diff --git a/panther_docking/config/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml index 5725a323..c9de95c0 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 8e0648b6..88d86105 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 8f2c54c2..bd59bbc2 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; }