From 20905cf0fd4e6d87e505bb45bf04b07e1f0bda57 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Mon, 29 Nov 2021 15:18:23 +0300 Subject: [PATCH] Prohibit of simultaneous usage of callbacks accessing to pf_ object --- 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 9deb55045a..e2446e0c7b 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -238,6 +238,7 @@ class AmclNode : public nav2_util::LifecycleNode */ 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 2433309150..293bbdb417 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -461,6 +461,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( @@ -485,6 +487,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 == "") { @@ -582,6 +586,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;}