def callback_gps(gps):
global x_charge
global y_charge
global z_charge
global battery_constant
global battery_percentage
global old_location_x
global old_location_y
global old_location_z
time_begin = rospy.Time(1075)
time_now = rospy.get_rostime()
if time_now<time_begin:
print('not_yet_started')
old_location_x=gps.pose.position.x
old_location_y=gps.pose.position.y
old_location_z=gps.pose.position.z
battery = BatteryState()
battery_percentage=100
battery.percentage = battery_percentage
battery_pub.publish(battery)
if time_now>time_begin:
print('started')
new_location_x = gps.pose.position.x
new_location_y = gps.pose.position.y
new_location_z = gps.pose.position.z
percentage_loss=battery_constant*(math.pow((new_location_x-old_location_x), 2) + math.pow((new_location_y-old_location_y), 2)+ math.pow((new_location_z-old_location_z), 2))
print("percentage lossp", percentage_loss)
battery_percentage=battery_percentage-percentage_loss
charge_diff=(math.pow((new_location_x-x_charge), 2) + math.pow((new_location_y-y_charge), 2)+ math.pow((new_location_z-z_charge), 2))
if battery_percentage < 0.1:
battery_percentage = 0
print("battery drained")
if charge_diff<0.5:
battery_percentage=battery_percentage+0.1
if battery_percentage>100:
battery_percentage=100
print("fully charged")
battery = BatteryState()
battery.percentage = battery_percentage
battery_pub.publish(battery)
old_location_x=new_location_x
old_location_y=new_location_y
old_location_z=new_location_z
print('battery_percentage', battery_percentage)
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 publish(self):
battery_msg = BatteryState()
battery_msg.percentage = 1.0
pose_msg = PoseStamped()
pose_msg.header.frame_id = "map"
pose_msg.pose.position.x = self.id
pose_msg.pose.position.y = self.id
pose_msg.pose.position.z = 0
self.battery_pub.publish(battery_msg)
self.pose_pub.publish(pose_msg)
self.state_pub.publish(self.state_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 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 _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 _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 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 step(self):
if self._rnet._battery is not None:
bs_msg = BatteryState()
bs_msg.header.stamp = rospy.Time.now()
bs_msg.percentage = 1.0 * self._rnet._battery
self._bat_pub.publish(bs_msg)
if (rospy.Time.now() - self._last_cmd).to_sec() > self._cmd_timeout:
# zero-out velocity commands
self._cmd_vel.linear.x = 0
self._cmd_vel.angular.z = 0
self._rnet._cmd_vel = self._cmd_vel
if self._disable_chair_joy:
cf = self._joy_frame
else:
cf = self._rnet.recvfrom(timeout=0.1) #16)
if cf is not None:
cf = aid_str(cf)
# TODO : calibrate to m/s and scale accordingly
# currently, v / w are expressed in fractions where 1 = max fw, -1 = max bw
v = np.clip(self._v_scale * self._cmd_vel.linear.x, -1.0, 1.0)
w = np.clip(self._w_scale * self._cmd_vel.angular.z, -1.0, 1.0)
if cf == self._joy_frame:
# for joy : y=fw, x=turn; 0-256
cmd_y = 0x100 + int(v * 100) & 0xFF
cmd_x = 0x100 + int(-w * 100) & 0xFF
if np.abs(v) > self._min_v or np.abs(w) > self._min_w:
self._rnet.send(self._joy_frame + '#' + dec2hex(cmd_x, 2) +
dec2hex(cmd_y, 2))
else:
# below thresh, stop
self._rnet.send(self._joy_frame + '#' + dec2hex(0, 2) +
dec2hex(0, 2))
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 step(self):
if self._rnet._battery is not None:
bs_msg = BatteryState()
bs_msg.header.stamp = rospy.Time.now()
bs_msg.percentage = 1.0 * self._rnet._battery
self._bat_pub.publish(bs_msg)
now = rospy.Time.now()
if (now - self._last_cmd).to_sec() > self._cmd_timeout:
# zero-out velocity commands
#print((now - self._last_cmd).to_sec(), 'timeout')
self._cmd_vel.linear.x = 0
self._cmd_vel.angular.z = 0
if self._disable_chair_joy:
cf = self._joy_frame
else:
cf = self._rnet.recvfrom(timeout=0.1) #16)
if cf is not None:
cf = aid_str(cf)
# TODO : calibrate to m/s and scale accordingly
# currently, v / w are expressed in fractions where 1 = max fw, -1 = max bw
v = self._v_scale * self._cmd_vel.linear.x
w = self._w_scale * self._cmd_vel.angular.z
v1 = int(np.clip(v * 100, -100, 100))
w1 = int(np.clip(w * 100, -100, 100))
v2 = int(np.clip(v * 25, -25, 25))
w2 = int(np.clip(w * 25, -25, 25))
if cf == self._joy_frame:
# for joy : y=fw, x=turn; 0-256
cmd_y1 = 0x100 + int(v1) & 0xFF
cmd_x1 = 0x100 - int(w1) & 0xFF
cmd_y2 = 0x100 + int(v2) & 0xFF
cmd_x2 = 0x100 - int(w2) & 0xFF
try:
if np.abs(v) > self._min_v or np.abs(w) > self._min_w:
#self._rnet.send(self._joy_frame + '#' + dec2hex(cmd_x, 2) + dec2hex(cmd_y, 2))
self._rnet.send('02001100' + '#' + dec2hex(cmd_x1, 2) +
dec2hex(cmd_y1, 2))
self._rnet.send('02000200' + '#' + dec2hex(cmd_x2, 2) +
dec2hex(cmd_y2, 2))
else:
# below thresh, stop
#self._rnet.send(self._joy_frame + '#' + dec2hex(0, 2) + dec2hex(0, 2))
self._rnet.send('02001100' + '#' + dec2hex(0, 2) +
dec2hex(0, 2))
self._rnet.send('02000200' + '#' + dec2hex(0, 2) +
dec2hex(0, 2))
except Exception as e:
#print e
#print self._cmd_vel
#print cmd_x, cmd_y
#print dec2hex(cmd_x, 2), dec2hex(cmd_y, 2)
raise e
# heartbeat
if (now - self._last_hb).to_sec() > 0.1:
self._rnet.send('03C30F0F#87878787878787')
self._last_hb = now
#!/usr/bin/env python
import random
import rospy
from sensor_msgs.msg import BatteryState
if __name__ == "__main__":
rospy.init_node("fake_battery", anonymous=True)
fake_battery_publisher = rospy.Publisher("battery_status",
BatteryState,
queue_size=10)
r = rospy.Rate(1)
r.sleep()
while not rospy.is_shutdown():
battery_msg = BatteryState()
battery_msg.percentage = round(random.uniform(0.95, 0.98), 2)
fake_battery_publisher.publish(battery_msg)
r.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 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")
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
try:
if device_input_current - device_output_current > 0.0:
bsmsg.power_supply_status = bsmsg.POWER_SUPPLY_STATUS_CHARGING
# TODO check for full
else:
bsmsg.power_supply_status = bsmsg.POWER_SUPPLY_STATUS_DISCHARGING
except:
pass
pub.publish(bsmsg)
r.sleep()
# 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)
