diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 850a059b5d..2360d09c25 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -105,6 +105,12 @@ class AmclNode : public nav2_util::LifecycleNode // respond until we're in the active state std::atomic active_{false}; + // Dedicated callback group and executor for services and subscriptions in AmclNode, + // in order to isolate TF timer used in message filter. + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + std::unique_ptr executor_thread_; + // Pose hypothesis typedef struct { @@ -165,7 +171,8 @@ class AmclNode : public nav2_util::LifecycleNode * @brief Initialize incoming data message subscribers and filters */ void initMessageFilters(); - std::unique_ptr> laser_scan_sub_; + std::unique_ptr> laser_scan_sub_; std::unique_ptr> laser_scan_filter_; message_filters::Connection laser_scan_connection_; diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index c645737b2a..44d016fac8 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -59,7 +59,7 @@ namespace nav2_amcl using nav2_util::geometry_utils::orientationAroundZAxis; AmclNode::AmclNode(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("amcl", "", true, options) +: nav2_util::LifecycleNode("amcl", "", false, options) { RCLCPP_INFO(get_logger(), "Creating"); @@ -236,7 +236,8 @@ nav2_util::CallbackReturn AmclNode::on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring"); - + callback_group_ = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); initParameters(); initTransforms(); initParticleFilter(); @@ -245,7 +246,9 @@ AmclNode::on_configure(const rclcpp_lifecycle::State & /*state*/) initPubSub(); initServices(); initOdometry(); - + executor_ = std::make_shared(); + executor_->add_callback_group(callback_group_, get_node_base_interface()); + executor_thread_ = std::make_unique(executor_); return nav2_util::CallbackReturn::SUCCESS; } @@ -317,6 +320,8 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); + executor_thread_.reset(); + // Get rid of the inputs first (services and message filter input), so we // don't continue to process incoming messages global_loc_srv_.reset(); @@ -1426,13 +1431,14 @@ AmclNode::initTransforms() RCLCPP_INFO(get_logger(), "initTransforms"); // Initialize transform listener and broadcaster - tf_buffer_ = std::make_shared(rclcpp_node_->get_clock()); + tf_buffer_ = std::make_shared(get_clock()); auto timer_interface = std::make_shared( - rclcpp_node_->get_node_base_interface(), - rclcpp_node_->get_node_timers_interface()); + get_node_base_interface(), + get_node_timers_interface(), + callback_group_); tf_buffer_->setCreateTimerInterface(timer_interface); tf_listener_ = std::make_shared(*tf_buffer_); - tf_broadcaster_ = std::make_shared(rclcpp_node_); + tf_broadcaster_ = std::make_shared(shared_from_this(), true); sent_first_transform_ = false; latest_tf_valid_ = false; @@ -1442,11 +1448,18 @@ AmclNode::initTransforms() void AmclNode::initMessageFilters() { - laser_scan_sub_ = std::make_unique>( - rclcpp_node_.get(), scan_topic_, rmw_qos_profile_sensor_data); + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group_; + laser_scan_sub_ = std::make_unique>( + shared_from_this(), scan_topic_, rmw_qos_profile_sensor_data, sub_opt); laser_scan_filter_ = std::make_unique>( - *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_, transform_tolerance_); + *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, + get_node_logging_interface(), + get_node_clock_interface(), + transform_tolerance_); + laser_scan_connection_ = laser_scan_filter_->registerCallback( std::bind(