From f0fa9d7985c4a824911e0ba5d230fdf743d3bd05 Mon Sep 17 00:00:00 2001 From: Dawid Kmak <73443304+KmakD@users.noreply.github.com> Date: Tue, 25 Jul 2023 12:03:28 +0200 Subject: [PATCH] ROS 1 Panther manager BehaviorTree (#145) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * BT manager node draft (#100) * BT manager node draft * rename package | add AsyncSpinner * update package version * ROS 1 manager bt add basic plugins (#101) * add basic ROS service plugins * improve logging and naming * Ros1 manager bt create basic trees (#104) * create basic trees and update plugins * update README | small fixes * change shutdown nodes to stateful * change param naming * fix shutdown hosts node * small fixes * Update panther_manager/include/panther_manager/moving_average.hpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/include/panther_manager/moving_average.hpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/include/panther_manager/shutdown_host_node.hpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/src/manager_bt_node.cpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/src/manager_bt_node.cpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/README.md Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/config/PantherManagerBT.xml Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/config/PantherManagerBT.xml Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/config/PantherManagerBT.xml Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/src/manager_bt_node.cpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * review fixes * move nh | change set up tree method * Update panther_manager/src/manager_bt_node.cpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/src/manager_bt_node.cpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/src/manager_bt_node.cpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/src/manager_bt_node.cpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/src/manager_bt_node.cpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/src/manager_bt_node.cpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/include/panther_manager/plugins/action/call_trigger_service_node.hpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/plugins/action/call_trigger_service_node.cpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/include/panther_manager/plugins/ros_service_node.hpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * review changes * moving average initial value * Update panther_manager/include/panther_manager/plugins/ros_service_node.hpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * use bat percent average * update includes | unify sleep * review fixes * README draft * moving average unique ptr * Update panther_manager/include/panther_manager/manager_bt_node.hpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/include/panther_manager/manager_bt_node.hpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/plugins/action/call_set_led_animation_service_node.cpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/plugins/action/call_trigger_service_node.cpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/include/panther_manager/moving_average.hpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * review fixes * fix moving avg | review fixes --------- Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * ros plugin | pass nh by reference (#116) * ros plugin | pass nh by reference * formating * Ros1 manager bt signal shutdown (#117) * add SignalShutdown node * name type * review fixes * revert output port * Ros1 bt shutdown nodes (#114) * new shutdown structure * small fixes * remove old file * small fix * review fixes * move Host class * add fsm to ShutdownHost * hosts list as shared_ptr * change remove duplicates method * remove shutdown_timeout bb entry * Update panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/include/panther_manager/plugins/shutdown_host.hpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * review fixes * Update panther_manager/include/panther_manager/plugins/shutdown_host.hpp Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * change ssh_execute_command * fix --------- Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * update shutdown nodes (#120) * update shutdown nodes * review fixes * review fix * Ros1 manager bt update (#121) * Light dimmer (#105) * Light dimmer * Update * delete debug * add brake * fix * formated * battery_animation 3 stage * fix * add david suggestion * Update panther_lights/src/animation/battery_animation.py Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> --------- Co-authored-by: aayli Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * add battery health info (#102) * add health info * Update README.md * add logs * add voltage mean count * update republisher node * fix typo * Update panther_battery/src/adc_node.py Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update adc_node.py * Update roboteq_republisher_node.py * fix typo --------- Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update changelog * 1.0.1 * fix bug (#112) * fix bug * Pawel sugestions --------- Co-authored-by: aayli * Update changelog * 1.0.2 * Ros1 motor enable srv (#113) * add motors enable srv * add reseting can callback times * disable motors if script reset fails * working solution * fix * IO initial state pth1.05 * remove unneccessary method * fix type * update README * Update panther_power_control/README.md Co-authored-by: Paweł Kowalski <82044322+pkowalsk1@users.noreply.github.com> * review changes --------- Co-authored-by: Paweł Kowalski <82044322+pkowalsk1@users.noreply.github.com> * Update changelog * 1.0.3 * Thread lock (#110) * thread lock * undo fcn naming * few more lock * revert nodes * Paweł suggestions * change * e_stop --------- Co-authored-by: aayli * Update changelog * 1.0.4 * APA102 cpp implementation (#108) * Use CPP for APA102 * Add color correction * Utilise gpiod * Remove :: * Fix lights after tests * Fix default pin number * Add set brightness service * Invert LED_SBC_SEL pin * Change SPI mode * Change clock polarity * Reduce number of ROS lgger instances * Celan up code, update to libgpiod 2.0 * Update panther_lights/include/panther_lights/apa102.hpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * Update panther_lights/src/lights_driver_node.cpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * Update panther_lights/src/lights_driver_node.cpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * Update panther_lights/include/panther_lights/apa102.hpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * Review changes * Update panther_lights/src/apa102.cpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * Review cleanup * Add build dependencies * FIx launchfile * Update panther_lights/CMakeLists.txt Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * Update panther_lights/CMakeLists.txt Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * Update panther_lights/src/apa102.cpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * Update panther_lights/src/driver_node.cpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * Update panther_lights/src/driver_node.cpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * Review changes * Fix inlude order * Fix references and APA102 constructor * Add build dependencies for libgpiod v2 * Bring back libgpiod v1 --------- Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * Update changelog * 1.0.5 * Rviz lights plugin (#111) * basic plugin usage - POC * add rear animation texture * fix rviz config * fix .rviz file * Update changelog * 1.0.6 * check can err (#119) * Update changelog * 1.0.7 * update bt project and launch files * remove scheduler_node * fix queuing * remove old variable --------- Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Co-authored-by: aayli Co-authored-by: Paweł Kowalski <82044322+pkowalsk1@users.noreply.github.com> Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Co-authored-by: action-bot * ROS 1 manager bt dosc (#126) * update docs * update docs * ad medyfying bt section * update lights tree png * update README * Update panther_manager/README.md Co-authored-by: Paweł Kowalski <82044322+pkowalsk1@users.noreply.github.com> * Update panther_manager/README.md Co-authored-by: Paweł Kowalski <82044322+pkowalsk1@users.noreply.github.com> * Update panther_manager/README.md Co-authored-by: Paweł Kowalski <82044322+pkowalsk1@users.noreply.github.com> * Update panther_manager/README.md Co-authored-by: Paweł Kowalski <82044322+pkowalsk1@users.noreply.github.com> * Update panther_manager/README.md Co-authored-by: Paweł Kowalski <82044322+pkowalsk1@users.noreply.github.com> * Update panther_manager/README.md Co-authored-by: Paweł Kowalski <82044322+pkowalsk1@users.noreply.github.com> * review fixes --------- Co-authored-by: Paweł Kowalski <82044322+pkowalsk1@users.noreply.github.com> * add battery health check (#141) * add battery health check * update readme * fix typo * fix args * Ros1 update bt images (#143) * add battery health check * update readme * fix typo * fix args * update README imgs * update README * merge fix * Update fan on/off thresholds (#146) * install BT from rosdep * fix rosdep install * fix manager linking --------- Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Co-authored-by: aayli Co-authored-by: Paweł Kowalski <82044322+pkowalsk1@users.noreply.github.com> Co-authored-by: action-bot --- panther_battery/README.md | 2 +- panther_battery/src/adc_node.py | 2 +- panther_bringup/launch/bringup.launch | 23 +- panther_lights/README.md | 23 - panther_lights/launch/lights.launch | 4 - panther_lights/src/controller_node.py | 2 +- panther_lights/src/scheduler_node.py | 206 --------- panther_manager/CMakeLists.txt | 69 ++- panther_manager/README.md | 208 +++++++-- panther_manager/config/Panther106BT.btproj | 17 + panther_manager/config/Panther12BT.btproj | 42 ++ panther_manager/config/lights.xml | 108 +++++ panther_manager/config/manager_bt_config.yaml | 28 ++ .../config/manager_bt_config_106.yaml | 14 + panther_manager/config/safety.xml | 68 +++ panther_manager/config/shutdown.xml | 38 ++ .../panther_manager/manager_bt_node.hpp | 82 ++++ .../panther_manager/moving_average.hpp | 38 ++ .../action/call_set_bool_service_node.hpp | 38 ++ .../call_set_led_animation_service_node.hpp | 42 ++ .../action/call_trigger_service_node.hpp | 35 ++ .../action/shutdown_hosts_from_file_node.hpp | 39 ++ .../action/shutdown_single_host_node.hpp | 43 ++ .../plugins/action/signal_shutdown_node.hpp | 34 ++ .../decorator/tick_after_timeout_node.hpp | 32 ++ .../panther_manager/plugins/plugin.hpp | 61 +++ .../plugins/ros_service_node.hpp | 93 ++++ .../panther_manager/plugins/shutdown_host.hpp | 257 +++++++++++ .../plugins/shutdown_hosts_node.hpp | 156 +++++++ panther_manager/launch/manager.launch | 12 - panther_manager/launch/manager_bt.launch | 18 + panther_manager/package.xml | 12 +- .../action/call_set_bool_service_node.cpp | 37 ++ .../call_set_led_animation_service_node.cpp | 51 +++ .../action/call_trigger_service_node.cpp | 27 ++ .../action/shutdown_hosts_from_file_node.cpp | 68 +++ .../action/shutdown_single_host_node.cpp | 57 +++ .../plugins/action/signal_shutdown_node.cpp | 30 ++ .../decorator/tick_after_timeout_node.cpp | 51 +++ panther_manager/src/main.cpp | 19 + panther_manager/src/manager_bt_node.cpp | 350 +++++++++++++++ panther_manager/src/manager_node.py | 403 ------------------ 42 files changed, 2239 insertions(+), 700 deletions(-) delete mode 100755 panther_lights/src/scheduler_node.py create mode 100644 panther_manager/config/Panther106BT.btproj create mode 100644 panther_manager/config/Panther12BT.btproj create mode 100644 panther_manager/config/lights.xml create mode 100644 panther_manager/config/manager_bt_config.yaml create mode 100644 panther_manager/config/manager_bt_config_106.yaml create mode 100644 panther_manager/config/safety.xml create mode 100644 panther_manager/config/shutdown.xml create mode 100644 panther_manager/include/panther_manager/manager_bt_node.hpp create mode 100644 panther_manager/include/panther_manager/moving_average.hpp create mode 100644 panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp create mode 100644 panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp create mode 100644 panther_manager/include/panther_manager/plugins/action/call_trigger_service_node.hpp create mode 100644 panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp create mode 100644 panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp create mode 100644 panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp create mode 100644 panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp create mode 100644 panther_manager/include/panther_manager/plugins/plugin.hpp create mode 100644 panther_manager/include/panther_manager/plugins/ros_service_node.hpp create mode 100644 panther_manager/include/panther_manager/plugins/shutdown_host.hpp create mode 100644 panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp delete mode 100644 panther_manager/launch/manager.launch create mode 100644 panther_manager/launch/manager_bt.launch create mode 100644 panther_manager/plugins/action/call_set_bool_service_node.cpp create mode 100644 panther_manager/plugins/action/call_set_led_animation_service_node.cpp create mode 100644 panther_manager/plugins/action/call_trigger_service_node.cpp create mode 100644 panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp create mode 100644 panther_manager/plugins/action/shutdown_single_host_node.cpp create mode 100644 panther_manager/plugins/action/signal_shutdown_node.cpp create mode 100644 panther_manager/plugins/decorator/tick_after_timeout_node.cpp create mode 100644 panther_manager/src/main.cpp create mode 100644 panther_manager/src/manager_bt_node.cpp delete mode 100755 panther_manager/src/manager_node.py diff --git a/panther_battery/README.md b/panther_battery/README.md index 7118bb3e4..20b4729eb 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. diff --git a/panther_battery/src/adc_node.py b/panther_battery/src/adc_node.py index 99b5ecdaf..0eccd7f56 100755 --- a/panther_battery/src/adc_node.py +++ b/panther_battery/src/adc_node.py @@ -23,7 +23,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_bringup/launch/bringup.launch b/panther_bringup/launch/bringup.launch index b1d9c7b62..4876cf0f6 100644 --- a/panther_bringup/launch/bringup.launch +++ b/panther_bringup/launch/bringup.launch @@ -6,7 +6,8 @@ - + + @@ -20,6 +21,16 @@ + + @@ -30,7 +41,7 @@ - + - @@ -63,13 +73,14 @@ - + + - + @@ -80,4 +91,4 @@ - + \ No newline at end of file diff --git a/panther_lights/README.md b/panther_lights/README.md index da6f8a4ec..b5ac1ab01 100644 --- a/panther_lights/README.md +++ b/panther_lights/README.md @@ -52,29 +52,6 @@ This node is responsible for displaying frames on the Husarion Panther robot LED - `~global_brightness` [*float*, default: **1.0**]: LED global brightness. Range between [0,1]. - `~num_led` [*int*, default: **46**]: number of LEDs in a single panel. -### scheduler_node.py - -This node is responsible for scheduling animations displayed on LED panels based on the Husarion Panther robot's system state. - -#### Subscribes - -- `/panther/battery` [*sensor_msgs/BatteryState*]: robot battery state. -- `/panther/hardware/e_stop` [*std_msgs/Bool*]: informs if robot is in emergency stop state. - -#### Services subscribed - -- `/panther/lights/controller/set/animation` [*panther_msgs/SetLEDAnimation*]: allows setting animation on LED panel based on animation ID. - -#### Parameters - -- `~battery_state_anim_period` [*float*, default: **120.0**]: time in seconds to wait before repeating animation representing current battery percentage. -- `~charging_battery_anim_period` [*float*, default: **20.0**]: time in seconds to wait before updating the charging battery animation if the battery percentage has changed by the value specified in the `update_charging_anim_step` param. -- `~critical_battery_anim_period` [*float*, default: **15.0**]: time in seconds to wait before repeating animation indicating a critical battery state. -- `~critical_battery_threshold_percent` [*float*, default: **0.1**]: if battery percentage drops below this value, animation indicating a critical battery state will start being displayed. -- `~low_battery_anim_period` [*float*, default: **30.0**]: time in seconds to wait before repeating animation indicating a low battery state. -- `~low_battery_threshold_percent` [*float*, default: **0.4**]: if the battery percentage drops below this value, animation indicating a low battery state will start being displayed. -- `~update_charging_anim_step` [*float*, default: **0.1**]: percentage value representing a step for updating the charging battery animation. - ## Animations Basic animations provided by Husarion are loaded upon node start from [`panther_lights_animations.yaml`](config/panther_lights_animations.yaml) and parsed as a list using the ROS parameter. Supported keys are: diff --git a/panther_lights/launch/lights.launch b/panther_lights/launch/lights.launch index 3d6418303..d0909f29e 100644 --- a/panther_lights/launch/lights.launch +++ b/panther_lights/launch/lights.launch @@ -1,5 +1,4 @@ - @@ -14,7 +13,4 @@ - - \ No newline at end of file diff --git a/panther_lights/src/controller_node.py b/panther_lights/src/controller_node.py index 1cc04d448..ff3c66a52 100755 --- a/panther_lights/src/controller_node.py +++ b/panther_lights/src/controller_node.py @@ -304,7 +304,7 @@ def _rgb_frame_to_img_msg(self, rgb_frame: list, brightness: int, frame_id: str) def _add_animation_to_queue(self, animation: PantherAnimation) -> None: if animation.repeating: interupting_animation = deepcopy(animation) - interupting_animation.init_time = float('inf') + interupting_animation.init_time -= 0.0001 if interupting_animation.priority > 2: interupting_animation.priority = 2 self._anim_queue.put(interupting_animation) diff --git a/panther_lights/src/scheduler_node.py b/panther_lights/src/scheduler_node.py deleted file mode 100755 index 709065dbc..000000000 --- a/panther_lights/src/scheduler_node.py +++ /dev/null @@ -1,206 +0,0 @@ -#!/usr/bin/env python3 - -import rospy -from threading import Lock - -from sensor_msgs.msg import BatteryState -from std_msgs.msg import Bool - -from panther_msgs.msg import LEDAnimation -from panther_msgs.srv import SetLEDAnimation, SetLEDAnimationRequest - - -class LightsSchedulerNode: - def __init__(self, name: str) -> None: - rospy.init_node(name, anonymous=False) - - self._lock = Lock() - - self._battery_percentage = 1.0 - self._battery_status = None - self._charging_percentage = -1.0 # -1.0 to trigger animation when charger gets connected - self._e_stop_state = None - self._led_e_stop_state = None - self._charging_battery_timer = None - self._charger_connected_states = [ - BatteryState.POWER_SUPPLY_STATUS_FULL, - BatteryState.POWER_SUPPLY_STATUS_CHARGING, - BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING, - ] - - self._critical_battery_anim_period = rospy.get_param('~critical_battery_anim_period', 15.0) - self._critical_battery_threshold_percent = rospy.get_param( - '~critical_battery_threshold_percent', 0.1 - ) - self._battery_state_anim_period = rospy.get_param('~battery_state_anim_period', 120.0) - self._low_battery_anim_period = rospy.get_param('~low_battery_anim_period', 30.0) - self._low_battery_threshold_percent = rospy.get_param('~low_battery_threshold_percent', 0.4) - self._update_charging_anim_step = rospy.get_param('~update_charging_anim_step', 0.1) - - # ------------------------------- - # Subscribers - # ------------------------------- - - self._battery_sub = rospy.Subscriber( - 'battery', BatteryState, self._battery_cb, queue_size=1 - ) - self._e_stop_sub = rospy.Subscriber('hardware/e_stop', Bool, self._e_stop_cb, queue_size=1) - - # ------------------------------- - # Service clients - # ------------------------------- - - self._set_led_client = rospy.ServiceProxy( - 'lights/controller/set/animation', SetLEDAnimation - ) - - # Wait for set/animation service - while not rospy.is_shutdown(): - try: - self._set_led_client.wait_for_service(5.0) - break - except rospy.ROSException: - rospy.loginfo(f'[{rospy.get_name()}] Waiting for lights controller node...') - - # ------------------------------- - # Timers - # ------------------------------- - - self._scheduler_timer = rospy.Timer(rospy.Duration(0.2), self._scheduler_timer_cb) # 5 Hz - self._critical_battery_timer = rospy.Timer( - rospy.Duration(self._critical_battery_anim_period), self._critical_battery_timer_cb - ) - self._battery_state_timer = rospy.Timer( - rospy.Duration(self._battery_state_anim_period), self._battery_state_timer_cb - ) - self._low_battery_timer = rospy.Timer( - rospy.Duration(self._low_battery_anim_period), self._low_battery_timer_cb - ) - - rospy.loginfo(f'[{rospy.get_name()}] Node started') - - def _scheduler_timer_cb(self, *args) -> None: - # call animation service only when e_stop state changes - with self._lock: - if ( - self._led_e_stop_state != self._e_stop_state - and not self._battery_status in self._charger_connected_states - ): - req = SetLEDAnimationRequest() - req.repeating = True - if self._e_stop_state: - req.animation.id = LEDAnimation.E_STOP - success = self._call_led_animation_srv(req) - else: - req.animation.id = LEDAnimation.READY - success = self._call_led_animation_srv(req) - if success: - self._led_e_stop_state = self._e_stop_state - - if ( - self._battery_status in self._charger_connected_states - and not self._charging_battery_timer - ): - self._charging_battery_timer_cb() # manually trigger timers callback - self._charging_battery_timer = rospy.Timer( - rospy.Duration(6.0), - self._charging_battery_timer_cb, - ) - elif ( - not self._battery_status in self._charger_connected_states - and self._charging_battery_timer - ): - self._charging_battery_timer.shutdown() - self._charging_battery_timer.join() - del self._charging_battery_timer - self._charging_battery_timer = None - self._charging_percentage = -1.0 - self._led_e_stop_state = None - - def _charging_battery_timer_cb(self, *args) -> None: - if self._e_stop_state: - req = SetLEDAnimationRequest() - req.animation.id = LEDAnimation.E_STOP - self._call_led_animation_srv(req) - - if ( - abs(self._battery_percentage - self._charging_percentage) - >= self._update_charging_anim_step - ): - self._charging_percentage = ( - round(self._battery_percentage / self._update_charging_anim_step) - * self._update_charging_anim_step - ) - req = SetLEDAnimationRequest() - req.repeating = True - req.animation.id = LEDAnimation.CHARGING_BATTERY - req.animation.param = str(self._battery_percentage) - self._call_led_animation_srv(req) - - def _critical_battery_timer_cb(self, *args) -> None: - with self._lock: - if ( - self._battery_status == BatteryState.POWER_SUPPLY_STATUS_DISCHARGING - and self._battery_percentage < self._critical_battery_threshold_percent - ): - req = SetLEDAnimationRequest() - req.animation.id = LEDAnimation.CRITICAL_BATTERY - self._call_led_animation_srv(req) - - def _battery_state_timer_cb(self, *args) -> None: - with self._lock: - if self._battery_status in [ - BatteryState.POWER_SUPPLY_STATUS_DISCHARGING, - BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING, - ]: - req = SetLEDAnimationRequest() - req.animation.id = LEDAnimation.BATTERY_STATE - req.animation.param = str(self._battery_percentage) - self._call_led_animation_srv(req) - - def _low_battery_timer_cb(self, *args) -> None: - with self._lock: - if ( - self._battery_status == BatteryState.POWER_SUPPLY_STATUS_DISCHARGING - and self._critical_battery_threshold_percent - <= self._battery_percentage - < self._low_battery_threshold_percent - ): - req = SetLEDAnimationRequest() - req.animation.id = LEDAnimation.LOW_BATTERY - self._call_led_animation_srv(req) - - def _battery_cb(self, battery_state: BatteryState) -> None: - with self._lock: - self._battery_percentage = battery_state.percentage - self._battery_status = battery_state.power_supply_status - - def _e_stop_cb(self, e_stop_state: Bool) -> None: - with self._lock: - self._e_stop_state = e_stop_state.data - - def _call_led_animation_srv(self, req: SetLEDAnimationRequest) -> bool: - try: - self._set_led_client.wait_for_service(5.0) - response = self._set_led_client.call(req) - rospy.logdebug( - f'[{rospy.get_name()}] Setting animation with ID: {req.animation.id}. Response: ({response})' - ) - return response.success - except (rospy.ServiceException, rospy.ROSException) as err: - rospy.logerr( - f'Calling {self._set_led_client.resolved_name} service for message id {req.animation.id} failed. Error: {err}' - ) - return False - - -def main(): - lights_scheduler_node = LightsSchedulerNode('lights_scheduler_node') - rospy.spin() - - -if __name__ == '__main__': - try: - main() - except rospy.ROSInterruptException: - pass diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index 91fec81e7..92a585369 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -1,14 +1,77 @@ cmake_minimum_required(VERSION 3.0.2) project(panther_manager) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package( + libssh REQUIRED + yaml-cpp REQUIRED +) + find_package(catkin REQUIRED COMPONENTS - rospy + behaviortree_cpp panther_msgs + roscpp + roslib + rospy + sensor_msgs + std_msgs + std_srvs ) -catkin_package() +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS panther_msgs roscpp +) + +# actions +add_library(call_set_bool_service_bt_node SHARED plugins/action/call_set_bool_service_node.cpp) +list(APPEND plugin_libs call_set_bool_service_bt_node) + +add_library(call_trigger_service_bt_node SHARED plugins/action/call_trigger_service_node.cpp) +list(APPEND plugin_libs call_trigger_service_bt_node) + +add_library(call_set_led_animation_service_bt_node SHARED plugins/action/call_set_led_animation_service_node.cpp) +list(APPEND plugin_libs call_set_led_animation_service_bt_node) + +add_library(shutdown_single_host_bt_node SHARED plugins/action/shutdown_single_host_node.cpp) +list(APPEND plugin_libs shutdown_single_host_bt_node) +target_link_libraries(shutdown_single_host_bt_node ssh) + +add_library(shutdown_hosts_from_file_bt_node SHARED plugins/action/shutdown_hosts_from_file_node.cpp) +list(APPEND plugin_libs shutdown_hosts_from_file_bt_node) +target_link_libraries(shutdown_hosts_from_file_bt_node ssh yaml-cpp) + +add_library(signal_shutdown_bt_node SHARED plugins/action/signal_shutdown_node.cpp) +list(APPEND plugin_libs signal_shutdown_bt_node) + +# decorators +add_library(tick_after_timeout_bt_node SHARED plugins/decorator/tick_after_timeout_node.cpp) +list(APPEND plugin_libs tick_after_timeout_bt_node) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +foreach(bt_plugin ${plugin_libs}) + add_dependencies(${bt_plugin} ${catkin_EXPORTED_TARGETS}) + target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) + target_link_libraries(${bt_plugin} ${CMAKE_DL_LIBS} ${catkin_LIBRARIES} ${GCC_COVERAGE_LINK_FLAGS}) +endforeach() + +add_executable(manager_bt_node + src/main.cpp + src/manager_bt_node.cpp +) +add_dependencies(manager_bt_node ${catkin_EXPORTED_TARGETS}) +target_link_libraries(manager_bt_node + ${catkin_LIBRARIES} + ${plugin_libs} +) -install(DIRECTORY +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/panther_manager/README.md b/panther_manager/README.md index a4a109b4c..27254c39e 100644 --- a/panther_manager/README.md +++ b/panther_manager/README.md @@ -4,17 +4,17 @@ A package containing nodes responsible for high-level control of Husarion Panthe ## ROS Nodes -### manager_node.py +### manager_bt_node -Node responsible for managing the Husarion Panther robot. Controls built-in fan and software shutdown of components. +Node responsible for managing the Husarion Panther robot. Composes control of three behavior trees responsible for handling LED panels, safety features and software shutdown of components. To set up connection with a new user computer, login to the built-in computer with `ssh husarion@10.15.20.2`. -Add built-in computer's public key to **known_hosts** of a computer you want to shut down automatically: +Add built-in computer's public key to **known_hosts** of a computer you want to shutdown automatically: ``` bash ssh-copy-id username@10.15.20.XX ``` -To allow your computer to be shut down without sudo password, ssh into it and execute: +To allow your computer to be shutdown without sudo password, ssh into it and execute: ``` bash echo $USERNAME 'ALL=(ALL) NOPASSWD: /sbin/poweroff, /sbin/reboot, /sbin/shutdown' | sudo EDITOR='tee -a' visudo ``` @@ -22,50 +22,186 @@ echo $USERNAME 'ALL=(ALL) NOPASSWD: /sbin/poweroff, /sbin/reboot, /sbin/shutdown #### Subscribes - `/panther/battery` [*sensor_msgs/BatteryState*]: state of internal battery. -- `/panther/driver/motor_controllers_state` [*panther_msgsDriverState*]: state of motor controllers. +- `/panther/driver/motor_controllers_state` [*panther_msgs/DriverState*]: state of motor controllers. - `/panther/hardware/e_stop` [*std_msgs/Bool*]: state of emergency stop. - `/panther/hardware/io_state` [*panther_msgs/IOState*]: state of IO pins. -- `/panther/system_status` [*panther_msgs/SystemStatus*]: state of system including CPU temperature and load. +- `/panther/system_status` [*panther_msgs/SystemStatus*]: state of the system including CPU temperature and load. -#### Services +#### Services subscribed (for default trees) -- `/panther/manager/overwrite_fan_control` [*std_srvs/SetBool*]: overwrites fan control, setting it to always on. - -#### Service clients - -- `hardware/aux_power_enable` [*std_srvs/SetBool*]: enables aux power output. -- `hardware/e_stop_trigger` [*std_srvs/Trigger*]: triggers e-stop. -- `hardware/fan_enable` [*std_srvs/SetBool*]: enables fan. +- `/panther/hardware/aux_power_enable` [*std_srvs/SetBool*]: enables aux power output. +- `/panther/hardware/e_stop_trigger` [*std_srvs/Trigger*]: triggers e-stop. +- `/panther/hardware/fan_enable` [*std_srvs/SetBool*]: enables fan. +- `/panther/lights/controller/set/animation` [*panther_msgs/SetLEDAnimation*]: allows setting animation on LED panel based on animation ID. #### Parameters -- `~battery_window_len` [*int*, default: **6**]: moving average window length used to smooth out temperature readings of battery. -- `~cpu_fan_off_temp` [*float*, default: **60.0**]: temperature in **deg C** of CPU, above which the fan is turned off. -- `~cpu_fan_on_temp` [*float*, default: **70.0**]: temperature in **deg C** of any drivers above which the fan is turned on. -- `~cpu_window_len` [*int*, default: **6**]: moving average window length used to smooth out temperature readings of CPU. -- `~critical_bat_temp` [*float*, default: **59.0**]: extends `high_bat_temp` by turning off AUX power. -- `~default_identity_file` [*string*, default: **~/.ssh/id_rsa**]: path to find identity file for SSH to be used as default. -- `~driver_fan_off_temp` [*float*, default: **35.0**]: temperature in **deg C** of any drivers below which the fan is turned off. -- `~driver_fan_on_temp` [*float*, default: **45.0**]: temperature in **deg C** of any drivers above which the fan is turned on. -- `~driver_window_len` [*int*, default: **6**]: moving average window length used to smooth out temperature readings of each driver. -- `~fatal_bat_temp` [*float*, default: **62.0**]: temperature of battery above which robot shuts down. -- `~high_bat_temp` [*float*, default: **55.0**]: temperature of battery above which robots starts displaying warning log and e-stop is triggered. -- `~hosts` [*list*, default: **None**]: list of hosts to request shutdown. - - `cmd` [*string*, default: **sudo shutdown now**]: command executed on shutdown of given device. - - `identity_file` [*string*, default: **None**]: SSH identity file global path. If not set, defaults to `~default_identity_file`. +- `~battery_percent_window_len` [*int*, default: **6**]: moving average window length used to smooth out battery percentage readings. +- `~battery_temp_window_len` [*int*, default: **6**]: moving average window length used to smooth out temperature readings of battery. +- `~bt_project_file` [*string*, default: **$(find panther_manager)/config/PantherBT.btproj**]: path to a BehaviorTree project. +- `~cpu_temp_window_len` [*int*, default: **6**]: moving average window length used to smooth out temperature readings of CPU. +- `~driver_temp_window_len` [*int*, default: **6**]: moving average window length used to smooth out temperature readings of each driver. +- `~launch_lights_tree` [*bool*, default: **true**]: launch behavior tree responsible for scheduling animations on Panther LED panels. +- `~launch_safety_tree` [*bool*, default: **true**]: launch behavior tree responsible for managing Panther safety measures. +- `~launch_shutdown_tree` [*bool*, default: **true**]: launch behavior tree responsible for the gentle shutdown of robot components. +- `~lights/battery_state_anim_period` [*float*, default: **120.0**]: time in seconds to wait before repeating animation representing current battery percentage. +- `~lights/critical_battery_anim_period` [*float*, default: **15.0**]: time in seconds to wait before repeating animation indicating a critical battery state. +- `~lights/critical_battery_threshold_percent` [*float*, default: **0.1**]: if battery percentage drops below this value, animation indicating a critical battery state will start being displayed. +- `~lights/low_battery_anim_period` [*float*, default: **30.0**]: time in seconds to wait before repeating animation indicating a low battery state. +- `~lights/low_battery_threshold_percent` [*float*, default: **0.4**]: if the battery percentage drops below this value, animation indicating a low battery state will start being displayed. +- `~lights/update_charging_anim_step` [*float*, default: **0.1**]: percentage value representing a step for updating the charging battery animation. +- `~plugin_libs` [*list*, default: **Empty list**]: list with names of plugins that are used in BT project. +- `~ros_plugin_libs` [*list*, default: **Empty list**]: list with names of ROS plugins that are used in a BT project. +- `~safety/cpu_fan_off_temp` [*float*, default: **60.0**]: temperature in **deg C** of CPU, below which the fan is turned off. +- `~safety/cpu_fan_on_temp` [*float*, default: **70.0**]: temperature in **deg C** of CPU, above which the fan is turned on. +- `~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/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. - `ip` [*string*, default: **None**]: IP of a host to shutdown over SSH. + - `ping_for_success` [*bool*, default: **true**]: ping host until it is not available or timeout is reached. + - `port` [*string*, default: **22**]: SSH communication port. + - `timeout` [*string*, default: **5.0**]: time in seconds to wait for the host to shutdown. - `username` [*string*, default: **None**]: username used to log in to over SSH. -- `~fun_enable_hysteresis` [*float*, default: **60.0**]: minimum time of fan being turned on. -- `~overwrite_fan_control` [*bool*, default: **false**]: enable the fan to be always on at start of the node. It can be turned off later by `/panther/manager/overwrite_fan_control` service. -- `~self_identity_file` [*float*, default: **~/.ssh/id_rsa**]: identity file global path to shutdown device running this node. If not, set defaults to `~default_identity_file`. -- `~self_ip` [*string*, default: **127.0.0.1**]: IP used to shutdown device running this node. -- `~self_username` [*string*, default: **husarion**]: username used to shutdown device running this node. -- `~shutdown_timeout` [*float*, default: **15.0**]: time in seconds to wait for graceful shutdown. After timeout power will be cut from all devices. ### system_status_node.py -Publishes stats status of the internal computer. Stats include CPU utilization and temperature, as well as disc and RAM usage. +Publishes stats status of the built-in computer. Stats include CPU utilization and temperature, as well as disc and RAM usage. #### Publishes -- `/panther/system_status` [*panther_msgs/SystemStatus*]: information about internal computer CPU temperature, utilization and disc and RAM usage. \ No newline at end of file +- `/panther/system_status` [*panther_msgs/SystemStatus*]: information about internal computer CPU temperature, utilization, disc, and RAM usage. + +## BehaviorTree + +For a BehaviorTree project to work correctly, it must contain three trees with names as described below. However, if any of the parameters (`~launch_lights_tree`, `~launch_safety_tree`, `~launch_shutdown_tree`) is set to false, the corresponding tree becomes unnecessary and can be omitted from the project. Files with trees XML descriptions can be shared between projects. Each tree is provided with a set of default blackboard entries (described below) which can be used to specify the behavior of a given tree. + +### Nodes + +#### Actions + +- `CallSetBoolService` - allows calling standard **std_srvs/SetBool** ROS service. Provided ports are: + - `data` [*input*, *bool*, default: **None**]: service data - `true` or `false` value. + - `service_name` [*input*, *string*, default: **None**]: ROS service name. + - `timeout` [*input*, *unsigned*, default: **100**]: time in seconds to wait for service to become available. +- `CallSetLedAnimationService` - allows calling custom type **panther_msgs/SetLEDAnimation** ROS service. Provided ports are: + - `id` [*input*, *unsigned*, default: **None**]: animation ID. + - `param` [*input*, *string*, default: **None**]: optional parameter. + - `repeating` [*input*, *bool*, default: **false**]: indicates if animation should repeat. + - `service_name` [*input*, *string*, default: **None**]: ROS service name. + - `timeout` [*input*, *unsigned*, default: **100**]: time in seconds to wait for service to become available. +- `CallTriggerService` - allows calling standard **std_srvs/Trigger** ROS service. Provided ports are: + - `service_name` [*input*, *string*, default: **None**]: ROS service name. + - `timeout` [*input*, *unsigned*, default: **100**]: time in seconds to wait for service to become available. +- `ShutdownHostsFromFile` - allows to shutdown devices based on a YAML file. Returns `SUCCESS` only when a YAML file is valid and the shutdown of all defined hosts was successful. Provided ports are: + - `shutdown_host_file` [*input*, *string*, default: **None**]: global path to YAML file with hosts to shutdown. +- `ShutdownSingleHost` - allows to shutdown single device. Will return `SUCCESS` only when the device was successfully shutdown. Provided ports are: + - `command` [*input*, *string*, default: **sudo shutdown now**]: command to execute on shutdown. + - `ip` [*input*, *string*, default: **None**]: IP of the host to shutdown. + - `ping_for_success` [*input*, *bool*, default: **true**]: ping host until it is not available or timeout is reached. + - `port` [*input*, *string*, default: **22**]: SSH communication port. + - `timeout` [*input*, *string*, default: **5.0**]: time in seconds to wait for the host to shutdown. + - `user` [*input*, *string*, default: **None**]: user to log into while executing shutdown command. +- `SignalShutdown` - signals shutdown of the robot. Provided ports are: + - `message` [*input*, *string*, default: **None**]: message with reason for robot to shutdown. + +#### Decorators + +- `TickAfterTimeout` - will skip a child until the specified time has passed. It can be used to specify the frequency at which a node or subtree is triggered. Provided ports are: + - `timeout` [*input*, *unsigned*, default: **None**]: time in seconds to wait before ticking child again. + +### Trees + +#### Lights + +Tree responsible for scheduling animations displayed on LED panels, based on the Husarion Panther robot's system state. + +

+ +

+ +Default blackboard entries: +- `battery_percent` [*float*, defautl: **None**]: moving average of battery percentage. +- `battery_percent_round` [*string*, default: **None**] battery percentage raunded to a value specified with `~lights/update_charging_anim_step` parameter and casted to string. +- `battery_status` [*unsigned*, defautl: **None**]: current battery status. +- `charging_anim_percent` [*string*, default: **None**]: the charging animation battery percentage value, casted to a string. +- `current_anim_id` [*int*, default: **-1**]: ID of currently displayed animation. +- `e_stop_state` [*bool*, defautl: **None**]: state of emergency stop. + +Default constant blackboard entries: +- `BATTERY_STATE_ANIM_PERIOD` [*float*, default: **120.0**]: refers to `battery_state_anim_period` ROS parameter. +- `CRITICAL_BATTERY_ANIM_PERIOD` [*float*, default: **15.0**]: refers to `critical_battery_anim_period` ROS parameter. +- `CRITICAL_BATTERY_THRESHOLD_PERCENT` [*float*, default: **0.1**]: refers to `critical_battery_threshold_percent` ROS parameter. +- `LOW_BATTERY_ANIM_PERIOD` [*float*, default: **30.0**]: refers to `low_battery_anim_period` ROS parameter. +- `LOW_BATTERY_THRESHOLD_PERCENT` [*float*, default: **0.4**]: refers to `low_battery_threshold_percent` ROS parameter. +- `E_STOP_ANIM_ID` [*unsigned*, value: **0**]: animation ID constant parsed from `panther_msgs::LEDAnimation::E_STOP`. +- `READY_ANIM_ID` [*unsigned*, value: **1**]: animation ID constant parsed from `panther_msgs::LEDAnimation::READY`. +- `ERROR_ANIM_ID` [*unsigned*, value: **2**]: animation ID constant parsed from `panther_msgs::LEDAnimation::ERROR`. +- `MANUAL_ACTION_ANIM_ID` [*unsigned*, value: **3**]: animation ID constant parsed from `panther_msgs::LEDAnimation::MANUAL_ACTION`. +- `AUTONOMOUS_ACTION_ANIM_ID` [*unsigned*, value: **4**]: animation ID constant parsed from `panther_msgs::LEDAnimation::AUTONOMOUS_ACTION`. +- `GOAL_ACHIEVED_ANIM_ID` [*unsigned*, value: **5**]: animation ID constant parsed from `panther_msgs::LEDAnimation::GOAL_ACHIEVED`. +- `LOW_BATTERY_ANIM_ID` [*unsigned*, value: **6**]: animation ID constant parsed from `panther_msgs::LEDAnimation::LOW_BATTERY`. +- `CRITICAL_BATTERY_ANIM_ID` [*unsigned*, value: **7**]: animation ID constant parsed from `panther_msgs::LEDAnimation::CRITICAL_BATTERY`. +- `BATTERY_STATE_ANIM_ID` [*unsigned*, value: **8**]: animation ID constant parsed from `panther_msgs::LEDAnimation::BATTERY_STATE`. +- `CHARGING_BATTERY_ANIM_ID` [*unsigned*, value: **9**]: animation ID constant parsed from `panther_msgs::LEDAnimation::CHARGING_BATTERY`. +- `POWER_SUPPLY_STATUS_UNKNOWN` [*unsigned*, value: **0**]: power supply status constant parsed from `sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN`. +- `POWER_SUPPLY_STATUS_CHARGING` [*unsigned*, value: **1**]: power supply status constant parsed from `sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_CHARGING`. +- `POWER_SUPPLY_STATUS_DISCHARGING` [*unsigned*, value: **2**]: power supply status constant parsed from `sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING`. +- `POWER_SUPPLY_STATUS_NOT_CHARGING` [*unsigned*, value: **3**]: power supply status constant parsed from `sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_NOT_CHARGING`. +- `POWER_SUPPLY_STATUS_FULL` [*unsigned*, value: **4**]: power supply status constant parsed from `sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_FULL`. + +### Safety + +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. + +

+ +

+ +Default blackboard entries: +- `aux_state` [*bool*, default: **None**]: state of AUX power. +- `bat_temp` [*double*, default: **None**]: moving average of battery temperature. +- `cpu_temp` [*double*, default: **None**]: moving average of cpu temperature +- `driver_temp` [*double*, default: **None**]: moving average of driver temperature, for condition simplification only motor driver with higher temperature is considered. +- `e_stop_state` [*bool*, default: **None**]: state of emergency stop. +- `fan_state` [*bool*, default: **None**]: state of fan. + +Default constant blackboard entries: +- `CPU_FAN_OFF_TEMP` [*float*, default: **60.0**]: refers to `cpu_fan_off_temp` ROS parameter. +- `CPU_FAN_ON_TEMP` [*float*, default: **70.0**]: refers to `cpu_fan_on_temp` ROS parameter. +- `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. +- `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. + +

+ +

+ +Default constant blackboard entries: + - `SHUTDOWN_HOSTS_FILE` [*string*, default: **None**]: refers to `shutdown_hosts_file` ROS parameter. + +### Modifying behavior trees + +Each behavior tree can be easily customized to enhance its functions and capabilities. To achieve this, we recommend using Groot2, a powerful tool for developing and modifying behavior trees. To install Groot2 and learn how to use it, please refer to the official guidelines provided [here](https://www.behaviortree.dev/groot). + +When creating a new BehaviorTree project, it is advised to use an existing project as a guideline and leverage it for reference. You can study the structure and implementation of the behavior trees in the existing project to inform your own development process. The project should consist of three behavior trees: `Lights`, `Safety`, `Shutdown`. Additionally, you have the option to incorporate some of the files used in the existing project into your own project. By utilizing these files, you can benefit from the work already done and save time and effort in developing certain aspects of the behavior trees. + +> **Note**: +> It is essential to exercise caution when modifying the trees responsible for safety or shutdown and ensure that default behaviors are not removed. +> +> Remember to use the files from the existing project in a way that avoids conflicts, such as by saving them under new names to ensure they don't overwrite any existing files. + +When modifying behavior trees, you have the flexibility to use standard BehaviorTree.CPP nodes or leverage nodes created specifically for Panther, as detailed in the [Nodes](#nodes) section. Additionally, if you have more specific requirements, you can even create your own custom Behavior Tree nodes. However, this will involve modifying the package and rebuilding the project accordingly. + +To use your customized project, you need to provide the `bt_project_file` launch argument when running `panther_bringup.launch` file. Here's an example of how to launch the project with the specified BehaviorTree project file: + +``` +roslaunch --wait panther_bringup bringup.launch bt_project_file:=/path/to/bt/project/file +``` diff --git a/panther_manager/config/Panther106BT.btproj b/panther_manager/config/Panther106BT.btproj new file mode 100644 index 000000000..56d5f2f8f --- /dev/null +++ b/panther_manager/config/Panther106BT.btproj @@ -0,0 +1,17 @@ + + + + + + + animation ID + optional parameter + indicates if animation should repeat + ROS service name + time in ms to wait for service to be active + + + time in s to wait before ticking child again + + + diff --git a/panther_manager/config/Panther12BT.btproj b/panther_manager/config/Panther12BT.btproj new file mode 100644 index 000000000..10d436042 --- /dev/null +++ b/panther_manager/config/Panther12BT.btproj @@ -0,0 +1,42 @@ + + + + + + + + + true / false value + ROS service name + time in ms to wait for service to be active + + + animation ID + optional parameter + indicates if animation should repeat + ROS service name + time in ms to wait for service to be active + + + ROS service name + timeout in ms to wait for service to be active + + + global path to YAML file with hosts to shutdown + + + command to execute on shutdown + ip of the host to shutdown + ping host unitl it is not available or timeout is reached + SSH communication port + time in s to wait for host to shutdown + user to log into while executing shutdown command + + + reason to shutdown robot + + + time in s to wait before ticking child again + + + diff --git a/panther_manager/config/lights.xml b/panther_manager/config/lights.xml new file mode 100644 index 000000000..9fbae5b13 --- /dev/null +++ b/panther_manager/config/lights.xml @@ -0,0 +1,108 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + animation ID + optional parameter + indicates if animation should repeat + ROS service name + time in ms to wait for service to be active + + + time in s to wait before ticking child again + + + + diff --git a/panther_manager/config/manager_bt_config.yaml b/panther_manager/config/manager_bt_config.yaml new file mode 100644 index 000000000..d3dca045b --- /dev/null +++ b/panther_manager/config/manager_bt_config.yaml @@ -0,0 +1,28 @@ +battery_temp_window_len: 6 +battery_percent_window_len: 6 +cpu_temp_window_len: 6 +driver_temp_window_len: 6 +shutdown_timeout: 15.0 +lights: + critical_battery_anim_period: 15.0 + critical_battery_threshold_percent: 0.1 + battery_state_anim_period: 120.0 + low_battery_anim_period: 30.0 + low_battery_threshold_percent: 0.4 + update_charging_anim_step: 0.1 +safety: + high_bat_temp: 55.0 + critical_bat_temp: 59.0 + cpu_fan_on_temp: 70.0 + cpu_fan_off_temp: 60.0 + driver_fan_on_temp: 50.0 + driver_fan_off_temp: 45.0 +plugin_libs: + - tick_after_timeout_bt_node + - shutdown_single_host_bt_node + - shutdown_hosts_from_file_bt_node + - signal_shutdown_bt_node +ros_plugin_libs: + - call_set_bool_service_bt_node + - call_trigger_service_bt_node + - call_set_led_animation_service_bt_node diff --git a/panther_manager/config/manager_bt_config_106.yaml b/panther_manager/config/manager_bt_config_106.yaml new file mode 100644 index 000000000..4a915465d --- /dev/null +++ b/panther_manager/config/manager_bt_config_106.yaml @@ -0,0 +1,14 @@ +launch_safety_tree: false +launch_shutdown_tree: false +battery_percent_window_len: 6 +lights: + critical_battery_anim_period: 15.0 + critical_battery_threshold_percent: 0.1 + battery_state_anim_period: 120.0 + low_battery_anim_period: 30.0 + low_battery_threshold_percent: 0.4 + update_charging_anim_step: 0.1 +plugin_libs: + - tick_after_timeout_bt_node +ros_plugin_libs: + - call_set_led_animation_service_bt_node diff --git a/panther_manager/config/safety.xml b/panther_manager/config/safety.xml new file mode 100644 index 000000000..f01fe733c --- /dev/null +++ b/panther_manager/config/safety.xml @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + true / false value + ROS service name + time in ms to wait for service to be active + + + ROS service name + timeout in ms to wait for service to be active + + + reason to shutdown robot + + + + diff --git a/panther_manager/config/shutdown.xml b/panther_manager/config/shutdown.xml new file mode 100644 index 000000000..335f93303 --- /dev/null +++ b/panther_manager/config/shutdown.xml @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + global path to YAML file with hosts to shutdown + + + (optional) command to execute on shutdown + ip of the host to shutdown + + + + user to log into while executing shutdown command + + + + diff --git a/panther_manager/include/panther_manager/manager_bt_node.hpp b/panther_manager/include/panther_manager/manager_bt_node.hpp new file mode 100644 index 000000000..26770b5cd --- /dev/null +++ b/panther_manager/include/panther_manager/manager_bt_node.hpp @@ -0,0 +1,82 @@ +#ifndef PANTHER_MANAGER_MANAGER_BT_NODE_HPP_ +#define PANTHER_MANAGER_MANAGER_BT_NODE_HPP_ + +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include + +#include +#include +#include + +#include + +namespace panther_manager +{ + +class ManagerBTNode +{ +public: + ManagerBTNode( + const std::shared_ptr & nh, const std::shared_ptr & ph); + ~ManagerBTNode() {} + +private: + bool launch_shutdown_tree_; + 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_; + + ros::Subscriber battery_sub_; + ros::Subscriber driver_state_sub_; + ros::Subscriber e_stop_sub_; + ros::Subscriber io_state_sub_; + ros::Subscriber system_status_sub_; + ros::Timer lights_tree_timer_; + ros::Timer safety_tree_timer_; + std::shared_ptr nh_; + std::shared_ptr ph_; + + BT::BehaviorTreeFactory factory_; + BT::NodeConfig lights_config_; + BT::NodeConfig safety_config_; + BT::NodeConfig shutdown_config_; + BT::NodeStatus lights_tree_status_; + BT::NodeStatus safety_tree_status_; + BT::NodeStatus shutdown_tree_status_; + BT::Tree lights_tree_; + BT::Tree safety_tree_; + BT::Tree shutdown_tree_; + + std::unique_ptr> battery_temp_ma_; + std::unique_ptr> battery_percent_ma_; + std::unique_ptr> cpu_temp_ma_; + std::unique_ptr> front_driver_temp_ma_; + std::unique_ptr> rear_driver_temp_ma_; + + void battery_cb(const sensor_msgs::BatteryState::ConstPtr & battery); + void driver_state_cb(const panther_msgs::DriverState::ConstPtr & driver_state); + void e_stop_cb(const std_msgs::Bool::ConstPtr & e_stop); + void io_state_cb(const panther_msgs::IOState::ConstPtr & io_state); + void system_status_cb(const panther_msgs::SystemStatus::ConstPtr & system_status); + void safety_tree_timer_cb(); + void lights_tree_timer_cb(); + void shutdown_robot(const std::string & reason); + BT::NodeConfig create_bt_config(const std::map & bb_values = {}) const; +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_MANAGER_BT_NODE_HPP_ \ No newline at end of file diff --git a/panther_manager/include/panther_manager/moving_average.hpp b/panther_manager/include/panther_manager/moving_average.hpp new file mode 100644 index 000000000..74db97af1 --- /dev/null +++ b/panther_manager/include/panther_manager/moving_average.hpp @@ -0,0 +1,38 @@ +#ifndef PANTHER_MANAGER_MOVING_AVERAGE_HPP_ +#define PANTHER_MANAGER_MOVING_AVERAGE_HPP_ + +#include + +namespace panther_manager +{ + +template +class MovingAverage +{ +public: + MovingAverage(const unsigned window_size = 5, const T initial_value = T(0)) + : window_size_(window_size), + sum_(initial_value * static_cast(window_size_)), + values_(window_size, initial_value) + { + } + + void roll(const T value) + { + sum_ -= values_.front(); + values_.pop_front(); + values_.push_back(value); + sum_ += value; + } + + T get_average() const { return sum_ / static_cast(window_size_); } + +private: + const unsigned window_size_; + std::deque values_; + T sum_; +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_MOVING_AVERAGE_HPP_ \ No newline at end of file diff --git a/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp b/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp new file mode 100644 index 000000000..74666f919 --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp @@ -0,0 +1,38 @@ +#ifndef PANTHER_MANAGER_CALL_SET_BOOL_SERVICE_NODE_HPP_ +#define PANTHER_MANAGER_CALL_SET_BOOL_SERVICE_NODE_HPP_ + +#include +#include + +#include +#include + +#include + +#include + +namespace panther_manager +{ + +class CallSetBoolService : public RosServiceNode +{ +public: + CallSetBoolService( + const std::string & name, const BT::NodeConfig & conf, + const std::shared_ptr & nh) + : RosServiceNode(name, conf, nh) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({BT::InputPort("data", "true / false value")}); + } + + void update_request(std_srvs::SetBool::Request & request) override; + virtual BT::NodeStatus on_response(const std_srvs::SetBool::Response & response); +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_CALL_SET_BOOL_SERVICE_NODE_HPP_ \ No newline at end of file diff --git a/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp b/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp new file mode 100644 index 000000000..7752e4ca7 --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp @@ -0,0 +1,42 @@ +#ifndef PANTHER_MANAGER_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_ +#define PANTHER_MANAGER_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_ + +#include +#include + +#include +#include + +#include + +#include + +namespace panther_manager +{ + +class CallSetLedAnimationService : public RosServiceNode +{ +public: + CallSetLedAnimationService( + const std::string & name, const BT::NodeConfig & conf, + const std::shared_ptr & nh) + : RosServiceNode(name, conf, nh) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("id", "animation ID"), + BT::InputPort("param", "optional parameter"), + BT::InputPort("repeating", "indicates if animation should repeat"), + }); + } + + void update_request(panther_msgs::SetLEDAnimation::Request & request) override; + virtual BT::NodeStatus on_response(const panther_msgs::SetLEDAnimation::Response & response); +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_CALL_SET_LED_ANIMATION_SERVICE_NODE_HPP_ \ No newline at end of file diff --git a/panther_manager/include/panther_manager/plugins/action/call_trigger_service_node.hpp b/panther_manager/include/panther_manager/plugins/action/call_trigger_service_node.hpp new file mode 100644 index 000000000..2c0d14a82 --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/action/call_trigger_service_node.hpp @@ -0,0 +1,35 @@ +#ifndef PANTHER_MANAGER_CALL_TRIGGER_SERVICE_HPP_ +#define PANTHER_MANAGER_CALL_TRIGGER_SERVICE_HPP_ + +#include +#include + +#include +#include + +#include + +#include + +namespace panther_manager +{ + +class CallTriggerService : public RosServiceNode +{ +public: + CallTriggerService( + const std::string & name, const BT::NodeConfig & conf, + const std::shared_ptr & nh) + : RosServiceNode(name, conf, nh) + { + } + + static BT::PortsList providedPorts() { return providedBasicPorts({}); } + + void update_request(std_srvs::Trigger::Request & request) {} + virtual BT::NodeStatus on_response(const std_srvs::Trigger::Response & response); +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_CALL_TRIGGER_SERVICE_HPP_ \ No newline at end of file diff --git a/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp b/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp new file mode 100644 index 000000000..0bb8b6689 --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp @@ -0,0 +1,39 @@ +#ifndef PANTHER_MANAGER_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_ +#define PANTHER_MANAGER_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_ + +#include +#include +#include + +#include +#include + +#include +#include + +namespace panther_manager +{ + +class ShutdownHostsFromFile : public ShutdownHosts +{ +public: + ShutdownHostsFromFile(const std::string & name, const BT::NodeConfig & conf) + : ShutdownHosts(name, conf) + { + } + + static BT::PortsList providedPorts() + { + return { + BT::InputPort( + "shutdown_hosts_file", "global path to YAML file with hosts to shutdown"), + }; + } + +private: + void update_hosts(std::vector> & hosts) override; +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_SHUTDOWN_HOST_FROM_FILE_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp b/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp new file mode 100644 index 000000000..e91431770 --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp @@ -0,0 +1,43 @@ +#ifndef PANTHER_MANAGER_SHUTDOWN_SINGLE_HOST_NODE_HPP_ +#define PANTHER_MANAGER_SHUTDOWN_SINGLE_HOST_NODE_HPP_ + +#include +#include +#include + +#include + +#include +#include + +namespace panther_manager +{ + +class ShutdownSingleHost : public ShutdownHosts +{ +public: + ShutdownSingleHost(const std::string & name, const BT::NodeConfig & conf) + : ShutdownHosts(name, conf) + { + } + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("ip", "ip of the host to shutdown"), + BT::InputPort("user", "user to log into while executing shutdown command"), + BT::InputPort("port", "SSH communication port"), + BT::InputPort("command", "command to execute on shutdown"), + BT::InputPort("timeout", "time in seconds to wait for host to shutdown"), + BT::InputPort( + "ping_for_success", "ping host unitl it is not available or timeout is reached"), + }; + } + +private: + void update_hosts(std::vector> & hosts) override; +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_SHUTDOWN_SINGLE_HOST_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp b/panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp new file mode 100644 index 000000000..4f8763080 --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp @@ -0,0 +1,34 @@ +#ifndef PANTHER_MANAGER_SIGNAL_SHUTDOWN_NODE_HPP_ +#define PANTHER_MANAGER_SIGNAL_SHUTDOWN_NODE_HPP_ + +#include + +#include +#include +#include + +namespace panther_manager +{ + +class SignalShutdown : public BT::SyncActionNode +{ +public: + explicit SignalShutdown(const std::string & name, const BT::NodeConfig & conf) + : BT::SyncActionNode(name, conf) + { + } + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("reason", "", "reason to shutdown robot"), + }; + } + +private: + virtual BT::NodeStatus tick() override; +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_SIGNAL_SHUTDOWN_NODE_HPP_ \ No newline at end of file diff --git a/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp b/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp new file mode 100644 index 000000000..477e7d072 --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp @@ -0,0 +1,32 @@ +#ifndef PANTHER_MANAGER_TICK_AFTER_TIMEOUT_NODE_HPP_ +#define PANTHER_MANAGER_TICK_AFTER_TIMEOUT_NODE_HPP_ + +#include + +#include +#include +#include + +#include + +namespace panther_manager +{ +class TickAfterTimeout : public BT::DecoratorNode +{ +public: + TickAfterTimeout(const std::string & name, const BT::NodeConfig & conf); + + static BT::PortsList providedPorts() + { + return {BT::InputPort("timeout", "time in s to wait before ticking child again")}; + } + +private: + ros::Duration timeout_; + ros::Time last_success_time_; + + BT::NodeStatus tick() override; +}; +} // namespace panther_manager + +#endif // PANTHER_MANAGER_TICK_AFTER_TIMEOUT_NODE_HPP_ \ No newline at end of file diff --git a/panther_manager/include/panther_manager/plugins/plugin.hpp b/panther_manager/include/panther_manager/plugins/plugin.hpp new file mode 100644 index 000000000..4338b96d6 --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/plugin.hpp @@ -0,0 +1,61 @@ +// Copyright (c) 2023 Davide Faconti +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" + +#include "ros/ros.h" + +// Use this macro to generate a plugin for any BT node requiring NodeHandle +// +// - BT::RosActionNode +// - BT::RosServiceNode +// - BT::RosServiceNode +// - BT::RosTopicPubNode +// - BT::RosTopicSubNode +// +// - First argument: type to register (name of the class) +// - Second argument: string with the registration name +// +// Usage example: +// CreateRosNodePlugin(MyClassName, "MyClassName"); + +#define CreateRosNodePlugin(TYPE, REGISTRATION_NAME) \ + BTCPP_EXPORT void BT_RegisterRosNodeFromPlugin( \ + BT::BehaviorTreeFactory & factory, const std::shared_ptr & nh) \ + { \ + factory.registerNodeType(REGISTRATION_NAME, nh); \ + } + +/** + * @brief RegisterRosNode function used to load a plugin and register + * the containing Node definition. + * + * @param factory the factory where the node should be registered. + * @param filepath path to the plugin. + * @param nh parameters to pass to the instances of the Node. + */ +inline void RegisterRosNode( + BT::BehaviorTreeFactory & factory, const std::filesystem::path & filepath, + const std::shared_ptr & nh) +{ + BT::SharedLibrary loader(filepath); + typedef void (*Func)(BT::BehaviorTreeFactory &, const std::shared_ptr &); + auto func = (Func)loader.getSymbol("BT_RegisterRosNodeFromPlugin"); + func(factory, nh); +} diff --git a/panther_manager/include/panther_manager/plugins/ros_service_node.hpp b/panther_manager/include/panther_manager/plugins/ros_service_node.hpp new file mode 100644 index 000000000..369adf57f --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/ros_service_node.hpp @@ -0,0 +1,93 @@ +#ifndef PANTHER_MANAGER_ROS_SERVICE_NODE_HPP_ +#define PANTHER_MANAGER_ROS_SERVICE_NODE_HPP_ + +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace panther_manager +{ + +template +class RosServiceNode : public BT::SyncActionNode +{ +public: + explicit RosServiceNode(const std::string & name, const BT::NodeConfig & conf, const std::shared_ptr & nh) + : BT::SyncActionNode(name, conf), nh_(nh) + { + node_name_ = ros::this_node::getName(); + } + + virtual ~RosServiceNode() = default; + + using ServiceType = ServiceT; + using RequestType = typename ServiceT::Request; + using ResponseType = typename ServiceT::Response; + + static BT::PortsList providedBasicPorts(const BT::PortsList & addition) + { + BT::PortsList ports = { + BT::InputPort("service_name", "ROS service name"), + BT::InputPort("timeout", 100, "time in ms to wait for service to be active"), + }; + ports.insert(addition.begin(), addition.end()); + return ports; + } + + // methods to be implemented by user + virtual void update_request(RequestType & request) = 0; + virtual BT::NodeStatus on_response(const ResponseType & response) = 0; + + std::string get_node_name() const { return node_name_; } + std::string get_srv_name() const { return srv_name_; } + +private: + std::string node_name_; + std::string srv_name_; + + ros::Duration srv_timeout_; + ros::ServiceClient srv_client_; + std::shared_ptr nh_; + + BT::NodeStatus tick() override + { + if (!getInput("service_name", srv_name_) || srv_name_ == "") { + throw BT::RuntimeError("[", name(), "] Failed to get input [service_name]"); + } + + unsigned srv_timeout_ms; + if (!getInput("timeout", srv_timeout_ms)) { + throw BT::RuntimeError("[", name(), "] Failed to get input [timeout]"); + } + srv_timeout_ = ros::Duration(static_cast(srv_timeout_ms) * 1e-3); + + if (!srv_client_.isValid()) { + srv_client_ = nh_->serviceClient(srv_name_); + } + + if (!srv_client_.waitForExistence(srv_timeout_)) { + ROS_ERROR("[%s] Timeout waiting for service %s", node_name_.c_str(), srv_name_.c_str()); + return BT::NodeStatus::FAILURE; + } + + RequestType request; + ResponseType response; + update_request(request); + if (!srv_client_.call(request, response)) { + ROS_ERROR("[%s] Failed to call service %s", node_name_.c_str(), srv_name_.c_str()); + return BT::NodeStatus::FAILURE; + } + return on_response(response); + } +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_ROS_SERVICE_NODE_HPP_ \ No newline at end of file diff --git a/panther_manager/include/panther_manager/plugins/shutdown_host.hpp b/panther_manager/include/panther_manager/plugins/shutdown_host.hpp new file mode 100644 index 000000000..87ca224c7 --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/shutdown_host.hpp @@ -0,0 +1,257 @@ +#ifndef PANTHER_MANAGER_SHUTDOWN_HOST_HPP_ +#define PANTHER_MANAGER_SHUTDOWN_HOST_HPP_ + +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace panther_manager +{ + +enum class ShutdownHostState { + IDLE = 0, + COMMAND_EXECUTED, + RESPONSE_RECEIVED, + PINGING, + SKIPPED, + SUCCESS, + FAILURE, +}; + +class ShutdownHost +{ +public: + // default constructor + ShutdownHost() + : ip_(""), + user_(""), + port_(22), + command_(""), + timeout_(5.0), + ping_for_success_(true), + hash_(std::hash{}("")) + { + } + ShutdownHost( + const std::string ip, const std::string user, const int port = 22, + const std::string command = "sudo shutdown now", const float timeout = 5.0, + const bool ping_for_success = true) + : ip_(ip), + user_(user), + port_(port), + command_(command), + timeout_(timeout), + ping_for_success_(ping_for_success), + hash_(std::hash{}(ip + user + std::to_string(port))), + state_(ShutdownHostState::IDLE) + { + } + + ~ShutdownHost() {} + + void call() + { + switch (state_) { + case ShutdownHostState::IDLE: + if (!is_available()) { + state_ = ShutdownHostState::SKIPPED; + break; + } + + try { + request_shutdown(); + } catch (std::runtime_error err) { + state_ = ShutdownHostState::FAILURE; + failure_reason_ = err.what(); + break; + } + state_ = ShutdownHostState::COMMAND_EXECUTED; + break; + + case ShutdownHostState::COMMAND_EXECUTED: + try { + if (update_response()) { + break; + } + } catch (std::runtime_error err) { + state_ = ShutdownHostState::FAILURE; + failure_reason_ = err.what(); + break; + } + state_ = ShutdownHostState::RESPONSE_RECEIVED; + break; + + case ShutdownHostState::RESPONSE_RECEIVED: + state_ = ShutdownHostState::PINGING; + break; + case ShutdownHostState::PINGING: + if (ping_for_success_ ? !is_available() : true) { + state_ = ShutdownHostState::SUCCESS; + break; + } + if (timeout_exceeded()) { + state_ = ShutdownHostState::FAILURE; + failure_reason_ = "Timeout exceeded"; + } + break; + + default: + break; + } + } + + bool is_available() const + { + return system(("ping -c 1 -w 1 " + ip_ + " > /dev/null").c_str()) == 0; + } + + void close_connection() + { + if (ssh_channel_is_closed(channel_)) { + return; + } + + ssh_channel_send_eof(channel_); + ssh_channel_close(channel_); + ssh_channel_free(channel_); + ssh_disconnect(session_); + ssh_free(session_); + } + + int get_port() const { return port_; } + + std::string get_ip() const { return ip_; } + + std::string get_user() const { return user_; } + + std::string get_command() const { return command_; } + + std::string get_error() const { return failure_reason_; } + + std::string get_response() const { return output_; } + + ShutdownHostState get_state() const { return state_; } + + bool operator==(const ShutdownHost & other) const { return hash_ == other.hash_; } + + bool operator!=(const ShutdownHost & other) const { return hash_ != other.hash_; } + + bool operator<(const ShutdownHost & other) const { return hash_ < other.hash_; } + +private: + const std::string ip_; + const std::string user_; + const std::string command_; + const std::size_t hash_; + const int port_; + const bool ping_for_success_; + const float timeout_; + + char buffer_[1024]; + const int verbosity_ = SSH_LOG_NOLOG; + int nbytes_; + std::string output_; + std::string failure_reason_; + ros::Time command_time_; + ShutdownHostState state_; + + ssh_session session_; + ssh_channel channel_; + + void request_shutdown() + { + ssh_execute_command(command_); + command_time_ = ros::Time::now(); + } + + bool update_response() + { + if (!is_available()) { + close_connection(); + throw std::runtime_error("Lost connection"); + } + + if (!ssh_channel_is_open(channel_)) { + throw std::runtime_error("Channel closed"); + } + + if (timeout_exceeded()) { + close_connection(); + throw std::runtime_error("Timeout exceeded"); + } + + if ((nbytes_ = ssh_channel_read_nonblocking(channel_, buffer_, sizeof(buffer_), 0)) >= 0) { + output_.append(buffer_, nbytes_); + return true; + } + close_connection(); + return false; + } + + bool timeout_exceeded() + { + return (ros::Time::now() - command_time_) > ros::Duration(timeout_) && is_available(); + } + + void ssh_execute_command(const std::string & command) + { + session_ = ssh_new(); + if (session_ == NULL) { + throw std::runtime_error("Failed to open session"); + }; + + ssh_options_set(session_, SSH_OPTIONS_HOST, ip_.c_str()); + ssh_options_set(session_, SSH_OPTIONS_USER, user_.c_str()); + ssh_options_set(session_, SSH_OPTIONS_PORT, &port_); + ssh_options_set(session_, SSH_OPTIONS_LOG_VERBOSITY, &verbosity_); + + if (ssh_connect(session_) != SSH_OK) { + std::string err = ssh_get_error(session_); + ssh_free(session_); + throw std::runtime_error("Error connecting to host: " + err); + } + + if (ssh_userauth_publickey_auto(session_, NULL, NULL) != SSH_AUTH_SUCCESS) { + std::string err = ssh_get_error(session_); + ssh_disconnect(session_); + ssh_free(session_); + throw std::runtime_error("Error authenticating with public key: " + err); + } + + channel_ = ssh_channel_new(session_); + if (channel_ == NULL) { + std::string err = ssh_get_error(session_); + ssh_disconnect(session_); + ssh_free(session_); + throw std::runtime_error("Failed to create ssh channel: " + err); + } + + if (ssh_channel_open_session(channel_) != SSH_OK) { + std::string err = ssh_get_error(session_); + ssh_channel_free(channel_); + ssh_disconnect(session_); + ssh_free(session_); + throw std::runtime_error("Failed to open ssh channel: " + err); + } + + if (ssh_channel_request_exec(channel_, command.c_str()) != SSH_OK) { + std::string err = ssh_get_error(session_); + ssh_channel_close(channel_); + ssh_channel_free(channel_); + ssh_disconnect(session_); + ssh_free(session_); + throw std::runtime_error("Failed to execute ssh command: " + err); + } + } +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_SHUTDOWN_HOST_NODE_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp b/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp new file mode 100644 index 000000000..534b9051d --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/shutdown_hosts_node.hpp @@ -0,0 +1,156 @@ +#ifndef PANTHER_MANAGER_SHUTDOWN_HOSTS_NODE_HPP_ +#define PANTHER_MANAGER_SHUTDOWN_HOSTS_NODE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include + +namespace panther_manager +{ + +class ShutdownHosts : public BT::StatefulActionNode +{ +public: + explicit ShutdownHosts(const std::string & name, const BT::NodeConfig & conf) + : BT::StatefulActionNode(name, conf) + { + node_name_ = ros::this_node::getName(); + } + + virtual ~ShutdownHosts() = default; + + // method to be implemented by user + virtual void update_hosts(std::vector> & hosts) = 0; + + // method that can be overriden by user + virtual BT::NodeStatus post_process() + { + // return success only when all hosts succeded + if (failed_hosts_.size() == 0) { + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::FAILURE; + } + + std::string get_node_name() const { return node_name_; } + std::vector const get_failed_hosts() { return failed_hosts_; } + +private: + int check_host_index_ = 0; + std::string node_name_; + std::vector> hosts_; + std::vector hosts_to_check_; + std::vector skipped_hosts_; + std::vector succeeded_hosts_; + std::vector failed_hosts_; + + BT::NodeStatus onStart() + { + update_hosts(hosts_); + remove_duplicate_hosts(hosts_); + if (hosts_.size() <= 0) { + ROS_ERROR("[%s] Hosts list is empty! Check configuration!", node_name_.c_str()); + return BT::NodeStatus::FAILURE; + } + hosts_to_check_.resize(hosts_.size()); + std::iota(hosts_to_check_.begin(), hosts_to_check_.end(), 0); + return BT::NodeStatus::RUNNING; + } + + BT::NodeStatus onRunning() + { + if (hosts_to_check_.size() <= 0) { + return post_process(); + } + + if (check_host_index_ >= hosts_to_check_.size()) { + check_host_index_ = 0; + } + + auto host_index = hosts_to_check_[check_host_index_]; + auto host = hosts_[host_index]; + host->call(); + + switch (host->get_state()) { + case ShutdownHostState::RESPONSE_RECEIVED: + ROS_INFO( + "[%s] Device at: %s response:\n%s", node_name_.c_str(), host->get_ip().c_str(), + host->get_response().c_str()); + check_host_index_++; + break; + + case ShutdownHostState::SUCCESS: + ROS_INFO( + "[%s] Successfuly shutdown device at: %s", node_name_.c_str(), host->get_ip().c_str()); + succeeded_hosts_.push_back(host_index); + hosts_to_check_.erase(hosts_to_check_.begin() + check_host_index_); + break; + + case ShutdownHostState::FAILURE: + ROS_WARN( + "[%s] Failed to shutdown device at: %s. Error: %s", node_name_.c_str(), host->get_ip().c_str(), + host->get_error().c_str()); + failed_hosts_.push_back(host_index); + hosts_to_check_.erase(hosts_to_check_.begin() + check_host_index_); + break; + + case ShutdownHostState::SKIPPED: + ROS_WARN( + "[%s] Davice at: %s not available, skipping", node_name_.c_str(), host->get_ip().c_str()); + skipped_hosts_.push_back(host_index); + hosts_to_check_.erase(hosts_to_check_.begin() + check_host_index_); + break; + + default: + check_host_index_++; + break; + } + + return BT::NodeStatus::RUNNING; + } + + void remove_duplicate_hosts(std::vector> & hosts) + { + std::set seen; + + hosts.erase( + std::remove_if( + hosts.begin(), hosts.end(), + [&](const std::shared_ptr & host) { + if (!seen.count(*host)) { + seen.insert(*host); + return false; + } else { + ROS_WARN( + "Found duplicate host: %s\nProcessing only the first occurence.", + host->get_ip().c_str()); + return true; + } + }), + hosts.end()); + } + + void onHalted() + { + for (auto & host : hosts_) { + host->close_connection(); + } + } +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_SHUTDOWN_HOSTS_NODE_HPP_ diff --git a/panther_manager/launch/manager.launch b/panther_manager/launch/manager.launch deleted file mode 100644 index 64c662957..000000000 --- a/panther_manager/launch/manager.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/panther_manager/launch/manager_bt.launch b/panther_manager/launch/manager_bt.launch new file mode 100644 index 000000000..bf1cda230 --- /dev/null +++ b/panther_manager/launch/manager_bt.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/panther_manager/package.xml b/panther_manager/package.xml index 449bb8a8d..92a9851f4 100644 --- a/panther_manager/package.xml +++ b/panther_manager/package.xml @@ -15,15 +15,19 @@ https://github.com/husarion/panther_ros/issues catkin - + + behaviortree_cpp + iputils-ping + libssh-dev panther_msgs - rospy + roscpp + roslib sensor_msgs std_msgs std_srvs + yaml-cpp - iputils-ping - python3-paramiko + python3-psutil \ No newline at end of file diff --git a/panther_manager/plugins/action/call_set_bool_service_node.cpp b/panther_manager/plugins/action/call_set_bool_service_node.cpp new file mode 100644 index 000000000..32483972f --- /dev/null +++ b/panther_manager/plugins/action/call_set_bool_service_node.cpp @@ -0,0 +1,37 @@ +#include + +#include +#include + +#include + +namespace panther_manager +{ + +void CallSetBoolService::update_request(std_srvs::SetBool::Request & request) +{ + bool data; + if (!getInput("data", data)) { + throw BT::RuntimeError("[", name(), "] Failed to get input [data]"); + } + request.data = data; +} + +BT::NodeStatus CallSetBoolService::on_response(const std_srvs::SetBool::Response & response) +{ + if (!response.success) { + ROS_ERROR( + "[%s] Failed to call %s service, message: %s", get_node_name().c_str(), + get_srv_name().c_str(), response.message.c_str()); + return BT::NodeStatus::FAILURE; + } + ROS_DEBUG( + "[%s] Successfuly called %s service, message: %s", get_node_name().c_str(), + get_srv_name().c_str(), response.message.c_str()); + return BT::NodeStatus::SUCCESS; +} + +} // namespace panther_manager + +#include +CreateRosNodePlugin(panther_manager::CallSetBoolService, "CallSetBoolService"); \ No newline at end of file diff --git a/panther_manager/plugins/action/call_set_led_animation_service_node.cpp b/panther_manager/plugins/action/call_set_led_animation_service_node.cpp new file mode 100644 index 000000000..0663f4066 --- /dev/null +++ b/panther_manager/plugins/action/call_set_led_animation_service_node.cpp @@ -0,0 +1,51 @@ +#include + +#include + +#include +#include + +#include + +namespace panther_manager +{ + +void CallSetLedAnimationService::update_request(panther_msgs::SetLEDAnimation::Request & request) +{ + bool repeating; + unsigned animation_id; + std::string param; + + if (!getInput("id", animation_id)) { + throw(BT::RuntimeError("[", name(), "] Failed to get input [id]")); + } + if (!getInput("repeating", repeating)) { + throw(BT::RuntimeError("[", name(), "] Failed to get input [repeating]")); + } + if (!getInput("param", param)) { + throw(BT::RuntimeError("[", name(), "] Failed to get input [param]")); + } + + request.animation.id = animation_id; + request.animation.param = param; + request.repeating = repeating; +} + +BT::NodeStatus CallSetLedAnimationService::on_response(const panther_msgs::SetLEDAnimation::Response & response) +{ + if (!response.success) { + ROS_ERROR( + "[%s] Failed to call %s service, message: %s", get_node_name().c_str(), + get_srv_name().c_str(), response.message.c_str()); + return BT::NodeStatus::FAILURE; + } + ROS_DEBUG( + "[%s] Successfuly called %s service, message: %s", get_node_name().c_str(), + get_srv_name().c_str(), response.message.c_str()); + return BT::NodeStatus::SUCCESS; +} + +} // namespace panther_manager + +#include +CreateRosNodePlugin(panther_manager::CallSetLedAnimationService, "CallSetLedAnimationService"); \ No newline at end of file diff --git a/panther_manager/plugins/action/call_trigger_service_node.cpp b/panther_manager/plugins/action/call_trigger_service_node.cpp new file mode 100644 index 000000000..02eb52725 --- /dev/null +++ b/panther_manager/plugins/action/call_trigger_service_node.cpp @@ -0,0 +1,27 @@ +#include + +#include + +#include + +namespace panther_manager +{ + +BT::NodeStatus CallTriggerService::on_response(const std_srvs::Trigger::Response & response) +{ + if (!response.success) { + ROS_ERROR( + "[%s] Failed to call %s service, message: %s", get_node_name().c_str(), + get_srv_name().c_str(), response.message.c_str()); + return BT::NodeStatus::FAILURE; + } + ROS_DEBUG( + "[%s] Successfuly called %s service, message: %s", get_node_name().c_str(), + get_srv_name().c_str(), response.message.c_str()); + return BT::NodeStatus::SUCCESS; +} + +} // namespace panther_manager + +#include +CreateRosNodePlugin(panther_manager::CallTriggerService, "CallTriggerService"); \ No newline at end of file diff --git a/panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp b/panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp new file mode 100644 index 000000000..810f6392b --- /dev/null +++ b/panther_manager/plugins/action/shutdown_hosts_from_file_node.cpp @@ -0,0 +1,68 @@ +#include + +#include +#include +#include + +#include +#include +#include + +#include + +namespace panther_manager +{ + +void ShutdownHostsFromFile::update_hosts(std::vector> & hosts) +{ + std::string shutdown_hosts_file; + if ( + !getInput("shutdown_hosts_file", shutdown_hosts_file) || + shutdown_hosts_file == "") { + throw(BT::RuntimeError("[", name(), "] Failed to get input [shutdown_hosts_file]")); + } + + YAML::Node shutdown_hosts; + try { + shutdown_hosts = YAML::LoadFile(shutdown_hosts_file); + } catch (const YAML::Exception & e) { + throw BT::RuntimeError("[" + name() + "] Error loading YAML file: " + e.what()); + } + + for (const auto & host : shutdown_hosts["hosts"]) { + if (!host["ip"] || !host["username"]) { + ROS_ERROR("[%s] Missing info for remote host!", get_node_name().c_str()); + continue; + } + + auto ip = host["ip"].as(); + auto user = host["username"].as(); + unsigned port = 22; + if (host["port"]) { + port = host["port"].as(); + } + std::string command = "sudo shutdown now"; + if (host["command"]) { + command = host["command"].as(); + } + float timeout = 5.0; + if (host["timeout"]) { + timeout = host["timeout"].as(); + } + bool ping_for_success = true; + if (host["ping_for_success"]) { + ping_for_success = host["ping_for_success"].as(); + } + + hosts.push_back( + std::make_shared(ip, user, port, command, timeout, ping_for_success)); + } +} + +} // namespace panther_manager + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("ShutdownHostsFromFile"); +} \ No newline at end of file diff --git a/panther_manager/plugins/action/shutdown_single_host_node.cpp b/panther_manager/plugins/action/shutdown_single_host_node.cpp new file mode 100644 index 000000000..0bdfdc6ac --- /dev/null +++ b/panther_manager/plugins/action/shutdown_single_host_node.cpp @@ -0,0 +1,57 @@ +#include + +#include +#include +#include + +#include +#include + +#include + +namespace panther_manager +{ + +void ShutdownSingleHost::update_hosts(std::vector> & hosts) +{ + std::string ip; + if (!getInput("ip", ip) || ip == "") { + throw(BT::RuntimeError("[", name(), "] Failed to get input [ip]")); + } + + std::string user; + if (!getInput("user", user) || user == "") { + throw(BT::RuntimeError("[", name(), "] Failed to get input [user]")); + } + + unsigned port; + if (!getInput("port", port)) { + throw(BT::RuntimeError("[", name(), "] Failed to get input [port]")); + } + + std::string command; + if (!getInput("command", command) || command == "") { + throw(BT::RuntimeError("[", name(), "] Failed to get input [command]")); + } + + float timeout; + if (!getInput("timeout", timeout)) { + throw(BT::RuntimeError("[", name(), "] Failed to get input [timeout]")); + } + + bool ping_for_success; + if (!getInput("ping_for_success", ping_for_success)) { + throw(BT::RuntimeError("[", name(), "] Failed to get input [ping_for_success]")); + } + + hosts.push_back( + std::make_shared(ip, user, port, command, timeout, ping_for_success)); +} + +} // namespace panther_manager + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("ShutdownSingleHost"); +} \ No newline at end of file diff --git a/panther_manager/plugins/action/signal_shutdown_node.cpp b/panther_manager/plugins/action/signal_shutdown_node.cpp new file mode 100644 index 000000000..2968581e6 --- /dev/null +++ b/panther_manager/plugins/action/signal_shutdown_node.cpp @@ -0,0 +1,30 @@ +#include + +#include +#include + +#include +#include + +namespace panther_manager +{ + +BT::NodeStatus SignalShutdown::tick() +{ + auto reason = getInput("reason").value(); + + std::pair signal_shutdown; + signal_shutdown.first = true; + signal_shutdown.second = reason; + config().blackboard->set>("signal_shutdown", signal_shutdown); + + return BT::NodeStatus::SUCCESS; +} + +} // namespace panther_manager + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("SignalShutdown"); +} \ No newline at end of file diff --git a/panther_manager/plugins/decorator/tick_after_timeout_node.cpp b/panther_manager/plugins/decorator/tick_after_timeout_node.cpp new file mode 100644 index 000000000..ce1364908 --- /dev/null +++ b/panther_manager/plugins/decorator/tick_after_timeout_node.cpp @@ -0,0 +1,51 @@ +#include + +#include + +#include +#include + +#include + +namespace panther_manager +{ + +TickAfterTimeout::TickAfterTimeout(const std::string & name, const BT::NodeConfig & conf) +: BT::DecoratorNode(name, conf) +{ + last_success_time_ = ros::Time::now(); +} + +BT::NodeStatus TickAfterTimeout::tick() +{ + float timeout; + if (!getInput("timeout", timeout)) { + throw("[", name(), "] Failed to get input [timeout]"); + } + timeout_ = ros::Duration(timeout); + + if (ros::Time::now() - last_success_time_ < timeout_) { + return BT::NodeStatus::SKIPPED; + } + + setStatus(BT::NodeStatus::RUNNING); + auto child_status = child()->executeTick(); + + if (child_status == BT::NodeStatus::SUCCESS) { + last_success_time_ = ros::Time::now(); + } + + if (child_status != BT::NodeStatus::RUNNING) { + resetChild(); + } + + return child_status; +} + +} // namespace panther_manager + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("TickAfterTimeout"); +} \ No newline at end of file diff --git a/panther_manager/src/main.cpp b/panther_manager/src/main.cpp new file mode 100644 index 000000000..cefda30ad --- /dev/null +++ b/panther_manager/src/main.cpp @@ -0,0 +1,19 @@ +#include "panther_manager/manager_bt_node.hpp" + +#include + +#include + +int main(int argc, char ** argv) +{ + ros::init(argc, argv, "manager_bt_node"); + ros::AsyncSpinner spinner(0); + auto nh = std::make_shared(); + auto ph = std::make_shared("~"); + + panther_manager::ManagerBTNode manager_bt_node(nh, ph); + + spinner.start(); + ros::waitForShutdown(); + return 0; +} \ No newline at end of file diff --git a/panther_manager/src/manager_bt_node.cpp b/panther_manager/src/manager_bt_node.cpp new file mode 100644 index 000000000..6e4dac025 --- /dev/null +++ b/panther_manager/src/manager_bt_node.cpp @@ -0,0 +1,350 @@ +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace panther_manager +{ + +ManagerBTNode::ManagerBTNode( + const std::shared_ptr & nh, const std::shared_ptr & ph) +: nh_(std::move(nh)), ph_(std::move(ph)) +{ + node_name_ = ros::this_node::getName(); + + const std::string default_bt_project_file = + ros::package::getPath("panther_manager") + "/config/Panther12BT.btproj"; + const std::vector default_plugin_libs = {}; + + const auto launch_lights_tree = ph_->param("launch_lights_tree", true); + const auto launch_safety_tree = ph_->param("launch_safety_tree", true); + launch_shutdown_tree_ = ph_->param("launch_shutdown_tree", true); + const auto bt_project_file = ph_->param("bt_project_file", default_bt_project_file); + const auto plugin_libs = ph_->param>("plugin_libs", default_plugin_libs); + const auto ros_plugin_libs = + ph_->param>("ros_plugin_libs", default_plugin_libs); + const auto battery_temp_window_len = ph_->param("battery_temp_window_len", 6); + const auto battery_percent_window_len = ph->param("battery_percent_window_len", 6); + const auto cpu_temp_window_len = ph_->param("cpu_temp_window_len", 6); + const auto driver_temp_window_len = ph_->param("driver_temp_window_len", 6); + const auto shutdown_hosts_file = ph_->param("shutdown_hosts_file", ""); + + // lights tree params + const auto critical_battery_anim_period = + ph_->param("lights/critical_battery_anim_period", 15.0); + const auto critical_battery_threshold_percent = + ph_->param("lights/critical_battery_threshold_percent", 0.1); + const auto battery_state_anim_period = + ph_->param("lights/battery_state_anim_period", 120.0); + const auto low_battery_anim_period = ph_->param("lights/low_battery_anim_period", 30.0); + const auto low_battery_threshold_percent = + ph_->param("lights/low_battery_threshold_percent", 0.4); + update_charging_anim_step_ = ph_->param("lights/update_charging_anim_step", 0.1); + + // 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 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); + const auto driver_fan_off_temp = ph_->param("safety/driver_fan_off_temp", 35.0); + + battery_temp_ma_ = std::make_unique>(battery_temp_window_len); + battery_percent_ma_ = std::make_unique>(battery_percent_window_len, 1.0); + cpu_temp_ma_ = std::make_unique>(cpu_temp_window_len); + front_driver_temp_ma_ = std::make_unique>(driver_temp_window_len); + rear_driver_temp_ma_ = std::make_unique>(driver_temp_window_len); + + ROS_INFO("[%s] Register BehaviorTree from: %s", node_name_.c_str(), bt_project_file.c_str()); + + // export plugins for a behaviour tree + for (const auto & p : plugin_libs) { + factory_.registerFromPlugin(BT::SharedLibrary::getOSName(p)); + } + + for (const auto & p : ros_plugin_libs) { + RegisterRosNode(factory_, BT::SharedLibrary::getOSName(p), nh_); + } + + factory_.registerBehaviorTreeFromFile(bt_project_file); + + if (launch_safety_tree && !launch_shutdown_tree_) { + ROS_ERROR( + "[%s] Can't launch safety tree without shutdown tree. Killing node.", node_name_.c_str()); + ros::requestShutdown(); + } + + if (launch_lights_tree) { + const std::map lights_initial_bb = { + {"charging_anim_percent", ""}, + {"current_anim_id", -1}, + {"BATTERY_STATE_ANIM_PERIOD", battery_state_anim_period}, + {"CRITICAL_BATTERY_ANIM_PERIOD", critical_battery_anim_period}, + {"CRITICAL_BATTERY_THRESHOLD_PERCENT", critical_battery_threshold_percent}, + {"LOW_BATTERY_ANIM_PERIOD", low_battery_anim_period}, + {"LOW_BATTERY_THRESHOLD_PERCENT", low_battery_threshold_percent}, + // anim constants + {"E_STOP_ANIM_ID", unsigned(panther_msgs::LEDAnimation::E_STOP)}, + {"READY_ANIM_ID", unsigned(panther_msgs::LEDAnimation::READY)}, + {"ERROR_ANIM_ID", unsigned(panther_msgs::LEDAnimation::ERROR)}, + {"MANUAL_ACTION_ANIM_ID", unsigned(panther_msgs::LEDAnimation::MANUAL_ACTION)}, + {"AUTONOMOUS_ACTION_ANIM_ID", unsigned(panther_msgs::LEDAnimation::AUTONOMOUS_ACTION)}, + {"GOAL_ACHIEVED_ANIM_ID", unsigned(panther_msgs::LEDAnimation::GOAL_ACHIEVED)}, + {"LOW_BATTERY_ANIM_ID", unsigned(panther_msgs::LEDAnimation::LOW_BATTERY)}, + {"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 status constants + {"POWER_SUPPLY_STATUS_UNKNOWN", + unsigned(sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN)}, + {"POWER_SUPPLY_STATUS_CHARGING", + unsigned(sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_CHARGING)}, + {"POWER_SUPPLY_STATUS_DISCHARGING", + unsigned(sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING)}, + {"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)}, + }; + + lights_config_ = create_bt_config(lights_initial_bb); + lights_tree_ = factory_.createTree("Lights", lights_config_.blackboard); + } + + if (launch_safety_tree) { + const std::map 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}, + // 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); + safety_tree_ = factory_.createTree("Safety", safety_config_.blackboard); + } + + if (launch_shutdown_tree_) { + const std::map shutdown_initial_bb = { + {"SHUTDOWN_HOSTS_FILE", shutdown_hosts_file.c_str()}, + }; + + shutdown_config_ = create_bt_config(shutdown_initial_bb); + shutdown_tree_ = factory_.createTree("Shutdown", shutdown_config_.blackboard); + } + + battery_sub_ = nh_->subscribe("battery", 10, &ManagerBTNode::battery_cb, this); + driver_state_sub_ = + nh_->subscribe("driver/motor_controllers_state", 10, &ManagerBTNode::driver_state_cb, this); + e_stop_sub_ = nh_->subscribe("hardware/e_stop", 2, &ManagerBTNode::e_stop_cb, this); + io_state_sub_ = nh_->subscribe("hardware/io_state", 2, &ManagerBTNode::io_state_cb, this); + system_status_sub_ = nh_->subscribe("system_status", 10, &ManagerBTNode::system_status_cb, this); + + ros::Rate rate(10.0); // 10Hz + while (ros::ok() && !e_stop_state_.has_value()) { + ROS_INFO_THROTTLE(5.0, "[%s] Waiting for e_stop message to arrive", node_name_.c_str()); + rate.sleep(); + ros::spinOnce(); + } + + while (ros::ok() && !io_state_.has_value()) { + ROS_INFO_THROTTLE(5.0, "[%s] Waiting for io_state message to arrive", node_name_.c_str()); + rate.sleep(); + ros::spinOnce(); + } + + while (ros::ok() && !battery_status_.has_value()) { + ROS_INFO_THROTTLE(5.0, "[%s] Waiting for battery message to arrive", node_name_.c_str()); + rate.sleep(); + ros::spinOnce(); + } + + if (launch_lights_tree) { + lights_tree_timer_ = + nh_->createTimer(ros::Duration(0.1), std::bind(&ManagerBTNode::lights_tree_timer_cb, this)); + } + if (launch_safety_tree) { + safety_tree_timer_ = + nh_->createTimer(ros::Duration(0.1), std::bind(&ManagerBTNode::safety_tree_timer_cb, this)); + } + + ROS_INFO("[%s] Node started", node_name_.c_str()); +} + +BT::NodeConfig ManagerBTNode::create_bt_config( + const std::map & bb_values) const +{ + BT::NodeConfig config; + config.blackboard = BT::Blackboard::create(); + // update blackboard + config.blackboard->set("nh", nh_); + for (auto & item : bb_values) { + const std::type_info & type = item.second.type(); + if (type == typeid(bool)) { + config.blackboard->set(item.first, std::any_cast(item.second)); + } else if (type == typeid(int)) { + config.blackboard->set(item.first, std::any_cast(item.second)); + } else if (type == typeid(unsigned)) { + config.blackboard->set(item.first, std::any_cast(item.second)); + } else if (type == typeid(float)) { + config.blackboard->set(item.first, std::any_cast(item.second)); + } else if (type == typeid(double)) { + config.blackboard->set(item.first, std::any_cast(item.second)); + } else if (type == typeid(long)) { + config.blackboard->set(item.first, std::any_cast(item.second)); + } else if (type == typeid(const char *)) { + config.blackboard->set(item.first, std::any_cast(item.second)); + } else if (type == typeid(std::string)) { + config.blackboard->set(item.first, std::any_cast(item.second)); + } else { + throw std::invalid_argument( + "Invalid type for blackboard entry. Valid types are: bool, int, unsigned, float, double," + " long, const char*, std::string"); + } + } + + return 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); +} + +void ManagerBTNode::driver_state_cb(const panther_msgs::DriverState::ConstPtr & driver_state) +{ + front_driver_temp_ma_->roll(driver_state->front.temperature); + rear_driver_temp_ma_->roll(driver_state->rear.temperature); +} + +void ManagerBTNode::e_stop_cb(const std_msgs::Bool::ConstPtr & e_stop) +{ + e_stop_state_ = e_stop->data; +} + +void ManagerBTNode::io_state_cb(const panther_msgs::IOState::ConstPtr & io_state) +{ + if (io_state->power_button && launch_shutdown_tree_) { + shutdown_robot("Power button pressed"); + } + io_state_ = io_state; +} + +void ManagerBTNode::system_status_cb(const panther_msgs::SystemStatus::ConstPtr & system_status) +{ + cpu_temp_ma_->roll(system_status->cpu_temp); +} + +void ManagerBTNode::lights_tree_timer_cb() +{ + // update blackboard + lights_config_.blackboard->set("e_stop_state", e_stop_state_.value()); + lights_config_.blackboard->set("battery_status", battery_status_.value()); + lights_config_.blackboard->set("battery_percent", battery_percent_ma_->get_average()); + lights_config_.blackboard->set( + "battery_percent_round", + std::to_string( + round(battery_percent_ma_->get_average() / update_charging_anim_step_) * + update_charging_anim_step_)); + + // BT::StdCoutLogger logger_cout(lights_tree_); // debugging + lights_tree_status_ = lights_tree_.tickOnce(); +} + +void ManagerBTNode::safety_tree_timer_cb() +{ + // update blackboard + 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 + safety_config_.blackboard->set( + "driver_temp", + std::max({front_driver_temp_ma_->get_average(), rear_driver_temp_ma_->get_average()})); + + // BT::StdCoutLogger logger_cout(safety_tree_); // debugging + safety_tree_status_ = safety_tree_.tickOnce(); + + std::pair signal_shutdown; + if (safety_config_.blackboard->get>( + "signal_shutdown", signal_shutdown)) { + if (signal_shutdown.first) shutdown_robot(signal_shutdown.second); + } +} + +void ManagerBTNode::shutdown_robot(const std::string & reason) +{ + ROS_WARN("[%s] Soft shutdown initialized. %s", node_name_.c_str(), reason.c_str()); + lights_tree_timer_.stop(); + lights_tree_.haltTree(); + safety_tree_timer_.stop(); + safety_tree_.haltTree(); + + // tick shutdown tree + // BT::StdCoutLogger logger_cout(shutdown_tree_); // debugging + shutdown_tree_status_ = BT::NodeStatus::RUNNING; + auto start_time = ros::Time::now(); + ros::Rate rate(30.0); // 30 Hz + while (ros::ok() && shutdown_tree_status_ == BT::NodeStatus::RUNNING) { + shutdown_tree_status_ = shutdown_tree_.tickOnce(); + rate.sleep(); + } + ros::requestShutdown(); +} + +} // namespace panther_manager diff --git a/panther_manager/src/manager_node.py b/panther_manager/src/manager_node.py deleted file mode 100755 index d841c6813..000000000 --- a/panther_manager/src/manager_node.py +++ /dev/null @@ -1,403 +0,0 @@ -#!/usr/bin/python3 - -from threading import Lock - -import os -import paramiko -import socket - -import rospy - -from sensor_msgs.msg import BatteryState -from std_msgs.msg import Bool -from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse, Trigger - -from panther_msgs.msg import DriverState, IOState, SystemStatus - - -class ManagerNode: - def __init__(self, name: str) -> None: - rospy.init_node(name, anonymous=False) - - self._lock = Lock() - - self._aux_power_state = None - self._e_stop_state = None - self._fan_state = None - self._power_button_state = None - - self._high_bat_temp = rospy.get_param('~high_bat_temp', 55.0) - self._critical_bat_temp = rospy.get_param('~critical_bat_temp', 59.0) - self._fatal_bat_temp = rospy.get_param('~fatal_bat_temp', 62.0) - self._battery_window_len = rospy.get_param('~battery_window_len', 6) - - self._shutdown_timeout = rospy.get_param('~shutdown_timeout', 15.0) - self._ip = rospy.get_param('~self_ip', '127.0.0.1') - self._username = rospy.get_param('~self_username', 'husarion') - - dif = rospy.get_param('~default_identity_file', '~/.ssh/id_rsa') - self._default_identity_file = os.path.expanduser(dif) - if rospy.has_param('~self_identity_file'): - sif = rospy.get_param('~self_identity_file') - self._identity_file = os.path.expanduser(sif) - else: - self._identity_file = self._default_identity_file - - self._hosts = rospy.get_param('~hosts', []) - for host in self._hosts: - # check if all keys are provided - if {'ip', 'username'} != set(host.keys()): - rospy.logerr(f'[{rospy.get_name()}] Missing info for remote host!') - raise KeyError - if 'identity_file' not in host.keys(): - host['identity_file'] = self._default_identity_file - else: - host['identity_file'] = os.path.expanduser(host['identity_file']) - - if not os.path.exists(host['identity_file']): - rospy.logerr( - f'[{rospy.get_name()}]' - f' Can\'t find provided identity file for host {host["ip"]}!' - f' Path \'{host["identity_file"]}\' doesn\'t exist!' - ) - raise ValueError - - if 'cmd' not in host.keys(): - host['cmd'] = 'sudo shutdown now' - - self._battery_temp_window = None - self._cpu_temp_window = None - self._front_driver_temp_window = None - self._rear_driver_temp_window = None - self._turn_on_time = rospy.Time.now() - - self._critical_driver_temp = 60.0 - - self._cpu_fan_on_temp = rospy.get_param('~cpu_fan_on_temp', 70.0) - self._cpu_fan_off_temp = rospy.get_param('~cpu_fan_off_temp', 60.0) - self._driver_fan_on_temp = rospy.get_param('~driver_fan_on_temp', 45.0) - self._driver_fan_off_temp = rospy.get_param('~driver_fan_off_temp', 35.0) - self._fun_enable_hysteresis = rospy.get_param('~fun_enable_hysteresis', 60.0) - self._cpu_window_len = rospy.get_param('~cpu_window_len', 6) - self._driver_window_len = rospy.get_param('~driver_window_len', 6) - self._overwrite_fan_control = rospy.get_param('~overwrite_fan_control', False) - - if self._cpu_fan_on_temp < self._cpu_fan_off_temp: - rospy.logerr( - f'[{rospy.get_name()}] ' - f'Turning off temperature for CPU is higher than turning on temperature!' - ) - raise ValueError - - if self._driver_fan_on_temp < self._driver_fan_off_temp: - rospy.logerr( - f'[{rospy.get_name()}] ' - f'Turning off temperature for driver is higher than turning on temperature!' - ) - raise ValueError - - if self._fun_enable_hysteresis < 0.0: - rospy.logerr(f'[{rospy.get_name()}] Fan enable hysteresis can not be negative!') - raise ValueError - - if self._cpu_window_len <= 0 or self._driver_window_len <= 0: - rospy.logerr(f'[{rospy.get_name()}] Smoothing window has to be positive!') - raise ValueError - - # ------------------------------- - # Subscribers - # ------------------------------- - - self._battery_sub = rospy.Subscriber('battery', BatteryState, self._battery_cb) - self._driver_state_sub = rospy.Subscriber( - 'driver/motor_controllers_state', DriverState, self._driver_state_cb - ) - self._e_stop_sub = rospy.Subscriber('hardware/e_stop', Bool, self._e_stop_cb) - self._io_state_sub = rospy.Subscriber('hardware/io_state', IOState, self._io_state_cb) - self._system_status_sub = rospy.Subscriber( - 'system_status', SystemStatus, self._system_status_cb - ) - - # ------------------------------- - # Service servers - # ------------------------------- - - self._overwrite_fan_control_server = rospy.Service( - 'manager/overwrite_fan_control', SetBool, self._overwrite_fan_control_cb - ) - - # ------------------------------- - # Service clients - # ------------------------------- - - self._aux_power_enable_client = rospy.ServiceProxy('hardware/aux_power_enable', SetBool) - self._e_stop_trigger_client = rospy.ServiceProxy('hardware/e_stop_trigger', Trigger) - self._fan_enable_client = rospy.ServiceProxy('hardware/fan_enable', SetBool) - - # ------------------------------- - # Timers - # ------------------------------- - - self._manager_timer = rospy.Timer(rospy.Duration(0.1), self._manager_timer_cb) - self._fan_control_timer = rospy.Timer(rospy.Duration(2.0), self._fan_control_timer_cb) - - self._call_set_bool_service(self._fan_enable_client, self._overwrite_fan_control) - - rospy.loginfo(f'[{rospy.get_name()}] Node started') - - def _battery_cb(self, battery_state: BatteryState) -> None: - with self._lock: - self._battery_status = battery_state.power_supply_status - if self._battery_temp_window is not None: - self._battery_temp_window = self._move_window( - self._battery_temp_window, battery_state.temperature - ) - else: - self._battery_temp_window = [battery_state.temperature] * self._battery_window_len - - def _driver_state_cb(self, driver_state: DriverState) -> None: - with self._lock: - if ( - self._front_driver_temp_window is not None - and self._rear_driver_temp_window is not None - ): - self._front_driver_temp_window = self._move_window( - self._front_driver_temp_window, driver_state.front.temperature - ) - self._rear_driver_temp_window = self._move_window( - self._rear_driver_temp_window, driver_state.rear.temperature - ) - else: - self._front_driver_temp_window = [ - driver_state.front.temperature - ] * self._driver_window_len - self._rear_driver_temp_window = [ - driver_state.rear.temperature - ] * self._driver_window_len - - def _e_stop_cb(self, e_stop_state: Bool) -> None: - with self._lock: - self._e_stop_state = e_stop_state.data - - def _io_state_cb(self, io_state: IOState) -> None: - with self._lock: - self._aux_power_state = io_state.aux_power - self._fan_state = io_state.fan - self._power_button_state = io_state.power_button - - def _system_status_cb(self, system_status: SystemStatus) -> None: - with self._lock: - if self._cpu_temp_window is not None: - self._cpu_temp_window = self._move_window( - self._cpu_temp_window, system_status.cpu_temp - ) - else: - self._cpu_temp_window = [system_status.cpu_temp] * self._cpu_window_len - - def _overwrite_fan_control_cb(self, req: SetBoolRequest) -> SetBoolResponse: - with self._lock: - self._overwrite_fan_control = req.data - if req.data: - self._call_set_bool_service(self._fan_enable_client, req.data) - return SetBoolResponse(True, f'Overwrite fan control set to: {req.data}') - - def _manager_timer_cb(self, *args) -> None: - with self._lock: - if self._power_button_state: - rospy.loginfo(f'[{rospy.get_name()}] Power button pressed, shutting down robot') - self._shutdown() - return - - if self._battery_temp_window is None: - rospy.loginfo_throttle( - 5.0, f'[{rospy.get_name()}] Waiting for battery message to arrive.' - ) - return - - if self._battery_status == BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING: - rospy.logwarn_throttle(5.0, f'[{rospy.get_name()}] Battery is not charging.') - - self._battery_avg_temp = self._get_mean(self._battery_temp_window) - if self._battery_avg_temp > self._fatal_bat_temp: - rospy.logerr_throttle( - 5.0, f'[{rospy.get_name()}] Fatal battery temperature, shutting down robot!' - ) - self._shutdown() - elif self._battery_avg_temp > self._high_bat_temp: - if self._battery_avg_temp > self._critical_bat_temp: - rospy.logerr_throttle( - 5.0, - f'[{rospy.get_name()}] Critical battery temperature, triggering E-STOP and disabling AUX!', - ) - if self._aux_power_state: - self._call_set_bool_service(self._aux_power_enable_client, False) - else: - rospy.logerr_throttle( - 5.0, f'[{rospy.get_name()}] High battery temperature, triggering E-STOP!' - ) - if not self._e_stop_state: - self._call_trigger_service(self._e_stop_trigger_client) - - def _shutdown(self) -> None: - rospy.logwarn(f'[{rospy.get_name()}] Soft shutdown initialized.') - # create new list of computers that confirmed shutdown procedure - hosts_to_check = [ - h - for h in self._hosts - if self._request_shutdown(h['ip'], h['identity_file'], h['username'], h['cmd']) - ] - - start_time = rospy.get_time() - if len(hosts_to_check) > 0: - while rospy.get_time() - start_time < self._shutdown_timeout: - # reduce list to all computers that are still available - hosts_to_check = [h for h in hosts_to_check if self._check_ip(h['ip'])] - if len(hosts_to_check) == 0: - rospy.loginfo(f'[{rospy.get_name()}] All computes shut down gracefully.') - break - else: - rospy.loginfo( - f'[{rospy.get_name()}] ' - 'Shutdown timeout reached. Cutting out power from computers.' - ) - - # ensure all computers did full shutdown - rospy.loginfo(f'[{rospy.get_name()}] Shutting down itself.') - self._request_shutdown(self._ip, self._identity_file, self._username, 'sudo shutdown now') - - def _request_shutdown(self, ip: str, identity_file: str, username: str, cmd: str) -> bool: - # shutdown only if host available - if self._check_ip(ip): - try: - pkey = paramiko.RSAKey.from_private_key_file(identity_file) - client = paramiko.SSHClient() - policy = paramiko.AutoAddPolicy() - client.set_missing_host_key_policy(policy) - client.connect(ip, username=username, pkey=pkey, timeout=0.5) - rospy.loginfo(f'[{rospy.get_name()}] Shutting down device at {ip}.') - try: - client.exec_command(cmd, timeout=0.5) - except socket.timeout: - # some systems do not close SSH connection on shut down - # this will handle the timeout to allow shutting other devices - pass - client.close() - return True - except Exception: - rospy.logerr(f'[{rospy.get_name()}] Can\'t SSH to device at {ip}!') - return False - else: - rospy.loginfo(f'[{rospy.get_name()}] Device at {ip} not available.') - return False - - def _check_ip(self, host: str) -> bool: - return os.system('ping -c 1 -w 1 ' + host + ' > /dev/null') == 0 - - def _fan_control_timer_cb(self, *args) -> None: - with self._lock: - if self._fan_state is None: - rospy.loginfo(f'[{rospy.get_name()}] Waiting for fan state message to arrive.') - return - - if self._overwrite_fan_control: - return - - if self._cpu_temp_window is None: - rospy.loginfo(f'[{rospy.get_name()}] Waiting for system_status message to arrive.') - return - - if self._front_driver_temp_window is None or self._rear_driver_temp_window is None: - rospy.loginfo( - f'[{rospy.get_name()}] Waiting for motor_controllers_state message to arrive.' - ) - return - - self._cpu_avg_temp = self._get_mean(self._cpu_temp_window) - self._front_driver_avg_temp = self._get_mean(self._front_driver_temp_window) - self._rear_driver_avg_temp = self._get_mean(self._rear_driver_temp_window) - - if self._front_driver_avg_temp > self._critical_driver_temp: - rospy.logerr_throttle( - 60, - f'[{rospy.get_name()}] Front driver reached critical ', - f'temperature of {int(round(self._front_driver_avg_temp) + 0.1)} deg C!', - ) - if self._rear_driver_avg_temp > self._critical_driver_temp: - rospy.logerr_throttle( - 60, - f'[{rospy.get_name()}] Rear driver reached critical ', - f'temperature of {int(round(self._rear_driver_avg_temp) + 0.1)} deg C!', - ) - - if not self._fan_state and ( - self._cpu_avg_temp > self._cpu_fan_on_temp - or self._front_driver_avg_temp > self._driver_fan_on_temp - or self._rear_driver_avg_temp > self._driver_fan_on_temp - ): - rospy.loginfo(f'[{rospy.get_name()}] Turning on fan. Cooling the robot.') - self._turn_on_time = rospy.Time.now() - self._call_set_bool_service(self._fan_enable_client, True) - return - - if ( - self._fan_state - and (rospy.Time.now() - self._turn_on_time).secs > self._fun_enable_hysteresis - and ( - self._cpu_avg_temp < self._cpu_fan_off_temp - and self._front_driver_avg_temp < self._driver_fan_off_temp - and self._rear_driver_avg_temp < self._driver_fan_off_temp - ) - ): - rospy.loginfo(f'[{rospy.get_name()}] Turning off fan.') - self._call_set_bool_service(self._fan_enable_client, False) - return - - def _move_window(self, window: list, elem: float) -> list: - window = window[1:] - window.append(elem) - return window - - def _get_mean(self, window: list) -> float: - return sum(window) / len(window) - - def _call_trigger_service(self, service: rospy.ServiceProxy) -> None: - try: - service.wait_for_service(5.0) - res = service.call() - if res.success: - rospy.loginfo( - f'[{rospy.get_name()}] Successfuly triggered {service.resolved_name} service. Response: {res.message}.' - ) - else: - rospy.logerr( - f'[{rospy.get_name()}] Failed to trigger {service.resolved_name} service. Response: {res.message}.' - ) - except (rospy.exceptions.ROSException, rospy.service.ServiceException) as err: - rospy.logerr(f'[{rospy.get_name()}] {err}') - - def _call_set_bool_service(self, service: rospy.ServiceProxy, state: bool) -> None: - try: - service.wait_for_service(5.0) - res = service.call(SetBoolRequest(state)) - if res.success: - rospy.loginfo( - f'[{rospy.get_name()}] Successfuly called {service.resolved_name} service. Response: {res.message}.' - ) - else: - rospy.logerr( - f'[{rospy.get_name()}] Failed to call {service.resolved_name} service. Response: {res.message}.' - ) - except (rospy.exceptions.ROSException, rospy.service.ServiceException) as err: - rospy.logerr(f'[{rospy.get_name()}] {err}') - - -def main(): - manager_node = ManagerNode('manager_node') - rospy.spin() - - -if __name__ == '__main__': - try: - main() - except rospy.ROSInterruptException: - pass