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)
@@ -66,11 +85,13 @@ add_executable(safety_manager_node src/safety_manager_node_main.cpp
 ament_target_dependencies(
   safety_manager_node
   behaviortree_ros2
-  panther_msgs
+  geometry_msgs
   husarion_ugv_utils
+  panther_msgs
   rclcpp
   sensor_msgs
-  std_msgs)
+  std_msgs
+  tf2_geometry_msgs)
 
 generate_parameter_library(safety_manager_parameters
                            config/safety_manager_parameters.yaml)
@@ -82,11 +103,13 @@ add_executable(lights_manager_node src/lights_manager_node_main.cpp
 ament_target_dependencies(
   lights_manager_node
   behaviortree_ros2
-  panther_msgs
+  geometry_msgs
   husarion_ugv_utils
+  panther_msgs
   rclcpp
   sensor_msgs
-  std_msgs)
+  std_msgs
+  tf2_geometry_msgs)
 
 generate_parameter_library(lights_manager_parameters
                            config/lights_manager_parameters.yaml)
@@ -137,6 +160,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
@@ -179,7 +226,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)
@@ -200,11 +247,13 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_lights_manager_node
     behaviortree_cpp
     behaviortree_ros2
-    panther_msgs
+    geometry_msgs
     husarion_ugv_utils
+    panther_msgs
     rclcpp
     sensor_msgs
-    std_msgs)
+    std_msgs
+    tf2_geometry_msgs)
   target_link_libraries(${PROJECT_NAME}_test_lights_manager_node
                         lights_manager_parameters)
 
@@ -219,11 +268,13 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_lights_behavior_tree
     behaviortree_cpp
     behaviortree_ros2
-    panther_msgs
+    geometry_msgs
     husarion_ugv_utils
+    panther_msgs
     rclcpp
     sensor_msgs
-    std_msgs)
+    std_msgs
+    tf2_geometry_msgs)
   target_link_libraries(${PROJECT_NAME}_test_lights_behavior_tree
                         lights_manager_parameters)
 
@@ -237,11 +288,13 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_safety_manager_node
     behaviortree_cpp
     behaviortree_ros2
-    panther_msgs
+    geometry_msgs
     husarion_ugv_utils
+    panther_msgs
     rclcpp
     sensor_msgs
-    std_msgs)
+    std_msgs
+    tf2_geometry_msgs)
   target_link_libraries(${PROJECT_NAME}_test_safety_manager_node
                         safety_manager_parameters)
 
@@ -256,12 +309,14 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_safety_behavior_tree
     behaviortree_cpp
     behaviortree_ros2
-    panther_msgs
+    geometry_msgs
     husarion_ugv_utils
+    panther_msgs
     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: