From 7120e879df5ef17c89e7ff52e85e7a0c26736e2d Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 22 Nov 2024 09:32:23 +0000 Subject: [PATCH] Fixed after merge Signed-off-by: Jakub Delicat --- panther_docking/config/panther_docking_server.yaml | 4 +++- .../include/panther_docking/panther_charging_dock.hpp | 4 +++- panther_docking/src/dock_pose_publisher_node.cpp | 9 ++++----- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/panther_docking/config/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml index 5725a323e..c9de95c09 100644 --- a/panther_docking/config/panther_docking_server.yaml +++ b/panther_docking/config/panther_docking_server.yaml @@ -16,11 +16,13 @@ dock_plugins: ["panther_charging_dock"] panther_charging_dock: plugin: panther_docking::PantherChargingDock - external_detection_timeout: 0.1 + external_detection_timeout: 0.2 docking_distance_threshold: 0.12 docking_yaw_threshold: 0.2 staging_x_offset: -0.8 filter_coef: 0.1 + use_wibotic_info: + wibotic_info_timeout: 1.0 docks: ["main", "backup"] main: diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index 8e0648b61..88d86105c 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -149,8 +149,10 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock * @brief Method to update and publish the staging pose. * * Uses staging_x_offset_ and staging_yaw_offset_ to calculate the staging pose. + * + * @param frame The frame to publish the staging pose in. */ - void updateAndPublishStagingPose(); + void updateAndPublishStagingPose(const std::string & frame); /** * @brief Dock pose callback, used for external detection. diff --git a/panther_docking/src/dock_pose_publisher_node.cpp b/panther_docking/src/dock_pose_publisher_node.cpp index 8f2c54c2c..bd59bbc27 100644 --- a/panther_docking/src/dock_pose_publisher_node.cpp +++ b/panther_docking/src/dock_pose_publisher_node.cpp @@ -41,7 +41,7 @@ class DockPosePublisherNode : public rclcpp::Node const auto publish_rate = get_parameter("publish_rate").as_double(); const auto publish_period = std::chrono::duration(1.0 / publish_rate); - timeout_ = get_parameter("panther_charging_dock.external_detection_timeout").as_double(); + timeout_ = get_parameter("panther_charging_dock.external_detection_timeout").as_double() * 0.1; base_frame_ = get_parameter("base_frame").as_string(); for (const auto & dock : docks) { @@ -87,7 +87,7 @@ class DockPosePublisherNode : public rclcpp::Node base_transform_stamped = tf_buffer_->lookupTransform( target_frame_, base_frame_, tf2::TimePointZero); } catch (tf2::TransformException & ex) { - RCLCPP_DEBUG(this->get_logger(), "Could not get transform: %s", ex.what()); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Could not get transform: " << ex.what()); return; } @@ -95,10 +95,9 @@ class DockPosePublisherNode : public rclcpp::Node geometry_msgs::msg::TransformStamped transform_stamped; try { transform_stamped = tf_buffer_->lookupTransform( - target_frame_, source_frames_[i], tf2::TimePointZero, - tf2::durationFromSec(timeout_ * 0.1)); + target_frame_, source_frames_[i], tf2::TimePointZero, tf2::durationFromSec(timeout_)); } catch (tf2::TransformException & ex) { - RCLCPP_DEBUG(this->get_logger(), "Could not get transform: %s", ex.what()); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Could not get transform: " << ex.what()); continue; }