Added mutex to prevent SEGFAULT on map change in AMCL (#2933)

* Added mutex to prevent SEGFAULT on map change * Removing redundant mutex Co-authored-by: Antonio Marangi <[email protected]>
ros-navigation · May 6, 2022 · 7378f9f · 7378f9f
1 parent c712244
commit 7378f9f
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 6 deletions.
diff --git 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<bool> first_map_received_{false};
   amcl_hyp_t * initial_pose_hyp_;
-  std::recursive_mutex configuration_mutex_;
+  std::recursive_mutex mutex_;
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::ConstSharedPtr map_sub_;
 #if NEW_UNIFORM_SAMPLING
   static std::vector<std::pair<int, int>> 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
@@ -504,7 +504,7 @@ AmclNode::globalLocalizationCallback(
   const std::shared_ptr<std_srvs::srv::Empty::Request>/*req*/,
   std::shared_ptr<std_srvs::srv::Empty::Response>/*res*/)
 {
-  std::lock_guard<std::mutex> lock(pf_mutex_);
+  std::lock_guard<std::recursive_mutex> 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<std::mutex> lock(pf_mutex_);
+  std::lock_guard<std::recursive_mutex> 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<std::recursive_mutex> 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<std::mutex> lock(pf_mutex_);
+  std::lock_guard<std::recursive_mutex> 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<std::recursive_mutex> cfl(configuration_mutex_);
+  std::lock_guard<std::recursive_mutex> cfl(mutex_);
 
   RCLCPP_INFO(
     get_logger(), "Received a %d X %d map @ %.3f m/pix",