Add new logic to safety tree

panther_battery/README.md

@@ -29,7 +29,3 @@ Node publishing Panther battery state read from motor controllers. Used in Panth
 #### Subscribes
 
 - `/panther/driver/motor_controllers_state` [*panther_msgs/DriverState*]: current motor controllers' state and error flags.
-
-#### Parameters 
-
-- `~fatal_bat_temp` [*float*, default: **62.0**]: The temperature of the battery at which the battery health state is incorrect.

panther_battery/src/adc_node.py

@@ -19,17 +19,17 @@ class ADCNode:
     V_BAT_FATAL_MAX = 43.0
     V_BAT_FULL = 41.4
     V_BAT_MIN = 32.0
+    LOW_BAT_TEMP = -10.0
+    OVERHEAT_BAT_TEMP = 45.0
 
     def __init__(self, name: str) -> None:
         rospy.init_node(name, anonymous=False)
 
-        self._high_bat_temp = rospy.get_param('~fatal_bat_temp', 62.0)
-
         self._driver_battery_last_info_time: Optional[float] = None
         self._I_driv: Optional[float] = None
         self._V_driv: Optional[float] = None
 
-        self._mean_length = 10
+        self._mean_length = 20
         self._V_bat_hist = defaultdict(lambda: [37.0] * self._mean_length)
         self._V_bat_mean = defaultdict(lambda: 37.0)
         self._I_bat_charge_hist = defaultdict(lambda: [0.0] * self._mean_length)
@@ -42,10 +42,10 @@ def __init__(self, name: str) -> None:
         self._u_supply = 3.28
 
         self._battery_count = self._check_battery_count()
-        
+
         self._I_bat_charging_thresh = {}
         self._charger_connected = False
-        
+
         self._lock = Lock()
 
         # -------------------------------
@@ -150,7 +150,11 @@ def _battery_timer_cb(self, *args) -> None:
         else:
             temp_bat_1 = self._voltage_to_deg(V_temp_bat_1)
             self._publish_battery_msg(
-                self._battery_pub, V_bat_1, temp_bat_1, -(I_bat_1 + I_bat_2) + I_charge_bat_1, I_charge_bat_1
+                self._battery_pub,
+                V_bat_1,
+                temp_bat_1,
+                -(I_bat_1 + I_bat_2) + I_charge_bat_1,
+                I_charge_bat_1,
             )
 
     def _check_battery_count(self) -> int:
@@ -228,13 +232,16 @@ def _publish_battery_msg(
             error_msg = None
             if V_bat_mean < self.V_BAT_FATAL_MIN:
                 battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_DEAD
-                error_msg = 'Battery voltage is critically low!'
+                error_msg = 'The battery voltage is critically low!'
             elif V_bat_mean > self.V_BAT_FATAL_MAX:
                 battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_OVERVOLTAGE
-                error_msg = 'Battery overvoltage!'
-            elif temp_bat >= self._high_bat_temp:
+                error_msg = 'The battery overvoltage!'
+            elif temp_bat >= self.OVERHEAT_BAT_TEMP:
                 battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_OVERHEAT
-                error_msg = 'Battery is overheating!'
+                error_msg = 'The battery is overheating!'
+            elif temp_bat <= self.LOW_BAT_TEMP:
+                battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_COLD
+                error_msg = 'The battery is too cold! It may result in reduced effectiveness.'
             elif self._driver_battery_last_info_time is None:
                 battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNKNOWN
             else:

panther_manager/config/lights.xml

@@ -5,25 +5,37 @@
       <Sequence name="ChargingSequence"
                 _skipIf="battery_status != POWER_SUPPLY_STATUS_CHARGING \
 &amp;&amp; battery_status != POWER_SUPPLY_STATUS_FULL">
-        <CallSetLedAnimationService name="SetChargingAnimation"
-                                    id="{CHARGING_BATTERY_ANIM_ID}"
-                                    param="{battery_percent_round}"
+        <CallSetLedAnimationService name="SerErrorAnimation"
+                                    id="{ERROR_ANIM_ID}"
+                                    param=""
                                     repeating="true"
                                     service_name="lights/controller/set/animation"
                                     timeout="100"
-                                    _skipIf="battery_percent_round == charging_anim_percent \
+                                    _skipIf="battery_health != POWER_SUPPLY_HEALTH_OVERHEAT \
+|| current_anim_id == ERROR_ANIM_ID"
+                                    _onSuccess="current_anim_id = ERROR_ANIM_ID"/>
+        <Sequence name="DisplayChargingAnimationSequence"
+                  _skipIf="battery_health == POWER_SUPPLY_HEALTH_OVERHEAT">
+          <CallSetLedAnimationService name="SetChargingAnimation"
+                                      id="{CHARGING_BATTERY_ANIM_ID}"
+                                      param="{battery_percent_round}"
+                                      repeating="true"
+                                      service_name="lights/controller/set/animation"
+                                      timeout="100"
+                                      _skipIf="battery_percent_round == charging_anim_percent \
 &amp;&amp; current_anim_id == CHARGING_BATTERY_ANIM_ID"
-                                    _onSuccess="charging_anim_percent = battery_percent_round; \
+                                      _onSuccess="charging_anim_percent = battery_percent_round; \
 current_anim_id = CHARGING_BATTERY_ANIM_ID"/>
-        <TickAfterTimeout timeout="20.0"
-                          _skipIf="!e_stop_state">
-          <CallSetLedAnimationService name="SetEStopAnimation"
-                                      id="{E_STOP_ANIM_ID}"
-                                      param=""
-                                      repeating="false"
-                                      service_name="lights/controller/set/animation"
-                                      timeout="100"/>
-        </TickAfterTimeout>
+          <TickAfterTimeout timeout="20.0"
+                            _skipIf="!e_stop_state">
+            <CallSetLedAnimationService name="SetEStopAnimation"
+                                        id="{E_STOP_ANIM_ID}"
+                                        param=""
+                                        repeating="false"
+                                        service_name="lights/controller/set/animation"
+                                        timeout="100"/>
+          </TickAfterTimeout>
+        </Sequence>
       </Sequence>
       <Sequence name="DischargingSequence"
                 _skipIf="battery_status != POWER_SUPPLY_STATUS_DISCHARGING \

panther_manager/config/manager_bt_config.yaml

@@ -13,8 +13,8 @@ lights:
 safety:
   high_bat_temp: 55.0
   critical_bat_temp: 59.0
-  cpu_fan_on_temp: 70.0
-  cpu_fan_off_temp: 60.0
+  cpu_fan_on_temp: 75.0
+  cpu_fan_off_temp: 70.0
   driver_fan_on_temp: 50.0
   driver_fan_off_temp: 45.0
 plugin_libs:

panther_manager/config/safety.xml

@@ -2,45 +2,70 @@
 <root BTCPP_format="4">
   <BehaviorTree ID="Safety">
     <Sequence>
-      <Sequence name="BatteryStatusSequence"
-                _skipIf="battery_health == POWER_SUPPLY_HEALTH_GOOD \
-|| battery_health == POWER_SUPPLY_HEALTH_UNKNOWN">
-        <SignalShutdown reason="Battery status unhealthy"/>
+      <Sequence name="BatteryOverheatSequence"
+                _skipIf="battery_health != POWER_SUPPLY_HEALTH_OVERHEAT">
+        <Sequence name="FatalBatTempSequence"
+                  _skipIf="bat_temp &lt;= FATAL_BAT_TEMP">
+          <SignalShutdown reason="Fatal battery temperature"/>
+        </Sequence>
+        <Sequence name="CriticalBatTempSequence"
+                  _skipIf="bat_temp &lt;= CRITICAL_BAT_TEMP">
+          <CallTriggerService name="TriggerEStop"
+                              service_name="hardware/e_stop_trigger"
+                              timeout="100"
+                              _skipIf="e_stop_state"/>
+          <CallSetBoolService name="DisableAUX"
+                              data="false"
+                              service_name="hardware/aux_power_enable"
+                              timeout="100"
+                              _skipIf="!aux_state"/>
+        </Sequence>
+        <CallSetBoolService name="EnableFanIfHighBatTemp"
+                            data="true"
+                            service_name="hardware/fan_enable"
+                            timeout="100"
+                            _skipIf="fan_state"/>
       </Sequence>
-      <Sequence name="HighBatTempSequence"
-                _skipIf="bat_temp &lt; HIGH_BAT_TEMP">
+      <Sequence name="BatteryDeadSequence"
+                _skipIf="battery_health != POWER_SUPPLY_HEALTH_DEAD">
+        <SignalShutdown reason="Battery dead"/>
+      </Sequence>
+      <Sequence name="BatteryOvervoltageSequence"
+                _skipIf="battery_health != POWER_SUPPLY_HEALTH_OVERVOLTAGE">
         <CallTriggerService name="TriggerEStop"
                             service_name="hardware/e_stop_trigger"
                             timeout="100"
                             _skipIf="e_stop_state"/>
-        <CallSetBoolService name="DisableAUX"
-                            data="false"
-                            service_name="hardware/aux_power_enable"
-                            timeout="100"
-                            _skipIf="!aux_state || bat_temp &lt;= CRITICAL_BAT_TEMP"/>
-        <CallSetBoolService name="EnableFan"
-                            data="true"
-                            service_name="hardware/fan_enable"
-                            timeout="100"
-                            _skipIf="fan_state"/>
       </Sequence>
-      <Sequence name="TempControlSequence"
-                _skipIf="bat_temp &gt;= HIGH_BAT_TEMP">
-        <CallSetBoolService name="DisableFan"
-                            data="false"
+      <RunOnce name="TurnOnFunAtStartup"
+               then_skip="true">
+        <CallSetBoolService name="EnableFanIfHighBatTemp"
+                            data="true"
                             service_name="hardware/fan_enable"
-                            timeout="100"
-                            _skipIf="cpu_temp &gt; CPU_FAN_OFF_TEMP \
+                            timeout="100"/>
+      </RunOnce>
+      <TickAfterTimeout name="FanOffWithHysteresis"
+                        timeout="60.0">
+        <Sequence>
+          <Sequence name="HighSysTempSequence"
+                    _skipIf="(cpu_temp &lt;= CPU_FAN_ON_TEMP \
+&amp;&amp; driver_temp &lt;= DRIVER_FAN_ON_TEMP)">
+            <CallSetBoolService name="EnableFanIfHighBatTemp"
+                                data="true"
+                                service_name="hardware/fan_enable"
+                                timeout="100"
+                                _skipIf="fan_state"/>
+          </Sequence>
+          <CallSetBoolService name="DisableFan"
+                              data="false"
+                              service_name="hardware/fan_enable"
+                              timeout="100"
+                              _skipIf="cpu_temp &gt; CPU_FAN_OFF_TEMP \
 || driver_temp &gt; DRIVER_FAN_OFF_TEMP \
+|| battery_health == POWER_SUPPLY_HEALTH_OVERHEAT \
 || !fan_state"/>
-        <CallSetBoolService name="EnableFan"
-                            data="true"
-                            service_name="hardware/fan_enable"
-                            timeout="100"
-                            _skipIf="(cpu_temp &lt;= CPU_FAN_ON_TEMP \
-&amp;&amp; driver_temp &lt;= DRIVER_FAN_ON_TEMP) \
-|| fan_state"/>
-      </Sequence>
+        </Sequence>
+      </TickAfterTimeout>
     </Sequence>
   </BehaviorTree>
 
@@ -63,6 +88,10 @@
             editable="true">
       <input_port name="reason">reason to shutdown robot</input_port>
     </Action>
+    <Decorator ID="TickAfterTimeout"
+               editable="true">
+      <input_port name="timeout">time in s to wait before ticking child again</input_port>
+    </Decorator>
   </TreeNodesModel>
 
 </root>

panther_manager/include/panther_manager/manager_bt_node.hpp

@@ -31,6 +31,9 @@ class ManagerBTNode
   ~ManagerBTNode() {}
 
 private:
+  static constexpr float critical_bat_temp_ = 55.0;
+  static constexpr float fatal_bat_temp_ = 62.0;
+
   bool launch_shutdown_tree_;
   float update_charging_anim_step_;
   std::string node_name_;

panther_manager/src/manager_bt_node.cpp

@@ -67,8 +67,6 @@ ManagerBTNode::ManagerBTNode(
   update_charging_anim_step_ = ph_->param<float>("lights/update_charging_anim_step", 0.1);
 
   // safety tree params
-  const auto high_bat_temp = ph_->param<float>("safety/high_bat_temp", 55.0);
-  const auto critical_bat_temp = ph_->param<float>("safety/critical_bat_temp", 59.0);
   const auto cpu_fan_on_temp = ph_->param<float>("safety/cpu_fan_on_temp", 70.0);
   const auto cpu_fan_off_temp = ph_->param<float>("safety/cpu_fan_off_temp", 60.0);
   const auto driver_fan_on_temp = ph_->param<float>("safety/driver_fan_on_temp", 45.0);
@@ -129,6 +127,9 @@ ManagerBTNode::ManagerBTNode(
       {"POWER_SUPPLY_STATUS_NOT_CHARGING",
        unsigned(sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_NOT_CHARGING)},
       {"POWER_SUPPLY_STATUS_FULL", unsigned(sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_FULL)},
+      // battery health constants
+      {"POWER_SUPPLY_HEALTH_OVERHEAT",
+       unsigned(sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_OVERHEAT)},
     };
 
     lights_config_ = create_bt_config(lights_initial_bb);
@@ -139,10 +140,10 @@ ManagerBTNode::ManagerBTNode(
     const std::map<std::string, std::any> safety_initial_bb = {
       {"CPU_FAN_OFF_TEMP", cpu_fan_off_temp},
       {"CPU_FAN_ON_TEMP", cpu_fan_on_temp},
-      {"CRITICAL_BAT_TEMP", critical_bat_temp},
       {"DRIVER_FAN_OFF_TEMP", driver_fan_off_temp},
       {"DRIVER_FAN_ON_TEMP", driver_fan_on_temp},
-      {"HIGH_BAT_TEMP", high_bat_temp},
+      {"CRITICAL_BAT_TEMP", critical_bat_temp_},
+      {"FATAL_BAT_TEMP", fatal_bat_temp_},
       // battery health constants
       {"POWER_SUPPLY_HEALTH_UNKNOWN",
        unsigned(sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN)},
@@ -292,6 +293,7 @@ void ManagerBTNode::lights_tree_timer_cb()
   // update blackboard
   lights_config_.blackboard->set<bool>("e_stop_state", e_stop_state_.value());
   lights_config_.blackboard->set<unsigned>("battery_status", battery_status_.value());
+  lights_config_.blackboard->set<unsigned>("battery_health", battery_health_.value());
   lights_config_.blackboard->set<float>("battery_percent", battery_percent_ma_->get_average());
   lights_config_.blackboard->set<std::string>(
     "battery_percent_round",
@@ -309,6 +311,7 @@ void ManagerBTNode::safety_tree_timer_cb()
   safety_config_.blackboard->set<bool>("aux_state", io_state_.value()->aux_power);
   safety_config_.blackboard->set<bool>("e_stop_state", e_stop_state_.value());
   safety_config_.blackboard->set<bool>("fan_state", io_state_.value()->fan);
+  safety_config_.blackboard->set<unsigned>("battery_status", battery_status_.value());
   safety_config_.blackboard->set<unsigned>("battery_health", battery_health_.value());
   safety_config_.blackboard->set<double>("bat_temp", battery_temp_ma_->get_average());
   safety_config_.blackboard->set<double>("cpu_temp", cpu_temp_ma_->get_average());