+ if volt < 7 and (rospy.Time.now() - self.volt_last_warn).to_sec() > 10:
+ rospy.logerr("Voltage critical: %.2fV" % (volt))
+ self.volt_last_warn = rospy.Time.now()
+
+ self.bDocked = volt > 10.1
+ self.update_capacity(volt, current)
+
+ if self.pub_battery.get_num_connections() > 0:
+ batmsg = BatteryState()
+ batmsg.header.stamp = rospy.Time.now()
+ batmsg.voltage = volt
+ batmsg.current = current
+ batmsg.charge = self.battery_capacity/3600.0
+ batmsg.capacity = float('nan')
+ batmsg.design_capacity = BATTERY_CAPACITY_FULL
+ batmsg.percentage = batmsg.charge/batmsg.design_capacity
+ batmsg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
+ batmsg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNKNOWN
+ batmsg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_NIMH
+ batmsg.present = True
+ self.pub_battery.publish(batmsg)
+
+ def update_capacity(self, volt, current):
+ if self.bDocked:
+ self.battery_capacity = BATTERY_CAPACITY_FULL*3600
+ else:
+ self.battery_capacity -= current/UPDATE_RATE