diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index 6da348c5f1..7f1ef07157 100644 --- 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_{nullptr}; + double bond_heartbeat_period; }; } // namespace nav2_util diff --git a/nav2_util/src/lifecycle_node.cpp b/nav2_util/src/lifecycle_node.cpp index e09ae54b5d..5976d098a8 100644 --- a/nav2_util/src/lifecycle_node.cpp +++ b/nav2_util/src/lifecycle_node.cpp @@ -19,6 +19,7 @@ #include #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( - std::string("bond"), - this->get_name(), - shared_from_this()); + bond_ = std::make_unique( + 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(); + } } }