[nav2_costmap_2d] add the std::unique_lock before layered_costmap->isCurrent()

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -169,6 +169,11 @@ class Costmap2DROS : public nav2_util::LifecycleNode
   /** @brief Same as getLayeredCostmap()->isCurrent(). */
   bool isCurrent()
   {
+    //lock because no ptr-access is allowed until other ptr-free finished
+    std::unique_lock<Costmap2D::mutex_t> lock(*access_);
+    if (layered_costmap_ == nullptr) {
+      return false;                                // to avoid nullptr accessed
+    }
     return layered_costmap_->isCurrent();
   }
 
@@ -316,6 +321,13 @@ class Costmap2DROS : public nav2_util::LifecycleNode
    */
   double getRobotRadius() {return robot_radius_;}
 
+  // Provide a typedef to ease future code maintenance
+  typedef std::recursive_mutex mutex_t;
+  mutex_t * getMutex()
+  {
+    return access_;
+  }
+
 protected:
   // Publishers and subscribers
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PolygonStamped>::SharedPtr
@@ -397,6 +409,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode
    */
   rcl_interfaces::msg::SetParametersResult
   dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+
+private:
+  mutex_t * access_;
 };
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -135,10 +135,13 @@ void Costmap2DROS::init()
   declare_parameter("update_frequency", rclcpp::ParameterValue(5.0));
   declare_parameter("use_maximum", rclcpp::ParameterValue(false));
   declare_parameter("clearable_layers", rclcpp::ParameterValue(clearable_layers));
+
+  access_ = new mutex_t();
 }
 
 Costmap2DROS::~Costmap2DROS()
 {
+  delete access_;
 }
 
 nav2_util::CallbackReturn
@@ -360,6 +363,8 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
 
   layer_publishers_.clear();
 
+  //lock because no ptr-access is allowed until this ptr-free finished
+  std::unique_lock<Costmap2D::mutex_t> lock(*access_);
   layered_costmap_.reset();
 
   tf_listener_.reset();
@@ -370,6 +375,8 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
 
 
   executor_thread_.reset();
+
+  lock.unlock();
   return nav2_util::CallbackReturn::SUCCESS;
 }