Charging Dock Plugin based on dock pose subscriber

panther_description/config/docking_components.yaml

@@ -9,12 +9,12 @@ components:
   - type: WCH02
     parent_link: world
     xyz: 3.0 -2.0 1.0
-    rpy: -1.57  0.0 1.57
+    rpy: 1.57  0.0 -1.57
     device_namespace: wireless_charger
 
   - type: CAM01
     name: camera
-    parent_link: lights_channel_1_link
-    xyz: 0.05 0.0 0.0
+    parent_link: front_bumper_link
+    xyz: 0.0 0.0 -0.06
     rpy: 0.0 0.0 0.0
     device_namespace: camera

panther_docking/CMakeLists.txt

@@ -35,10 +35,15 @@ pluginlib_export_plugin_description_file(opennav_docking_core plugin.xml)
 
 add_library(panther_charging_dock SHARED src/panther_charging_dock.cpp)
 ament_target_dependencies(panther_charging_dock ${PACKAGE_DEPENDENCIES})
+
 # TODO @delihus how to link the library what is not a name of a package
 target_link_libraries(panther_charging_dock
                       /opt/ros/humble/lib/libpose_filter.so)
 
+target_include_directories(
+  panther_charging_dock PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+                               $<INSTALL_INTERFACE:include>)
+
 install(TARGETS panther_charging_dock LIBRARY DESTINATION lib)
 
 install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME})
@@ -47,13 +52,19 @@ if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
 
   ament_add_gtest(${PROJECT_NAME}_test_panther_charging_dock
-                  test/test_panther_charging_dock.cpp)
+                  test/unit/test_panther_charging_dock.cpp)
   target_link_libraries(${PROJECT_NAME}_test_panther_charging_dock
                         panther_charging_dock)
   ament_target_dependencies(${PROJECT_NAME}_test_panther_charging_dock
                             ${PACKAGE_DEPENDENCIES})
+
 endif()
 
+add_executable(dock_pose_publisher src/dock_pose_publisher_node.cpp)
+ament_target_dependencies(dock_pose_publisher ${PACKAGE_DEPENDENCIES})
+
+install(TARGETS dock_pose_publisher DESTINATION lib/${PROJECT_NAME})
+
 ament_export_include_directories(include)
 ament_export_libraries(panther_charging_dock)
 

panther_docking/README.md

@@ -13,37 +13,41 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht
 
 ## ROS Nodes
 
+- `DockPosePublisherNode`: A node listens to `tf` and republishes position of `dock_pose` in the fixed frame.
 - `PantherChargingDock`:  A plugin for a Panther robot what is responsible for a charger service.
 
-### PantherChargingDock
+### DockPosePublisherNode
 
 #### Publishes
 
 - `docking/dock_pose` [*geometry_msgs/PoseStamped*]: An offset dock pose.
-- `docking/staging_pose` [*geometry_msgs/PoseStamped*]: An offset staging pose next to a charging station.
 
 #### Subscribers
 
-- `battery/charging_status` [*panther_msgs/ChargingStatus*]: A charging status of Panther robot.
-- `hardware/io_state` [*panther_msgs/IOState*]: States of GPIOs of Panther robot. The `PantherChargingDock` subscribes this topic to check if a robot charges.
+- `tf` [*tf2_msgs/TFMessage*]: Tf tree with a detected dock transform.
+
+#### Parameters
 
-#### Service Clients
+- `fixed_frame` [*string*, default: **odom**]: A fixed frame id of a robot.
+- `<dock_name>.type` [*string*, default: **panther_charging_dock**]: It checks if this dock with name `dock_name` is a type of  `panther_charging_dock`.
+- `<dock_name>.frame` [*string*, default: **main_wibotic_receiver_requested_pose_link** ]: Then look for transformation between `fixed_frame` and `<dock_name>.frame`  to publish `dock_pose`
 
-- `hardware/charger_enable` [*std_srvs/SetBoot*]: This service client enables charging in a robot.
+### PantherChargingDock
+
+#### Publishes
+
+- `docking/staging_pose` [*geometry_msgs/PoseStamped*]: An offset staging pose next to a charging station.
+
+#### Subscribers
+
+- `docking/dock_pose` [*geometry_msgs/PoseStamped*]: An offset dock pose.
 
 #### Parameters
 
-- `~<dock_name>.base_frame` [*string*, default: **base_link**]: A base frame id of a robot.
-- `~<dock_name>.external_detection_timeout` [*double*, default: **0.2**]: A timeout in seconds for looking up a transformation from an april tag of a dock to a base frame id.
-- `~<dock_name>.external_detection_translation_x` [*double*, default: **0.0**]: A translation over an X axis between a detected frame and a dock pose.
-- `~<dock_name>.external_detection_translation_y` [*double*, default: **0.0**]: A translation over an Y axis between a detected frame and a dock pose.
-- `~<dock_name>.external_detection_translation_x` [*double*, default: **0.0**]: A translation over a Z axis between a detected frame and a dock pose.
-- `~<dock_name>.external_detection_rotation_roll` [*double*, default: **0.0**]: A rotation over an X axis between a detected frame and a dock pose.
-- `~<dock_name>.external_detection_rotation_pitch` [*double*, default: **0.0**]: A rotation over an Y axis between a detected frame and a dock pose.
-- `~<dock_name>.external_detection_rotation_yaw` [*double*, default: **0.0**]: A rotation over a Z axis between a detected frame and a dock pose.
-- `~<dock_name>.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.
-- `~<dock_name>.docking_distance_threshold` [*double*, default: **0.05**]: A threshold of a distance between a robot pose and a dock pose to declare if docking succeed.
-- `~<dock_name>.docking_yaw_threshold` [*double*, default: **0.3**]: A threshold of a difference of yaw angles between a robot pose and a dock pose to declare if docking succeed.
-- `~<dock_name>.staging_x_offset` [*double*, default: **-0.7**]: A staging pose is defined by offsetting a dock pose in axis X.
-- `~<dock_name>.staging_yaw_offset` [*double*, default: **0.0**]: A staging pose is defined by offsetting a yaw angle.
-- `~<dock_name>.enable_charger_service_call_timeout` [*double*, default: **0.2**]: A timeout for calling enable charging service. A robot is unable to dock if excised.
+- `base_frame` [*string*, default: **base_link**]: A base frame id of a robot.
+- `fixed_frame` [*string*, default: **odom**]: A fixed frame id of a robot.
+- `<dock_type>.external_detection_timeout` [*double*, default: **0.2**]: A timeout in seconds for looking up a transformation from an april tag of a dock to a base frame id.
+- `<dock_type>.docking_distance_threshold` [*double*, default: **0.05**]: A threshold of a distance between a robot pose and a dock pose to declare if docking succeed.
+- `<dock_type>.docking_yaw_threshold` [*double*, default: **0.3**]: A threshold of a difference of yaw angles between a robot pose and a dock pose to declare if docking succeed.
+- `<dock_type>.staging_x_offset` [*double*, default: **-0.7**]: A staging pose is defined by offsetting a dock pose in axis X.
+- `<dock_type>.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_docking/config/panther_docking_server.yaml

@@ -16,33 +16,21 @@
     dock_plugins: ["panther_charging_dock"]
     panther_charging_dock:
       plugin: panther_docking::PantherChargingDock
-      base_frame: "<robot_namespace>/base_link"
-      docking_distance_threshold: 0.15
-      docking_yaw_threshold: 0.15
-      staging_x_offset: -0.8
-      staging_yaw_offset: 0.0
+      external_detection_timeout: 0.1
+      docking_distance_threshold: 0.08
+      docking_yaw_threshold: 0.1
+      staging_x_offset: -0.5
+      filter_coef: 0.1
 
-      # TODO: @delihus Try to remove this parameters by using docking station description in the ros_components_description
-      # Transform between april tag frame and dock pose. An april tag Z+ faces always a camera
-      external_detection_timeout: 0.3
-      external_detection_translation_x: 0.6
-      external_detection_translation_y: 0.0
-      external_detection_translation_z: 0.0
-      external_detection_rotation_roll: 0.0
-      external_detection_rotation_pitch: 0.0
-      external_detection_rotation_yaw: 3.14
-
-      enable_charger_service_call_timeout: 1.0
-
-    docks: ["main_dock"]
-    main_dock:
+    docks: ["main"]
+    main:
       type: panther_charging_dock
-      frame: <robot_namespace>/main_wibotic_station_link
+      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
 
     controller:
       k_phi: 1.0
       k_delta: 2.0
-      v_linear_min: 0.05
-      v_linear_max: 0.2
-      v_angular_max: 0.3
+      v_linear_min: 0.0
+      v_linear_max: 0.3
+      v_angular_max: 0.15

panther_docking/include/panther_docking/panther_charging_dock.hpp

@@ -25,6 +25,7 @@
 #include <opennav_docking_core/charging_dock.hpp>
 #include <opennav_docking_core/docking_exceptions.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
@@ -139,56 +140,12 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock
    */
   void getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node);
 
-  /**
-   * @brief Offset the staging pose.
-   *
-   * This method offsets the staging dock pose by values described in a configuration.
-   *
-   * @param dock_pose The dock pose to offset.
-   *
-   * @return The offset staging pose.
-   */
-  PoseStampedMsg offsetStagingPoseToDockPose(const PoseStampedMsg & dock_pose);
+  void updateAndPublishStagingPose();
 
-  /**
-   * @brief Offset the detected dock pose.
-   *
-   * This method offsets the detected dock pose by values described in a configuration.
-   *
-   * @param detected_dock_pose The detected dock pose to offset.
-   *
-   * @return The offset detected dock pose.
-   */
-  PoseStampedMsg offsetDetectedDockPose(const PoseStampedMsg & detected_dock_pose);
-
-  /**
-   * @brief Get the dock pose in a detection dock frame.
-   *
-   * This method retrieves the dock pose in a detection dock frame.
-   *
-   * @param frame The detection frame to get the dock pose in.
-   * @return The dock pose in the detection frame.
-   */
-  PoseStampedMsg getDockPose(const std::string & frame);
-
-  /**
-   * @brief Method to update the dock pose and publish it.
-   *
-   * This method makes all necessary transformations to update the dock pose and publishes it.
-   *
-   */
-  void updateDockPoseAndPublish();
-
-  /**
-   * @brief Update the staging pose and publish it.
-   *
-   * This method makes all necessary transformations to update the staging pose and publishes it.
-   *
-   * @param frame The frame to publish the staging pose in.
-   */
-  void updateStagingPoseAndPublish(const std::string & frame);
+  void setDockPose(const PoseStampedMsg::SharedPtr pose);
 
   std::string base_frame_name_;
+  std::string fixed_frame_name_;
   std::string dock_frame_;
 
   rclcpp::Logger logger_{rclcpp::get_logger("PantherChargingDock")};
@@ -198,16 +155,12 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock
   tf2_ros::Buffer::SharedPtr tf2_buffer_;
 
   rclcpp::Publisher<PoseStampedMsg>::SharedPtr staging_pose_pub_;
-  rclcpp::Publisher<PoseStampedMsg>::SharedPtr dock_pose_pub_;
+  rclcpp::Subscription<PoseStampedMsg>::SharedPtr dock_pose_sub_;
 
   PoseStampedMsg dock_pose_;
   PoseStampedMsg staging_pose_;
 
   double external_detection_timeout_;
-  tf2::Quaternion external_detection_rotation_;
-  double external_detection_translation_x_;
-  double external_detection_translation_y_;
-  double external_detection_translation_z_;
 
   std::shared_ptr<opennav_docking::PoseFilter> pose_filter_;
 

panther_docking/launch/docking.launch.py

@@ -50,6 +50,14 @@ def generate_launch_description():
         description=("Path to docking server configuration file."),
     )
 
+    log_level = LaunchConfiguration("log_level")
+    declare_log_level = DeclareLaunchArgument(
+        "log_level",
+        default_value="info",
+        description="Logging level",
+        choices=["debug", "info", "warning", "error"],
+    )
+
     namespaced_docking_server_config = ReplaceString(
         source_file=docking_server_config_path,
         replacements={"<robot_namespace>": namespace, "//": "/"},
@@ -62,6 +70,7 @@ def generate_launch_description():
             namespaced_docking_server_config,
             {"use_sim_time": use_sim},
         ],
+        arguments=["--ros-args", "--log-level", log_level, "--log-level", "rcl:=INFO"],
         namespace=namespace,
         emulate_tty=True,
     )
@@ -82,6 +91,19 @@ def generate_launch_description():
         namespace=namespace,
     )
 
+    dock_pose_publisher = Node(
+        package="panther_docking",
+        executable="dock_pose_publisher",
+        parameters=[
+            namespaced_docking_server_config,
+            {"use_sim_time": use_sim},
+        ],
+        name="dock_pose_publisher",
+        namespace=namespace,
+        emulate_tty=True,
+        arguments=["--ros-args", "--log-level", log_level, "--log-level", "rcl:=INFO"],
+    )
+
     station_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
@@ -100,8 +122,10 @@ def generate_launch_description():
             declare_use_sim_arg,
             declare_namespace_arg,
             declare_docking_server_config_path_arg,
+            declare_log_level,
             station_launch,
             docking_server_node,
             docking_server_activate_node,
+            dock_pose_publisher,
         ]
     )

panther_docking/launch/station.launch.py

@@ -33,7 +33,7 @@
 
 def generate_apriltag_and_get_path(tag_id):
     tag_generator = TagGenerator2("tag36h11")
-    tag_image = tag_generator.generate(tag_id, scale=100)
+    tag_image = tag_generator.generate(tag_id, scale=1000)
 
     path = f"/tmp/tag_{tag_id}.png"
 
@@ -99,13 +99,13 @@ def generate_launch_description():
 
     declare_apriltag_id = DeclareLaunchArgument(
         "apriltag_id",
-        default_value="1",
+        default_value="0",
         description="ID of a generated apriltag on the station",
     )
 
     declare_apriltag_size = DeclareLaunchArgument(
         "apriltag_size",
-        default_value="0.15",
+        default_value="0.06",
         description="Size in meters of a generated apriltag on the station",
     )
 

panther_docking/package.xml

@@ -28,6 +28,7 @@
 
   <exec_depend>nav2_lifecycle_manager</exec_depend>
   <exec_depend>python3-imageio</exec_depend>
+  <exec_depend>xacro</exec_depend>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <test_depend>ament_cmake_gtest</test_depend>