diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 723a243a4a..65b40ddb9f 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -130,7 +130,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 c2a46b4f87..8be37dbeb1 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -504,7 +504,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"); @@ -530,7 +530,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"); @@ -565,6 +565,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; @@ -629,7 +630,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 @@ -1166,7 +1167,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",