ROS2 docking utils (#462)

* added half tested panther_docking package Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added tests Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added redme Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * run precommit Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Fix typo Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * After coderabbit review Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * fixed parameters | fixed exporting_package Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Adjust cmake Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added getParameters and declareParameters | review suggestions Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * added offsetPose | moved transformPose to ros_utils Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * removed hardware deps Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Moved TransformPose to tf2_utils Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * added filter description Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * removed realtime_tools Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Modef OffsetFunction utils Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * added and applied ArePosesNear() Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * fix tf utils header guard Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added comment Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added locking node Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Update docking to develop (#399) * Implement testing POC * Namespace refactor * Add EStop to Gazebo * unify CMakeLists.txt files * Add dependencies * Add remapping * Rename files in panther_diagnostics package * Update after changes in panther_diagnostics * Rename config and launch file in manager package * Correct include guards in manager package * Restructure files tree in manager tests * Ros2 estop sim gui (#384) * New format of documentation (#369) * Change 3 package for demo * Improve ROS_API * fix links * Update * Update * Table improvements * Format * Save work * Save work * update * fix * fix * fix * fix * fix * Add API warning * Improve links * lights simplify * Create CONFIGURATION.md files * Typos * pre-commit * Apply suggestions from code review Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> * Save work * Final unification * Delete trash * typos * Update README.md * Update ROS_API.md * Update ROS_API.md * Update README.md Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> * Change initial warning to beta warning * improve warn rendering * rendering * Update Diagram * Add Dawid suggestions * Dot * Change diagram ext and typos * Do not describe external nodes * Add Dawid suggestons * Add last Dawid suggestions * Format * Pawel suggestions * Diagram improvements * Update * Diagram Visual --------- Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> * Change scheme theme (#380) * unify CMakeLists.txt files (#381) * First working version * Ros2 increase bt service timeout (#382) * Parametrize and increase service timeout in managers * Format panther API drawio file * Add Estop GUI and docs --------- Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> Co-authored-by: Paweł Irzyk <108666440+pawelirh@users.noreply.github.com> Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * Update panther_gazebo/panther_hardware_plugins.xml Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> * Add david suggestion and change gui layout * Typos in Readme + estop publish on service call * Reorganize files in panther_lights * UPdate include guards in panther_lights * Reorganize files in panther_battery * Move estop to plugins folder * add nmea gps * Rename battery driver files * Rename shutdown hosts config * Inherit from IgnitionSystem * Change to Estop -> EStop * Reorganize panther_hardware_interfaces files * Dawid suggestions part 1 * Rename PantherSystem -> GzPantherSystem * Update references to files * Rename battery exec * Fix links in documentations (#387) * Refer to header files * Update panther_gazebo/include/panther_gazebo/gz_panther_system.hpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * Update panther_gazebo/src/gz_panther_system.cpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * Update panther_gazebo/src/gz_panther_system.cpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * Dawid suggestions * delete typo * Minor modifications * Move BT plugins to src directory * Reorganize test utilities in hardware_interfaces * Merge remote-tracking branch 'origin/ros2-devel' into ros2-testing-poc * Add missing module configuration * Update panther_gazebo/include/panther_gazebo/gz_panther_system.hpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * Dawid suggestions * update docs * Extend filesystem responsibility * Update ROS_API.md Co-authored-by: Paweł Irzyk <108666440+pawelirh@users.noreply.github.com> * update names * Add new common utility functions * System monitor improvements * Round temperature precision * Implement filesystem unit tests * Formatting * Add integration tests condition * Update ROS_API.md * Review changes * Add pre-commit workflow (#395) --------- Co-authored-by: pawelirh <pawel.irzyk@husarion.com> Co-authored-by: rafal-gorecki <rafal.gorecki@husarion.com> Co-authored-by: Dawid <kmakd197@gmail.com> Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> Co-authored-by: Paweł Irzyk <108666440+pawelirh@users.noreply.github.com> Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * ROS2 docking manager plugins (#394) * Added base of BT docking plugins Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Teste dock and undock plugins Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Removed test dock manager Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * run precommit Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * moved plugins Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * changed names of files and classes | changed DockRobotAction to DockRobot Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Fixed copyright Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Changed timeouts | changed using Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> --------- Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * JoySubscription BT node (#397) * changed main in manager's tests Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Changed to AreButtonsPressed Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> --------- Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Fixed model Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Revert "Fixed model" This reverts commit 97d85c2. * Fixed model Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Readme update Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Update panther_docking/README.md Co-authored-by: Paweł Irzyk <108666440+pawelirh@users.noreply.github.com> * Added exec depend and pip install to cmake Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * add python as interpreter Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Fixed namespaces and updated ros_component_description (#424) * Fixed namespaces and updated ros_component_description Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Remove panther_version --------- Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> Co-authored-by: rafal-gorecki <rafal.gorecki@husarion.com> * Ros2 docking manager no lights (#430) * Manage lights * Simple working version * Clean up * Clean up and add types enums * Simplifications * Change ports * Add use_docking arg * Fix port * Fix port * Add more cases to tests * update * Fix ports * Add dock_pose field, improve description, add dependencies * Docking manager without lights * Auto review * Update CONFIGURATION.md * Typo * Coderabbit suggestions * Jakub suggestions * Fix tests * Jakub suggestions * ROS2 docking fix joy (#439) * Fixed utils test, fixed joy Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Revert "Fixed utils test, fixed joy" This reverts commit 3bd5e30. * Fixed bt utils test, changed dock jot input Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Joy input and typo Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> --------- Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Charging Dock Plugin based on dock pose subscriber (#431) * Fixed namespaces and updated ros_component_description Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Changed GetDockPose approach Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Cleanup charger Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added unit tests Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Update docs: Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Fix upside down Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added updateAndPublishStagingPose method Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added suggestins | added reading docking_server parameter is dock pose publisher Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Remove redundant tests Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added aprilros launch to simulation Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * update vsc and changed dock name Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> --------- Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Ros2 docking charging dock wibotic msgs (#436) * Fixed namespaces and updated ros_component_description Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Changed GetDockPose approach Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Cleanup charger Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added unit tests Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Update docs: Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Fix upside down Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added updateAndPublishStagingPose method Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added suggestins | added reading docking_server parameter is dock pose publisher Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Remove redundant tests Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added reading wibotic_info Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added launching wibotic_connector_can Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added right condition to wibotic_connector_can Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * wibotic_ros is needed to build Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Update repose, update parameters Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added condition for use_docking Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * update repose Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Fixed use_wibotic_info param Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Rafal's suggestions applied Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * README update and comment delete Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Update docs Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Update .repose Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> --------- Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * ROS2 docking nav2 (#443) * Fixed namespaces and updated ros_component_description Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Changed GetDockPose approach Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Cleanup charger Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added unit tests Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Update docs: Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Fix upside down Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added updateAndPublishStagingPose method Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added suggestins | added reading docking_server parameter is dock pose publisher Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Remove redundant tests Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added aprilros launch to simulation Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * update vsc and changed dock name Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Tested with nav2 in simulation | Added docking to multibple stations | Spawning multiple stations Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Fixed after merge Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * described parameters | applied codeguides Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * coderabbitai suggestions Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Review Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * wibotic_connector_ros depends on hardware | fixed launch args Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * wibotic_connector_ros depends on hardware | fixed launch args Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added default values Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Update panther_manager/launch/manager.launch.py --------- Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> Co-authored-by: Rafal Gorecki <126687345+rafal-gorecki@users.noreply.github.com> * Ros2 docking tf compare (#451) * Changed camera pose Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Fixed test Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Update docs Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * typo Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * typo Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Update panther_docking/README.md Co-authored-by: Rafal Gorecki <126687345+rafal-gorecki@users.noreply.github.com> * updated vcs Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> --------- Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> Co-authored-by: Rafal Gorecki <126687345+rafal-gorecki@users.noreply.github.com> * Ros2 docking lifecycle node (#453) * Changed camera pose Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Fixed test Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Update docs Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * typo Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * typo Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Changed to lifecyclenode | added transition service client Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Updated docs Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added timeout function Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> --------- Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Changed names in package Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Revert "Changed names in package" This reverts commit 31ae7ce. * ROS2 Docking - panther_docking to husarion_ugv_docking (#459) * Changed names in package Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * fixed param name Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> --------- Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Removed docking Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Removed docking manager Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Removed docking manager Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Fixed after merge Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Code rabbit ai suggestions Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Remove accident panther_manager Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Removed dock plugins Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Fixed path Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Fixed path Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Fixed path Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Removed docking files Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Removed tf2_utils Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * removed station launch Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Fixed HEADER GUARD Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> --------- Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> Co-authored-by: pawelirh <pawel.irzyk@husarion.com> Co-authored-by: rafal-gorecki <rafal.gorecki@husarion.com> Co-authored-by: Dawid <kmakd197@gmail.com> Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> Co-authored-by: Paweł Irzyk <108666440+pawelirh@users.noreply.github.com> Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com>
husarion · Dec 20, 2024 · 08f556b · 08f556b
1 parent 4a7e624
commit 08f556b
Show file tree

Hide file tree

Showing 36 changed files with 854 additions and 68 deletions.
diff --git a/husarion_ugv/hardware_deps.repos b/husarion_ugv/hardware_deps.repos
@@ -10,7 +10,7 @@ repositories:
   ros_components_description:
     type: git
     url: https://github.com/husarion/ros_components_description.git
-    version: b29f5637a243b0008ac197032575c8df47883b2c
+    version: 517350ac672b7383c3377e63244353a104096c39
   ros2_controllers: # Caused by two error: 1. https://github.com/ros-controls/ros2_controllers/pull/1104 2. There is no nice way to change `sensor_name` imu_bradcaster param when spawning multiple robots -> ros2_control refer only to single imu entity
     type: git
     url: https://github.com/husarion/ros2_controllers/

diff --git a/husarion_ugv/simulation_deps.repos b/husarion_ugv/simulation_deps.repos
@@ -14,7 +14,7 @@ repositories:
   ros_components_description:
     type: git
     url: https://github.com/husarion/ros_components_description.git
-    version: b29f5637a243b0008ac197032575c8df47883b2c
+    version: 517350ac672b7383c3377e63244353a104096c39
   ros2_controllers: # Caused by two error: 1. https://github.com/ros-controls/ros2_controllers/pull/1104 2. There is no nice way to change `sensor_name` imu_bradcaster param when spawning multiple robots -> ros2_control refer only to single imu entity
     type: git
     url: https://github.com/husarion/ros2_controllers/

diff --git a/husarion_ugv_manager/CMakeLists.txt b/husarion_ugv_manager/CMakeLists.txt
@@ -12,12 +12,15 @@ set(PACKAGE_DEPENDENCIES
     behaviortree_ros2
     generate_parameter_library
     libssh
+    geometry_msgs
     husarion_ugv_msgs
     husarion_ugv_utils
     rclcpp
     rclcpp_action
     sensor_msgs
+    std_msgs
     std_srvs
+    tf2_geometry_msgs
     yaml-cpp)
 
 foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES})
@@ -52,6 +55,14 @@ add_library(shutdown_hosts_from_file_bt_node SHARED
 target_link_libraries(shutdown_hosts_from_file_bt_node ssh yaml-cpp)
 list(APPEND plugin_libs shutdown_hosts_from_file_bt_node)
 
+add_library(check_bool_msg_bt_node SHARED
+            src/plugins/condition/check_bool_msg.cpp)
+list(APPEND plugin_libs check_bool_msg_bt_node)
+
+add_library(check_joy_msg_bt_node SHARED
+            src/plugins/condition/check_joy_msg.cpp)
+list(APPEND plugin_libs check_joy_msg_bt_node)
+
 add_library(tick_after_timeout_bt_node SHARED
             src/plugins/decorator/tick_after_timeout_node.cpp)
 list(APPEND plugin_libs tick_after_timeout_bt_node)
@@ -66,11 +77,13 @@ add_executable(safety_manager_node src/safety_manager_node_main.cpp
 ament_target_dependencies(
   safety_manager_node
   behaviortree_ros2
+  geometry_msgs
   husarion_ugv_msgs
   husarion_ugv_utils
   rclcpp
   sensor_msgs
-  std_msgs)
+  std_msgs
+  tf2_geometry_msgs)
 
 generate_parameter_library(safety_manager_parameters
                            config/safety_manager_parameters.yaml)
@@ -82,11 +95,13 @@ add_executable(lights_manager_node src/lights_manager_node_main.cpp
 ament_target_dependencies(
   lights_manager_node
   behaviortree_ros2
+  geometry_msgs
   husarion_ugv_msgs
   husarion_ugv_utils
   rclcpp
   sensor_msgs
-  std_msgs)
+  std_msgs
+  tf2_geometry_msgs)
 
 generate_parameter_library(lights_manager_parameters
                            config/lights_manager_parameters.yaml)
@@ -102,6 +117,7 @@ install(DIRECTORY behavior_trees config launch
         DESTINATION share/${PROJECT_NAME})
 
 install(DIRECTORY include/ DESTINATION include/)
+install(DIRECTORY test/ DESTINATION include/${PROJECT_NAME}/test/)
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
@@ -139,6 +155,18 @@ if(BUILD_TESTING)
     src/plugins/action/shutdown_hosts_from_file_node.cpp)
   list(APPEND plugin_tests ${PROJECT_NAME}_test_shutdown_hosts_from_file_node)
 
+  ament_add_gtest(
+    ${PROJECT_NAME}_test_check_bool_msg
+    test/plugins/condition/test_check_bool_msg.cpp
+    src/plugins/condition/check_bool_msg.cpp)
+  list(APPEND plugin_tests ${PROJECT_NAME}_test_check_bool_msg)
+
+  ament_add_gtest(
+    ${PROJECT_NAME}_test_check_joy_msg
+    test/plugins/condition/test_check_joy_msg.cpp
+    src/plugins/condition/check_joy_msg.cpp)
+  list(APPEND plugin_tests ${PROJECT_NAME}_test_check_joy_msg)
+
   ament_add_gtest(
     ${PROJECT_NAME}_test_tick_after_timeout_node
     test/plugins/decorator/test_tick_after_timeout_node.cpp
@@ -181,7 +209,7 @@ if(BUILD_TESTING)
            $<INSTALL_INTERFACE:include>)
   ament_target_dependencies(
     ${PROJECT_NAME}_test_behavior_tree_utils behaviortree_cpp behaviortree_ros2
-    husarion_ugv_utils)
+    husarion_ugv_utils geometry_msgs tf2_geometry_msgs)
 
   ament_add_gtest(${PROJECT_NAME}_test_behavior_tree_manager
                   test/test_behavior_tree_manager.cpp)
@@ -202,11 +230,13 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_lights_manager_node
     behaviortree_cpp
     behaviortree_ros2
+    geometry_msgs
     husarion_ugv_msgs
     husarion_ugv_utils
     rclcpp
     sensor_msgs
-    std_msgs)
+    std_msgs
+    tf2_geometry_msgs)
   target_link_libraries(${PROJECT_NAME}_test_lights_manager_node
                         lights_manager_parameters)
 
@@ -221,11 +251,13 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_lights_behavior_tree
     behaviortree_cpp
     behaviortree_ros2
+    geometry_msgs
     husarion_ugv_msgs
     husarion_ugv_utils
     rclcpp
     sensor_msgs
-    std_msgs)
+    std_msgs
+    tf2_geometry_msgs)
   target_link_libraries(${PROJECT_NAME}_test_lights_behavior_tree
                         lights_manager_parameters)
 
@@ -239,11 +271,13 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_safety_manager_node
     behaviortree_cpp
     behaviortree_ros2
+    geometry_msgs
     husarion_ugv_msgs
     husarion_ugv_utils
     rclcpp
     sensor_msgs
-    std_msgs)
+    std_msgs
+    tf2_geometry_msgs)
   target_link_libraries(${PROJECT_NAME}_test_safety_manager_node
                         safety_manager_parameters)
 
@@ -258,12 +292,14 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_safety_behavior_tree
     behaviortree_cpp
     behaviortree_ros2
+    geometry_msgs
     husarion_ugv_msgs
     husarion_ugv_utils
     rclcpp
     sensor_msgs
     std_msgs
-    std_srvs)
+    std_srvs
+    tf2_geometry_msgs)
   target_link_libraries(${PROJECT_NAME}_test_safety_behavior_tree
                         safety_manager_parameters)
 endif()

diff --git a/husarion_ugv_manager/config/lights_manager.yaml b/husarion_ugv_manager/config/lights_manager.yaml
@@ -2,6 +2,7 @@
   lights_manager:
     ros__parameters:
       timer_frequency: 10.0
+      bt_server_port: 5555
       battery:
         percent:
           window_len: 6

diff --git a/husarion_ugv_manager/config/lights_manager_parameters.yaml b/husarion_ugv_manager/config/lights_manager_parameters.yaml
@@ -44,6 +44,12 @@ lights_manager:
     default_value: ""
     description: Path to a BehaviorTree project.
 
+  bt_server_port:
+    type: int
+    default_value: 5555
+    description: Port number for the BehaviorTree server.
+    validation: { gt<>: 0 }
+
   plugin_libs:
     type: string_array
     default_value: []

diff --git a/husarion_ugv_manager/config/safety_manager.yaml b/husarion_ugv_manager/config/safety_manager.yaml
@@ -2,6 +2,7 @@
   safety_manager:
     ros__parameters:
       timer_frequency: 10.0
+      bt_server_port: 6666
       fan_turn_off_timeout: 60.0
       battery:
         temp:

diff --git a/husarion_ugv_manager/config/safety_manager_parameters.yaml b/husarion_ugv_manager/config/safety_manager_parameters.yaml
@@ -12,6 +12,12 @@ safety_manager:
     default_value: ""
     description: Path to a BehaviorTree project.
 
+  bt_server_port:
+    type: int
+    default_value: 6666
+    description: Port number for the BehaviorTree server.
+    validation: { gt<>: 0 }
+
   cpu:
     temp:
       fan_off:

diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp
@@ -26,6 +26,9 @@
 #include "behaviortree_cpp/utils/shared_library.h"
 #include "behaviortree_ros2/plugins.hpp"
 
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
 namespace husarion_ugv_manager::behavior_tree_utils
 {
 
@@ -90,4 +93,78 @@ inline std::string GetLoggerPrefix(const std::string & bt_node_name)
 }
 }  // namespace husarion_ugv_manager
 
+namespace BT
+{
+/**
+ * @brief Converts a string to a vector of float.
+ *
+ * @param str The string to convert.
+ * @return std::vector<float> The vector of float.
+ *
+ * @throw BT::RuntimeError Throws when there is no input or cannot parse float.
+ */
+template <>
+inline std::vector<float> convertFromString<std::vector<float>>(StringView str)
+{
+  auto parts = splitString(str, ';');
+  std::vector<float> output;
+  output.reserve(parts.size());
+  for (const StringView & part : parts) {
+    output.push_back(convertFromString<float>(part));
+  }
+  return output;
+}
+
+/**
+ * @brief Converts a string to a PoseStamped message.
+ *
+ * The string format should be "x;y;z;roll;pitch;yaw;frame_id" where:
+ *  - x, y, z: Position coordinates.
+ *  - roll, pitch, yaw: Rotation around axes XYZ.
+ *  - frame_id: Coordinate frame ID (string).
+ *
+ * @param str The string to convert.
+ * @return geometry_msgs::msg::PoseStamped The converted PoseStamped message.
+ *
+ * @throw BT::RuntimeError Throws if the input is invalid or cannot be parsed.
+ */
+template <>
+inline geometry_msgs::msg::PoseStamped convertFromString<geometry_msgs::msg::PoseStamped>(
+  StringView str)
+{
+  constexpr std::size_t expected_parts_size = 7;
+
+  auto parts = splitString(str, ';');
+  if (parts.size() != expected_parts_size) {
+    throw BT::RuntimeError(
+      "Invalid input for PoseStamped. Expected " + std::to_string(expected_parts_size) +
+      " values: x;y;z;roll;pitch;yaw;frame_id");
+  }
+
+  geometry_msgs::msg::PoseStamped pose_stamped;
+
+  try {
+    pose_stamped.pose.position.x = convertFromString<double>(parts[0]);
+    pose_stamped.pose.position.y = convertFromString<double>(parts[1]);
+    pose_stamped.pose.position.z = convertFromString<double>(parts[2]);
+
+    const auto roll = convertFromString<double>(parts[3]);
+    const auto pitch = convertFromString<double>(parts[4]);
+    const auto yaw = convertFromString<double>(parts[5]);
+    tf2::Quaternion quaternion;
+    quaternion.setRPY(roll, pitch, yaw);
+    pose_stamped.pose.orientation = tf2::toMsg(quaternion);
+
+    pose_stamped.header.frame_id = convertFromString<std::string>(parts[6]);
+    pose_stamped.header.stamp = rclcpp::Clock().now();
+
+  } catch (const std::exception & e) {
+    throw BT::RuntimeError("Failed to convert string to PoseStamped: " + std::string(e.what()));
+  }
+
+  return pose_stamped;
+}
+
+}  // namespace BT
+
 #endif  // HUSARION_UGV_MANAGER_BEHAVIOR_TREE_UTILS_HPP_
diff --git a/...on_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp b/...on_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp
@@ -36,7 +36,8 @@ class CallSetBoolService : public BT::RosServiceNode<std_srvs::srv::SetBool>
 
   static BT::PortsList providedPorts()
   {
-    return providedBasicPorts({BT::InputPort<bool>("data", "true / false value")});
+    return providedBasicPorts(
+      {BT::InputPort<bool>("data", "Boolean value to send with the service request.")});
   }
 
   virtual bool setRequest(typename Request::SharedPtr & request) override;

diff --git a/...nager/include/husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp b/...nager/include/husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp
@@ -37,11 +37,11 @@ class CallSetLedAnimationService
 
   static BT::PortsList providedPorts()
   {
-    return providedBasicPorts({
-      BT::InputPort<unsigned>("id", "animation ID"),
-      BT::InputPort<std::string>("param", "optional parameter"),
-      BT::InputPort<bool>("repeating", "indicates if animation should repeat"),
-    });
+    return providedBasicPorts(
+      {BT::InputPort<unsigned>("id", "Animation ID to trigger."),
+       BT::InputPort<std::string>("param", "Optional animation parameter."),
+       BT::InputPort<bool>(
+         "repeating", "Specifies whether the animation should repeated continuously.")});
   }
 
   virtual bool setRequest(typename Request::SharedPtr & request) override;

diff --git a/...ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_hosts_from_file_node.hpp b/...ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_hosts_from_file_node.hpp
@@ -39,10 +39,8 @@ class ShutdownHostsFromFile : public ShutdownHosts
 
   static BT::PortsList providedPorts()
   {
-    return {
-      BT::InputPort<std::string>(
-        "shutdown_hosts_file", "global path to YAML file with hosts to shutdown"),
-    };
+    return {BT::InputPort<std::string>(
+      "shutdown_hosts_file", "Absolute path to a YAML file listing the hosts to shut down.")};
   }
 
 private:

diff --git a/...ion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_single_host_node.hpp b/...ion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_single_host_node.hpp
@@ -38,14 +38,17 @@ class ShutdownSingleHost : public ShutdownHosts
   static BT::PortsList providedPorts()
   {
     return {
-      BT::InputPort<std::string>("ip", "ip of the host to shutdown"),
-      BT::InputPort<std::string>("username", "user to log into while executing shutdown command"),
-      BT::InputPort<unsigned>("port", "SSH communication port"),
-      BT::InputPort<std::string>("command", "command to execute on shutdown"),
-      BT::InputPort<float>("timeout", "time in seconds to wait for host to shutdown"),
+      BT::InputPort<std::string>("ip", "IP address of the host to shut down."),
+      BT::InputPort<std::string>(
+        "username", "Username to use for logging in and executing the shutdown command."),
+      BT::InputPort<unsigned>("port", "SSH port used for communication (default is usually 22)."),
+      BT::InputPort<std::string>("command", "A command to execute on the remote host."),
+      BT::InputPort<float>(
+        "timeout", "Maximum time (in seconds) to wait for the host to shut down."),
       BT::InputPort<bool>(
-        "ping_for_success", "ping host until it is not available or timeout is reached"),
-    };
+        "ping_for_success",
+        "Whether to continuously ping the host until it becomes unreachable or the timeout is "
+        "reached.")};
   }
 
 private:

diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/signal_shutdown_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/signal_shutdown_node.hpp
@@ -35,7 +35,7 @@ class SignalShutdown : public BT::SyncActionNode
   static BT::PortsList providedPorts()
   {
     return {
-      BT::InputPort<std::string>("reason", "", "reason to shutdown robot"),
+      BT::InputPort<std::string>("reason", "", "A reason to shutdown a robot."),
     };
   }