husarion · KmakD · Mar 26, 2024 · Mar 12, 2024 · Mar 13, 2024 · Mar 13, 2024
diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt
@@ -7,57 +7,77 @@ endif()
 
 set(PACKAGE_INCLUDE_DEPENDS
     ament_cmake
-    rclcpp
-    rclcpp_action
     ament_index_cpp
     behaviortree_cpp
-    std_srvs
-    panther_msgs
-    libssh
-    yaml-cpp
     behaviortree_ros2
-    panther_utils)
+    libssh
+    panther_msgs
+    panther_utils
+    rclcpp
+    rclcpp_action
+    sensor_msgs
+    std_srvs
+    yaml-cpp)
 
 foreach(Dependency IN ITEMS ${PACKAGE_INCLUDE_DEPENDS})
   find_package(${Dependency} REQUIRED)
 endforeach()
 
 include_directories(include)
 
-add_library(call_set_bool_service_node SHARED
+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_node)
+list(APPEND plugin_libs call_set_bool_service_bt_node)
 
-add_library(call_trigger_service_node SHARED
+add_library(call_trigger_service_bt_node SHARED
             plugins/action/call_trigger_service_node.cpp)
-list(APPEND plugin_libs call_trigger_service_node)
+list(APPEND plugin_libs call_trigger_service_bt_node)
 
-add_library(call_set_led_animation_service_node SHARED
+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_node)
+list(APPEND plugin_libs call_set_led_animation_service_bt_node)
 
-add_library(signal_shutdown_node SHARED plugins/action/signal_shutdown_node.cpp)
-list(APPEND plugin_libs signal_shutdown_node)
+add_library(signal_shutdown_bt_node SHARED
+            plugins/action/signal_shutdown_node.cpp)
+list(APPEND plugin_libs signal_shutdown_bt_node)
 
-add_library(shutdown_single_host_node SHARED
+add_library(shutdown_single_host_bt_node SHARED
             plugins/action/shutdown_single_host_node.cpp)
-target_link_libraries(shutdown_single_host_node ssh)
-list(APPEND plugin_libs shutdown_single_host_node)
+target_link_libraries(shutdown_single_host_bt_node ssh)
+list(APPEND plugin_libs shutdown_single_host_bt_node)
 
-add_library(shutdown_hosts_from_file_node SHARED
+add_library(shutdown_hosts_from_file_bt_node SHARED
             plugins/action/shutdown_hosts_from_file_node.cpp)
-target_link_libraries(shutdown_hosts_from_file_node ssh yaml-cpp)
-list(APPEND plugin_libs shutdown_hosts_from_file_node)
+target_link_libraries(shutdown_hosts_from_file_bt_node ssh yaml-cpp)
+list(APPEND plugin_libs shutdown_hosts_from_file_bt_node)
 
-add_library(tick_after_timeout_node SHARED
+add_library(tick_after_timeout_bt_node SHARED
             plugins/decorator/tick_after_timeout_node.cpp)
-list(APPEND plugin_libs tick_after_timeout_node)
+list(APPEND plugin_libs tick_after_timeout_bt_node)
 
-foreach(bt_node ${plugin_libs})
-  target_compile_definitions(${bt_node} PRIVATE BT_PLUGIN_EXPORT)
-  ament_target_dependencies(${bt_node} ${PACKAGE_INCLUDE_DEPENDS})
+foreach(bt_plugin ${plugin_libs})
+  target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
+  ament_target_dependencies(${bt_plugin} ${PACKAGE_INCLUDE_DEPENDS})
 endforeach()
 
+add_executable(manager_bt_node src/main.cpp src/manager_bt_node.cpp)
+ament_target_dependencies(
+  manager_bt_node
+  behaviortree_ros2
+  panther_msgs
+  panther_utils
+  rclcpp
+  sensor_msgs
+  std_msgs)
+target_link_libraries(manager_bt_node ${plugin_libs})
+
+install(TARGETS ${plugin_libs} DESTINATION lib)
+
+install(TARGETS manager_bt_node DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY behavior_trees config launch
+        DESTINATION share/${PROJECT_NAME})
+
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
 
@@ -68,13 +88,13 @@ if(BUILD_TESTING)
 
   target_link_libraries(
     ${PROJECT_NAME}_test_plugin_utils
-    call_set_bool_service_node
-    call_trigger_service_node
-    call_set_led_animation_service_node
-    signal_shutdown_node
-    shutdown_single_host_node
-    shutdown_hosts_from_file_node
-    tick_after_timeout_node)
+    call_set_bool_service_bt_node
+    call_trigger_service_bt_node
+    call_set_led_animation_service_bt_node
+    signal_shutdown_bt_node
+    shutdown_single_host_bt_node
+    shutdown_hosts_from_file_bt_node
+    tick_after_timeout_bt_node)
 
   ament_target_dependencies(${PROJECT_NAME}_test_plugin_utils
                             ${PACKAGE_INCLUDE_DEPENDS})
@@ -112,11 +132,30 @@ if(BUILD_TESTING)
     target_link_libraries(${bt_node_test} ${PROJECT_NAME}_test_plugin_utils)
     ament_target_dependencies(${bt_node_test} ${PACKAGE_INCLUDE_DEPENDS})
   endforeach()
-endif()
-
-install(TARGETS DESTINATION lib/${PROJECT_NAME})
 
-ament_export_include_directories(include)
-ament_export_dependencies(${PACKAGE_INCLUDE_DEPENDS})
+  ament_add_gtest(${PROJECT_NAME}_test_bt_utils test/test_bt_utils.cpp)
+  target_include_directories(
+    ${PROJECT_NAME}_test_bt_utils
+    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+           $<INSTALL_INTERFACE:include>)
+  ament_target_dependencies(${PROJECT_NAME}_test_bt_utils behaviortree_cpp
+                            panther_utils)
+
+  ament_add_gtest(${PROJECT_NAME}_test_manager_bt_node
+                  test/test_manager_bt_node.cpp src/manager_bt_node.cpp)
+  target_include_directories(
+    ${PROJECT_NAME}_test_manager_bt_node
+    PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+           $<INSTALL_INTERFACE:include>)
+  ament_target_dependencies(
+    ${PROJECT_NAME}_test_manager_bt_node
+    behaviortree_cpp
+    behaviortree_ros2
+    panther_msgs
+    panther_utils
+    rclcpp
+    sensor_msgs
+    std_msgs)
+endif()
 
 ament_package()
diff --git a/panther_manager/behavior_trees/Panther106BT.btproj b/panther_manager/behavior_trees/Panther106BT.btproj
@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4" project_name="Panther106BT">
+    <include path="lights.xml"/>
+    <!-- Description of Node Models (used by Groot) -->
+    <TreeNodesModel>
+        <Action ID="CallSetLedAnimationService" editable="true">
+            <input_port name="id">animation ID</input_port>
+            <input_port name="param">optional parameter</input_port>
+            <input_port name="repeating" default="false">indicates if animation should repeat</input_port>
+            <input_port name="service_name">ROS service name</input_port>
+            <input_port name="timeout" default="100">time in ms to wait for service to be active</input_port>
+        </Action>
+        <Decorator ID="TickAfterTimeout" editable="true">
+            <input_port name="timeout">time in s to wait before ticking child again</input_port>
+        </Decorator>
+    </TreeNodesModel>
+</root>
diff --git a/panther_manager/behavior_trees/Panther12BT.btproj b/panther_manager/behavior_trees/Panther12BT.btproj
@@ -0,0 +1,39 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4" project_name="Panther12BT">
+    <include path="lights.xml"/>
+    <include path="safety.xml"/>
+    <include path="shutdown.xml"/>
+    <!-- Description of Node Models (used by Groot) -->
+    <TreeNodesModel>
+        <Action ID="CallSetBoolService" editable="true">
+            <input_port name="data">true / false value</input_port>
+            <input_port name="service_name">ROS service name</input_port>
+        </Action>
+        <Action ID="CallSetLedAnimationService" editable="true">
+            <input_port name="id">animation ID</input_port>
+            <input_port name="param">optional parameter</input_port>
+            <input_port name="repeating" default="false">indicates if animation should repeat</input_port>
+            <input_port name="service_name">ROS service name</input_port>
+        </Action>
+        <Action ID="CallTriggerService" editable="true">
+            <input_port name="service_name">ROS service name</input_port>
+        </Action>
+        <Action ID="ShutdownHostsFromFile" editable="true">
+            <input_port name="shutdown_hosts_file">global path to YAML file with hosts to shutdown</input_port>
+        </Action>
+        <Action ID="ShutdownSingleHost" editable="true">
+            <input_port name="command" default="sudo shutdown now">command to execute on shutdown</input_port>
+            <input_port name="ip">ip of the host to shutdown</input_port>
+            <input_port name="ping_for_success" default="true">ping host until it is not available or timeout is reached</input_port>
+            <input_port name="port" default="22">SSH communication port</input_port>
+            <input_port name="timeout" default="5.0">time in s to wait for host to shutdown</input_port>
+            <input_port name="user">user to log into while executing shutdown command</input_port>
+        </Action>
+        <Action ID="SignalShutdown" editable="true">
+            <input_port name="reason">reason to shutdown robot</input_port>
+        </Action>
+        <Decorator ID="TickAfterTimeout" editable="true">
+            <input_port name="timeout">time in s to wait before ticking child again</input_port>
+        </Decorator>
+    </TreeNodesModel>
+</root>
diff --git a/panther_manager/behavior_trees/lights.xml b/panther_manager/behavior_trees/lights.xml
@@ -0,0 +1,101 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="Lights">
+    <Sequence>
+      <Sequence name="ChargingSequence"
+                _skipIf="battery_status != POWER_SUPPLY_STATUS_CHARGING \
+&amp;&amp; battery_status != POWER_SUPPLY_STATUS_FULL">
+        <CallSetLedAnimationService name="SetErrorAnimation"
+                                    id="{ERROR_ANIM_ID}"
+                                    param=""
+                                    repeating="true"
+                                    service_name="lights/controller/set/animation"
+                                    _skipIf="battery_health != POWER_SUPPLY_HEALTH_OVERHEAT \
+|| current_anim_id == ERROR_ANIM_ID"
+                                    _onSuccess="current_anim_id = ERROR_ANIM_ID"/>
+        <Sequence name="DisplayChargingAnimationSequence"
+                  _skipIf="battery_health == POWER_SUPPLY_HEALTH_OVERHEAT">
+          <CallSetLedAnimationService name="SetChargingAnimation"
+                                      id="{CHARGING_BATTERY_ANIM_ID}"
+                                      param="{battery_percent_round}"
+                                      repeating="true"
+                                      service_name="lights/controller/set/animation"
+                                      _skipIf="battery_percent_round == charging_anim_percent \
+&amp;&amp; current_anim_id == CHARGING_BATTERY_ANIM_ID"
+                                      _onSuccess="charging_anim_percent = battery_percent_round; \
+current_anim_id = CHARGING_BATTERY_ANIM_ID"/>
+          <TickAfterTimeout timeout="20.0"
+                            _skipIf="!e_stop_state">
+            <CallSetLedAnimationService name="SetEStopAnimation"
+                                        id="{E_STOP_ANIM_ID}"
+                                        param=""
+                                        repeating="false"
+                                        service_name="lights/controller/set/animation"/>
+          </TickAfterTimeout>
+        </Sequence>
+      </Sequence>
+      <Sequence name="DischargingSequence"
+                _skipIf="battery_status != POWER_SUPPLY_STATUS_DISCHARGING \
+&amp;&amp; battery_status != POWER_SUPPLY_STATUS_NOT_CHARGING">
+        <CallSetLedAnimationService name="SetReadyAnimation"
+                                    id="{READY_ANIM_ID}"
+                                    param=""
+                                    repeating="true"
+                                    service_name="lights/controller/set/animation"
+                                    _skipIf="e_stop_state || current_anim_id == READY_ANIM_ID"
+                                    _onSuccess="current_anim_id = READY_ANIM_ID"/>
+        <CallSetLedAnimationService name="SetEStopAnimation"
+                                    id="{E_STOP_ANIM_ID}"
+                                    param=""
+                                    repeating="true"
+                                    service_name="lights/controller/set/animation"
+                                    _skipIf="!e_stop_state || current_anim_id == E_STOP_ANIM_ID"
+                                    _onSuccess="current_anim_id = E_STOP_ANIM_ID"/>
+        <Sequence name="DisplayAnimationStatusSequence">
+          <TickAfterTimeout timeout="{LOW_BATTERY_ANIM_PERIOD}"
+                            _skipIf="battery_percent &lt; CRITICAL_BATTERY_THRESHOLD_PERCENT \
+|| battery_percent &gt;= LOW_BATTERY_THRESHOLD_PERCENT">
+            <CallSetLedAnimationService name="SetLowBatteryAnimation"
+                                        id="{LOW_BATTERY_ANIM_ID}"
+                                        param=""
+                                        repeating="false"
+                                        service_name="lights/controller/set/animation"/>
+          </TickAfterTimeout>
+          <TickAfterTimeout timeout="{CRITICAL_BATTERY_ANIM_PERIOD}"
+                            _skipIf="battery_percent &gt;= CRITICAL_BATTERY_THRESHOLD_PERCENT">
+            <CallSetLedAnimationService name="SetCriticalBatteryAnimation"
+                                        id="{CRITICAL_BATTERY_ANIM_ID}"
+                                        param=""
+                                        repeating="false"
+                                        service_name="lights/controller/set/animation"/>
+          </TickAfterTimeout>
+        </Sequence>
+      </Sequence>
+      <CallSetLedAnimationService name="SerErrorAnimation"
+                                  id="{ERROR_ANIM_ID}"
+                                  param=""
+                                  repeating="true"
+                                  service_name="lights/controller/set/animation"
+                                  _skipIf="battery_status != POWER_SUPPLY_STATUS_UNKNOWN \
+|| current_anim_id == ERROR_ANIM_ID"
+                                  _onSuccess="current_anim_id = ERROR_ANIM_ID"/>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Description of Node Models (used by Groot) -->
+  <TreeNodesModel>
+    <Action ID="CallSetLedAnimationService"
+            editable="true">
+      <input_port name="id">animation ID</input_port>
+      <input_port name="param">optional parameter</input_port>
+      <input_port name="repeating"
+                  default="false">indicates if animation should repeat</input_port>
+      <input_port name="service_name">ROS service name</input_port>
+    </Action>
+    <Decorator ID="TickAfterTimeout"
+               editable="true">
+      <input_port name="timeout">time in s to wait before ticking child again</input_port>
+    </Decorator>
+  </TreeNodesModel>
+
+</root>