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 looping(self): rate = rospy.Rate(50) while not rospy.is_shutdown(): # Generate Commands LEDs = np.array([0, 0, 0, 0, 0, 0, 1, 1]) self.command = np.array([self.throttle, self.steering]) current, batteryVoltage, encoderCounts = self.myCar.read_write_std(self.command, LEDs) battery_state = BatteryState() battery_state.header.stamp = rospy.Time.now() battery_state.header.frame_id = 'battery_voltage' battery_state.voltage = batteryVoltage self.battery_pub_.publish(battery_state) longitudinal_car_speed = basic_speed_estimation(encoderCounts) velocity_state = Vector3Stamped() velocity_state.header.stamp = rospy.Time.now() velocity_state.header.frame_id = 'car_velocity' velocity_state.vector.x = float(np.cos(self.command[1]) * longitudinal_car_speed) velocity_state.vector.y = float(np.sin(self.command[1]) * longitudinal_car_speed) self.carvel_pub_.publish(velocity_state) rate.sleep() self.myCar.terminate()
def callback(self, msg):
rpi_battery_msg = BatteryState()
voltages = [msg.cell1, msg.cell2, msg.cell3, msg.cell4]
rpi_battery_msg.header.stamp = rospy.Time.now()
rpi_battery_msg.voltage = sum(voltages)
rpi_battery_msg.cell_voltage = voltages
rpi_battery_msg.present = True
self.bridge_pub.publish(rpi_battery_msg)
def PublishStatus(self):
battery_state = BatteryState()
# Read the battery voltage
try:
battery_state.voltage = self.__thunderborg.GetBatteryReading()
self.__status_pub.publish(battery_state)
except:
rospy.logwarn("Thunderborg node: Failed to read battery voltage")
def talker():
pub = rospy.Publisher("test_topic_ufro", BatteryState, queue_size=10)
rospy.init_node('publisher_ufro', anonymous=True)
rate = rospy.Rate(1) # 10hz
while not rospy.is_shutdown():
battery = BatteryState()
battery.voltage = random.random() * 10
rospy.loginfo(battery.voltage)
pub.publish(battery)
rate.sleep()
def publish_battery_state_msg(self):
battery_state_msg = BatteryState()
battery_state_msg.header.stamp = rospy.get_rostime()
battery_state_msg.voltage = self.battery_mv / 1000.0
# estimate remaining battery life
# 1.2v * 6 batteries = 7.2 v
# 6v is depletion point
battery_state_msg.percentage = (battery_state_msg.voltage - 6.0) / 1.2
battery_state_msg.present = True if self.battery_mv > 0 else False
self.battery_pub.publish(battery_state_msg)
def _publish_battery(self):
battery = BatteryState()
battery.header.stamp = rospy.Time.now()
battery.voltage = self._cozmo.battery_voltage
battery.present = True
if self._cozmo.is_on_charger: # is_charging always return False
battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING
else:
battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING
self._battery_pub.publish(battery)
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 timerBatCB(self, event):
try:
data = self.data
if len(data) == 8:
voltage = float( int(data[6].encode('hex'),16)*256 \
+ int(data[7].encode('hex'),16) ) / 100.0
else:
print("Received wrong data")
bat = BatteryState()
bat.voltage = voltage
self.bat_pub.publish(bat)
except:
print('publish battery sate error')
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 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 _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 publish_battery_state_msg(publisher):
# The Romi is powered by six 1.5V 2400 mAh AA NiMH batteries. Their combined
# operating voltage is about 7.2V.
# Define the message class for battery_state_msg:
battery_state_msg = BatteryState()
# Time tag:
battery_state_msg.header.stamp = rospy.get_rostime()
# Capacity in amp-hours:
battery_state_msg.design_capacity = 2.4
# Read the battery voltage over the I2C interface:
battery_state_msg.voltage = romi.read_battery_millivolts() / 1000.0
if( romi.read_battery_millivolts() >= 0.00 ):
battery_state_msg.present = True
else:
battery_state_msg.present = False
# Publish the message
publisher.publish(battery_state_msg)
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 _publish_battery(self):
"""
Publish battery as BatteryState message.
"""
# only publish if we have a subscriber
if self._battery_pub.get_num_connections() == 0:
return
battery = BatteryState()
battery.header.stamp = rospy.Time.now()
battery.voltage = self._vector.get_battery_state().battery_volts
battery.present = True
if self._vector.get_battery_state().is_on_charger_platform:
battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING
else:
battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING
self._battery_pub.publish(battery)
def _publish_battery(self):
"""
Publish battery as BatteryState message.
"""
## TODO: use get_num_connections when rclpy supports it
# only publish if we have a subscriber
#if self._battery_pub.get_num_connections() == 0:
# return
battery = BatteryState()
#battery.header.stamp = rospy.Time.now()
battery.header.stamp = TimeStamp.now()
battery.voltage = self._cozmo.battery_voltage
battery.present = True
if self._cozmo.is_on_charger: # is_charging always return False
battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING
else:
battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING
self._battery_pub.publish(battery)
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 _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.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
except:
pass
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()
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()
def battery_callback(voltage):
state = BatteryState()
state.voltage = voltage
battery_pub.publish(state)
count = 0
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)
def publish_state(self):
current_time = rospy.Time.now()
self.encoder_left = self.motor_driver.encoder1_get()
self.encoder_right = self.motor_driver.encoder2_get()
self.battery_voltage = self.motor_driver.bat_voltage_get()
msg_battery = BatteryState()
msg_battery.voltage = self.battery_voltage
msg_battery.power_supply_status = 2
msg_battery.power_supply_health = 1
msg_battery.power_supply_technology = 0
msg_battery.present = True
msg_left = JointState()
msg_left.header.frame_id = self.joint_frames[0] # + "_hinge"
msg_left.header.stamp = current_time
msg_left.header.seq = self.seq
msg_right = JointState()
msg_right.header.frame_id = self.joint_frames[1] # + "_hinge"
msg_right.header.stamp = current_time
msg_right.header.seq = self.seq
self.encoder_left = self.encoder_left
self.encoder_right = self.encoder_right
# print("Left",left,"Right",right)
msg_left.name.append(self.joint_frames[0] +
"_hinge") # 360 ticks/ per wheel turn
msg_left.position.append(
self.encoder_left) # 360 ticks/ per wheel turn
msg_right.name.append(self.joint_frames[1] + "_hinge")
msg_right.position.append(
self.encoder_right) # 360 ticks/ per wheel turn
self.pub_joint_right.publish(msg_right)
self.pub_joint_left.publish(msg_left)
self.pub_battery_state.publish(msg_battery)
# extract the wheel velocities from the tick signals count
#
if (current_time - self.past_time).to_sec() != 0 and (
self.encoder_left != self._PreviousLeftEncoderCounts
or self.encoder_right != self._PreviousRightEncoderCounts):
msg_odom = Odometry()
# --- Get the distance delta since the last period ---
deltaLeft = (self.encoder_left - self._PreviousLeftEncoderCounts
) * self.distance_per_count
deltaRight = (self.encoder_right - self._PreviousRightEncoderCounts
) * self.distance_per_count
# --- Update the local position and orientation ---
self.local_pose["x"] = (
deltaLeft + deltaRight) / 2.0 # distance in X direction
self.local_pose["y"] = 0.0
# distance in Y direction
self.local_pose["th"] = (
deltaRight - deltaLeft
) / self.length_between_two_wheels # Change in orientation
if -360 > self.local_pose["th"] or self.local_pose["th"] > 360:
return
# self.yaw += self.local_pose["th"]
# --- Update the velocity ---
leftDistance = (deltaLeft - self.pos_left_past)
rightDistance = (deltaRight - self.pos_right_past)
delta_distance = (leftDistance + rightDistance) / 2.0
delta_theta = (rightDistance - leftDistance
) / self.length_between_two_wheels # in radians
self.local_vel["x_lin"] = delta_distance / (
current_time - self.past_time).to_sec() # Linear x velocity
self.local_vel["y_lin"] = 0.0
self.local_vel["y_ang"] = (
delta_theta / (current_time - self.past_time).to_sec()
) # In radians per/sec
odom_quat = tf.transformations.quaternion_from_euler(
0, 0, self.local_pose["th"])
# first, we'll publish the transform over tf
# send the transform
self.odom_broadcaster.sendTransform(
(self.local_pose["x"], self.local_pose["y"], 0.), odom_quat,
current_time, self.base_link_topic[1:], self.odom_topic[1:])
msg_odom.header.stamp = current_time
msg_odom.header.frame_id = self.odom_topic[1:]
msg_odom.header.seq = self.seq
# set the position
msg_odom.pose.pose = Pose(
Point(self.local_pose["x"], self.local_pose["y"], 0.),
Quaternion(*odom_quat))
# set the velocity
msg_odom.child_frame_id = self.base_link_topic[1:]
msg_odom.twist.twist = Twist(
Vector3(self.local_vel["x_lin"], self.local_vel["y_lin"], 0),
Vector3(0, 0, self.local_vel["y_ang"]))
# publish the message
self.odom_pub.publish(msg_odom)
self.seq += 1
# --- Save the last position values ---
self.pos_left_past = deltaLeft
self.pos_right_past = deltaRight
self.past_time = current_time
self._PreviousLeftEncoderCounts = self.encoder_left
self._PreviousRightEncoderCounts = self.encoder_right
# initializes node
rospy.init_node("r1_publisher", anonymous=False)
# starts subscribers and publishers
rospy.Subscriber("/hospital/vital_signal", String, upload)
# rospy.Subscriber("/aramis/pose", Odometry, update_pose1)
print('subscribers on!')
speaker_pub = rospy.Publisher("/R1/speaker", String, queue_size=10)
batt_pub = rospy.Publisher("/R1/battery", BatteryState, queue_size=10)
signal_pub = rospy.Publisher("/R1/vital_signal", String, queue_size=10)
signal_pub = rospy.Publisher("/R1/vital_signal", String, queue_size=10)
print('publishers on!')
rate = rospy.Rate(10)
spk = String()
spk.data = "Hello World Hospital"
batt = BatteryState()
batt.voltage = 12.06
signal = String()
signal.data = "Its alive!"
while not rospy.is_shutdown():
speaker_pub.publish(spk)
batt_pub.publish(batt)
signal_pub.publish(signal)
rate.sleep()
