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