From 0e7017189acb252c9c7dfdf68fc9a7e312928587 Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Fri, 6 Dec 2024 11:27:11 +0100 Subject: [PATCH] Ros2 docking tf compare (#451) * Changed camera pose Signed-off-by: Jakub Delicat * Fixed test Signed-off-by: Jakub Delicat * Update docs Signed-off-by: Jakub Delicat * typo Signed-off-by: Jakub Delicat * typo Signed-off-by: Jakub Delicat * Update panther_docking/README.md Co-authored-by: Rafal Gorecki <126687345+rafal-gorecki@users.noreply.github.com> * updated vcs Signed-off-by: Jakub Delicat --------- Signed-off-by: Jakub Delicat Co-authored-by: Rafal Gorecki <126687345+rafal-gorecki@users.noreply.github.com> --- panther/panther_hardware.repos | 2 +- panther/panther_simulation.repos | 2 +- panther_description/config/docking_components.yaml | 6 +++--- panther_docking/README.md | 8 ++++---- panther_docking/config/panther_docking_server.yaml | 14 +++++++------- panther_docking/src/dock_pose_publisher_node.cpp | 2 +- panther_docking/src/panther_charging_dock.cpp | 2 ++ .../test/unit/test_panther_charging_dock.cpp | 14 +++----------- .../launch/spawn_charging_docks.launch.py | 2 +- 9 files changed, 23 insertions(+), 29 deletions(-) diff --git a/panther/panther_hardware.repos b/panther/panther_hardware.repos index 87fd010d..2b900239 100644 --- 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: 0813c3eebec410c2635b1db3ab87b094c38658c6 + 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/ diff --git a/panther/panther_simulation.repos b/panther/panther_simulation.repos index 50c918d6..b4b54fcb 100644 --- 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: 0813c3eebec410c2635b1db3ab87b094c38658c6 + 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/ diff --git a/panther_description/config/docking_components.yaml b/panther_description/config/docking_components.yaml index eb497cea..d51cbe62 100644 --- a/panther_description/config/docking_components.yaml +++ b/panther_description/config/docking_components.yaml @@ -4,12 +4,12 @@ components: parent_link: lights_channel_1_link xyz: -0.02 0.0 -0.0185 rpy: 0.0 0.0 0.0 - device_namespace: wireless_charger + device_namespace: None - type: CAM01 name: camera - parent_link: front_bumper_link - xyz: 0.0 0.0 -0.06 + parent_link: cover_link + xyz: 0.18 0.0 0.12 rpy: 0.0 0.0 0.0 device_namespace: camera diff --git a/panther_docking/README.md b/panther_docking/README.md index 47ab38ea..dd30304f 100644 --- a/panther_docking/README.md +++ b/panther_docking/README.md @@ -31,7 +31,7 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht - `fixed_frame` [*string*, default: **odom**]: A fixed frame id of a robot. - `.type` [*string*, default: **panther_charging_dock**]: It checks if this dock with name `dock_name` is a type of `panther_charging_dock`. -- `.frame` [*string*, default: **main_wibotic_receiver_requested_pose_link** ]: Then look for transformation between `fixed_frame` and `.frame` to publish `dock_pose` +- `.frame` [*string*, default: **main_wibotic_transmitter_link** ]: Then look for transformation between `fixed_frame` and `.frame` to publish `dock_pose`. A frame id of a wireless transmitter. ### PantherChargingDock @@ -45,7 +45,7 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht #### Parameters -- `base_frame` [*string*, default: **base_link**]: A base frame id of a robot. +- `base_frame` [*string*, default: **base_link**]: A frame id of a wireless receiver. - `fixed_frame` [*string*, default: **odom**]: A fixed frame id of a robot. - `panther_charging_dock.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. - `panther_charging_dock.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. @@ -53,7 +53,7 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht - `panther_charging_dock.staging_x_offset` [*double*, default: **-0.7**]: A staging pose is defined by offsetting a dock pose in axis X. - `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. +- `panther_charging_dock.wibotic_info_timeout` [*double*, default: **1.5**]: A timeout in seconds to receive a wibotic_info. - `.apriltag_id` [*int*, default: **0**]: AprilTag ID of a dock. -- `.dock_frame` [*string*, default: **main_wibotic_receiver_requested_pose_link**]: A frame id to compare with fixed frame if docked. +- `.dock_frame` [*string*, default: **main_wibotic_transmitter_link**]: A frame id to compare with fixed frame if docked. - `.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/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml index 2f7b223e..5f295466 100644 --- a/panther_docking/config/panther_docking_server.yaml +++ b/panther_docking/config/panther_docking_server.yaml @@ -4,10 +4,10 @@ initial_perception_timeout: 5.0 wait_charge_timeout: 10.0 dock_approach_timeout: 20.0 - undock_linear_tolerance: 0.08 + undock_linear_tolerance: 0.19 undock_angular_tolerance: 0.08 max_retries: 3 - base_frame: "/base_link" + base_frame: "/wibotic_receiver_link" # Distance to the dock is calculated from this frame fixed_frame: "/odom" dock_backwards: false dock_prestaging_tolerance: 0.5 @@ -17,7 +17,7 @@ panther_charging_dock: plugin: panther_docking::PantherChargingDock external_detection_timeout: 0.2 - docking_distance_threshold: 0.12 + docking_distance_threshold: 0.04 docking_yaw_threshold: 0.2 staging_x_offset: -1.3 filter_coef: 0.1 @@ -28,15 +28,15 @@ main: type: panther_charging_dock frame: /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. + dock_frame: main_wibotic_transmitter_link + pose: [1.0, 1.20, 1.57] # [x, y, yaw] of the dock on the map. Used also for spawning dock in the simulation. apriltag_id: 0 backup: type: panther_charging_dock frame: /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. + dock_frame: backup_wibotic_transmitter_link + pose: [-1.0, 1.20, 1.57] # [x, y, yaw] of the dock on the map. Used also for spawning dock in the simulation. apriltag_id: 1 controller: diff --git a/panther_docking/src/dock_pose_publisher_node.cpp b/panther_docking/src/dock_pose_publisher_node.cpp index 7fd97319..685c807c 100644 --- a/panther_docking/src/dock_pose_publisher_node.cpp +++ b/panther_docking/src/dock_pose_publisher_node.cpp @@ -45,7 +45,7 @@ DockPosePublisherNode::DockPosePublisherNode(const std::string & name) : Node(na for (const auto & dock : docks) { declare_parameter(dock + ".type", "panther_charging_dock"); - declare_parameter(dock + ".dock_frame", "main_wibotic_receiver_requested_pose_link"); + declare_parameter(dock + ".dock_frame", "main_wibotic_transmitter_link"); const auto dock_type = get_parameter(dock + ".type").as_string(); if (dock_type == "panther_charging_dock") { diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index 744db722..48bb5235 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -243,6 +243,8 @@ void PantherChargingDock::updateAndPublishStagingPose(const std::string & frame) staging_pose_.header.stamp = node_.lock()->now(); staging_pose_.pose.position.x += std::cos(yaw) * staging_x_offset_; staging_pose_.pose.position.y += std::sin(yaw) * staging_x_offset_; + staging_pose_.pose.position.z = 0.0; + tf2::Quaternion orientation; orientation.setRPY(0.0, 0.0, yaw); staging_pose_.pose.orientation = tf2::toMsg(orientation); diff --git a/panther_docking/test/unit/test_panther_charging_dock.cpp b/panther_docking/test/unit/test_panther_charging_dock.cpp index 80a7a759..4d3711e0 100644 --- a/panther_docking/test/unit/test_panther_charging_dock.cpp +++ b/panther_docking/test/unit/test_panther_charging_dock.cpp @@ -116,26 +116,18 @@ TEST_F(TestPantherChargingDock, GetStagingPoseLocal) dock_pose->pose.position.y = 1.0; dock_pose->pose.position.z = 0.0; dock_pose->pose.orientation.w = 1.0; - - dock_->setDockPose(dock_pose); - geometry_msgs::msg::PoseStamped pose; - - geometry_msgs::msg::PoseStamped staging_pose; - ASSERT_THROW( - { staging_pose = dock_->getStagingPose(pose.pose, kOdomFrame); }, - opennav_docking_core::FailedToDetectDock); - dock_pose->header.frame_id = kOdomFrame; dock_->setDockPose(dock_pose); - staging_pose = dock_->getStagingPose(pose.pose, kOdomFrame); + geometry_msgs::msg::PoseStamped pose; + geometry_msgs::msg::PoseStamped staging_pose = dock_->getStagingPose(pose.pose, kOdomFrame); ASSERT_FLOAT_EQ(staging_pose.pose.position.x, 0.3); ASSERT_FLOAT_EQ(staging_pose.pose.position.y, 1.0); ASSERT_FLOAT_EQ(staging_pose.pose.position.z, 0.0); } -// TODO: fill after nav2 tests +// TODO: @delihus fill after nav2 tests // TEST_F(TestPantherChargingDock, GetStagingPoseGlobal){ // } diff --git a/panther_gazebo/launch/spawn_charging_docks.launch.py b/panther_gazebo/launch/spawn_charging_docks.launch.py index 2f626618..2d5720d3 100644 --- a/panther_gazebo/launch/spawn_charging_docks.launch.py +++ b/panther_gazebo/launch/spawn_charging_docks.launch.py @@ -72,7 +72,7 @@ def spawn_stations(context, *args, **kwargs): "-y", str(pose[1] - 2.0), # -2.0 is the offset between world and map "-z", - "0.2", # station z is not in 0.0 + "0.5", # station z is not in 0.0 "-R", "1.57", "-P",