From a2d464dcd315be8a864ad4a51645d707a63358f1 Mon Sep 17 00:00:00 2001 From: Janne Date: Wed, 19 Jan 2022 14:51:36 +0200 Subject: [PATCH] backport pull-request #2712 to foxy --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 1 + nav2_amcl/src/amcl_node.cpp | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index dc86baaf6e1..772a63dd519 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -160,6 +160,7 @@ class AmclNode : public nav2_util::LifecycleNode // Pose-generating function used to uniformly distribute particles over the map static pf_vector_t uniformPoseGenerator(void * arg); pf_t * pf_{nullptr}; + std::mutex pf_mutex_; bool pf_init_; pf_vector_t pf_odom_pose_; int resample_count_{0}; diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 3a4e0492d06..3dea8569c2c 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -505,6 +505,8 @@ AmclNode::globalLocalizationCallback( const std::shared_ptr/*req*/, std::shared_ptr/*res*/) { + std::lock_guard lock(pf_mutex_); + RCLCPP_INFO(get_logger(), "Initializing with uniform distribution"); pf_init_model( @@ -529,6 +531,8 @@ AmclNode::nomotionUpdateCallback( void AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { + std::lock_guard lock(pf_mutex_); + RCLCPP_INFO(get_logger(), "initialPoseReceived"); if (msg->header.frame_id == "") { @@ -626,6 +630,8 @@ AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg) void AmclNode::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) { + std::lock_guard lock(pf_mutex_); + // Since the sensor data is continually being published by the simulator or robot, // we don't want our callbacks to fire until we're in the active state if (!active_) {return;}