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>
husarion · Nov 29, 2024 · b9e11a5 · b9e11a5
1 parent 0327ac4
commit b9e11a5
Show file tree

Hide file tree

Showing 19 changed files with 468 additions and 240 deletions.
diff --git a/panther_description/config/docking_components.yaml b/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
diff --git a/panther_docking/CMakeLists.txt b/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})

diff --git a/panther_docking/README.md b/panther_docking/README.md
@@ -9,7 +9,8 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht
 
 ## Configuration Files
 
-- [`panther_docking_server.yaml`](./config/panther_docking_server.yaml): Defines parameters for a `docking_server` and a `PantherChargingDock` plugin.
+- [`panther_docking_server.yaml`](./config/panther_docking_server.yaml): Defines parameters for a `docking_server` and a `PantherChargingDock` plugin. Defines poses where charging docks are spawned in the Gazebo.
+
 
 ## ROS Nodes
 
@@ -53,3 +54,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.
diff --git a/panther_docking/config/apriltag.yaml b/panther_docking/config/apriltag.yaml
@@ -0,0 +1,21 @@
+---
+/**:
+  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]
diff --git a/panther_docking/config/panther_docking_server.yaml b/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: -1.3
       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]  # [x, y, yaw] pose 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
diff --git a/panther_docking/include/panther_docking/dock_pose_publisher_node.hpp b/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_
diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/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.

diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py
@@ -36,16 +36,18 @@ def generate_launch_description():
         description=("Path to docking server configuration file."),
     )
 
-    declare_use_docking_arg = DeclareLaunchArgument(
-        "use_docking",
-        default_value="True",
-        description="Enable docking server.",
-        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")
+    namespace = LaunchConfiguration("namespace", default="")
+    use_docking = LaunchConfiguration("use_docking", default="True")
+    use_sim = LaunchConfiguration("use_sim", default="False")
 
     log_level = LaunchConfiguration("log_level")
     declare_log_level = DeclareLaunchArgument(
@@ -119,6 +121,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 +159,17 @@ 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_wibotic_info_arg,
             station_launch,
             docking_server_node,
             docking_server_activate_node,
             dock_pose_publisher,
+            apriltag_node,
             wibotic_connector_can,
-            docking_manager_node,
         ]
     )