diff --git a/panther_battery/README.md b/panther_battery/README.md
index 7118bb3e4..4c15e2e2f 100644
--- a/panther_battery/README.md
+++ b/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.
\ No newline at end of file
+- `~fatal_bat_temp` [*float*, default: **62.0**]: The temperature of the battery at which the battery health state is incorrect.
\ No newline at end of file
diff --git a/panther_battery/src/adc_node.py b/panther_battery/src/adc_node.py
index 5092c0a4d..fdda343a1 100755
--- a/panther_battery/src/adc_node.py
+++ b/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
diff --git a/panther_manager/README.md b/panther_manager/README.md
index 435ca058f..ddc886365 100644
--- a/panther_manager/README.md
+++ b/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.
@@ -175,7 +174,6 @@ 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
diff --git a/panther_manager/config/manager_bt_config.yaml b/panther_manager/config/manager_bt_config.yaml
index 2c9c4f92d..42c87b1c6 100644
--- a/panther_manager/config/manager_bt_config.yaml
+++ b/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
diff --git a/panther_manager/config/safety.xml b/panther_manager/config/safety.xml
index 9c40608a5..f01fe733c 100644
--- a/panther_manager/config/safety.xml
+++ b/panther_manager/config/safety.xml
@@ -2,30 +2,30 @@
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+ _skipIf="bat_temp >= HIGH_BAT_TEMP">
timeout in ms to wait for service to be active
+
+ reason to shutdown robot
+
diff --git a/panther_manager/include/panther_manager/manager_bt_node.hpp b/panther_manager/include/panther_manager/manager_bt_node.hpp
index a7d96e186..26770b5cd 100644
--- a/panther_manager/include/panther_manager/manager_bt_node.hpp
+++ b/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 battery_status_;
+ std::optional battery_health_;
std::optional e_stop_state_;
std::optional io_state_;
diff --git a/panther_manager/src/manager_bt_node.cpp b/panther_manager/src/manager_bt_node.cpp
index b1461cdd6..6e4dac025 100644
--- a/panther_manager/src/manager_bt_node.cpp
+++ b/panther_manager/src/manager_bt_node.cpp
@@ -69,7 +69,6 @@ ManagerBTNode::ManagerBTNode(
// safety tree params
const auto high_bat_temp = ph_->param("safety/high_bat_temp", 55.0);
const auto critical_bat_temp = ph_->param("safety/critical_bat_temp", 59.0);
- const auto fatal_bat_temp = ph_->param("safety/fatal_bat_temp", 62.0);
const auto cpu_fan_on_temp = ph_->param("safety/cpu_fan_on_temp", 70.0);
const auto cpu_fan_off_temp = ph_->param("safety/cpu_fan_off_temp", 60.0);
const auto driver_fan_on_temp = ph_->param("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("aux_state", io_state_.value()->aux_power);
safety_config_.blackboard->set("e_stop_state", e_stop_state_.value());
safety_config_.blackboard->set("fan_state", io_state_.value()->fan);
+ safety_config_.blackboard->set("battery_health", battery_health_.value());
safety_config_.blackboard->set("bat_temp", battery_temp_ma_->get_average());
safety_config_.blackboard->set("cpu_temp", cpu_temp_ma_->get_average());
// to simplify conditions pass only higher temp of motor drivers