[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]>
Signed-off-by: enricosutera <[email protected]>

Author: 2 people

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

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();
+    }
   }
 }