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 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 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 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
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()
#!/usr/bin/env python
# license removed for brevity
#import roslib
import rospy
import struct
#from std_msgs.msg import Int32
from std_msgs.msg import String
from std_msgs.msg import Float64
from sensor_msgs.msg import BatteryState
import serial
ser = serial.Serial('/dev/ttyUSB1', 19200)
bat = BatteryState()
bat.design_capacity = 105
# Power supply status constants
bat.POWER_SUPPLY_STATUS_UNKNOWN
bat.POWER_SUPPLY_STATUS_CHARGING
bat.POWER_SUPPLY_STATUS_DISCHARGING
bat.POWER_SUPPLY_STATUS_NOT_CHARGING
bat.POWER_SUPPLY_STATUS_FULL
# Power supply health constants
bat.POWER_SUPPLY_HEALTH_UNKNOWN
bat.POWER_SUPPLY_HEALTH_GOOD
bat.POWER_SUPPLY_HEALTH_OVERHEAT
bat.POWER_SUPPLY_HEALTH_DEAD
bat.POWER_SUPPLY_HEALTH_OVERVOLTAGE
bat.POWER_SUPPLY_HEALTH_UNSPEC_FAILURE
bat.POWER_SUPPLY_HEALTH_COLD
bat.POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE
bat.POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE
# Power supply technology (chemistry) constants
# This resulted in a ADC 'value' of 1465.
# The conversion factor for the battery voltage is then: 1465 / 13.44 = 109.00297619047619
#
voltage = (value / 109.00297619047619)
else:
# simulated value!
voltage = demoVoltage
# 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)
