Charging Dock Plugin based on dock pose subscriber (#431)

* 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> --------- Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>
husarion · Nov 8, 2024 · 4ee2f68 · 4ee2f68
1 parent 5fc7560
commit 4ee2f68
Show file tree

Hide file tree

Showing 19 changed files with 583 additions and 672 deletions.
diff --git a/panther/panther_hardware.repos b/panther/panther_hardware.repos
@@ -14,7 +14,7 @@ repositories:
   ros_components_description:
     type: git
     url: https://github.com/husarion/ros_components_description.git
-    version: 99e0189970293bb0110aad0a3b609bc659ecc663
+    version: b29f41ac00ab1a6fbac3e1d03602575094de277a
   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/

diff --git a/panther/panther_simulation.repos b/panther/panther_simulation.repos
@@ -14,7 +14,7 @@ repositories:
   ros_components_description:
     type: git
     url: https://github.com/husarion/ros_components_description.git
-    version: 99e0189970293bb0110aad0a3b609bc659ecc663
+    version: b29f41ac00ab1a6fbac3e1d03602575094de277a
   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/

diff --git a/panther_description/config/docking_components.yaml b/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
diff --git a/panther_docking/CMakeLists.txt b/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)
 

diff --git a/panther_docking/README.md b/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.
diff --git a/panther_docking/config/panther_docking_server.yaml b/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
diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/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_;
 

diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py
@@ -13,13 +13,11 @@
 # limitations under the License.
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.conditions import IfCondition
-from launch.substitutions import (
-    EnvironmentVariable,
-    LaunchConfiguration,
-    PathJoinSubstitution,
-)
+from launch.actions import DeclareLaunchArgument  # , IncludeLaunchDescription
+from launch.conditions import IfCondition  # , UnlessCondition
+
+# from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 from nav2_common.launch import ReplaceString
@@ -35,27 +33,23 @@ def generate_launch_description():
         description=("Path to docking server configuration file."),
     )
 
-    namespace = LaunchConfiguration("namespace")
-    declare_namespace_arg = DeclareLaunchArgument(
-        "namespace",
-        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
-        description="Add namespace to all launched nodes.",
-    )
-
-    use_docking = LaunchConfiguration("use_docking")
     declare_use_docking_arg = DeclareLaunchArgument(
         "use_docking",
         default_value="True",
         description="Enable docking server.",
         choices=["True", "False", "true", "false"],
     )
 
+    namespace = LaunchConfiguration("namespace")
+    use_docking = LaunchConfiguration("use_docking")
     use_sim = LaunchConfiguration("use_sim")
-    declare_use_sim_arg = DeclareLaunchArgument(
-        "use_sim",
-        default_value="False",
-        description="Whether simulation is used.",
-        choices=["True", "False", "true", "false"],
+
+    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(
@@ -71,6 +65,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"],
         remappings=[("~/transition_event", "~/_transition_event")],
         emulate_tty=True,
         condition=IfCondition(use_docking),
@@ -93,13 +88,43 @@ def generate_launch_description():
         condition=IfCondition(use_docking),
     )
 
+    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"],
+        condition=IfCondition(use_docking),
+    )
+
+    # FIXME: This launch does not work with the simulation. It can be caused by different versions of opencv
+    # station_launch = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         PathJoinSubstitution(
+    #             [
+    #                 FindPackageShare("panther_docking"),
+    #                 "launch",
+    #                 "station.launch.py",
+    #             ]
+    #         ),
+    #     ),
+    #     launch_arguments={"namespace": namespace}.items(),
+    #     condition=UnlessCondition(use_sim),
+    # )
+
     return LaunchDescription(
         [
-            declare_docking_server_config_path_arg,
-            declare_namespace_arg,
             declare_use_docking_arg,
-            declare_use_sim_arg,
+            declare_docking_server_config_path_arg,
+            declare_log_level,
+            # station_launch,
             docking_server_node,
             docking_server_activate_node,
+            dock_pose_publisher,
         ]
     )