Merge pull request #82 from hello-robot/feature/charging_state

Provide charging status in BatteryState message

stretch_core/README.md

@@ -1,6 +1,6 @@
 ![](../images/banner.png)
 
-## Overview
+# Overview
 
 *stretch_core* provides the core ROS interfaces to the Stretch RE1 mobile manipulator from Hello Robot Inc. It includes the following nodes:
 
@@ -12,7 +12,21 @@
 
 *keyboard_teleop* : node that provides a keyboard interface to control the robot's joints
 
-## Testing
+# API
+
+## Nodes
+
+### [stretch_driver](./nodes/stretch_driver)
+
+#### Published Topics
+
+##### /battery ([sensor_msgs/BatteryState](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/BatteryState.html))
+
+This topic publishes Stretch's battery and charge status. Charging status, the `power_supply_status` field, is estimated by looking at changes in voltage readings over time, where plugging-in causes the voltage to jump up (i.e. status becomes 'charging') and pulling the plug out is detected by a voltage dip (i.e. status becomes 'discharging'). Therefore, charging status is unknown at boot of this node. Consequently, the `current` field is positive at boot of this node, regardless of whether the robot is charging/discharging. After a charging state change, there is a ~10 second timeout where state changes won't be detected. Additionally, outlier voltage readings can slip past the filters and incorrectly indicate a charging state change (albeit rarely). Finally, voltage readings are affected by power draw (e.g. the onboard computer starts a computationally taxing program), which can lead to incorrect indications of charging state change. Stretch RE2s have a hardware switch in the charging port that can detect when a plug has been plugged in, regardless of whether the plug is providing any power. Therefore, this node combines the previous voltage-based estimate with readings from this hardware switch to make better charging state estimates on RE2s (effectively eliminating the false positive case where a computational load draws more power).
+
+Since a battery is always present on a Stretch system, we instead misuse the `present` field to indicate whether a plug is plugged in to the charging port (regardless of whether it's providing power) on RE2 robots. This field is always false on RE1s. The unmeasured fields (e.g. charge in Ah) return a NaN, or 'not a number'.
+
+# Testing
 
 The primary testing framework being used within *stretch_ros* is pytest. Pytest is an open source testing framework that scales well and takes a functional approach resulting in minimal boiler plate code. First we should ensure that pytest is installed and up to date:
 
@@ -122,6 +136,6 @@ Results can be visualized by typing in the following command:
 This will show descriptive messages based on how many tests were run, errors, failures, skipped tests, and the respective package where the failure occurred. However, when running tests with *catkin_tools* some plugins will lose functionality such as Pytest Clarity.
 
 
-## License
+# License
 
 For license information, please see the LICENSE files.

stretch_core/nodes/stretch_driver

@@ -31,12 +31,9 @@ from joint_trajectory_server import JointTrajectoryAction
 from stretch_diagnostics import StretchDiagnostics
 
 
-class StretchBodyNode:
+class StretchDriverNode:
 
     def __init__(self):
-        self.use_robotis_head = True
-        self.use_robotis_end_of_arm = True
-
         self.default_goal_timeout_s = 10.0
         self.default_goal_timeout_duration = rospy.Duration(self.default_goal_timeout_s)
 
@@ -46,6 +43,10 @@ class StretchBodyNode:
         self.robot_mode_rwlock = RWLock()
         self.robot_mode = None
 
+        self.voltage_history = []
+        self.charging_state_history = [BatteryState.POWER_SUPPLY_STATUS_UNKNOWN] * 10
+        self.charging_state = BatteryState.POWER_SUPPLY_STATUS_UNKNOWN
+
     ###### MOBILE BASE VELOCITY METHODS #######
 
     def set_mobile_base_velocity_callback(self, twist):
@@ -122,18 +123,50 @@ class StretchBodyNode:
         odom.twist.twist.angular.z = theta_vel
         self.odom_pub.publish(odom)
 
-        # TODO: Add way to determine if the robot is charging
-        # TODO: Calculate the percentage
-        battery_state = BatteryState()
+        pimu_hardware_id = self.robot.pimu.board_info['hardware_id']
         invalid_reading = float('NaN')
+        v = float(robot_status['pimu']['voltage'])
+        self.voltage_history.append(v)
+        if len(self.voltage_history) > 100:
+            self.voltage_history.pop(0)
+            self.charging_state_history.pop(0)
+            if v > np.mean(self.voltage_history) + 3 * np.std(self.voltage_history):
+                self.charging_state_history.append(BatteryState.POWER_SUPPLY_STATUS_CHARGING)
+            elif v < np.mean(self.voltage_history) - 3 * np.std(self.voltage_history):
+                self.charging_state_history.append(BatteryState.POWER_SUPPLY_STATUS_DISCHARGING)
+            else:
+                self.charging_state_history.append(BatteryState.POWER_SUPPLY_STATUS_UNKNOWN)
+        filtered_charging_state = max(set(self.charging_state_history), key=self.charging_state_history.count)
+        if filtered_charging_state != BatteryState.POWER_SUPPLY_STATUS_UNKNOWN:
+            if pimu_hardware_id == 0:
+                self.charging_state = filtered_charging_state
+            elif pimu_hardware_id == 1:
+                if robot_status['pimu']['charger_connected'] == True and filtered_charging_state == BatteryState.POWER_SUPPLY_STATUS_CHARGING:
+                    self.charging_state = BatteryState.POWER_SUPPLY_STATUS_CHARGING
+                elif robot_status['pimu']['charger_connected'] == False and filtered_charging_state == BatteryState.POWER_SUPPLY_STATUS_DISCHARGING:
+                    self.charging_state = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
+
+        i = float(robot_status['pimu']['current'])
+        if self.charging_state == BatteryState.POWER_SUPPLY_STATUS_CHARGING:
+            i = float(robot_status['pimu']['current'])
+        elif self.charging_state == BatteryState.POWER_SUPPLY_STATUS_DISCHARGING:
+            i = -1 * float(robot_status['pimu']['current'])
+
+        battery_state = BatteryState()
         battery_state.header.stamp = current_time
-        battery_state.voltage = float(robot_status['pimu']['voltage'])
-        battery_state.current = float(robot_status['pimu']['current'])
+        battery_state.voltage = v
+        battery_state.current = i
+        battery_state.temperature = invalid_reading
         battery_state.charge = invalid_reading
         battery_state.capacity = invalid_reading
-        battery_state.percentage = invalid_reading
+        battery_state.percentage = invalid_reading # TODO: Calculate the percentage
         battery_state.design_capacity = 18.0
-        battery_state.present = True
+        battery_state.power_supply_status = self.charging_state
+        # misuse the 'present' flag to indicated whether the barrel jack button is pressed (i.e. charger is present, but may or may not be providing power)
+        if pimu_hardware_id == 0:
+            battery_state.present = False
+        elif pimu_hardware_id == 1:
+            battery_state.present = robot_status['pimu']['charger_connected']
         self.power_pub.publish(battery_state)
 
         calibration_status = Bool()
@@ -467,5 +500,5 @@ class StretchBodyNode:
 
 
 if __name__ == '__main__':
-    node = StretchBodyNode()
+    node = StretchDriverNode()
     node.main()