From d1ed5e466286898abebbc1bf80dd51e009670fe5 Mon Sep 17 00:00:00 2001 From: Jannkar Date: Wed, 19 Jan 2022 19:41:48 +0200 Subject: [PATCH] backport pull-request #2712 to foxy (#2776) --- 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 dc86baaf6e..772a63dd51 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 3a4e0492d0..3dea8569c2 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;}