ROS2 docking utils

husarion_ugv/hardware_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/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_gazebo/launch/simulation.launch.py

@@ -93,6 +93,14 @@ def generate_launch_description():
         ),
     )
 
+    spawn_charging_docks = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [FindPackageShare("panther_gazebo"), "launch", "spawn_charging_docks.launch.py"]
+            )
+        ),
+    )
+
     actions = [
         declare_gz_gui,
         declare_namespace_arg,
@@ -102,6 +110,7 @@ def generate_launch_description():
         gz_sim,
         rviz_launch,
         simulate_robots,
+        spawn_charging_docks,
     ]
 
     return LaunchDescription(actions)

husarion_ugv_gazebo/launch/spawn_charging_docks.launch.py

@@ -0,0 +1,125 @@
+#!/usr/bin/env python3
+
+# Copyright 2024 Husarion sp. z o.o.
+#
+# 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.
+
+import os
+from tempfile import NamedTemporaryFile
+
+import imageio
+import yaml
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_apriltag_and_get_path(tag_id):
+    from moms_apriltag import TagGenerator2
+
+    tag_generator = TagGenerator2("tag36h11")
+    tag_image = tag_generator.generate(tag_id, scale=1000)
+    temp_file = NamedTemporaryFile(suffix=f"_tag_{tag_id}.png", delete=False)
+
+    imageio.imwrite(temp_file.name, tag_image)
+    return temp_file.name
+
+
+def spawn_stations(context, *args, **kwargs):
+    docking_server_config_path = LaunchConfiguration("docking_server_config_path").perform(context)
+    use_docking = LaunchConfiguration("use_docking").perform(context)
+    docking_server_config = None
+
+    try:
+        with open(os.path.join(docking_server_config_path)) as file:
+            docking_server_config = yaml.safe_load(file)
+        if not isinstance(docking_server_config, dict) or "/**" not in docking_server_config:
+            raise ValueError("Invalid configuration structure")
+    except Exception as e:
+        print(f"Error loading docking server config: {str(e)}")
+        return []
+
+    actions = []
+
+    ros_parameters = docking_server_config["/**"]["ros__parameters"]
+    docks_names = ros_parameters["docks"]
+    for dock_name in docks_names:
+        pose = ros_parameters[dock_name]["pose"]
+
+        spawn_station = Node(
+            package="ros_gz_sim",
+            executable="create",
+            arguments=[
+                "-name",
+                [dock_name, "_station"],
+                "-topic",
+                [dock_name, "_station_description"],
+                "-x",
+                str(pose[0]),
+                "-y",
+                str(pose[1] - 2.0),  # -2.0 is the offset between world and map
+                "-z",
+                "0.5",  # station z is not in 0.0
+                "-R",
+                "1.57",
+                "-P",
+                "0.0",
+                "-Y",
+                str(pose[2] - 1.57),
+            ],
+            emulate_tty=True,
+            condition=IfCondition(use_docking),
+        )
+
+        actions.append(spawn_station)
+
+    return actions
+
+
+def generate_launch_description():
+    declare_device_namespace = DeclareLaunchArgument(
+        "device_namespace",
+        default_value="",
+        description="Device namespace that will appear before all non absolute topics and TF frames, used for distinguishing multiple cameras on the same robot.",
+    )
+
+    declare_docking_server_config_path_arg = DeclareLaunchArgument(
+        "docking_server_config_path",
+        default_value=PathJoinSubstitution(
+            [
+                FindPackageShare("husarion_ugv_docking"),
+                "config",
+                "docking_server.yaml",
+            ]
+        ),
+        description=("Path to docking server configuration file."),
+    )
+
+    declare_use_docking_arg = DeclareLaunchArgument(
+        "use_docking",
+        default_value="True",
+        description="Enable docking server and spawn docking stations in a simulation.",
+        choices=["True", "False", "true", "false"],
+    )
+
+    return LaunchDescription(
+        [
+            declare_docking_server_config_path_arg,
+            declare_device_namespace,
+            declare_use_docking_arg,
+            OpaqueFunction(function=spawn_stations),
+        ]
+    )

husarion_ugv_manager/CMakeLists.txt

@@ -12,12 +12,16 @@ set(PACKAGE_DEPENDENCIES
     behaviortree_ros2
     generate_parameter_library
     libssh
+    geometry_msgs
+    opennav_docking_msgs
     panther_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 +56,21 @@ 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(dock_robot_bt_node SHARED src/plugins/action/dock_robot_node.cpp)
+list(APPEND plugin_libs dock_robot_bt_node)
+
+add_library(undock_robot_bt_node SHARED
+            src/plugins/action/undock_robot_node.cpp)
+list(APPEND plugin_libs undock_robot_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)
@@ -137,6 +156,30 @@ 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_dock_robot_node
+    test/plugins/action/test_dock_robot_node.cpp
+    src/plugins/action/dock_robot_node.cpp)
+  list(APPEND plugin_tests ${PROJECT_NAME}_test_dock_robot_node)
+
+  ament_add_gtest(
+    ${PROJECT_NAME}_test_undock_robot_node
+    test/plugins/action/test_undock_robot_node.cpp
+    src/plugins/action/undock_robot_node.cpp)
+  list(APPEND plugin_tests ${PROJECT_NAME}_test_undock_robot_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

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

@@ -36,11 +36,11 @@ class CallSetLedAnimationService : public BT::RosServiceNode<panther_msgs::srv::
 
   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;