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 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 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 _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 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 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 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 _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 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 _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)
# 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)
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()
def AdcDataProcess(rawData):
battData = BatteryState()
battData.voltage = rawData[0] / 10.0
battData.present = True
BatteryStatePub.publish(battData)
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