Ros1 update bt images

panther_battery/README.md

@@ -32,4 +32,4 @@ Node publishing Panther battery state read from motor controllers. Used in Panth
 
 #### Parameters 
 
-- `~high_bat_temp` [*float*, default: **55.0**]: The temperature of the battery at which the battery health state is incorrect.
+- `~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

@@ -22,7 +22,7 @@ class ADCNode:
     def __init__(self, name: str) -> None:
         rospy.init_node(name, anonymous=False)
 
-        self._high_bat_temp = rospy.get_param('~high_bat_temp', 55.0)
+        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

panther_manager/README.md

@@ -57,7 +57,6 @@ echo $USERNAME 'ALL=(ALL) NOPASSWD: /sbin/poweroff, /sbin/reboot, /sbin/shutdown
 - `~safety/critical_bat_temp` [*float*, default: **59.0**]: extends `high_bat_temp` by turning off AUX power.
 - `~safety/driver_fan_off_temp` [*float*, default: **35.0**]: temperature in **deg C** of any drivers below which the fan is turned off.
 - `~safety/driver_fan_on_temp` [*float*, default: **45.0**]: temperature in **deg C** of any drivers above which the fan is turned on.
-- `~safety/fatal_bat_temp` [*float*, default: **62.0**]: battery temperature above which the robot is shutdown.
 - `~safety/high_bat_temp` [*float*, default: **55.0**]: battery temperature above which the robot starts displaying warning log and e-stop is triggered.
 - `~shutdown_hosts_file` [*string*, default: **None**]: path to a YAML file containing a list of hosts to request shutdown. To correctly format the YAML file, include a **hosts** field consisting of a list with the following fields:
   - `command` [*string*, default: **sudo shutdown now**]: command executed on shutdown of given device.
@@ -120,7 +119,7 @@ For a BehaviorTree project to work correctly, it must contain three trees with n
 Tree responsible for scheduling animations displayed on LED panels, based on the Husarion Panther robot's system state.
 
 <p align="center">
-  <img align="center" src="./docs/lights_tree.png">
+  <img align="center" src="https://github-readme-figures.s3.eu-central-1.amazonaws.com/panther/panther_ros/lights_tree.png">
 </p>
 
 Default blackboard entries:
@@ -158,7 +157,7 @@ Default constant blackboard entries:
 Tree responsible for monitoring the Panther robot's state and handling safety measures, such as cooling the robot in case of high CPU or battery temperature.
 
 <p align="center">
-  <img align="center" src="./docs/safety_tree.png">
+  <img align="center" src="https://github-readme-figures.s3.eu-central-1.amazonaws.com/panther/panther_ros/safety_tree.png">
 </p>
 
 Default blackboard entries:
@@ -175,15 +174,14 @@ Default constant blackboard entries:
 - `CRITICAL_BAT_TEMP` [*float*, default: **59.0**]: refers to `critical_bat_temp` ROS parameter.
 - `DRIVER_FAN_OFF_TEMP` [*float*, default: **35.0**]: refers to `driver_fan_off_temp` ROS parameter.
 - `DRIVER_FAN_ON_TEMP` [*float*, default: **45.0**]: refers to `driver_fan_on_temp` ROS parameter.
-- `FATAL_BAT_TEMP` [*float*, default: **62.0**]: refers to `fatal_bat_temp` ROS parameter.
 - `HIGH_BAT_TEMP` [*float*, default: **55.0**]: refers to `high_bat_temp` ROS parameter.
 
 #### Shutdown 
 
 Tree responsible for graceful shutdown of robot components, user computers, and the built-in computer. By default, it will proceed to shutdown all computers defined in a YAML file with a path defined by the `~shutdown_host_file` ROS parameter.
 
 <p align="center">
-  <img src="./docs/shutdown_tree.png">
+  <img src="https://github-readme-figures.s3.eu-central-1.amazonaws.com/panther/panther_ros/shutdown_tree.png">
 </p>
 
 Default constant blackboard entries:

panther_manager/config/manager_bt_config.yaml

@@ -13,7 +13,6 @@ lights:
 safety:
   high_bat_temp: 55.0
   critical_bat_temp: 59.0
-  fatal_bat_temp: 62.0
   cpu_fan_on_temp: 70.0
   cpu_fan_off_temp: 60.0
   driver_fan_on_temp: 45.0

panther_manager/config/safety.xml

@@ -2,30 +2,30 @@
 <root BTCPP_format="4">
   <BehaviorTree ID="Safety">
     <Sequence>
-      <Sequence name="ManagePowerSequence"
-                _skipIf="bat_temp &lt;= HIGH_BAT_TEMP">
-        <SignalShutdown reason="Battery reached fatal temperature"
-                        _skipIf="bat_temp &lt;= FATAL_BAT_TEMP"/>
-        <Sequence name="HighTempSequence"
-                  _skipIf="bat_temp &gt; FATAL_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 || bat_temp &lt;= CRITICAL_BAT_TEMP"/>
-          <CallSetBoolService name="EnableFan"
-                              data="true"
-                              service_name="hardware/fan_enable"
-                              timeout="100"
-                              _skipIf="fan_state"/>
-        </Sequence>
+      <Sequence name="BatteryStatusSequence"
+                _skipIf="battery_health == POWER_SUPPLY_HEALTH_GOOD \
+|| battery_health == POWER_SUPPLY_HEALTH_UNKNOWN">
+        <SignalShutdown reason="Battery status unhealthy"/>
+      </Sequence>
+      <Sequence name="HighBatTempSequence"
+                _skipIf="bat_temp &lt; HIGH_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 || 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">
+                _skipIf="bat_temp &gt;= HIGH_BAT_TEMP">
         <CallSetBoolService name="DisableFan"
                             data="false"
                             service_name="hardware/fan_enable"
@@ -59,6 +59,10 @@
       <input_port name="timeout"
                   default="100">timeout in ms to wait for service to be active</input_port>
     </Action>
+    <Action ID="SignalShutdown"
+            editable="true">
+      <input_port name="reason">reason to shutdown robot</input_port>
+    </Action>
   </TreeNodesModel>
 
 </root>

panther_manager/include/panther_manager/manager_bt_node.hpp

@@ -35,6 +35,7 @@ class ManagerBTNode
   float update_charging_anim_step_;
   std::string node_name_;
   std::optional<unsigned> battery_status_;
+  std::optional<unsigned> battery_health_;
   std::optional<bool> e_stop_state_;
   std::optional<panther_msgs::IOState::ConstPtr> io_state_;
 

panther_manager/src/manager_bt_node.cpp

@@ -69,7 +69,6 @@ ManagerBTNode::ManagerBTNode(
   // 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 fatal_bat_temp = ph_->param<float>("safety/fatal_bat_temp", 62.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);
@@ -120,7 +119,7 @@ ManagerBTNode::ManagerBTNode(
       {"CRITICAL_BATTERY_ANIM_ID", unsigned(panther_msgs::LEDAnimation::CRITICAL_BATTERY)},
       {"BATTERY_STATE_ANIM_ID", unsigned(panther_msgs::LEDAnimation::BATTERY_STATE)},
       {"CHARGING_BATTERY_ANIM_ID", unsigned(panther_msgs::LEDAnimation::CHARGING_BATTERY)},
-      // battery constants
+      // battery status constants
       {"POWER_SUPPLY_STATUS_UNKNOWN",
        unsigned(sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN)},
       {"POWER_SUPPLY_STATUS_CHARGING",
@@ -143,8 +142,23 @@ ManagerBTNode::ManagerBTNode(
       {"CRITICAL_BAT_TEMP", critical_bat_temp},
       {"DRIVER_FAN_OFF_TEMP", driver_fan_off_temp},
       {"DRIVER_FAN_ON_TEMP", driver_fan_on_temp},
-      {"FATAL_BAT_TEMP", fatal_bat_temp},
       {"HIGH_BAT_TEMP", high_bat_temp},
+      // battery health constants
+      {"POWER_SUPPLY_HEALTH_UNKNOWN",
+       unsigned(sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN)},
+      {"POWER_SUPPLY_HEALTH_GOOD", unsigned(sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_GOOD)},
+      {"POWER_SUPPLY_HEALTH_OVERHEAT",
+       unsigned(sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_OVERHEAT)},
+      {"POWER_SUPPLY_HEALTH_DEAD", unsigned(sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_DEAD)},
+      {"POWER_SUPPLY_HEALTH_OVERVOLTAGE",
+       unsigned(sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_OVERVOLTAGE)},
+      {"POWER_SUPPLY_HEALTH_UNSPEC_FAILURE",
+       unsigned(sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_UNSPEC_FAILURE)},
+      {"POWER_SUPPLY_HEALTH_COLD", unsigned(sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_COLD)},
+      {"POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE",
+       unsigned(sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE)},
+      {"POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE",
+       unsigned(sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE)},
     };
 
     safety_config_ = create_bt_config(safety_initial_bb);
@@ -236,10 +250,14 @@ BT::NodeConfig ManagerBTNode::create_bt_config(
 void ManagerBTNode::battery_cb(const sensor_msgs::BatteryState::ConstPtr & battery)
 {
   battery_status_ = battery->power_supply_status;
+  battery_health_ = battery->power_supply_health;
   // don't update battery data if unknown status
   if (battery_status_ == sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN) {
     return;
   }
+  if (battery_health_ == sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN) {
+    return;
+  }
 
   battery_temp_ma_->roll(battery->temperature);
   battery_percent_ma_->roll(battery->percentage);
@@ -291,6 +309,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_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());
   // to simplify conditions pass only higher temp of motor drivers