From 87ddca88c0f282bdb96b424b925deca5b9ce6d32 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 2 Dec 2024 19:22:22 +0000 Subject: [PATCH 1/7] Changed camera pose Signed-off-by: Jakub Delicat --- .../config/docking_components.yaml | 6 +++--- .../config/panther_docking_server.yaml | 18 +++++++++--------- .../src/dock_pose_publisher_node.cpp | 2 +- panther_docking/src/panther_charging_dock.cpp | 2 ++ .../launch/spawn_charging_docks.launch.py | 2 +- 5 files changed, 16 insertions(+), 14 deletions(-) diff --git a/panther_description/config/docking_components.yaml b/panther_description/config/docking_components.yaml index eb497cea0..d51cbe62e 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/config/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml index 2f7b223e9..1bc785605 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" 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 @@ -27,16 +27,16 @@ docks: ["main", "backup"] 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. + frame: /odom + 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. + frame: /odom + 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 7fd97319b..685c807cc 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 744db7227..48bb52351 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_gazebo/launch/spawn_charging_docks.launch.py b/panther_gazebo/launch/spawn_charging_docks.launch.py index 2f626618e..2d5720d3e 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", From 23b8089a62437b9bb5416c7dded4cf1f368757e4 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 2 Dec 2024 19:37:27 +0000 Subject: [PATCH 2/7] Fixed test Signed-off-by: Jakub Delicat --- .../test/unit/test_panther_charging_dock.cpp | 14 +++----------- 1 file changed, 3 insertions(+), 11 deletions(-) diff --git a/panther_docking/test/unit/test_panther_charging_dock.cpp b/panther_docking/test/unit/test_panther_charging_dock.cpp index 80a7a7593..4d3711e05 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){ // } From 3c4a0ef3453992ee693514cac4f18844a9180093 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 2 Dec 2024 19:57:04 +0000 Subject: [PATCH 3/7] Update docs Signed-off-by: Jakub Delicat --- panther_docking/README.md | 8 ++++---- panther_docking/config/panther_docking_server.yaml | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/panther_docking/README.md b/panther_docking/README.md index 47ab38ea5..41d68ec67 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_receiver_requested_pose_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 fixed 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 1bc785605..5f295466a 100644 --- a/panther_docking/config/panther_docking_server.yaml +++ b/panther_docking/config/panther_docking_server.yaml @@ -7,7 +7,7 @@ undock_linear_tolerance: 0.19 undock_angular_tolerance: 0.08 max_retries: 3 - base_frame: "/wibotic_receiver_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 @@ -27,14 +27,14 @@ docks: ["main", "backup"] main: type: panther_charging_dock - frame: /odom + frame: /map 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: /odom + frame: /map 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 From 2b2c273b3e6c0209cd6065fe2cc5c83b622c3b1c Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 2 Dec 2024 19:58:27 +0000 Subject: [PATCH 4/7] typo Signed-off-by: Jakub Delicat --- panther_docking/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_docking/README.md b/panther_docking/README.md index 41d68ec67..4dd05d528 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`.A frame id of a wireless transmitter. +- `.frame` [*string*, default: **main_wibotic_receiver_requested_pose_link** ]: Then look for transformation between `fixed_frame` and `.frame` to publish `dock_pose`. A frame id of a wireless transmitter. ### PantherChargingDock From 0c27878521f25111f55a31209cffa91b21e22bdd Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 2 Dec 2024 19:59:00 +0000 Subject: [PATCH 5/7] typo Signed-off-by: Jakub Delicat --- panther_docking/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_docking/README.md b/panther_docking/README.md index 4dd05d528..0f36b2572 100644 --- a/panther_docking/README.md +++ b/panther_docking/README.md @@ -45,7 +45,7 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht #### Parameters -- `base_frame` [*string*, default: **base_link**]: A fixed frame id of a wireless receiver. +- `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. From d6915314ee0ef43a8fd0f098e3291f2fea2dfd86 Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Fri, 6 Dec 2024 11:20:03 +0100 Subject: [PATCH 6/7] Update panther_docking/README.md Co-authored-by: Rafal Gorecki <126687345+rafal-gorecki@users.noreply.github.com> --- panther_docking/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_docking/README.md b/panther_docking/README.md index 0f36b2572..dd30304fb 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`. A frame id of a wireless transmitter. +- `.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 From 03ca6c29184b93e7f07a1e6ca87ccb060383c1c7 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 6 Dec 2024 10:22:51 +0000 Subject: [PATCH 7/7] updated vcs Signed-off-by: Jakub Delicat --- panther/panther_hardware.repos | 2 +- panther/panther_simulation.repos | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/panther/panther_hardware.repos b/panther/panther_hardware.repos index 87fd010dc..2b900239e 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 50c918d6d..b4b54fcb0 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/