From 0a84793dc84c9ea5b350055f2827be506b7dadd8 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 15 Nov 2024 10:47:36 +0000 Subject: [PATCH] Rafal's suggestions applied Signed-off-by: Jakub Delicat --- README.md | 1 + panther_docking/README.md | 14 +++++++------- .../panther_docking/panther_charging_dock.hpp | 6 ++---- panther_docking/launch/docking.launch.py | 10 ++++++---- panther_docking/package.xml | 2 +- panther_docking/src/panther_charging_dock.cpp | 3 +-- 6 files changed, 18 insertions(+), 18 deletions(-) diff --git a/README.md b/README.md index 3e7a1f2e..a61cfcda 100644 --- a/README.md +++ b/README.md @@ -109,6 +109,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev | 🤖🖥️ | `use_ekf` | Enable or disable EKF.
***bool:*** `True` | | 🤖🖥️ | `use_sim` | Whether simulation is used.
***bool:*** `False` | | 🤖🖥️ | `user_led_animations_file` | Path to a YAML file with a description of the user-defined animations.
***string:*** `''` | +| 🤖🖥️ | `panther_charging_dock.use_wibotic_info` | Use readings from `wibotic_info` topics to ensure that a robot is charging.
**bool**: `True` | | 🤖🖥️ | `wheel_config_path` | Path to wheel configuration file.
***string:*** [`{wheel_type}.yaml`](./panther_description/config) | | 🤖🖥️ | `wheel_type` | Type of wheel. If you choose a value from the preset options ('WH01', 'WH02', 'WH04'), you can ignore the 'wheel_config_path' and 'controller_config_path' parameters. For custom wheels, please define these parameters to point to files that accurately describe the custom wheels.
***string:*** `WH01` (choices: `WH01`, `WH02`, `WH04`, `custom`) | | 🖥️ | `x` | Initial robot position in the global 'x' axis.
***float:*** `0.0` | diff --git a/panther_docking/README.md b/panther_docking/README.md index 0fa6c19f..0e3059cd 100644 --- a/panther_docking/README.md +++ b/panther_docking/README.md @@ -46,10 +46,10 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht - `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. -- `.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. -- `.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. -- `.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. -- `.staging_x_offset` [*double*, default: **-0.7**]: A staging pose is defined by offsetting a dock pose in axis X. -- `.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. -- `.use_wibotic_info` [*bool*, default: **True**]: Use readings from `wibotic_info` topics to ensure that a robot is charging. -- `.wibotic_info_timeout` [*double*, default: **0.2**]: A timeout in seconds for wibotic_info. +- `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**]: Use readings from `wibotic_info` topics to ensure that a robot is charging. +- `panther_charging_dock.wibotic_info_timeout` [*double*, default: **1.5**]: A timeout in seconds to receive wibotic_info. diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index 7523d8fa..2897fda4 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -155,16 +155,14 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock /** * @brief Set the dock pose. * - * This method sets the dock pose. It can be used as a callback for a subscription. + * This method sets the dock pose. It can be used as a callback for a subscription. * * @param pose The dock pose. */ void setDockPose(const PoseStampedMsg::SharedPtr pose); /** - * @brief Set the Wibotic info. - * - * This method sets the Wibotic info. It can be used as a callback for a subscription. + * @brief Wibotic info callback, used when `use_wibotic_info` is enabled. * * @param msg The Wibotic info message. */ diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py index 189c9786..68690e3f 100644 --- a/panther_docking/launch/docking.launch.py +++ b/panther_docking/launch/docking.launch.py @@ -151,14 +151,16 @@ def generate_launch_description(): name="docking_manager", parameters=[ PathJoinSubstitution([panther_manager_dir, "config", "docking_manager.yaml"]), - {"bt_project_path": PathJoinSubstitution( - [panther_manager_dir, "behavior_trees", "DockingBT.btproj"])}, + { + "bt_project_path": PathJoinSubstitution( + [panther_manager_dir, "behavior_trees", "DockingBT.btproj"] + ) + }, ], namespace=namespace, emulate_tty=True, ) - return LaunchDescription( [ declare_use_docking_arg, @@ -170,6 +172,6 @@ def generate_launch_description(): docking_server_activate_node, dock_pose_publisher, wibotic_connector_can, - docking_manager_node + docking_manager_node, ] ) diff --git a/panther_docking/package.xml b/panther_docking/package.xml index 2f5b6573..eb9ece00 100644 --- a/panther_docking/package.xml +++ b/panther_docking/package.xml @@ -17,6 +17,7 @@ geometry_msgs nav2_util opennav_docking + panther_manager panther_utils pluginlib python3-pip @@ -28,7 +29,6 @@ wibotic_msgs - panther_manager nav2_lifecycle_manager python3-imageio diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index e9d16741..9890e97e 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -45,11 +45,10 @@ void PantherChargingDock::configure( declareParameters(node); getParameters(node); - if(!use_wibotic_info_){ + if (!use_wibotic_info_) { RCLCPP_INFO(logger_, "Wibotic info is disabled."); } - pose_filter_ = std::make_unique( pose_filter_coef_, external_detection_timeout_); }