def __init__(self):
# ros communications
self.battery_publisher = rospy.Publisher('~state',
sensor_msgs.BatteryState,
queue_size=1,
latch=True)
# initialisations
self.battery = sensor_msgs.BatteryState()
self.battery.header.stamp = rospy.Time.now()
self.battery.voltage = float('nan')
self.battery.current = float('nan')
self.battery.charge = float('nan')
self.battery.capacity = float('nan')
self.battery.design_capacity = float('nan')
self.battery.percentage = 100
self.battery.power_supply_health = sensor_msgs.BatteryState.POWER_SUPPLY_HEALTH_GOOD
self.battery.power_supply_technology = sensor_msgs.BatteryState.POWER_SUPPLY_TECHNOLOGY_LION
self.battery.power_supply_status = sensor_msgs.BatteryState.POWER_SUPPLY_STATUS_FULL
self.battery.present = True
self.battery.location = ""
self.battery.serial_number = ""
# immediately reconfigured by dynamic_reconfigure
self.charging_increment = 0.01
# dynamic_reconfigure
self.parameters = None
self.dynamic_reconfigure_server = dynamic_reconfigure.server.Server(
BatteryConfig, self.dynamic_reconfigure_callback)
def __init__(self):
# ros communications
self.node = rclpy.create_node("battery")
self.battery_publisher = self.node.create_publisher(
msg_type=sensor_msgs.BatteryState,
topic="~/state",
qos_profile=py_trees_ros.utilities.qos_profile_latched_topic())
# initialisations
self.battery = sensor_msgs.BatteryState()
self.battery.header.stamp = rclpy.clock.Clock().now().to_msg()
self.battery.voltage = float('nan')
self.battery.current = float('nan')
self.battery.charge = float('nan')
self.battery.capacity = float('nan')
self.battery.design_capacity = float('nan')
self.battery.percentage = 100.0
self.battery.power_supply_health = sensor_msgs.BatteryState.POWER_SUPPLY_HEALTH_GOOD
self.battery.power_supply_technology = sensor_msgs.BatteryState.POWER_SUPPLY_TECHNOLOGY_LION
self.battery.power_supply_status = sensor_msgs.BatteryState.POWER_SUPPLY_STATUS_FULL
self.battery.present = True
self.battery.location = ""
self.battery.serial_number = ""
# immediately reconfigured by dynamic_reconfigure
self.charging_increment = 0.01
def __init__(self, name, topic_name="/battery/state", threshold=30.0):
super(ToBlackboard, self).__init__(
name=name,
topic_name=topic_name,
topic_type=sensor_msgs.BatteryState,
blackboard_variables={"battery": None},
clearing_policy=py_trees.common.ClearingPolicy.NEVER)
self.blackboard = py_trees.blackboard.Blackboard()
self.blackboard.battery = sensor_msgs.BatteryState()
self.blackboard.battery.percentage = 0.0
self.blackboard.battery.power_supply_status = sensor_msgs.BatteryState.POWER_SUPPLY_STATUS_UNKNOWN
self.blackboard.battery_low_warning = False # decision making
self.threshold = threshold
def _Sensor_state(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 5):
pass
try:
S1 = int(lineParts[1])
S2 = int(lineParts[2])
S3 = int(lineParts[3])
S4 = float(lineParts[4])
S5 = float(lineParts[5])
S6 = float(lineParts[6])
if S2 < 1:
left_rotate = float(0.02)
if S1 < 1:
right_rotate = -0.02
self.left_ir = S2
self.right_ir = S1
self.rear_bumper = S3
ir_state = Ir_State()
ir_state.right = int(S1)
ir_state.left = int(S2)
ir_state.rear = int(S3)
battery = sensor_msgs.BatteryState()
battery.header.stamp = rospy.Time.now()
battery.voltage = S4
battery.current = S6
battery.charge = float(S3)
battery.capacity = float('nan')
battery.design_capacity = float('nan')
battery.percentage = S4 / 0.126
battery.power_supply_health = sensor_msgs.BatteryState.POWER_SUPPLY_HEALTH_GOOD
battery.power_supply_technology = sensor_msgs.BatteryState.POWER_SUPPLY_TECHNOLOGY_LION
battery.power_supply_status = sensor_msgs.BatteryState.POWER_SUPPLY_STATUS_FULL
battery.present = True
battery.location = ""
battery.serial_number = ""
#self._S1_Publisher.publish(left_rotate + right_rotate)# angle
#self._S2_Publisher.publish(S2)#right ir
self._S3_Publisher.publish(S3) #rear bumper
self._S4_Publisher.publish(battery) #voltage
self._S5_Publisher.publish(S5) #drive voltage
self._S6_Publisher.publish(S6) #current
self._S7_Publisher.publish(ir_state) #ir_state
#rospy.logwarn(S1)
except:
rospy.logwarn("Unexpected error:sensor state" +
str(sys.exc_info()[0]))
def __init__(self,
topic_name: str,
qos_profile: rclpy.qos.QoSProfile,
name: str = py_trees.common.Name.AUTO_GENERATED,
threshold: float = 30.0):
super().__init__(name=name,
topic_name=topic_name,
topic_type=sensor_msgs.BatteryState,
qos_profile=qos_profile,
blackboard_variables={"battery": None},
clearing_policy=py_trees.common.ClearingPolicy.NEVER)
self.blackboard.register_key(key="battery_low_warning",
access=py_trees.common.Access.WRITE)
self.blackboard.battery = sensor_msgs.BatteryState()
self.blackboard.battery.percentage = 0.0
self.blackboard.battery.power_supply_status = sensor_msgs.BatteryState.POWER_SUPPLY_STATUS_UNKNOWN # noqa
self.blackboard.battery_low_warning = False # decision making
self.threshold = threshold
def __init__(self):
# node
self.node = rclpy.create_node(
node_name="battery",
parameter_overrides=[
rclpy.parameter.Parameter('charging_percentage', rclpy.parameter.Parameter.Type.DOUBLE, 100.0),
rclpy.parameter.Parameter('charging_increment', rclpy.parameter.Parameter.Type.DOUBLE, 0.1),
rclpy.parameter.Parameter('charging', rclpy.parameter.Parameter.Type.BOOL, False),
],
automatically_declare_parameters_from_overrides=True
)
# publishers
not_latched = False # latched = True
self.publishers = py_trees_ros.utilities.Publishers(
self.node,
[
('state', "~/state", sensor_msgs.BatteryState, not_latched),
]
)
# initialisations
self.battery = sensor_msgs.BatteryState()
self.battery.header.stamp = rclpy.clock.Clock().now().to_msg()
self.battery.voltage = float('nan')
self.battery.current = float('nan')
self.battery.charge = float('nan')
self.battery.capacity = float('nan')
self.battery.design_capacity = float('nan')
self.battery.percentage = 100.0
self.battery.power_supply_health = sensor_msgs.BatteryState.POWER_SUPPLY_HEALTH_GOOD
self.battery.power_supply_technology = sensor_msgs.BatteryState.POWER_SUPPLY_TECHNOLOGY_LION
self.battery.power_supply_status = sensor_msgs.BatteryState.POWER_SUPPLY_STATUS_FULL
self.battery.present = True
self.battery.location = ""
self.battery.serial_number = ""
self.timer = self.node.create_timer(
timer_period_sec=0.2,
callback=self.update_and_publish
)
def publish_battery_state(self):
msg = sensor_msgs.BatteryState()
msg.percentage = 80.0
self.publisher.publish(msg)
def __init__(self):
# register node in ROS network
rospy.init_node('~single_robot_battery_mockup', anonymous=False)
# setup publisher with the battery state
self.battery_publisher = rospy.Publisher('~battery_state',
sensor_msgs.BatteryState,
queue_size=1)
# subscibe to the is_charging topic
rospy.Subscriber('~is_charging', Bool, self.chargingCallback)
rospy.Service('~check_percentage', CheckPercentage,
self.serviceCallback)
# initialisations
self.battery = sensor_msgs.BatteryState()
self.battery.header.stamp = rospy.Time.now()
self.battery.voltage = float('nan')
self.battery.current = float('nan')
self.battery.charge = float('nan')
self.battery.capacity = float('nan')
self.battery.design_capacity = float('nan')
self.battery.percentage = 1
self.battery.power_supply_health = sensor_msgs.BatteryState.POWER_SUPPLY_HEALTH_GOOD
self.battery.power_supply_technology = sensor_msgs.BatteryState.POWER_SUPPLY_TECHNOLOGY_LION
self.battery.power_supply_status = sensor_msgs.BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
self.battery.present = True
self.battery.location = ""
self.battery.serial_number = ""
# is the robot currently charging
self.is_charging = True
if rospy.has_param('~battery_autonomy'):
# retrieves the battery autonomy
self.battery_autonomy = rospy.get_param('~battery_autonomy')
else:
self.battery_autonomy = 30
rospy.loginfo('Battery autonomy: %f', self.battery_autonomy)
# battery half life and total life
self.bat_half_life = (10.0 -
np.log(1)) / (np.log(2) /
(self.battery_autonomy / 2) * 10)
self.bat_max_life = 2 * self.bat_half_life
# Set the initial battery level
initial_battery_level = 'full'
# Discharged
if initial_battery_level == 'empty':
self.seconds_elapsed = 0
# Half Charged
elif initial_battery_level == 'half':
self.seconds_elapsed = self.bat_half_life
# Fully Charged
elif initial_battery_level == 'full':
self.seconds_elapsed = self.bat_max_life
# Choice between 0, half or fully charged
elif initial_battery_level == 'random_choice':
self.seconds_elapsed = np.random.choice(
[0, self.bat_half_life, self.bat_max_life])
# Uniform distribution
elif initial_battery_level == 'random_uniform':
self.seconds_elapsed = np.random.uniform(0, self.bat_max_life)
rospy.loginfo('Seconds chosen : %f', self.seconds_elapsed)
# Charging speed (factor multiplied by each unit of time)
self.chargind_speed = 1
# Dischargind speed (factor multiplied by each unit of time)
self.dischargind_speed = 1
self.white_noise = False