def _check_battery_state(_battery_acpi_path):
"""
@return BatteryState
"""
rv = BatteryState()
if _battery_acpi_path.startswith('/proc'):
if os.access(_battery_acpi_path, os.F_OK):
o = slerp(_battery_acpi_path + '/state')
else:
raise Exception(_battery_acpi_path + ' does not exist')
batt_info = yaml.load(o)
state = batt_info.get('charging state', 'discharging')
rv.power_supply_status = state_to_val.get(state, 0)
rv.current = _strip_A(batt_info.get('present rate', '-1 mA'))
if rv.power_supply_status == BatteryState.POWER_SUPPLY_STATUS_DISCHARGING:
rv.current = math.copysign(
rv.current, -1) # Need to set discharging rate to negative
rv.charge = _strip_Ah(batt_info.get('remaining capacity',
'-1 mAh')) # /energy_now
rv.voltage = _strip_V(batt_info.get('present voltage',
'-1 mV')) # /voltage_now
rv.present = batt_info.get('present', False) # /present
rv.header.stamp = rospy.get_rostime()
else:
# Charging state; make lowercase and remove trailing eol
state = _read_string(_battery_acpi_path + '/status',
'discharging').lower().rstrip()
rv.power_supply_status = state_to_val.get(state, 0)
if os.path.exists(_battery_acpi_path + '/power_now'):
rv.current = _read_number(_battery_acpi_path + '/power_now')/10e5 / \
_read_number(_battery_acpi_path + '/voltage_now')
else:
rv.current = _read_number(_battery_acpi_path +
'/current_now') / 10e5
if rv.power_supply_status == BatteryState.POWER_SUPPLY_STATUS_DISCHARGING:
rv.current = math.copysign(
rv.current, -1) # Need to set discharging rate to negative
if os.path.exists(_battery_acpi_path + '/energy_now'):
rv.charge = _read_number(_battery_acpi_path + '/energy_now') / 10e5
else:
rv.charge = _read_number(_battery_acpi_path + '/charge_now') / 10e5
rv.voltage = _read_number(_battery_acpi_path + '/voltage_now') / 10e5
rv.present = _read_number(_battery_acpi_path + '/present') == 1
rv.header.stamp = rospy.get_rostime()
return rv
def get_battery_state(self, battery):
"""Get the current state of a battery."""
battery_config = BATTERIES[battery]
battery_state = BatteryState()
battery_voltage = PiPuckBatteryServer.get_battery_voltage(battery_config["path"])
self._battery_history[battery].insert(0, battery_voltage)
self._battery_history[battery] = self._battery_history[battery][:HISTORY_MAX]
split_point = len(self._battery_history[battery]) // 2
head_half = self._battery_history[battery][:split_point]
tail_half = self._battery_history[battery][split_point:]
try:
battery_delta = (sum(head_half) / len(head_half)) - (sum(tail_half) / len(tail_half))
except ZeroDivisionError:
battery_delta = 0.0
battery_state.voltage = battery_voltage
battery_state.present = True
battery_state.design_capacity = battery_config["design_capacity"]
battery_state.power_supply_technology = battery_config["power_supply_technology"]
average_battery_voltage = sum(self._battery_history[battery]) / len(
self._battery_history[battery])
if average_battery_voltage >= battery_config["max_voltage"] * CHARGED_VOLTAGE_MARGIN:
battery_state.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_FULL
elif battery_delta < 0:
battery_state.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
elif battery_delta > 0:
battery_state.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING
else:
battery_state.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING
if average_battery_voltage >= battery_config["max_voltage"] * OVER_VOLTAGE_MARGIN:
battery_state.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_OVERVOLTAGE
elif average_battery_voltage <= battery_config["min_voltage"]:
# It is unclear whether this means "out of charge" or "will never charge again", we
# assume here that it means "out of charge".
battery_state.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_DEAD
else:
battery_state.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_GOOD
battery_state.percentage = clamp(
(average_battery_voltage - battery_config["min_voltage"]) /
(battery_config["max_voltage"] - battery_config["min_voltage"]), 1.0, 0.0)
battery_state.current = NAN
battery_state.charge = NAN
battery_state.capacity = NAN
battery_state.location = battery_config["location"]
return battery_state
def main():
rospy.init_node("adc_reader")
battery_type=rospy.get_param("~type","Pb")
battery_volt=rospy.get_param("~V",12.0)
battery_design_capacity=rospy.get_param('~C',7) # capacity of battery in Ah
reading_pub = rospy.Publisher("energy/adc_raw",Readings,queue_size=10)
battery_pub = rospy.Publisher("energy/battery",BatteryState,queue_size=10)
hz = rospy.Rate(10)
rospy.loginfo('Reading ADS1x15 values, press Ctrl-C to quit...')
# Print nice channel column headers.
filters = []
cutoffs = rospy.get_param('~cutoffs',[4,4,4,4])
for i in range(4):
filters.append(lowpass.LowPass(10,cutoffs[i],10,order=3))
rospy.logdebug('| {0:>6} | {1:>6} | {2:>6} | {3:>6} |'.format(*range(4)))
rospy.logdebug('-' * 37)
battery_msg = BatteryState()
reading_msg = Readings()
try:
while not rospy.is_shutdown():
# Read all the ADC channel values in a list.
raw = np.zeros(4)
for i in range(4):
# Read the specified ADC channel using the previously set gain value.
#raw[i] = filters[i].update(adc.read_adc(i, gain=GAIN))
raw[i] = adc.read_adc(i, gain=GAIN)
# Note you can also pass in an optional data_rate parameter that controls
# the ADC conversion time (in samples/second). Each chip has a different
# set of allowed data rate values, see datasheet Table 9 config register
# DR bit values.
#values[i] = adc.read_adc(i, gain=GAIN, data_rate=128)
# Each value will be a 12 or 16 bit signed integer value depending on the
# ADC (ADS1015 = 12-bit, ADS1115 = 16-bit).
values = FACTORS*(raw)+ZEROS
for i in range(4):
values[i] = filters[i].update(values[i])
# Print the ADC values.
rospy.logdebug('| {0:>6} | {1:>6} | {2:>6} | {3:>6} |'.format(*values))
battery_msg.header.stamp = rospy.Time.now()
battery_msg.header.frame_id = 'adc'
battery_msg.voltage = values[3]
battery_msg.current = values[0]
battery_msg.design_capacity = battery_design_capacity
battery_msg.present = True
battery_pub.publish(battery_msg)
reading_msg.header = battery_msg.header
reading_msg.data = raw
reading_pub.publish(reading_msg)
hz.sleep()
except rospy.ROSInterruptException:
print("Exiting")
def update_state(self):
voltage_dec_, current_dec_, charge_dec_, percentage_dec_, temperature_dec_, power_supply_status_dec_, cell_voltage_dec_ = self.read_bms(
)
battery_msg = BatteryState()
# Power supply status constants
# uint8 POWER_SUPPLY_STATUS_UNKNOWN = 0
# uint8 POWER_SUPPLY_STATUS_CHARGING = 1
# uint8 POWER_SUPPLY_STATUS_DISCHARGING = 2
# uint8 POWER_SUPPLY_STATUS_NOT_CHARGING = 3
# uint8 POWER_SUPPLY_STATUS_FULL = 4
# Power supply health constants
# uint8 POWER_SUPPLY_HEALTH_UNKNOWN = 0
# uint8 POWER_SUPPLY_HEALTH_GOOD = 1
# uint8 POWER_SUPPLY_HEALTH_OVERHEAT = 2
# uint8 POWER_SUPPLY_HEALTH_DEAD = 3
# uint8 POWER_SUPPLY_HEALTH_OVERVOLTAGE = 4
# uint8 POWER_SUPPLY_HEALTH_UNSPEC_FAILURE = 5
# uint8 POWER_SUPPLY_HEALTH_COLD = 6
# uint8 POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE = 7
# uint8 POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE = 8
# Power supply technology (chemistry) constants
# uint8 POWER_SUPPLY_TECHNOLOGY_UNKNOWN = 0
# uint8 POWER_SUPPLY_TECHNOLOGY_NIMH = 1
# uint8 POWER_SUPPLY_TECHNOLOGY_LION = 2
# uint8 POWER_SUPPLY_TECHNOLOGY_LIPO = 3
# uint8 POWER_SUPPLY_TECHNOLOGY_LIFE = 4
# uint8 POWER_SUPPLY_TECHNOLOGY_NICD = 5
# uint8 POWER_SUPPLY_TECHNOLOGY_LIMN = 6
# Populate battery parameters.
battery_msg.voltage = voltage_dec_ # Voltage in Volts (Mandatory)
battery_msg.current = current_dec_ # Negative when discharging (A) (If unmeasured NaN)
battery_msg.charge = charge_dec_ # Current charge in Ah (If unmeasured NaN)
battery_msg.capacity = 150 # Capacity in Ah (last full capacity) (If unmeasured NaN)
battery_msg.design_capacity = 150 # Capacity in Ah (design capacity) (If unmeasured NaN)
battery_msg.percentage = percentage_dec_ # Charge percentage on 0 to 1 range (If unmeasured NaN)
battery_msg.power_supply_status = int(
power_supply_status_dec_
) # The charging status as reported. Values defined above
battery_msg.power_supply_health = 0 # The battery health metric. Values defined above
battery_msg.power_supply_technology = battery_msg.POWER_SUPPLY_TECHNOLOGY_LIFE # The battery chemistry. Values defined above
battery_msg.present = True # True if the battery is present
battery_msg.cell_voltage = cell_voltage_dec_
self.pub_batt_state.publish(battery_msg)
def battery_republisher():
global MotorDriverPort
#print "Battery republisher launched"
bat_status = BatteryState()
BAT_FULL = 6*4.2
BAT_MIN = 6*3.3
bat_status.header.frame_id = 'robot'
bat_status.current = float('nan')
bat_status.charge = float('nan')
bat_status.capacity = float('nan')
bat_status.design_capacity = float('nan')
bat_status.power_supply_technology = BatteryState().POWER_SUPPLY_TECHNOLOGY_LIPO
for cell in range(0, 6):
bat_status.cell_voltage.append(float('nan'))
bat_status.location = 'Main Battery'
bat_status.serial_number = "NA"
while 1:
MotorDriverPort.write('GetBatTotal\n')
sleep(0.05)
recv_data = MotorDriverPort.readline()
print recv_data
# print recv_data
bat_status.header.stamp = rospy.Time.now()
try:
bat_status.voltage = float(recv_data)
bat_status.percentage = bat_status.voltage/BAT_FULL
bat_status.present = bat_status.voltage<BAT_FULL and bat_status.voltage>BAT_MIN
battery_pub.publish(bat_status)
except:
if DEBUG:
print "Receive Error"
else:
pass
sleep(0.01)
def _create_battery_msg(self):
# Check if data is available
if 'SYS_STATUS' not in self.get_data():
raise Exception('no SYS_STATUS data')
if 'BATTERY_STATUS' not in self.get_data():
raise Exception('no BATTERY_STATUS data')
bat = BatteryState()
self._create_header(bat)
#http://docs.ros.org/jade/api/sensor_msgs/html/msg/BatteryState.html
bat.voltage = self.get_data()['SYS_STATUS']['voltage_battery'] / 1000
bat.current = self.get_data()['SYS_STATUS']['current_battery'] / 100
bat.percentage = self.get_data(
)['BATTERY_STATUS']['battery_remaining'] / 100
self.pub.set_data('/battery', bat)
def timerBatteryCB(self,event):
output = chr(0x5a) + chr(0x06) + chr(0x01) + chr(0x07) + chr(0x00) + chr(0xe4) #0xe4 is CRC-8 value
while(self.serialIDLE_flag):
time.sleep(0.01)
self.serialIDLE_flag = 3
try:
while self.serial.out_waiting:
pass
self.serial.write(output)
except:
rospy.logerr("Battery Command Send Faild")
self.serialIDLE_flag = 0
msg = BatteryState()
msg.header.stamp = self.current_time
msg.header.frame_id = self.baseId
msg.voltage = float(self.Vvoltage/1000.0)
msg.current = float(self.Icurrent/1000.0)
self.battery_pub.publish(msg)
def on_new_telemetry(self, message):
for servo in message.servos:
self.voltages[servo.id] = servo.voltage
if len(self.voltages) == SERVO_COUNT:
voltages = self.voltages.values()
self.voltages.clear()
voltage = max(voltages)
battery_state = BatteryState()
battery_state.header.stamp = rospy.Time.now()
battery_state.voltage = voltage
battery_state.current = float("nan")
battery_state.charge = float("nan")
battery_state.capacity = float("nan")
battery_state.design_capacity = float("nan")
battery_state.percentage = 100 - (MAX_VOLTAGE - voltage) / (
MAX_VOLTAGE - MIN_VOLTAGE) * 100
battery_state.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
battery_state.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNKNOWN
battery_state.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LIPO
battery_state.present = True
battery_state.cell_voltage = [float("nan")] * 3
battery_state.location = "Primary batter bay"
battery_state.serial_number = "N/A"
self.battery_publisher.publish(battery_state)
# skip the first check so that you don't get a warning if battery is already bellow some value
if self.first_check:
self.first_check = False
self.lowest_recorded_voltage = voltage
return
if voltage < 10.2:
if self.last_critical_voltage_warning + self.critical_voltage_warning_period < rospy.Time.now(
):
self.speech_publisher.publish("battery_critical")
self.face_color_publisher.publish("flash:red")
self.last_critical_voltage_warning = rospy.Time.now()
elif voltage < 11 and self.lowest_recorded_voltage >= 11:
self.speech_publisher.publish("battery_below_11")
elif voltage < 12 and self.lowest_recorded_voltage >= 12:
self.speech_publisher.publish("battery_below_12")
if voltage < self.lowest_recorded_voltage:
self.lowest_recorded_voltage = voltage
def publish_state(self):
state = BatteryState()
state.header.frame_id = "usv"
state.header.stamp = rospy.Time.now()
state.voltage = self.voltage
state.current = self.power_battery
state.charge = self.charging_current
#state.capacity = self.design_capacity * (self.percentage / 100.0)
state.design_capacity = self.design_capacity
state.percentage = (self.percentage/100.0)
state.power_supply_status = self.state_charging
state.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNKNOWN
state.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LIPO
state.present = True
state.cell_voltage = [self.voltage]
state.location = "Slot 1"
state.serial_number = "SUSV LIPO 3000mAH"
self.pub.publish(state)
def _create_battery_msg(self,topic):
""" Create battery message from ROV data
Raises:
Exception: No data available
"""
# Check if data is available
if 'SYS_STATUS' not in self.get_data():
raise Exception('no SYS_STATUS data')
if 'BATTERY_STATUS' not in self.get_data():
raise Exception('no BATTERY_STATUS data')
bat = BatteryState()
self._create_header(bat)
#http://docs.ros.org/jade/api/sensor_msgs/html/msg/BatteryState.html
bat.voltage = self.get_data()['SYS_STATUS']['voltage_battery']/1000
bat.current = self.get_data()['SYS_STATUS']['current_battery']/100
bat.percentage = self.get_data()['BATTERY_STATUS']['battery_remaining']/100
self.pub_topics[topic][3].publish(bat)
def batRead():
bat_dir = "/sys/devices/platform/7000c400.i2c/i2c-1/1-0042/iio_device/"
volt0_in = open(bat_dir + "in_voltage0_input")
curr0_in = open(bat_dir + "in_current0_input")
rospy.init_node('BatRead')
pub = rospy.Publisher('jetson_battery', BatteryState, queue_size=20)
rate = rospy.Rate(5) # 5hz
while not rospy.is_shutdown():
try:
# Read voltage in mV, store in V
voltage = float(volt0_in.read().strip()) / 1000
volt0_in.seek(0)
# Read voltage in mA, store in A
current = float(curr0_in.read().strip()) / 1000
curr0_in.seek(0)
except (IOError, ValueError) as e:
rospy.logerr("I/O error: {0}".format(e))
else:
bat_msg = BatteryState()
bat_msg.header.stamp = rospy.Time.now()
bat_msg.voltage = voltage
bat_msg.current = -current
bat_msg.charge = float('NaN')
bat_msg.capacity = float('NaN')
bat_msg.design_capacity = float('NaN')
bat_msg.percentage = float('NaN')
bat_msg.power_supply_status = bat_msg.POWER_SUPPLY_STATUS_UNKNOWN
bat_msg.power_supply_health = bat_msg.POWER_SUPPLY_HEALTH_UNKNOWN
bat_msg.power_supply_technology = bat_msg.POWER_SUPPLY_TECHNOLOGY_UNKNOWN
bat_msg.present = True
rospy.logdebug('New battery message: %s' % bat_msg)
pub.publish(bat_msg)
rate.sleep()
def test__on_receive_battery_state_over_threshold(self, mocked_rospy,
mocked_mqtt):
mocked_rospy.get_param.return_value = utils.get_attrs_params()
mocked_mqtt_client = mocked_mqtt.Client.return_value
with freezegun.freeze_time('2018-01-02T03:04:05.123456+09:00'):
bridge = AttrsBridge().connect()
battery = BatteryState()
battery.voltage = 0.1
battery.current = 0.2
battery.charge = 0.3
battery.capacity = 0.4
battery.design_capacity = 0.5
battery.percentage = 0.6
with freezegun.freeze_time('2018-01-02T03:04:06.123457+09:00'):
bridge._on_receive_battery_state(battery)
payload = '2018-01-02T03:04:06.123457+0900|voltage|0.1|current|0.2|charge|0.3|capacity|0.4|' \
'design_capacity|0.5|percentage|0.6'
mocked_mqtt_client.publish.assert_called_once_with(
'/robot/turtlebot3/attrs', payload)
def _hw_robot_state_cb(self, msg):
self._get_wallbanger_state_pub.publish(msg.in_autonomous_mode)
batteryMsg = BatteryState()
if msg.robot_power_state == RobotState.ESTOP_ACTIVE:
batteryMsg.voltage = float('nan')
batteryMsg.percentage = float('nan')
else:
batteryMsg.voltage = msg.voltage # volts
batteryMsg.percentage = max(0, min(msg.voltage / 12.0,
1)) # volts / volts-nominal
batteryMsg.current = float('nan')
batteryMsg.charge = float('nan')
batteryMsg.capacity = float('nan')
batteryMsg.design_capacity = 14.0 #AH (2x batteries)
batteryMsg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_UNKNOWN
batteryMsg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNKNOWN
batteryMsg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_UNKNOWN
batteryMsg.present = True
self._battery_pub.publish(batteryMsg)
self._watchdog_tripped_pub.publish(msg.watchdog_tripped)
strMsg = String()
if msg.robot_power_state == RobotState.POWER_GOOD:
strMsg.data = "power-good"
elif msg.robot_power_state == RobotState.POWER_LOW:
strMsg.data = "power-low"
elif msg.robot_power_state == RobotState.POWER_EMERGENCY:
strMsg.data = "power-emergency"
elif msg.robot_power_state == RobotState.ESTOP_ACTIVE:
strMsg.data = "estop-active"
self._robot_state_pub.publish(strMsg)
def _status_to_battery_(status):
"""
Converts a ds4_driver/Status to sensor_msgs/BatteryState
Reference: https://www.psdevwiki.com/ps4/DualShock_4#Specifications
:param status:
:type status: Status
:return:
"""
msg = BatteryState()
msg.header = status.header
msg.percentage = status.battery_percentage
msg.voltage = Controller.MAX_VOLTAGE * msg.percentage
msg.current = float('NaN')
msg.charge = float('NaN')
msg.capacity = float('NaN')
msg.design_capacity = 1.0
if not status.plug_usb:
msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING
elif not status.battery_full_charging:
msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING
elif status.battery_full_charging:
msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_FULL
msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LION
return msg
bsmsg.header.stamp = rospy.Time.now()
bsmsg.header.frame_id = 'base_link'
bsmsg.header.seq = count
count += 1
# voltage, current, charge capacity, design_capacity, percentage
for li in lines:
if "battery.capacity" in li:
bsmsg.capacity = float(li.split(' ')[-1])
elif "battery.charge" in li and not 'warning' in li and not 'low' in li:
bsmsg.charge = float(li.split(' ')[-1])
elif "battery.current" in li:
bsmsg.current = float(li.split(' ')[-1])
elif "battery.voltage" in li:
bsmsg.voltage = float(li.split(' ')[-1])
elif "device.serial" in li:
serial_number = str(li.split(' ')[-1])
elif "device.input_current" in li:
device_input_current = float(li.split(' ')[-1])
elif "device.output_current" in li:
device_output_current = float(li.split(' ')[-1])
elif "device.input_voltage" in li:
device_input_voltage = float(li.split(' ')[-1])
elif "device.output_voltage" in li:
device_output_voltage = float(li.split(' ')[-1])
try:
bsmsg.percentage = bmsg.charge / bmsg.capacity
def callback(self, timer):
# Simply print out values in our custom message.
batt_V = self.getBusVoltage_V(BATTERY_CHANNEL)
batt_A = self.getCurrent_A(BATTERY_CHANNEL)
total_W = batt_V * batt_A
pi4_V = self.getBusVoltage_V(PI4_CHANNEL)
pi4_A = self.getCurrent_A(PI4_CHANNEL)
pi4_W = pi4_V * pi4_A
#
pi4shunt_V = self.getShuntVoltage_V(PI4_CHANNEL)
#
power_file = open(
"/sys/bus/i2c/drivers/ina3221x/6-0040/iio_device/in_power0_input",
"r")
jetson_W = int(power_file.readline().rstrip()) / 1000
power_file.close()
rospy.loginfo(
rospy.get_name() +
" PI4 bus: {:.2f} V, PI4: {:.2f} V / {:.2f} A, Jetson {:.2f} W".
format(pi4_V, pi4_V - pi4shunt_V, pi4_A, jetson_W))
#
# Publish battery message
batteryState = BatteryState()
batteryState.voltage = batt_V
batteryState.current = batt_A
batteryState.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_UNKNOWN
self.pub.publish(batteryState)
#
# Log battery status
current_time = rospy.Time.now()
if (current_time >
self.last_log + rospy.Duration.from_sec(LOG_INTERVAL)):
self.last_log = current_time
if batt_V < MIN_BATT_VOLTAGE:
rospy.logwarn(
rospy.get_name() +
" Battery voltage critically low: {:.2f}".format(batt_V))
else:
rospy.loginfo(rospy.get_name() +
" Battery voltage ok: {:.2f}".format(batt_V))
# Send battery image
if (current_time >
self.last_blink + rospy.Duration.from_sec(BLINK_INTERVAL)):
self.last_blink = current_time
self.blink_flag = not self.blink_flag
img_color = WHITE_BGR
text_color = BLACK_BGR
if batt_V < MIN_BATT_VOLTAGE:
text_color = RED_BGR
if self.blink_flag:
img_color = ORANGE_BGR
img = 0xff * np.ones(shape=[270, 400, 3], dtype=np.uint8)
img[:] = img_color
cv2.putText(img, "Batt: {:.1f} V".format(batt_V), (25, 50),
cv2.FONT_HERSHEY_SIMPLEX, 1.5, text_color, 3)
cv2.putText(img, "PI4: {:.1f} W".format(pi4_W), (25, 120),
cv2.FONT_HERSHEY_SIMPLEX, 1.5, BLACK_BGR, 2)
cv2.putText(img, "Jetson: {:.1f} W".format(jetson_W), (25, 170),
cv2.FONT_HERSHEY_SIMPLEX, 1.5, BLACK_BGR, 2)
cv2.putText(img, "Total: {:.1f} W".format(total_W), (25, 220),
cv2.FONT_HERSHEY_SIMPLEX, 1.5, BLACK_BGR, 2)
self.image_pub.publish(self.bridge.cv2_to_imgmsg(img))
return
def driverNode():
rospy.init_node('~', anonymous=False)
kinematics_type = rospy.get_param('~wheel_type', "classic")
odom_frame = rospy.get_param('~odom_frame', "odom")
base_link_frame = rospy.get_param('~base_link_frame', "base_link")
publish_tf = rospy.get_param('~publish_tf', True)
publish_odometry = rospy.get_param('~publish_odometry', False)
publish_pose = rospy.get_param('~publish_pose', True)
RK = factory(kinematics_type)
br = tf2_ros.TransformBroadcaster()
tf = TransformStamped()
battery_publisher = rospy.Publisher('battery', BatteryState, queue_size=1)
battery_msg = BatteryState()
joint_state_publisher = rospy.Publisher('joint_states',
JointState,
queue_size=1)
joint_state_msg = JointState()
joint_state_msg.header.frame_id = base_link_frame
joint_state_msg.name = [
'front_left', 'front_right', 'rear_left', 'rear_right'
]
if publish_pose == True:
pose_publisher = rospy.Publisher('pose', Pose, queue_size=1)
pose_msg = Pose()
pose_msg.position.x = 0
pose_msg.position.y = 0
pose_msg.position.z = 0
pose_msg.orientation.x = 0
pose_msg.orientation.y = 0
pose_msg.orientation.z = 0
pose_msg.orientation.w = 1
if publish_odometry == True:
odom_publisher = rospy.Publisher('odom/wheel', Odometry, queue_size=1)
odom_msg = Odometry()
rospy.Subscriber("/cmd_vel", Twist, RK.cmdVelCallback, queue_size=1)
can_interface = rospy.get_param('~can_interface', 'panther_can')
RK.robot_width = rospy.get_param('~robot_width', 0.682)
RK.robot_length = rospy.get_param('~robot_length', 0.44)
RK.wheel_radius = rospy.get_param('~wheel_radius', 0.1825)
RK.encoder_resolution = rospy.get_param('~encoder_resolution',
30 * 400 * 4)
RK.power_factor = rospy.get_param('~power_factor', 0.04166667)
if rospy.has_param('~eds_file'):
eds_file = rospy.get_param('~eds_file')
else:
rospy.logerr("eds_file not defined, can not start CAN interface")
return
loop_rate = 50
rate = rospy.Rate(loop_rate)
rospy.loginfo("Start with creating a network representing one CAN bus")
network = canopen.Network()
rospy.loginfo("Add some nodes with corresponding Object Dictionaries")
front_controller = canopen.RemoteNode(1, eds_file)
rear_controller = canopen.RemoteNode(2, eds_file)
network.add_node(front_controller)
network.add_node(rear_controller)
rospy.loginfo("Connect to the CAN bus")
network.connect(channel=can_interface, bustype='socketcan')
# rospy.loginfo("Reset encoders")
# front_controller.sdo['Cmd_SENCNTR'][1].raw = 0
# front_controller.sdo['Cmd_SENCNTR'][2].raw = 0
robot_x_pos = 0.0
robot_y_pos = 0.0
robot_th_pos = 0.0
wheel_FL_ang_pos_last = 0.0
wheel_FR_ang_pos_last = 0.0
wheel_RL_ang_pos_last = 0.0
wheel_RR_ang_pos_last = 0.0
last_time = rospy.Time.now()
while not rospy.is_shutdown():
try:
rospy.get_master().getPid()
except:
rospy.logerr("Error getting master")
exit(1)
try:
now = rospy.Time.now()
dt_ = (now - last_time).to_sec()
last_time = now
if ((now - RK.cmd_vel_command_time) > rospy.Duration(secs=0.2)):
RK.FL_enc_speed = 0.0
RK.FR_enc_speed = 0.0
RK.RL_enc_speed = 0.0
RK.RR_enc_speed = 0.0
try:
front_controller.sdo['Cmd_CANGO'][2].raw = RK.FL_enc_speed
except:
rospy.logwarn("Error while writing to front left Cmd_CANGO")
try:
front_controller.sdo['Cmd_CANGO'][1].raw = RK.FR_enc_speed
except:
rospy.logwarn("Error while writing to front right Cmd_CANGO")
try:
rear_controller.sdo['Cmd_CANGO'][2].raw = RK.RL_enc_speed
except:
rospy.logwarn("Error while writing to rear left Cmd_CANGO")
try:
rear_controller.sdo['Cmd_CANGO'][1].raw = RK.RR_enc_speed
except:
rospy.logwarn("Error while writing to rear right Cmd_CANGO")
try:
battery_msg.voltage = float(
front_controller.sdo[0x210D][2].raw) / 10
battery_msg.current = float(
front_controller.sdo['Qry_BATAMPS'][1].raw) / 10
battery_publisher.publish(battery_msg)
except:
rospy.logwarn("Error getting battery data")
# inverse kinematics
try:
position_FL = front_controller.sdo['Qry_ABCNTR'][2].raw
except:
rospy.logwarn("Error reading front left controller sdo")
try:
position_FR = front_controller.sdo['Qry_ABCNTR'][1].raw
except:
rospy.logwarn("Error reading front right controller sdo")
try:
position_RL = rear_controller.sdo['Qry_ABCNTR'][2].raw
except:
rospy.logwarn("Error reading rear left controller sdo")
try:
position_RR = rear_controller.sdo['Qry_ABCNTR'][1].raw
except:
rospy.logwarn("Error reading rear right controller sdo")
# position_FL / RK.encoder_resolution - > full wheel rotations
wheel_FL_ang_pos = 2 * math.pi * position_FL / RK.encoder_resolution # radians
wheel_FR_ang_pos = 2 * math.pi * position_FR / RK.encoder_resolution
wheel_RL_ang_pos = 2 * math.pi * position_RL / RK.encoder_resolution
wheel_RR_ang_pos = 2 * math.pi * position_RR / RK.encoder_resolution
wheel_FL_ang_vel = (wheel_FL_ang_pos -
wheel_FL_ang_pos_last) * dt_ # rad/s
wheel_FR_ang_vel = (wheel_FR_ang_pos - wheel_FR_ang_pos_last) * dt_
wheel_RL_ang_vel = (wheel_RL_ang_pos - wheel_RL_ang_pos_last) * dt_
wheel_RR_ang_vel = (wheel_RR_ang_pos - wheel_RR_ang_pos_last) * dt_
wheel_FL_ang_pos_last = wheel_FL_ang_pos
wheel_FR_ang_pos_last = wheel_FR_ang_pos
wheel_RL_ang_pos_last = wheel_RL_ang_pos
wheel_RR_ang_pos_last = wheel_RR_ang_pos
try:
robot_x_pos, robot_y_pos, robot_th_pos = RK.inverseKinematics(
wheel_FL_ang_vel, wheel_FR_ang_vel, wheel_RL_ang_vel,
wheel_RR_ang_vel, dt_)
except:
rospy.logwarn("Couldn't get robot pose")
# rospy.loginfo("Robot pos: [x, y, th]: [%0.3f, %0.3f, %0.3f]" % (robot_x_pos, robot_y_pos, robot_th_pos*180/3.14))
RK.wheels_angular_velocity = [
wheel_FL_ang_vel, wheel_FR_ang_vel, wheel_RR_ang_vel,
wheel_RL_ang_vel
]
qx, qy, qz, qw = eulerToQuaternion(robot_th_pos, 0, 0)
if publish_pose == True:
pose_msg.position.x = robot_x_pos
pose_msg.position.y = robot_y_pos
pose_msg.orientation.x = qx
pose_msg.orientation.y = qy
pose_msg.orientation.z = qz
pose_msg.orientation.w = qw
pose_publisher.publish(pose_msg)
if publish_tf == True:
tf.header.stamp = rospy.Time.now()
tf.header.frame_id = odom_frame
tf.child_frame_id = base_link_frame
tf.transform.translation.x = robot_x_pos
tf.transform.translation.y = robot_y_pos
tf.transform.translation.z = 0.0
tf.transform.rotation.x = qx
tf.transform.rotation.y = qy
tf.transform.rotation.z = qz
tf.transform.rotation.w = qw
br.sendTransform(tf)
if publish_odometry == True:
odom_msg.header.frame_id = odom_frame
odom_msg.header.stamp = now
odom_msg.pose.pose.position.x = robot_x_pos
odom_msg.pose.pose.position.y = robot_y_pos
odom_msg.pose.pose.orientation.x = qx
odom_msg.pose.pose.orientation.y = qy
odom_msg.pose.pose.orientation.z = qz
odom_msg.pose.pose.orientation.w = qw
odom_msg.twist.twist.linear.x = 0
odom_msg.twist.twist.linear.y = 0
odom_msg.twist.twist.linear.z = 0
odom_msg.twist.twist.angular.x = 0
odom_msg.twist.twist.angular.y = 0
odom_msg.twist.twist.angular.z = 0
odom_publisher.publish(odom_msg)
except:
rospy.logerr("CAN protocol error")
rate.sleep()
log_idx = 0
bridge = CvBridge()
while not rospy.is_shutdown():
# Publish lat,lon
navmsg = NavSatFix()
navmsg.latitude = np.random.uniform(-33.0,-32.0)
navmsg.longitude = np.random.uniform(151.0,152.0)
navmsg.altitude = np.random.uniform(-5.0,0.0)
navpub.publish(navmsg)
# Publish battery state
batmsg = BatteryState()
batmsg.voltage = np.random.uniform(14.5,15.5)
batmsg.current = np.random.uniform(0.0, 2.0)
batmsg.capacity = np.random.uniform(0.0, 1.0)
battery_pub.publish(batmsg)
# Depth
vmsg = Vector3Stamped()
vmsg.header.frame_id = 'header'
vmsg.header.seq = count
vmsg.header.stamp = rospy.Time.now()
vmsg.vector.z = np.random.uniform(0, 5.0)
depth_pub.publish(vmsg)
# Image
path="/home/auv/Pictures/sirius.png"
img = cv2.imread(path, 1)
img = img.astype(np.uint8)
def spin(self):
encoders = [0, 0]
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
then = rospy.Time.now()
# things that don't ever change
scan_link = rospy.get_param('~frame_id', 'base_laser_link')
scan = LaserScan(header=rospy.Header(frame_id=scan_link))
scan.angle_min = 0.0
scan.angle_max = 359.0 * pi / 180.0
scan.angle_increment = pi / 180.0
scan.range_min = 0.020
scan.range_max = 5.0
odom = Odometry(header=rospy.Header(frame_id="odom"),
child_frame_id='base_footprint')
# main loop of driver
r = rospy.Rate(20)
cycle_count = 0
self.bumperEngaged = None
while not rospy.is_shutdown():
# Emergency shutdown checks.
if int(self.chargerValues["FuelPercent"]) < 10:
rospy.logerr(
"Neato battery is less than 10%. Terminating Node")
rospy.signal_shutdown(
"Neato battery is less than 10%. Terminating Node")
break
if self.chargerValues["BatteryFailure"] == "1":
rospy.logerr("Neato battery failure. Terminating Node")
rospy.signal_shutdown(
"Neato battery failure. Terminating Node")
break
if self.chargerValues["EmptyFuel"] == "1":
rospy.logerr("Neato battery is empty. Terminating Node")
break
# get motor encoder values
left, right = self.getMotors()
if not self.lifted and cycle_count % 2 == 0:
# bumper engaged procedure
# left or right bumpers
if self.moving_forward and self.bumperEngaged == 0:
# left bump
self.setMotors(-100, -110, MAX_SPEED / 2)
if self.moving_forward and self.bumperEngaged == 1:
# right bump
self.setMotors(-110, -100, MAX_SPEED / 2)
# all other bumpers
elif self.moving_forward and self.bumperEngaged > 1:
self.setMotors(-100, -100, MAX_SPEED / 2)
# undock proceedure
if self.cmd_vel[0] and self.chargerValues["ChargingActive"]:
self.setMotors(-400, -400, MAX_SPEED / 2)
else:
# send updated movement commands
self.setMotors(
self.cmd_vel[0], self.cmd_vel[1],
max(abs(self.cmd_vel[0]), abs(self.cmd_vel[1])))
self.old_vel = self.cmd_vel
# prepare laser scan
scan.header.stamp = rospy.Time.now()
self.getldsscan()
scan.ranges, scan.intensities = self.getScanRanges()
# now update position information
dt = (scan.header.stamp - then).to_sec()
then = scan.header.stamp
d_left = (left - encoders[0]) / 1000.0
d_right = (right - encoders[1]) / 1000.0
encoders = [left, right]
dx = (d_left + d_right) / 2
dth = (d_right - d_left) / (self.base_width / 1000.0)
x = cos(dth) * dx
y = -sin(dth) * dx
self.x += cos(self.th) * x - sin(self.th) * y
self.y += sin(self.th) * x + cos(self.th) * y
self.th += dth
# prepare tf from base_link to odom
quaternion = Quaternion()
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# prepare odometry
odom.header.stamp = rospy.Time.now()
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = dx / dt
odom.twist.twist.angular.z = dth / dt
# read sensors and data
# Neato cannot handle reads of all sensors every cycle.
# use cycle_count to rate limit the reads or
# you will get errors like:
# navigation costmap2DROS transform timeout.
# Could not get robot pose.
if cycle_count % 2 == 0:
self.getDigitalSensors()
for i, b in enumerate(
("LSIDEBIT", "RSIDEBIT", "LFRONTBIT", "RFRONTBIT")):
engaged = None
engaged = self.digitalSensors[b] # Bumper Switches
self.bumperHandler(b, engaged, i)
if cycle_count == 2:
self.getAnalogSensors()
for i, b in enumerate(("LeftDropInMM", "RightDropInMM",
"LeftMagSensor", "RightMagSensor")):
engaged = None
if i < 2:
# Optical Sensors (no drop: ~0-60)
engaged = (self.analogSensors[b] > 100)
else:
# Mag Sensors (no mag: ~ +/-20)
engaged = (abs(self.analogSensors[b]) > 20)
self.bumperHandler(b, engaged, i)
if cycle_count == 1:
self.getButtons()
# region Publish Button Events
for i, b in enumerate(
("BTN_SOFT_KEY", "BTN_SCROLL_UP", "BTN_START", "BTN_BACK",
"BTN_SCROLL_DOWN")):
engaged = (self.buttons[b] == 1)
if engaged != self.state[b]:
buttonEvent = ButtonEvent()
buttonEvent.button = i
buttonEvent.engaged = engaged
self.buttonEventPub.publish(buttonEvent)
self.state[b] = engaged
# endregion Publish Button Info
if cycle_count == 3:
self.getCharger()
# region Publish Battery Info
# pulls data from analogSensors and charger info to publish battery state
battery = BatteryState()
# http://docs.ros.org/en/api/sensor_msgs/html/msg/BatteryState.html
power_supply_health = 1 # POWER_SUPPLY_HEALTH_GOOD
if self.chargerValues["BatteryOverTemp"]:
power_supply_health = 2 # POWER_SUPPLY_HEALTH_OVERHEAT
elif self.chargerValues["EmptyFuel"]:
power_supply_health = 3 # POWER_SUPPLY_HEALTH_DEAD
elif self.chargerValues["BatteryFailure"]:
power_supply_health = 5 # POWER_SUPPLY_HEALTH_UNSPEC_FAILURE
power_supply_status = 3 # POWER_SUPPLY_STATUS_NOT_CHARGING
if self.chargerValues["ChargingActive"]:
power_supply_status = 1 # POWER_SUPPLY_STATUS_CHARGING
elif (self.chargerValues["FuelPercent"] == 100):
power_supply_status = 4 # POWER_SUPPLY_STATUS_FULL
battery.voltage = self.analogSensors[
"BatteryVoltageInmV"] // 1000
# battery.temperature = self.analogSensors["BatteryTemp0InC"]
battery.current = self.analogSensors["CurrentInmA"] // 1000
# battery.charge
# battery.capacity
# battery.design_capacity
battery.percentage = self.chargerValues["FuelPercent"]
battery.power_supply_status = power_supply_status
battery.power_supply_health = power_supply_health
battery.power_supply_technology = 1 # POWER_SUPPLY_TECHNOLOGY_NIMH
battery.present = self.chargerValues['FuelPercent'] > 0
# battery.cell_voltage
# battery.cell_temperature
# battery.location
# battery.serial_number
self.batteryPub.publish(battery)
# endregion Publish Battery Info
self.publishSensors()
# region publish lidar and odom
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w), then,
"base_footprint", "odom")
self.scanPub.publish(scan)
self.odomPub.publish(odom)
# endregion publish lidar and odom
# wait, then do it again
r.sleep()
cycle_count = cycle_count + 1
if cycle_count == 4:
cycle_count = 0
# shut down
self.setLed(LED.BacklightOff)
self.setLed(LED.ButtonOff)
self.setLdsRotation("Off")
self.testmode("Off")
def bs_err_inj(tb3_name):
#Create error-injected topic
rospy.init_node('batterystate_err_inj')
#########################################
#Create new message
batterystate_msg = BatteryState()
#Fill message with values
batterystate_msg.header.seq = 0
batterystate_msg.header.stamp.secs = 0
batterystate_msg.header.stamp.nsecs = 0
batterystate_msg.header.frame_id = ""
batterystate_msg.voltage = 0.0
batterystate_msg.current = 0.0
batterystate_msg.charge = 0.0
batterystate_msg.capacity = 0.0
batterystate_msg.design_capacity = 0.0
batterystate_msg.percentage = 0.0
batterystate_msg.power_supply_status = 0
batterystate_msg.power_supply_health = 0
batterystate_msg.power_supply_technology = 0
batterystate_msg.bool = 1
batterystate_msg.cell_voltage = []
batterystate_msg.location = ""
batterystate_msg.serial_number = ""
#########################################
rate = rospy.Rate(50)
#Publish message into new topic
while not rospy.is_shutdown():
my_pub = rospy.Publisher(tb3_name + 'batterystate_err_inj',
BatteryState,
queue_size=10)
my_sub = rospy.Subscriber(tb3_name + 'battery_state', BatteryState,
listener)
#########################################
#INJECT ERRORS HERE
batterystate_msg.header.seq = actual_seq
batterystate_msg.header.stamp.secs = actual_secs
batterystate_msg.header.stamp.nsecs = actual_nsecs
batterystate_msg.header.frame_id = actual_frameid
batterystate_msg.voltage = actual_voltage
batterystate_msg.current = actual_current
batterystate_msg.charge = actual_charge
batterystate_msg.capacity = actual_capacity
batterystate_msg.design_capacity = actual_designcapacity
batterystate_msg.percentage = actual_percentage
batterystate_msg.power_supply_status = actual_powersupplystatus
batterystate_msg.power_supply_health = actual_powersupplyhealth
batterystate_msg.power_supply_technology = actual_powersupplytechnology
batterystate_msg.present = actual_present
batterystate_msg.cell_voltage = actual_cellvoltage
batterystate_msg.location = actual_location
batterystate_msg.serial_number = actual_serialnumber
#########################################
my_pub.publish(batterystate_msg)
rate.sleep()
rospy.spin()