Fixed after merge

Signed-off-by: Jakub Delicat <[email protected]>
husarion · Nov 22, 2024 · 7120e87 · 7120e87
1 parent 1922ab5
commit 7120e87
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 7 deletions.
diff --git 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: <use_wibotic_info_param>
+      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
@@ -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
@@ -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<double>(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,18 +87,17 @@ 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;
     }
 
     for (size_t i = 0; i < source_frames_.size(); ++i) {
       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;
       }