Only publish the costmap if it's current. Also provide an additional …

…function in the costmap subscriber, to get the timestamp of the last received costmap
ros-navigation · Dec 17, 2024 · cf5f9f3 · cf5f9f3
1 parent 9d60bc0
commit cf5f9f3
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 3 deletions.
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp
@@ -56,6 +56,10 @@ class CostmapSubscriber
    * @brief Get current costmap
    */
   std::shared_ptr<Costmap2D> getCostmap();
+  /**
+   * @brief Get the timestamp of the last costmap update
+   */
+  rclcpp::Time getTimestampLastCostmapUpdate();
   /**
    * @brief Callback for the costmap topic
    */

diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp
@@ -535,9 +535,10 @@ Costmap2DROS::mapUpdateLoop(double frequency)
         }
 
         auto current_time = now();
-        if ((last_publish_ + publish_cycle_ < current_time) ||  // publish_cycle_ is due
-          (current_time <
-          last_publish_))      // time has moved backwards, probably due to a switch to sim_time // NOLINT
+        auto map_current = isCurrent();
+        if (map_current &&  // only update costmap if it's current
+          ((last_publish_ + publish_cycle_ < current_time) ||  // publish_cycle_ is due
+          (current_time < last_publish_)))     // time has moved backwards, probably due to a switch to sim_time // NOLINT
         {
           RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str());
           costmap_publisher_->publishCostmap();
@@ -548,6 +549,9 @@ Costmap2DROS::mapUpdateLoop(double frequency)
 
           last_publish_ = current_time;
         }
+        else if (!map_current) {
+          RCLCPP_WARN(get_logger(), "Costmap was not published because it`s not current");
+        }
       }
     }
 

diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp
@@ -68,6 +68,18 @@ std::shared_ptr<Costmap2D> CostmapSubscriber::getCostmap()
   return costmap_;
 }
 
+rclcpp::Time CostmapSubscriber::getTimestampLastCostmapUpdate()
+{
+  if (!costmap_received_) {
+    throw std::runtime_error("Costmap is not available");
+  }
+
+  std::lock_guard<std::mutex> lock(costmap_msg_mutex_);
+  auto stamp = costmap_msg_->header.stamp;
+
+  return stamp;
+}
+
 void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg)
 {
   {