From 29e7b39b81e0e0306cb602affe43b9e11fb78678 Mon Sep 17 00:00:00 2001 From: Gerardo Puga Date: Thu, 13 Feb 2025 18:31:51 -0300 Subject: [PATCH] Add ros parameter for bond timeout Signed-off-by: Gerardo Puga --- beluga_amcl/src/amcl_node.cpp | 2 ++ beluga_amcl/src/ros2_common.cpp | 33 ++++++++++++++++++++++++++++++--- 2 files changed, 32 insertions(+), 3 deletions(-) diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index 9e7ad3754..e9f07ae15 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -365,7 +365,9 @@ void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map) { if (!particle_filter_) { try { + RCLCPP_INFO(get_logger(), "Initializing particle filter instance"); particle_filter_ = make_particle_filter(std::move(map)); + RCLCPP_INFO(get_logger(), "Particle filter initialization completed"); } catch (const std::invalid_argument& error) { RCLCPP_ERROR(get_logger(), "Could not initialize particle filter: %s", error.what()); return; diff --git a/beluga_amcl/src/ros2_common.cpp b/beluga_amcl/src/ros2_common.cpp index 65e73aa29..2c6fcfedb 100644 --- a/beluga_amcl/src/ros2_common.cpp +++ b/beluga_amcl/src/ros2_common.cpp @@ -385,6 +385,12 @@ BaseAMCLNode::BaseAMCLNode( this->declare_parameter("autostart_delay", 0.0, descriptor); } + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Heartbeat timeout value for the bond connection."; + this->declare_parameter("bond_timeout", 4.0, descriptor); + } + common_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); common_subscription_options_ = rclcpp::SubscriptionOptions{}; common_subscription_options_.callback_group = common_callback_group_; @@ -451,12 +457,14 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn BaseAM particle_cloud_pub_->on_activate(); particle_markers_pub_->on_activate(); pose_pub_->on_activate(); + { initial_pose_sub_ = create_subscription( get_parameter("initial_pose_topic").as_string(), rclcpp::SystemDefaultsQoS(), std::bind(&BaseAMCLNode::initial_pose_callback, this, std::placeholders::_1), common_subscription_options_); RCLCPP_INFO(get_logger(), "Subscribed to initial_pose_topic: %s", initial_pose_sub_->get_topic_name()); } + { using namespace std::chrono_literals; // TODO(alon): create a parameter for the timer rate? @@ -464,12 +472,30 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn BaseAM } { - bond_ = std::make_unique("bond", get_name(), shared_from_this()); + auto on_bond_formed_callback = [this]() { + RCLCPP_INFO(get_logger(), "The bond connection to the lifecycle manager is now fully formed"); + }; + auto on_bond_broken_callback = [this]() { + RCLCPP_ERROR(get_logger(), "The bond connection to the lifecycle manager has been broken"); + }; + bond_ = std::make_unique( + "bond", get_name(), shared_from_this(), on_bond_broken_callback, on_bond_formed_callback); bond_->setHeartbeatPeriod(0.10); - bond_->setHeartbeatTimeout(4.0); + const auto heartbeat_timeout_value = get_parameter("bond_timeout").as_double(); + // we don't want to shorten the default connection timeout, but we want to be able to + // make it longer because it can fail while building the likelihood map for large maps + // just like the heartbeat timeout does + const auto connect_timeout_value = + std::max(heartbeat_timeout_value, static_cast(bond::msg::Constants::DEFAULT_CONNECT_TIMEOUT)); + bond_->setConnectTimeout(connect_timeout_value); + bond_->setHeartbeatTimeout(heartbeat_timeout_value); bond_->start(); - RCLCPP_INFO(get_logger(), "Created bond (%s) to lifecycle manager", get_name()); + RCLCPP_INFO( + get_logger(), + "The bond (%s) connection to the lifecycle manager has been started (heartbeat timeout: %.2lf seconds)", + get_name(), heartbeat_timeout_value); } + { tf_buffer_ = std::make_unique(get_clock()); tf_buffer_->setCreateTimerInterface( @@ -479,6 +505,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn BaseAM *tf_buffer_, this, false); // avoid using dedicated tf thread } + do_activate(state); return CallbackReturn::SUCCESS; }