From 469e1e6fa677794cb26b55c43ac53b6ad5bc6329 Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Fri, 6 Dec 2024 15:28:48 +0100 Subject: [PATCH] Ros2 docking lifecycle node (#453) * Changed camera pose Signed-off-by: Jakub Delicat * Fixed test Signed-off-by: Jakub Delicat * Update docs Signed-off-by: Jakub Delicat * typo Signed-off-by: Jakub Delicat * typo Signed-off-by: Jakub Delicat * Changed to lifecyclenode | added transition service client Signed-off-by: Jakub Delicat * Updated docs Signed-off-by: Jakub Delicat * Added timeout function Signed-off-by: Jakub Delicat --------- Signed-off-by: Jakub Delicat --- panther_docking/CMakeLists.txt | 2 + panther_docking/README.md | 2 +- .../dock_pose_publisher_node.hpp | 22 ++++- .../panther_docking/panther_charging_dock.hpp | 17 ++++ panther_docking/package.xml | 2 + .../src/dock_pose_publisher_main.cpp | 2 +- .../src/dock_pose_publisher_node.cpp | 87 ++++++++++++++----- panther_docking/src/panther_charging_dock.cpp | 73 ++++++++++++---- 8 files changed, 161 insertions(+), 46 deletions(-) diff --git a/panther_docking/CMakeLists.txt b/panther_docking/CMakeLists.txt index d0bb9d9a..243718fb 100644 --- a/panther_docking/CMakeLists.txt +++ b/panther_docking/CMakeLists.txt @@ -8,11 +8,13 @@ endif() set(PACKAGE_DEPENDENCIES ament_cmake geometry_msgs + lifecycle_msgs opennav_docking_core opennav_docking panther_utils pluginlib rclcpp + rclcpp_lifecycle sensor_msgs std_srvs tf2_geometry_msgs diff --git a/panther_docking/README.md b/panther_docking/README.md index dd30304f..2aa1b668 100644 --- a/panther_docking/README.md +++ b/panther_docking/README.md @@ -14,7 +14,7 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht ## ROS Nodes -- `DockPosePublisherNode`: A node listens to `tf` and republishes position of `dock_pose` in the fixed frame. +- `DockPosePublisherNode`: A lifecycle node listens to `tf` and republishes position of `dock_pose` in the fixed frame when it is activated. - `PantherChargingDock`: A plugin for a Panther robot what is responsible for a charger service. ### DockPosePublisherNode diff --git a/panther_docking/include/panther_docking/dock_pose_publisher_node.hpp b/panther_docking/include/panther_docking/dock_pose_publisher_node.hpp index 875a9327..6e7c579e 100644 --- a/panther_docking/include/panther_docking/dock_pose_publisher_node.hpp +++ b/panther_docking/include/panther_docking/dock_pose_publisher_node.hpp @@ -22,26 +22,40 @@ #include #include #include -#include +#include +#include #include namespace panther_docking { constexpr double kMinimalDetectionDistance = 1.0; -class DockPosePublisherNode : public rclcpp::Node +class DockPosePublisherNode : public rclcpp_lifecycle::LifecycleNode { public: - DockPosePublisherNode(const std::string & name); + explicit DockPosePublisherNode(const std::string & node_name); + +protected: + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & state) override; private: void publishPose(); double timeout_; + std::chrono::duration publish_period_; std::string target_frame_; std::vector source_frames_; std::string base_frame_; - rclcpp::Publisher::SharedPtr pose_publisher_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr pose_publisher_; std::shared_ptr tf_listener_; std::unique_ptr tf_buffer_; rclcpp::TimerBase::SharedPtr timer_; diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index 88d86105..c58ed184 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -29,6 +29,8 @@ #include #include +#include +#include #include #include @@ -168,6 +170,17 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock */ void setWiboticInfo(const WiboticInfoMsg::SharedPtr msg); + /** + * @brief Method to set the state of the dock pose publisher lifecycle node. + * + * Calls async service to change the state of the dock pose publisher lifecycle node. + * + * @param state The transition state to set the dock pose publisher to. + */ + void setDockPosePublisherState(std::uint8_t state); + + bool IsWiboticInfoTimeout(); + std::string base_frame_name_; std::string fixed_frame_name_; std::string dock_frame_; @@ -181,11 +194,15 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock rclcpp::Publisher::SharedPtr staging_pose_pub_; rclcpp::Subscription::SharedPtr dock_pose_sub_; rclcpp::Subscription::SharedPtr wibotic_info_sub_; + rclcpp::Client::SharedPtr + dock_pose_publisher_change_state_client_; PoseStampedMsg dock_pose_; PoseStampedMsg staging_pose_; WiboticInfoMsg::SharedPtr wibotic_info_; + std::uint8_t dock_pose_publisher_state_; + double external_detection_timeout_; std::shared_ptr pose_filter_; diff --git a/panther_docking/package.xml b/panther_docking/package.xml index 1c680705..2d68586d 100644 --- a/panther_docking/package.xml +++ b/panther_docking/package.xml @@ -15,6 +15,7 @@ Jakub Delicat geometry_msgs + lifecycle_msgs nav2_util opennav_docking panther_manager @@ -22,6 +23,7 @@ pluginlib python3-pip rclcpp + rclcpp_lifecycle ros_components_description sensor_msgs std_srvs diff --git a/panther_docking/src/dock_pose_publisher_main.cpp b/panther_docking/src/dock_pose_publisher_main.cpp index cd241dd5..1a1f5855 100644 --- a/panther_docking/src/dock_pose_publisher_main.cpp +++ b/panther_docking/src/dock_pose_publisher_main.cpp @@ -28,7 +28,7 @@ int main(int argc, char ** argv) std::make_shared("dock_pose_publisher_node"); try { - rclcpp::spin(dock_pose_publisher_node); + rclcpp::spin(dock_pose_publisher_node->get_node_base_interface()); } catch (const std::runtime_error & e) { std::cerr << "[" << dock_pose_publisher_node->get_name() << "] Caught exception: " << e.what() << std::endl; diff --git a/panther_docking/src/dock_pose_publisher_node.cpp b/panther_docking/src/dock_pose_publisher_node.cpp index 685c807c..c4558c6d 100644 --- a/panther_docking/src/dock_pose_publisher_node.cpp +++ b/panther_docking/src/dock_pose_publisher_node.cpp @@ -20,14 +20,18 @@ #include #include -#include - #include #include namespace panther_docking { -DockPosePublisherNode::DockPosePublisherNode(const std::string & name) : Node(name) +DockPosePublisherNode::DockPosePublisherNode(const std::string & name) +: rclcpp_lifecycle::LifecycleNode(name) +{ +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +DockPosePublisherNode::on_configure(const rclcpp_lifecycle::State &) { declare_parameter("publish_rate", 10.0); declare_parameter("docks", std::vector({"main"})); @@ -38,7 +42,7 @@ DockPosePublisherNode::DockPosePublisherNode(const std::string & name) : Node(na const auto fixed_frame = get_parameter("fixed_frame").as_string(); const auto docks = get_parameter("docks").as_string_array(); const auto publish_rate = get_parameter("publish_rate").as_double(); - const auto publish_period = std::chrono::duration(1.0 / publish_rate); + publish_period_ = std::chrono::duration(1.0 / publish_rate); timeout_ = get_parameter("panther_charging_dock.external_detection_timeout").as_double() * 0.1; base_frame_ = get_parameter("base_frame").as_string(); @@ -60,12 +64,52 @@ DockPosePublisherNode::DockPosePublisherNode(const std::string & name) : Node(na tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); - timer_ = this->create_wall_timer( - publish_period, std::bind(&DockPosePublisherNode::publishPose, this)); pose_publisher_ = this->create_publisher( "docking/dock_pose", 10); - RCLCPP_INFO(this->get_logger(), "DockPosePublisherNode initialized"); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Node configured successfully"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +DockPosePublisherNode::on_activate(const rclcpp_lifecycle::State &) +{ + pose_publisher_->on_activate(); + timer_ = this->create_wall_timer( + publish_period_, std::bind(&DockPosePublisherNode::publishPose, this)); + + RCLCPP_DEBUG_STREAM(this->get_logger(), "Node activated"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +DockPosePublisherNode::on_deactivate(const rclcpp_lifecycle::State &) +{ + pose_publisher_->on_deactivate(); + timer_.reset(); + + RCLCPP_DEBUG_STREAM(this->get_logger(), "Node deactivated"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +DockPosePublisherNode::on_cleanup(const rclcpp_lifecycle::State &) +{ + pose_publisher_.reset(); + timer_.reset(); + tf_listener_.reset(); + tf_buffer_.reset(); + source_frames_.clear(); + + RCLCPP_DEBUG_STREAM(this->get_logger(), "Node cleaned up"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +DockPosePublisherNode::on_shutdown(const rclcpp_lifecycle::State &) +{ + RCLCPP_DEBUG_STREAM(this->get_logger(), "Node shutting down"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } void DockPosePublisherNode::publishPose() @@ -78,7 +122,6 @@ void DockPosePublisherNode::publishPose() geometry_msgs::msg::TransformStamped base_transform_stamped; bool found = false; - double closest_dist = std::numeric_limits::max(); try { @@ -89,25 +132,24 @@ void DockPosePublisherNode::publishPose() return; } - for (size_t i = 0; i < source_frames_.size(); ++i) { - geometry_msgs::msg::TransformStamped transform_stamped; + for (const auto & source_frame : source_frames_) { try { - transform_stamped = tf_buffer_->lookupTransform( - target_frame_, source_frames_[i], tf2::TimePointZero, tf2::durationFromSec(timeout_)); + const auto transform_stamped = tf_buffer_->lookupTransform( + target_frame_, source_frame, tf2::TimePointZero, tf2::durationFromSec(timeout_)); + + const double dist = std::hypot( + transform_stamped.transform.translation.x - base_transform_stamped.transform.translation.x, + transform_stamped.transform.translation.y - base_transform_stamped.transform.translation.y); + + if (dist < kMinimalDetectionDistance && dist < closest_dist) { + closest_dist = dist; + closest_dock = transform_stamped; + found = true; + } } catch (tf2::TransformException & ex) { RCLCPP_DEBUG_STREAM(this->get_logger(), "Could not get transform: " << ex.what()); continue; } - - const double dist = std::hypot( - transform_stamped.transform.translation.x - base_transform_stamped.transform.translation.x, - transform_stamped.transform.translation.y - base_transform_stamped.transform.translation.y); - - if (dist < kMinimalDetectionDistance && dist < closest_dist) { - closest_dist = dist; - closest_dock = transform_stamped; - found = true; - } } if (!found) { @@ -121,4 +163,5 @@ void DockPosePublisherNode::publishPose() pose_msg.pose.orientation = closest_dock.transform.rotation; pose_publisher_->publish(pose_msg); } + } // namespace panther_docking diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index 48bb5235..b0eb1df5 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -67,17 +67,23 @@ void PantherChargingDock::activate() std::bind(&PantherChargingDock::setDockPose, this, std::placeholders::_1)); staging_pose_pub_ = node->create_publisher("docking/staging_pose", 1); + dock_pose_publisher_change_state_client_ = + node->create_client("dock_pose_publisher/change_state"); + if (use_wibotic_info_) { wibotic_info_sub_ = node->create_subscription( "wibotic_info", 1, std::bind(&PantherChargingDock::setWiboticInfo, this, std::placeholders::_1)); } + + setDockPosePublisherState(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); } void PantherChargingDock::deactivate() { dock_pose_sub_.reset(); staging_pose_pub_.reset(); + dock_pose_publisher_change_state_client_.reset(); } void PantherChargingDock::declareParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) @@ -144,10 +150,12 @@ PantherChargingDock::PoseStampedMsg PantherChargingDock::getStagingPose( bool PantherChargingDock::getRefinedPose(PoseStampedMsg & pose) { RCLCPP_DEBUG(logger_, "Getting refined pose"); + setDockPosePublisherState(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + rclcpp::Time request_detection_time; if (dock_pose_.header.frame_id.empty()) { - throw opennav_docking_core::FailedToDetectDock("No dock pose detected"); + return false; } { @@ -189,33 +197,24 @@ bool PantherChargingDock::isCharging() RCLCPP_DEBUG(logger_, "Checking if charging"); try { if (!use_wibotic_info_) { - return isDocked(); - } - - if (!wibotic_info_) { - throw opennav_docking_core::FailedToCharge("No Wibotic info received."); - } - - rclcpp::Time requested_wibotic_info_time; - { - auto node = node_.lock(); - requested_wibotic_info_time = node->now(); + if (isDocked()) { + setDockPosePublisherState(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); + return true; + } + return false; } - const auto duration = requested_wibotic_info_time - wibotic_info_->header.stamp; - if (duration > rclcpp::Duration::from_seconds(wibotic_info_timeout_)) { - RCLCPP_WARN_STREAM( - logger_, "Wibotic info is outdated. Time difference is: " - << duration.seconds() << "s but timeout is " << wibotic_info_timeout_ << "s."); + if (IsWiboticInfoTimeout()) { return false; } if (wibotic_info_->i_charger > kWiboticChargingCurrentThreshold) { + setDockPosePublisherState(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); return true; } } catch (const opennav_docking_core::FailedToDetectDock & e) { RCLCPP_ERROR_STREAM(logger_, "An occurred error while checking if charging: " << e.what()); - return false; + setDockPosePublisherState(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); } return false; @@ -257,6 +256,44 @@ void PantherChargingDock::setWiboticInfo(const WiboticInfoMsg::SharedPtr msg) wibotic_info_ = std::make_shared(*msg); } +void PantherChargingDock::setDockPosePublisherState(std::uint8_t state) +{ + if (dock_pose_publisher_state_ == state) { + return; + } + + RCLCPP_DEBUG_STREAM(logger_, "Setting dock pose publisher state to: " << static_cast(state)); + dock_pose_publisher_state_ = state; + + auto request = std::make_shared(); + request->transition.id = state; + dock_pose_publisher_change_state_client_->async_send_request(request); +} + +bool PantherChargingDock::IsWiboticInfoTimeout() +{ + if (!wibotic_info_) { + RCLCPP_ERROR_STREAM( + logger_, "Wibotic info is not set. This should not happen. Check the Wibotic info topic."); + return true; + } + + rclcpp::Time requested_wibotic_info_time; + { + auto node = node_.lock(); + requested_wibotic_info_time = node->now(); + } + + const auto duration = requested_wibotic_info_time - wibotic_info_->header.stamp; + if (duration > rclcpp::Duration::from_seconds(wibotic_info_timeout_)) { + RCLCPP_WARN_STREAM( + logger_, "Wibotic info is outdated. Time difference is: " + << duration.seconds() << "s but timeout is " << wibotic_info_timeout_ << "s."); + return true; + } + return false; +} + } // namespace panther_docking #include "pluginlib/class_list_macros.hpp"