diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index e2446e0c7bf..3cc8c90a006 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -132,7 +132,7 @@ class AmclNode : public nav2_util::LifecycleNode bool first_map_only_{true}; std::atomic first_map_received_{false}; amcl_hyp_t * initial_pose_hyp_; - std::recursive_mutex configuration_mutex_; + std::recursive_mutex mutex_; rclcpp::Subscription::ConstSharedPtr map_sub_; #if NEW_UNIFORM_SAMPLING static std::vector> free_space_indices; @@ -238,7 +238,6 @@ 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 b9770285bf5..6494967b563 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -465,7 +465,7 @@ AmclNode::globalLocalizationCallback( const std::shared_ptr/*req*/, std::shared_ptr/*res*/) { - std::lock_guard lock(pf_mutex_); + std::lock_guard cfl(mutex_); RCLCPP_INFO(get_logger(), "Initializing with uniform distribution"); @@ -491,7 +491,7 @@ AmclNode::nomotionUpdateCallback( void AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - std::lock_guard lock(pf_mutex_); + std::lock_guard cfl(mutex_); RCLCPP_INFO(get_logger(), "initialPoseReceived"); @@ -526,6 +526,7 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha void AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg) { + std::lock_guard cfl(mutex_); // In case the client sent us a pose estimate in the past, integrate the // intervening odometric change. geometry_msgs::msg::TransformStamped tx_odom; @@ -590,7 +591,7 @@ AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg) void AmclNode::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) { - std::lock_guard lock(pf_mutex_); + std::lock_guard cfl(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 @@ -1127,7 +1128,7 @@ AmclNode::mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) void AmclNode::handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg) { - std::lock_guard cfl(configuration_mutex_); + std::lock_guard cfl(mutex_); RCLCPP_INFO( get_logger(), "Received a %d X %d map @ %.3f m/pix",