[LifecycleNode] add bond_heartbeat_period (#4342)

* add bond_heartbeat_period Signed-off-by: Guillaume Doisy <[email protected]> * lint Signed-off-by: Guillaume Doisy <[email protected]> --------- Signed-off-by: Guillaume Doisy <[email protected]> Co-authored-by: Guillaume Doisy <[email protected]>
ros-navigation · May 14, 2024 · 2ae649b · 2ae649b
1 parent 4fa12ac
commit 2ae649b
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 11 deletions.
diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp
@@ -205,6 +205,7 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode
 
   // Connection to tell that server is still up
   std::unique_ptr<bond::Bond> bond_{nullptr};
+  double bond_heartbeat_period;
 };
 
 }  // namespace nav2_util

diff --git a/nav2_util/src/lifecycle_node.cpp b/nav2_util/src/lifecycle_node.cpp
@@ -19,6 +19,7 @@
 #include <vector>
 
 #include "lifecycle_msgs/msg/state.hpp"
+#include "nav2_util/node_utils.hpp"
 
 namespace nav2_util
 {
@@ -35,6 +36,10 @@ LifecycleNode::LifecycleNode(
     rclcpp::Parameter(
       bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true));
 
+  nav2_util::declare_parameter_if_not_declared(
+    this, "bond_heartbeat_period", rclcpp::ParameterValue(0.1));
+  this->get_parameter("bond_heartbeat_period", bond_heartbeat_period);
+
   printLifecycleNodeNotification();
 
   register_rcl_preshutdown_callback();
@@ -55,16 +60,18 @@ LifecycleNode::~LifecycleNode()
 
 void LifecycleNode::createBond()
 {
-  RCLCPP_INFO(get_logger(), "Creating bond (%s) to lifecycle manager.", this->get_name());
+  if (bond_heartbeat_period > 0.0) {
+    RCLCPP_INFO(get_logger(), "Creating bond (%s) to lifecycle manager.", this->get_name());
 
-  bond_ = std::make_unique<bond::Bond>(
-    std::string("bond"),
-    this->get_name(),
-    shared_from_this());
+    bond_ = std::make_unique<bond::Bond>(
+      std::string("bond"),
+      this->get_name(),
+      shared_from_this());
 
-  bond_->setHeartbeatPeriod(0.10);
-  bond_->setHeartbeatTimeout(4.0);
-  bond_->start();
+    bond_->setHeartbeatPeriod(bond_heartbeat_period);
+    bond_->setHeartbeatTimeout(4.0);
+    bond_->start();
+  }
 }
 
 void LifecycleNode::runCleanups()
@@ -110,10 +117,12 @@ void LifecycleNode::register_rcl_preshutdown_callback()
 
 void LifecycleNode::destroyBond()
 {
-  RCLCPP_INFO(get_logger(), "Destroying bond (%s) to lifecycle manager.", this->get_name());
+  if (bond_heartbeat_period > 0.0) {
+    RCLCPP_INFO(get_logger(), "Destroying bond (%s) to lifecycle manager.", this->get_name());
 
-  if (bond_) {
-    bond_.reset();
+    if (bond_) {
+      bond_.reset();
+    }
   }
 }