Ros2 docking lifecycle node (#453)

* Changed camera pose

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>

* Fixed test

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>

* Update docs

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>

* typo

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>

* typo

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>

* Changed to lifecyclenode | added transition service client

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>

* Updated docs

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>

* Added timeout function

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>

---------

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>

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

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

panther_docking/include/panther_docking/dock_pose_publisher_node.hpp

@@ -22,26 +22,40 @@
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 #include <geometry_msgs/msg/pose_stamped.hpp>
-#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
+#include <rclcpp_lifecycle/state.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 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<double> publish_period_;
   std::string target_frame_;
   std::vector<std::string> source_frames_;
   std::string base_frame_;
-  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publisher_;
+  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publisher_;
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
   rclcpp::TimerBase::SharedPtr timer_;

panther_docking/include/panther_docking/panther_charging_dock.hpp

@@ -29,6 +29,8 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
+#include <lifecycle_msgs/msg/state.hpp>
+#include <lifecycle_msgs/srv/change_state.hpp>
 #include <sensor_msgs/msg/battery_state.hpp>
 #include <std_srvs/srv/set_bool.hpp>
 
@@ -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<PoseStampedMsg>::SharedPtr staging_pose_pub_;
   rclcpp::Subscription<PoseStampedMsg>::SharedPtr dock_pose_sub_;
   rclcpp::Subscription<WiboticInfoMsg>::SharedPtr wibotic_info_sub_;
+  rclcpp::Client<lifecycle_msgs::srv::ChangeState>::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<opennav_docking::PoseFilter> pose_filter_;

panther_docking/package.xml

@@ -15,13 +15,15 @@
   <author email="jakub.delicat@husarion.com">Jakub Delicat</author>
 
   <depend>geometry_msgs</depend>
+  <depend>lifecycle_msgs</depend>
   <depend>nav2_util</depend>
   <depend>opennav_docking</depend>
   <depend>panther_manager</depend>
   <depend>panther_utils</depend>
   <depend>pluginlib</depend>
   <depend>python3-pip</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
   <depend>ros_components_description</depend>
   <depend>sensor_msgs</depend>
   <depend>std_srvs</depend>

panther_docking/src/dock_pose_publisher_main.cpp

@@ -28,7 +28,7 @@ int main(int argc, char ** argv)
     std::make_shared<panther_docking::DockPosePublisherNode>("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;

panther_docking/src/dock_pose_publisher_node.cpp

@@ -20,14 +20,18 @@
 
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
-#include <rclcpp/rclcpp.hpp>
-
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 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<std::string>({"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<double>(1.0 / publish_rate);
+  publish_period_ = std::chrono::duration<double>(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<tf2_ros::Buffer>(this->get_clock());
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 
-  timer_ = this->create_wall_timer(
-    publish_period, std::bind(&DockPosePublisherNode::publishPose, this));
   pose_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>(
     "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<double>::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