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 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 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 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()
# print out pure ADC voltage
# rospy.loginfo("Battery: %.1f Volt" % voltage)
# completing the ROS message
# @SA http://docs.ros.org/jade/api/sensor_msgs/html/msg/BatteryState.html
battery_msg.charge = 0.0
battery_msg.capacity = 0.0
battery_msg.design_capacity = 2.2 # 2.2 Ah
battery_msg.percentage = 0.0 # 0 to 1!
# battery_msg.power_supply_status = POWER_SUPPLY_STATUS_DISCHARGING
# battery_msg.power_supply_health = POWER_SUPPLY_HEALTH_GOOD
# battery_msg.power_supply_technology = POWER_SUPPLY_TECHNOLOGY_LIPO
battery_msg.power_supply_status = 2
battery_msg.power_supply_health = 1
battery_msg.power_supply_technology = 3
battery_msg.present = True
battery_msg.cell_voltage = [float(0)]
battery_msg.location = "1" # The location into which the battery is inserted. (slot number or plug)
battery_msg.serial_number = "1"
# this is the battery voltage
# @TODO strange, this assignment as to be exactly here...
battery_msg.voltage = float(voltage)
# publish voltage
pubBattery.publish(battery_msg)
# Sleep for a second until the next reading.
rospy.sleep(sleepTime)
