Add low battery check to the hcm

Author: ayin21

src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/check_hardware.py

@@ -1,3 +1,7 @@
+from typing import Optional
+
+from rclpy.time import Time
+
 from bitbots_hcm.hcm_dsd.decisions import AbstractHCMDecisionElement
 from bitbots_msgs.msg import RobotControlState
 
@@ -158,53 +162,24 @@ def get_reevaluate(self):
         return True
 
 
-class CheckPressureSensor(AbstractHCMDecisionElement):
+class CheckBattery(AbstractHCMDecisionElement):
     """
-    Checks connection to pressure sensors.
+    Checks the battery level
     """
 
     def __init__(self, blackboard, dsd, parameters):
         super().__init__(blackboard, dsd, parameters)
-        self.had_problem = False
+        self.last_battery_high_time: Optional[Time] = None
 
     def perform(self, reevaluate=False):
-        if self.blackboard.visualization_active:
-            # no pressure sensors is visualization, but thats okay
-            return "OKAY"
-
-        if not self.blackboard.pressure_sensors_installed:
-            # no pressure sensors installed, no check necessary
-            return "OKAY"
-
-        # Check if we get no messages due to an error or always the exact same one (no connection)
-        if (
-            not self.blackboard.pressure_diag_error
-            and not self.blackboard.previous_pressures == self.blackboard.pressures
-        ):
-            self.blackboard.last_different_pressure_state_time = self.blackboard.node.get_clock().now()
+        if self.blackboard.battery_voltage is None or self.blackboard.battery_voltage >= self.blackboard.battery_voltage_threshold:
+            self.last_battery_high_time = self.blackboard.node.get_clock().now()
 
-        # Check if we get no messages for a while
-        if (
-            self.blackboard.last_different_pressure_state_time is None
-            or self.blackboard.node.get_clock().now().nanoseconds
-            - self.blackboard.last_different_pressure_state_time.nanoseconds
-            > 0.1 * 1e9
+        if self.last_battery_high_time is None or (
+            self.blackboard.node.get_clock().now().nanoseconds - self.last_battery_high_time.nanoseconds
+            > self.blackboard.battery_debounce_time * 1e9
         ):
-            # Check if we are in the startup phase (not too long tho)
-            if (
-                self.blackboard.current_state == RobotControlState.STARTUP
-                and self.blackboard.node.get_clock().now().nanoseconds - self.blackboard.start_time.nanoseconds
-                < 10 * 1e9
-            ):
-                # wait for the pressure sensors to start
-                return "PRESSURE_NOT_STARTED"
-            else:
-                return "PROBLEM"
-
-        if self.had_problem:
-            # had problem before, just tell that this is solved now
-            self.blackboard.node.get_logger().info("Pressure sensors are now connected. Will resume.")
-            self.had_problem = False
+            return "LOW"
 
         return "OKAY"
 

src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd

@@ -9,48 +9,50 @@ $StartHCM
     START_UP --> $Simulation
         YES --> @RobotStateStartup, @PlayAnimationInit, @PlayAnimationWalkReady
         NO --> #INIT_PATTERN
-    RUNNING --> $CheckMotors
-        MOTORS_NOT_STARTED --> @RobotStateStartup, @Wait
-        OVERLOAD --> #EMERGENCY_FALL
-        OVERHEAT --> #EMERGENCY_FALL
-        PROBLEM --> @RobotStateHardwareProblem, @WaitForMotors
-        TURN_ON --> #INIT_PATTERN
-        OKAY --> $RecordAnimation
-            RECORD_ACTIVE --> @RobotStateRecord, @Wait
-            FREE --> $TeachingMode
-                TEACH --> @RobotStateRecord, @SetTorque + stiff:false, @Wait
-                HOLD --> @SetTorque + stiff:true, @Wait
-                FINISHED --> @SetTorque + stiff:true + r:false, @RobotStateControllable, @PlayAnimationWalkReady
-                OFF --> $Stop
-                    STOPPED --> @RobotStatePenalty, @CancelGoals, @StopWalking, @PlayAnimationWalkReady, @Wait
-                    FREE -->$CheckIMU
-                        IMU_NOT_STARTED --> @RobotStateStartup, @WaitForIMUStartup
-                        PROBLEM --> @RobotStateHardwareProblem, @WaitForIMU
-                        OKAY --> $Fallen
-                            FALLEN_FRONT --> $GameControllerStop
-                                STOPPED --> @RobotStateFallen, @CancelGoals, @StopWalking, @Wait
-                                FREE --> @RobotStateFallen, @CancelGoals, @StopWalking, @RobotStateGettingUp, @PlayAnimationStandupFront, @SetSquat + squat:true
-                            FALLEN_BACK --> $GameControllerStop
-                                STOPPED --> @RobotStateFallen, @CancelGoals, @StopWalking, @Wait
-                                FREE --> @RobotStateFallen, @CancelGoals, @StopWalking, @RobotStateGettingUp, @SetFootZero, @PlayAnimationFallingBack, @PlayAnimationStandupBack, @SetSquat + squat:true
-                            FALLEN_RIGHT --> $GameControllerStop
-                                STOPPED --> @RobotStateFallen, @CancelGoals, @StopWalking, @Wait
-                                FREE --> @RobotStateFallen, @CancelGoals, @StopWalking, @PlayAnimationTurningFrontRight
-                            FALLEN_LEFT --> $GameControllerStop
-                                STOPPED --> @RobotStateFallen, @CancelGoals, @StopWalking, @Wait
-                                FREE --> @RobotStateFallen, @CancelGoals, @StopWalking, @PlayAnimationTurningFrontLeft
-                            NOT_FALLEN --> $Falling
-                                FALLING_LEFT --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingLeft, @Wait
-                                FALLING_RIGHT --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingRight, @Wait
-                                FALLING_FRONT --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingFront, @Wait
-                                FALLING_BACK --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingBack, @Wait
-                                NOT_FALLING --> $InSquat
-                                    YES --> @RobotStateGettingUp, @Complain, @PlayAnimationRise, @SetSquat + squat:false
-                                    NO --> $PlayingExternalAnimation
-                                        ANIMATION_RUNNING --> @StopWalking, @RobotStateAnimationRunning, @Wait
-                                        ANIMATION_SERVER_TIMEOUT --> @CancelAnimation
-                                        FREE --> $RecentWalkingGoals
-                                            STAY_WALKING --> @RobotStateWalking, @Wait
-                                            NOT_WALKING --> $RecentKickGoals
-                                                KICKING --> @RobotStateKicking, @Wait
-                                                NOT_KICKING --> @RobotStateControllable, @Wait
+    RUNNING --> $CheckBattery
+        LOW --> @CancelGoals, @StopWalking, @Speak + text:Battery_low_so_please_power_me_off_gracefully, @Wait + time:10 + r:false
+        OKAY --> $CheckMotors
+            MOTORS_NOT_STARTED --> @RobotStateStartup, @Wait
+            OVERLOAD --> #EMERGENCY_FALL
+            OVERHEAT --> #EMERGENCY_FALL
+            PROBLEM --> @RobotStateHardwareProblem, @WaitForMotors
+            TURN_ON --> #INIT_PATTERN
+            OKAY --> $RecordAnimation
+                RECORD_ACTIVE --> @RobotStateRecord, @Wait
+                FREE --> $TeachingMode
+                    TEACH --> @RobotStateRecord, @SetTorque + stiff:false, @Wait
+                    HOLD --> @SetTorque + stiff:true, @Wait
+                    FINISHED --> @SetTorque + stiff:true + r:false, @RobotStateControllable, @PlayAnimationWalkReady
+                    OFF --> $Stop
+                        STOPPED --> @RobotStatePenalty, @CancelGoals, @StopWalking, @PlayAnimationWalkReady, @Wait
+                        FREE -->$CheckIMU
+                            IMU_NOT_STARTED --> @RobotStateStartup, @WaitForIMUStartup
+                            PROBLEM --> @RobotStateHardwareProblem, @WaitForIMU
+                            OKAY --> $Fallen
+                                FALLEN_FRONT --> $GameControllerStop
+                                    STOPPED --> @RobotStateFallen, @CancelGoals, @StopWalking, @Wait
+                                    FREE --> @RobotStateFallen, @CancelGoals, @StopWalking, @RobotStateGettingUp, @PlayAnimationStandupFront, @SetSquat + squat:true
+                                FALLEN_BACK --> $GameControllerStop
+                                    STOPPED --> @RobotStateFallen, @CancelGoals, @StopWalking, @Wait
+                                    FREE --> @RobotStateFallen, @CancelGoals, @StopWalking, @RobotStateGettingUp, @SetFootZero, @PlayAnimationFallingBack, @PlayAnimationStandupBack, @SetSquat + squat:true
+                                FALLEN_RIGHT --> $GameControllerStop
+                                    STOPPED --> @RobotStateFallen, @CancelGoals, @StopWalking, @Wait
+                                    FREE --> @RobotStateFallen, @CancelGoals, @StopWalking, @PlayAnimationTurningFrontRight
+                                FALLEN_LEFT --> $GameControllerStop
+                                    STOPPED --> @RobotStateFallen, @CancelGoals, @StopWalking, @Wait
+                                    FREE --> @RobotStateFallen, @CancelGoals, @StopWalking, @PlayAnimationTurningFrontLeft
+                                NOT_FALLEN --> $Falling
+                                    FALLING_LEFT --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingLeft, @Wait
+                                    FALLING_RIGHT --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingRight, @Wait
+                                    FALLING_FRONT --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingFront, @Wait
+                                    FALLING_BACK --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingBack, @Wait
+                                    NOT_FALLING --> $InSquat
+                                        YES --> @RobotStateGettingUp, @Complain, @PlayAnimationRise, @SetSquat + squat:false
+                                        NO --> $PlayingExternalAnimation
+                                            ANIMATION_RUNNING --> @StopWalking, @RobotStateAnimationRunning, @Wait
+                                            ANIMATION_SERVER_TIMEOUT --> @CancelAnimation
+                                            FREE --> $RecentWalkingGoals
+                                                STAY_WALKING --> @RobotStateWalking, @Wait
+                                                NOT_WALKING --> $RecentKickGoals
+                                                    KICKING --> @RobotStateKicking, @Wait
+                                                    NOT_KICKING --> @RobotStateControllable, @Wait

src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py

@@ -89,7 +89,7 @@ def __init__(self, node: Node):
         self.current_joint_state: Optional[JointState] = None
         self.previous_joint_state: Optional[JointState] = None
         self.last_different_joint_state_time: Optional[Time] = None
-        self.is_power_on: bool = False
+        self.is_power_on: bool = True
 
         # Motor Parameters
         self.motor_timeout_duration: float = self.node.get_parameter("motor_timeout_duration").value
@@ -114,6 +114,11 @@ def __init__(self, node: Node):
         self.previous_imu_msg: Optional[Imu] = None
         self.last_different_imu_state_time: Optional[Time] = None
 
+        # Battery state
+        self.battery_voltage_threshold: float = self.node.get_parameter("battery_voltage_threshold").value
+        self.battery_debounce_time: float = self.node.get_parameter("battery_debounce_time").value
+        self.battery_voltage: Optional[float] = None
+
         # Diagnostics state
         self.servo_diag_error: bool = False
         self.servo_overload: bool = False

src/bitbots_motion/bitbots_hcm/bitbots_hcm/humanoid_control_module.py

@@ -21,6 +21,7 @@
 from sensor_msgs.msg import Imu, JointState
 from std_msgs.msg import Bool
 from std_srvs.srv import SetBool
+from livelybot_msg.msg import PowerDetect
 
 from bitbots_hcm import hcm_dsd
 from bitbots_hcm.hcm_dsd.hcm_blackboard import HcmBlackboard
@@ -77,7 +78,7 @@ def __init__(self, use_sim_time, simulation_active, visualization_active):
         self.hcm_deactivated = False
 
         # Create subscribers
-        self.node.create_subscription(Bool, "core/power_switch_status", self.power_cb, 1)
+        self.node.create_subscription(PowerDetect, "power_detect_state", self.power_detect_cb, 1)
         self.node.create_subscription(Bool, "hcm_deactivate", self.deactivate_cb, 1)
         self.node.create_subscription(DiagnosticArray, "diagnostics_agg", self.diag_cb, 1)
         self.node.create_subscription(Bool, "game_controller/stop_msg", self.stop_cb, 1)
@@ -141,9 +142,9 @@ def set_manual_penalize_mode_callback(self, req: ManualPenalize.Request, resp: M
         resp.success = True
         return resp
 
-    def power_cb(self, msg: Bool):
-        """Updates the power state."""
-        self.blackboard.is_power_on = msg.data
+    def power_detect_cb(self, msg: PowerDetect):
+        """Recives data send by the battery management system."""
+        self.blackboard.battery_voltage = msg.voltage
 
     def diag_cb(self, msg: DiagnosticArray):
         """Updates the diagnostic state."""

src/bitbots_motion/bitbots_hcm/config/hcm_wolfgang.yaml

@@ -44,3 +44,9 @@
 
     # Pick up
     pick_up_accel_threshold: 7.0
+
+    # Battery
+    battery:
+    voltage_threshold: 18.0 # Voltage under which the battery is considered low
+    debounce_time: 5.0 # Time the battery has to be under the threshold to be considered low [s]
+