ROS2 docking utils

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/

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/

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()

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

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: []

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:

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:

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_

husarion_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;

husarion_ugv_manager/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;

husarion_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:

husarion_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:

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."),
     };
   }