Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Reduce amcl_node nodes #2483

Merged
merged 1 commit into from
May 26, 2022
Merged

Reduce amcl_node nodes #2483

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 8 additions & 1 deletion nav2_amcl/include/nav2_amcl/amcl_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -105,6 +105,12 @@ class AmclNode : public nav2_util::LifecycleNode
// respond until we're in the active state
std::atomic<bool> 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<nav2_util::NodeThread> executor_thread_;

// Pose hypothesis
typedef struct
{
Expand Down Expand Up @@ -165,7 +171,8 @@ class AmclNode : public nav2_util::LifecycleNode
* @brief Initialize incoming data message subscribers and filters
*/
void initMessageFilters();
std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan>> laser_scan_sub_;
std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
rclcpp_lifecycle::LifecycleNode>> laser_scan_sub_;
std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> laser_scan_filter_;
message_filters::Connection laser_scan_connection_;

Expand Down
33 changes: 23 additions & 10 deletions nav2_amcl/src/amcl_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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");

Expand Down Expand Up @@ -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();
Expand All @@ -245,7 +246,9 @@ AmclNode::on_configure(const rclcpp_lifecycle::State & /*state*/)
initPubSub();
initServices();
initOdometry();

executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
executor_->add_callback_group(callback_group_, get_node_base_interface());
executor_thread_ = std::make_unique<nav2_util::NodeThread>(executor_);
return nav2_util::CallbackReturn::SUCCESS;
}

Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -1426,13 +1431,14 @@ AmclNode::initTransforms()
RCLCPP_INFO(get_logger(), "initTransforms");

// Initialize transform listener and broadcaster
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(rclcpp_node_->get_clock());
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(get_clock());
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
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<tf2_ros::TransformListener>(*tf_buffer_);
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(rclcpp_node_);
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(shared_from_this(), true);

sent_first_transform_ = false;
latest_tf_valid_ = false;
Expand All @@ -1442,11 +1448,18 @@ AmclNode::initTransforms()
void
AmclNode::initMessageFilters()
{
laser_scan_sub_ = std::make_unique<message_filters::Subscriber<sensor_msgs::msg::LaserScan>>(
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<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
rclcpp_lifecycle::LifecycleNode>>(
shared_from_this(), scan_topic_, rmw_qos_profile_sensor_data, sub_opt);

laser_scan_filter_ = std::make_unique<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>>(
*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(),