Ros2 docking tf compare (#451)

* Changed camera pose

Signed-off-by: Jakub Delicat <[email protected]>

* Fixed test

Signed-off-by: Jakub Delicat <[email protected]>

* Update docs

Signed-off-by: Jakub Delicat <[email protected]>

* typo

Signed-off-by: Jakub Delicat <[email protected]>

* typo

Signed-off-by: Jakub Delicat <[email protected]>

* Update panther_docking/README.md

Co-authored-by: Rafal Gorecki <[email protected]>

* updated vcs

Signed-off-by: Jakub Delicat <[email protected]>

---------

Signed-off-by: Jakub Delicat <[email protected]>
Co-authored-by: Rafal Gorecki <[email protected]>

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/

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/

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
 

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.
 - `<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`
+- `<dock_name>.frame` [*string*, default: **main_wibotic_transmitter_link** ]: Then look for transformation between `fixed_frame` and `<dock_name>.frame`  to publish `dock_pose`. A frame id of a wireless transmitter.
 
 ### PantherChargingDock
 
@@ -45,15 +45,15 @@ 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.
 - `panther_charging_dock.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.
 - `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.
 - `<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>.dock_frame` [*string*, default: **main_wibotic_transmitter_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/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: "<robot_namespace>/base_link"
+    base_frame: "<robot_namespace>/wibotic_receiver_link" # Distance to the dock is calculated from this frame
     fixed_frame: "<robot_namespace>/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: <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.
+      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: <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.
+      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:

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") {

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);

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){
 // }
 

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",