ROS2 docking nav2

panther_description/config/docking_components.yaml

@@ -6,15 +6,15 @@ components:
     rpy: 0.0 0.0 0.0
     device_namespace: wireless_charger
 
-  - type: WCH02
-    parent_link: world
-    xyz: 3.0 -2.0 1.0
-    rpy: 1.57  0.0 -1.57
-    device_namespace: wireless_charger
-
   - type: CAM01
     name: camera
     parent_link: front_bumper_link
     xyz: 0.0 0.0 -0.06
     rpy: 0.0 0.0 0.0
     device_namespace: camera
+
+  - type: LDR06
+    parent_link: cover_link
+    xyz: 0.0 0.0 0.0
+    rpy: 0.0 0.0 0.0
+    device_namespace: main_lidar

panther_docking/CMakeLists.txt

@@ -61,7 +61,8 @@ if(BUILD_TESTING)
 
 endif()
 
-add_executable(dock_pose_publisher src/dock_pose_publisher_node.cpp)
+add_executable(dock_pose_publisher src/dock_pose_publisher_node.cpp
+                                   src/dock_pose_publisher_main.cpp)
 ament_target_dependencies(dock_pose_publisher ${PACKAGE_DEPENDENCIES})
 
 install(TARGETS dock_pose_publisher DESTINATION lib/${PROJECT_NAME})

panther_docking/README.md

@@ -53,3 +53,6 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht
 - `panther_charging_dock.filter_coef` [*double*, default: **0.1**]: A key parameter that influences the trade-off between the filter's responsiveness and its smoothness, balancing how quickly it reacts to new pose data pose how much it smooths out fluctuations.
 - `panther_charging_dock.use_wibotic_info` [*bool*, default: **True**]: Whether Wibotic information is used.
 - `panther_charging_dock.wibotic_info_timeout` [*double*, default: **1.5**]: A timeout in seconds to receive wibotic_info.
+- `<dock_name>.apriltag_id` [*int*, default: **0**]: AprilTag ID of a dock.
+- `<dock_name>.dock_frame` [*string*, default: **main_wibotic_receiver_requested_pose_link**]: A frame id to compare with fixed frame if docked.
+- `<dock_name>.pose` [*list*, default: **[0.0, 0.0, 0.0]**]: A pose of a dock on the map. If the simulation is used a dock is spawned in this pose.

panther_docking/config/apriltag.yaml

@@ -0,0 +1,23 @@
+---
+/**:
+  ros__parameters:
+    image_transport: raw
+    family: 36h11
+    size: 0.06
+    profile: false
+
+
+    max_hamming: 0
+    detector:
+      threads: 1
+      decimate: 2.0
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+
+
+    tag:
+      ids: [0, 1]
+      frames: [main_apriltag_link, backup_apriltag_link]
+      sizes: [0.06, 0.06]

panther_docking/config/panther_docking_server.yaml

@@ -16,23 +16,32 @@
     dock_plugins: ["panther_charging_dock"]
     panther_charging_dock:
       plugin: panther_docking::PantherChargingDock
-      external_detection_timeout: 0.1
+      external_detection_timeout: 0.2
       docking_distance_threshold: 0.12
-      docking_yaw_threshold: 0.15
-      staging_x_offset: -0.5
+      docking_yaw_threshold: 0.2
+      staging_x_offset: -0.8
       filter_coef: 0.1
       use_wibotic_info: <use_wibotic_info_param>
       wibotic_info_timeout: 1.0
 
-    docks: ["main"]
+    docks: ["main", "backup"]
     main:
       type: panther_charging_dock
-      frame: <robot_namespace>/main_wibotic_receiver_requested_pose_link
-      pose: [0.0, 0.0, 0.0] # position of the dock device (not the staging position), the front (X+) of the dock should point away from the robot
+      frame: <robot_namespace>/map
+      dock_frame: main_wibotic_receiver_requested_pose_link
+      pose: [1.0, 1.5, 1.57] # position of the dock on the map. Used also for spawning dock in the simulation.
+      apriltag_id: 0
+
+    backup:
+      type: panther_charging_dock
+      frame: <robot_namespace>/map
+      dock_frame: backup_wibotic_receiver_requested_pose_link
+      pose: [-1.0, 1.5, 1.57]  # position of the dock on the map. Used also for spawning dock in the simulation.
+      apriltag_id: 1
 
     controller:
       k_phi: 1.0
       k_delta: 2.0
       v_linear_min: 0.0
-      v_linear_max: 0.1
+      v_linear_max: 0.3
       v_angular_max: 0.15

panther_docking/include/panther_docking/dock_pose_publisher_node.hpp

@@ -0,0 +1,51 @@
+// Copyright (c) 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.
+
+#ifndef PANTHER_DOCKING_DOCK_POSE_PUBLISHER_NODE_HPP_
+#define PANTHER_DOCKING_DOCK_POSE_PUBLISHER_NODE_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+namespace panther_docking
+{
+constexpr double kMinimalDetectionDistance = 1.0;
+
+class DockPosePublisherNode : public rclcpp::Node
+{
+public:
+  DockPosePublisherNode(const std::string & name);
+
+private:
+  void publishPose();
+
+  double timeout_;
+  std::string target_frame_;
+  std::vector<std::string> source_frames_;
+  std::string base_frame_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publisher_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+}  // namespace panther_docking
+
+#endif  // PANTHER_DOCKING_DOCK_POSE_PUBLISHER_NODE_HPP_

panther_docking/include/panther_docking/panther_charging_dock.hpp

@@ -149,8 +149,10 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock
    * @brief Method to update and publish the staging pose.
    *
    * Uses staging_x_offset_ and staging_yaw_offset_ to calculate the staging pose.
+   *
+   * @param frame The frame to publish the staging pose in.
    */
-  void updateAndPublishStagingPose();
+  void updateAndPublishStagingPose(const std::string & frame);
 
   /**
    * @brief Dock pose callback, used for external detection.

panther_docking/launch/docking.launch.py

@@ -43,6 +43,15 @@ def generate_launch_description():
         choices=["True", "False", "true", "false"],
     )
 
+    apriltag_config_path = LaunchConfiguration("apriltag_config_path")
+    declare_apriltag_config_path_arg = DeclareLaunchArgument(
+        "apriltag_config_path",
+        default_value=PathJoinSubstitution(
+            [FindPackageShare("panther_docking"), "config", "apriltag.yaml"]
+        ),
+        description=("Path to apriltag configuration file. Only used in simulation."),
+    )
+
     namespace = LaunchConfiguration("namespace")
     use_docking = LaunchConfiguration("use_docking")
     use_sim = LaunchConfiguration("use_sim")
@@ -119,6 +128,20 @@ def generate_launch_description():
         condition=IfCondition(use_docking),
     )
 
+    apriltag_node = Node(
+        package="apriltag_ros",
+        executable="apriltag_node",
+        parameters=[{"use_sim_time": True}, apriltag_config_path],
+        namespace=namespace,
+        emulate_tty=True,
+        remappings={
+            "camera_info": "camera/color/camera_info",
+            "image_rect": "camera/color/image_raw",
+            "detections": "docking/april_tags",
+        }.items(),
+        condition=IfCondition(PythonExpression([use_docking, " and ", use_sim])),
+    )
+
     station_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
@@ -143,35 +166,18 @@ def generate_launch_description():
         ),
     )
 
-    # FIXME: Remove before release
-    panther_manager_dir = FindPackageShare("panther_manager")
-    docking_manager_node = Node(
-        package="panther_manager",
-        executable="docking_manager_node",
-        name="docking_manager",
-        parameters=[
-            PathJoinSubstitution([panther_manager_dir, "config", "docking_manager.yaml"]),
-            {
-                "bt_project_path": PathJoinSubstitution(
-                    [panther_manager_dir, "behavior_trees", "DockingBT.btproj"]
-                )
-            },
-        ],
-        namespace=namespace,
-        emulate_tty=True,
-    )
-
     return LaunchDescription(
         [
-            declare_use_docking_arg,
+            declare_apriltag_config_path_arg,
             declare_docking_server_config_path_arg,
-            declare_log_level,
+            declare_use_docking_arg,
             declare_use_wibotic_info_arg,
+            declare_log_level,
             station_launch,
             docking_server_node,
             docking_server_activate_node,
             dock_pose_publisher,
+            apriltag_node,
             wibotic_connector_can,
-            docking_manager_node,
         ]
     )

panther_docking/launch/station.launch.py

@@ -14,17 +14,19 @@
 # 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 (
     Command,
-    EnvironmentVariable,
     FindExecutable,
     LaunchConfiguration,
     PathJoinSubstitution,
-    PythonExpression,
 )
 from launch_ros.actions import Node
 from launch_ros.parameter_descriptions import ParameterValue
@@ -36,19 +38,13 @@ def generate_apriltag_and_get_path(tag_id):
 
     tag_generator = TagGenerator2("tag36h11")
     tag_image = tag_generator.generate(tag_id, scale=1000)
+    temp_file = NamedTemporaryFile(suffix=f"_tag_{tag_id}.png", delete=False)
 
-    path = f"/tmp/tag_{tag_id}.png"
-
-    imageio.imwrite(path, tag_image)
-    return path
+    imageio.imwrite(temp_file.name, tag_image)
+    return temp_file.name
 
 
-def launch_setup(context, *args, **kwargs):
-    namespace = LaunchConfiguration("namespace").perform(context)
-    apriltag_id = int(LaunchConfiguration("apriltag_id").perform(context))
-    apriltag_size = LaunchConfiguration("apriltag_size").perform(context)
-    use_docking = LaunchConfiguration("use_docking").perform(context)
-
+def generate_urdf(name, apriltag_id, apriltag_size):
     apriltag_image_path = generate_apriltag_and_get_path(apriltag_id)
 
     station_description_content = Command(
@@ -63,51 +59,63 @@ def launch_setup(context, *args, **kwargs):
                 ]
             ),
             " device_namespace:=",
-            "main",
+            name,
             " apriltag_image_path:=",
             apriltag_image_path,
             " apriltag_size:=",
             apriltag_size,
         ]
     )
+    return station_description_content
 
-    namespace_ext = PythonExpression(["'", namespace, "' + '/' if '", namespace, "' else ''"])
-
-    station_description = {
-        "robot_description": ParameterValue(station_description_content, value_type=str)
-    }
-
-    station_state_pub_node = Node(
-        package="robot_state_publisher",
-        executable="robot_state_publisher",
-        name="wibotic_station_state_publisher",
-        parameters=[
-            station_description,
-            {"frame_prefix": namespace_ext},
-        ],
-        remappings=[("robot_description", "station_description")],
-        namespace=namespace,
-        emulate_tty=True,
-        condition=IfCondition(use_docking),
-    )
 
-    return [station_state_pub_node]
+def launch_stations_descriptions(context, *args, **kwargs):
+    apriltag_id = int(LaunchConfiguration("apriltag_id").perform(context))
+    apriltag_size = LaunchConfiguration("apriltag_size").perform(context)
+    use_docking = LaunchConfiguration("use_docking").perform(context)
 
+    docking_server_config_path = LaunchConfiguration("docking_server_config_path").perform(context)
+    apriltag_size = LaunchConfiguration("apriltag_size").perform(context)
 
-def generate_launch_description():
-    declare_use_docking_arg = DeclareLaunchArgument(
-        "use_docking",
-        default_value="True",
-        description="Enable docking server.",
-        choices=["True", "False", "true", "false"],
-    )
+    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:
+        apriltag_id = ros_parameters[dock_name]["apriltag_id"]
+        station_description_content = generate_urdf(dock_name, apriltag_id, apriltag_size)
+        station_description = {
+            "robot_description": ParameterValue(station_description_content, value_type=str)
+        }
+
+        station_state_pub_node = Node(
+            package="robot_state_publisher",
+            executable="robot_state_publisher",
+            name=[dock_name, "_station_state_publisher"],
+            parameters=[
+                station_description,
+            ],
+            remappings=[("robot_description", [dock_name, "_station_description"])],
+            emulate_tty=True,
+            condition=IfCondition(use_docking),
+        )
+
+        actions.append(station_state_pub_node)
+
+    return actions
 
-    declare_namespace_arg = DeclareLaunchArgument(
-        "namespace",
-        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
-        description="Add namespace to all launched nodes.",
-    )
 
+def generate_launch_description():
     declare_apriltag_id = DeclareLaunchArgument(
         "apriltag_id",
         default_value="0",
@@ -122,10 +130,8 @@ def generate_launch_description():
 
     return LaunchDescription(
         [
-            declare_use_docking_arg,
-            declare_namespace_arg,
             declare_apriltag_id,
             declare_apriltag_size,
-            OpaqueFunction(function=launch_setup),
+            OpaqueFunction(function=launch_stations_descriptions),
         ]
     )