Dynamic base costmap

common/easynav_costmap_common/CMakeLists.txt

@@ -12,6 +12,7 @@ set(CMAKE_CXX_EXTENSIONS OFF)
 find_package(ament_cmake REQUIRED)
 find_package(easynav_common REQUIRED)
 find_package(nav_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
 
 
 add_library(${PROJECT_NAME} SHARED
@@ -24,6 +25,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC
 target_link_libraries(${PROJECT_NAME} PUBLIC
   easynav_common::easynav_common
   ${nav_msgs_TARGETS}
+  rclcpp::rclcpp
 )
 
 install(
@@ -56,5 +58,6 @@ ament_export_dependencies(
   easynav_common
   # pluginlib
   nav_msgs
+  rclcpp
 )
 ament_package()

common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp

@@ -44,6 +44,7 @@
 #include <climits>
 #include <vector>
 #include <mutex>
+#include "rclcpp/time.hpp"
 #include "geometry_msgs/msg/point.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
 
@@ -126,10 +127,29 @@ class Costmap2D
    *
    * The resulting OccupancyGrid message contains metadata such as resolution, size, and origin,
    * and its data field is populated with cost values from the costmap. Cells with a value of
-   * NO_INFORMATION are mapped to -1; other values are cast to int8_t directly.
+    * NO_INFORMATION are mapped to -1; other values are cast to int8_t directly.
+    *
+    * The output message header stamp is set to the internal last-modified timestamp.
    */
   void toOccupancyGridMsg(nav_msgs::msg::OccupancyGrid & msg) const;
 
+  /**
+   * @brief Get timestamp of last costmap modification.
+   *
+   * Set by constructors and copy/assignment, and may be updated by some mutating operations.
+   * For high-frequency per-cell updates (e.g., many `setCost()` calls in a loop), prefer calling
+   * `touch()` once after the batch update.
+   */
+  rclcpp::Time getLastModifiedStamp() const;
+
+  /**
+   * @brief Mark the costmap as modified.
+   *
+   * This bumps the internal last-modified stamp by 1 nanosecond while preserving the clock type.
+   * It is intended to be called after bulk updates to avoid per-cell overhead.
+   */
+  void touch();
+
   /**
    * @brief Copies the (x0,y0)..(xn,yn) window from source costmap into a current costmap
      @param source Source costmap where the window will be copied from
@@ -580,6 +600,10 @@ class Costmap2D
   unsigned char * costmap_;
   unsigned char default_value_;
 
+  // Timestamp indicating when the costmap was last modified.
+  // Set by constructors and copy/assignment, and bumped by some mutating operations.
+  rclcpp::Time last_modified_;
+
   // *INDENT-OFF* Uncrustify doesn't handle indented public/private labels
   class MarkCell
   {

common/easynav_costmap_common/package.xml

@@ -21,6 +21,7 @@
   <depend>easynav_common</depend>
   <!-- <depend>pluginlib</depend> -->
   <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
   <depend>ament_index_cpp</depend>
 
   <test_depend>ament_lint_auto</test_depend>

common/easynav_costmap_common/src/easynav_costmap_common/costmap_2d.cpp

@@ -51,7 +51,7 @@ Costmap2D::Costmap2D(
   unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
   double origin_x, double origin_y, unsigned char default_value)
 : resolution_(resolution), origin_x_(origin_x),
-  origin_y_(origin_y), costmap_(NULL), default_value_(default_value)
+  origin_y_(origin_y), costmap_(NULL), default_value_(default_value), last_modified_(0)
 {
   access_ = new mutex_t();
 
@@ -61,7 +61,7 @@ Costmap2D::Costmap2D(
 }
 
 Costmap2D::Costmap2D(const nav_msgs::msg::OccupancyGrid & map)
-: default_value_(FREE_SPACE)
+: default_value_(FREE_SPACE), last_modified_(map.header.stamp)
 {
   access_ = new mutex_t();
 
@@ -126,6 +126,9 @@ void Costmap2D::resetMaps()
 {
   std::unique_lock<mutex_t> lock(*access_);
   memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
+
+  const int64_t new_stamp_ns = last_modified_.nanoseconds() + 1;
+  last_modified_ = rclcpp::Time(new_stamp_ns, last_modified_.get_clock_type());
 }
 
 void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
@@ -141,6 +144,9 @@ void Costmap2D::resetMapToValue(
   for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_) {
     memset(costmap_ + y, value, len * sizeof(unsigned char));
   }
+
+  const int64_t new_stamp_ns = last_modified_.nanoseconds() + 1;
+  last_modified_ = rclcpp::Time(new_stamp_ns, last_modified_.get_clock_type());
 }
 
 bool Costmap2D::copyCostmapWindow(
@@ -179,6 +185,11 @@ bool Costmap2D::copyCostmapWindow(
     map.costmap_, lower_left_x, lower_left_y, map.size_x_, costmap_, 0, 0, size_x_,
     size_x_,
     size_y_);
+
+  {
+    std::unique_lock<mutex_t> lock(*access_);
+    last_modified_ = map.getLastModifiedStamp();
+  }
   return true;
 }
 
@@ -202,6 +213,14 @@ bool Costmap2D::copyWindow(
     source.costmap_, sx0, sy0, source.size_x_,
     costmap_, dx0, dy0, size_x_,
     sz_x, sz_y);
+
+  {
+    const auto source_stamp = source.getLastModifiedStamp();
+    std::unique_lock<mutex_t> lock(*access_);
+    const int64_t new_stamp_ns = std::max(last_modified_.nanoseconds(),
+        source_stamp.nanoseconds()) + 1;
+    last_modified_ = rclcpp::Time(new_stamp_ns, last_modified_.get_clock_type());
+  }
   return true;
 }
 
@@ -210,6 +229,10 @@ Costmap2D::toOccupancyGridMsg(nav_msgs::msg::OccupancyGrid & msg) const
 {
   std::lock_guard<mutex_t> lock(*access_);
 
+  const int64_t stamp_ns = last_modified_.nanoseconds();
+  msg.header.stamp.sec = static_cast<int32_t>(stamp_ns / 1000000000LL);
+  msg.header.stamp.nanosec = static_cast<uint32_t>(stamp_ns % 1000000000LL);
+
   msg.info.width = size_x_;
   msg.info.height = size_y_;
   msg.info.resolution = resolution_;
@@ -231,6 +254,19 @@ Costmap2D::toOccupancyGridMsg(nav_msgs::msg::OccupancyGrid & msg) const
   }
 }
 
+rclcpp::Time Costmap2D::getLastModifiedStamp() const
+{
+  std::lock_guard<mutex_t> lock(*access_);
+  return last_modified_;
+}
+
+void Costmap2D::touch()
+{
+  std::lock_guard<mutex_t> lock(*access_);
+  const int64_t new_stamp_ns = last_modified_.nanoseconds() + 1;
+  last_modified_ = rclcpp::Time(new_stamp_ns, last_modified_.get_clock_type());
+}
+
 Costmap2D & Costmap2D::operator=(const Costmap2D & map)
 {
   // check for self assignment
@@ -247,6 +283,7 @@ Costmap2D & Costmap2D::operator=(const Costmap2D & map)
   origin_x_ = map.origin_x_;
   origin_y_ = map.origin_y_;
   default_value_ = map.default_value_;
+  last_modified_ = map.last_modified_;
 
   // initialize our various maps
   initMaps(size_x_, size_y_);
@@ -258,15 +295,16 @@ Costmap2D & Costmap2D::operator=(const Costmap2D & map)
 }
 
 Costmap2D::Costmap2D(const Costmap2D & map)
-: costmap_(NULL)
+: costmap_(NULL), last_modified_(0)
 {
   access_ = new mutex_t();
   *this = map;
 }
 
 // just initialize everything to NULL by default
 Costmap2D::Costmap2D()
-: size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), costmap_(NULL)
+: size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0),
+  costmap_(NULL), last_modified_(0)
 {
   access_ = new mutex_t();
 }
@@ -305,7 +343,16 @@ unsigned char Costmap2D::getCost(unsigned int undex) const
 
 void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
 {
-  costmap_[getIndex(mx, my)] = cost;
+  std::lock_guard<mutex_t> lock(*access_);
+  const unsigned int index = getIndex(mx, my);
+
+  if (costmap_[index] == cost) {
+    return;
+  }
+
+  costmap_[index] = cost;
+  const int64_t new_stamp_ns = last_modified_.nanoseconds() + 1;
+  last_modified_ = rclcpp::Time(new_stamp_ns, last_modified_.get_clock_type());
 }
 
 void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double & wx, double & wy) const

common/easynav_costmap_common/tests/costmap_2d_tests.cpp

@@ -16,6 +16,7 @@
 #include <gtest/gtest.h>
 #include "easynav_costmap_common/costmap_2d.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
+#include "rclcpp/time.hpp"
 
 using easynav::Costmap2D;
 
@@ -121,3 +122,82 @@ TEST_F(Costmap2DTest, OccupancyGridConversion)
     EXPECT_EQ(grid.data[i], expected ? 100 : 0);
   }
 }
+
+TEST_F(Costmap2DTest, TimestampFromOccupancyGridConstructor)
+{
+  nav_msgs::msg::OccupancyGrid in;
+  in.header.stamp.sec = 123;
+  in.header.stamp.nanosec = 456u;
+
+  in.info.width = 2u;
+  in.info.height = 3u;
+  in.info.resolution = 0.5;
+  in.info.origin.position.x = 1.0;
+  in.info.origin.position.y = -2.0;
+  in.data.assign(in.info.width * in.info.height, 0);
+
+  Costmap2D map(in);
+  const int64_t expected_ns = 123LL * 1000000000LL + 456LL;
+  EXPECT_EQ(map.getLastModifiedStamp().nanoseconds(), expected_ns);
+
+  nav_msgs::msg::OccupancyGrid out;
+  map.toOccupancyGridMsg(out);
+
+  EXPECT_EQ(out.header.stamp.sec, in.header.stamp.sec);
+  EXPECT_EQ(out.header.stamp.nanosec, in.header.stamp.nanosec);
+}
+
+TEST_F(Costmap2DTest, TimestampFromCopyConstructor)
+{
+  nav_msgs::msg::OccupancyGrid in;
+  in.header.stamp.sec = 10;
+  in.header.stamp.nanosec = 20u;
+  in.info.width = 1u;
+  in.info.height = 1u;
+  in.info.resolution = 1.0;
+  in.info.origin.position.x = 0.0;
+  in.info.origin.position.y = 0.0;
+  in.data.assign(1u, 0);
+
+  Costmap2D original(in);
+  Costmap2D copy(original);
+
+  const int64_t expected_ns = 10LL * 1000000000LL + 20LL;
+  EXPECT_EQ(copy.getLastModifiedStamp().nanoseconds(), expected_ns);
+
+  nav_msgs::msg::OccupancyGrid out;
+  copy.toOccupancyGridMsg(out);
+  EXPECT_EQ(out.header.stamp.sec, in.header.stamp.sec);
+  EXPECT_EQ(out.header.stamp.nanosec, in.header.stamp.nanosec);
+}
+
+TEST_F(Costmap2DTest, TimestampUpdatedBySetCost)
+{
+  nav_msgs::msg::OccupancyGrid in;
+  in.header.stamp.sec = 7;
+  in.header.stamp.nanosec = 8u;
+  in.info.width = 3u;
+  in.info.height = 3u;
+  in.info.resolution = 1.0;
+  in.info.origin.position.x = 0.0;
+  in.info.origin.position.y = 0.0;
+  in.data.assign(in.info.width * in.info.height, 0);
+
+  Costmap2D map(in);
+  const int64_t expected_ns = 7LL * 1000000000LL + 8LL;
+
+  map.setCost(1, 1, 100);
+  EXPECT_EQ(map.getLastModifiedStamp().nanoseconds(), expected_ns + 1);
+
+  // Setting the same value should not change the stamp.
+  map.setCost(1, 1, 100);
+  EXPECT_EQ(map.getLastModifiedStamp().nanoseconds(), expected_ns + 1);
+
+  nav_msgs::msg::OccupancyGrid out;
+  map.toOccupancyGridMsg(out);
+
+  const int64_t out_ns =
+    static_cast<int64_t>(out.header.stamp.sec) * 1000000000LL +
+    static_cast<int64_t>(out.header.stamp.nanosec);
+  EXPECT_EQ(out_ns, expected_ns + 1);
+}

localizers/easynav_costmap_localizer/README.md

@@ -66,7 +66,7 @@ This package does not create service servers or clients.
 | Key | Type | Access | Notes |
 |---|---|---|---|
 | `points` | `PointPerceptions` | **Read** | Perception point clouds used in correction. |
-| `map.static` | `Costmap2D` | **Read** | Static costmap for likelihood evaluation. |
+| `map.base` | `Costmap2D` | **Read** | Base costmap for likelihood evaluation. |
 
 ## TF Frames
 

localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp

@@ -572,12 +572,12 @@ AMCLLocalizer::correct(NavState & nav_state)
     return;
   }
 
-  if (!nav_state.has("map.static")) {
-    RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.static map");
+  if (!nav_state.has("map.base")) {
+    RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.base map");
     return;
   }
 
-  const auto & map_static = nav_state.get<Costmap2D>("map.static");
+  const auto & map_static = nav_state.get<Costmap2D>("map.base");
 
   const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
 

localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp

@@ -67,7 +67,7 @@ TEST_F(AMCLLocalizerTest, BasicDynamicUpdate)
 
   navstate.set("perceptions", easynav::Perceptions());
   navstate.get_mutable<easynav::Perceptions>("perceptions")->push_back(perception);
-  navstate.set("map.static", *static_map);
+  navstate.set("map.base", *static_map);
 
   manager->update(navstate);
 

maps_managers/easynav_costmap_maps_manager/README.md

@@ -2,7 +2,7 @@
 
 ## Description
 
-Maps Manager that maintains 2D costmaps (static and dynamic), supports filter plugins (such as inflation and obstacle filters), and exposes maps through ROS topics and NavState integration.
+Maps Manager that maintains 2D costmaps (base and dynamic), supports filter plugins (such as inflation and obstacle filters), and exposes maps through ROS topics and NavState integration.
 
 At the core of this stack lies the Costmap2D data structure. `Costmap2D` extends the binary occupancy grid into a graded cost representation with values in the range [0–255]:
 
@@ -81,7 +81,7 @@ Each entry in `<plugin>.filters` defines a sub-namespace `<plugin>.<filter>` wit
 - **Type:** `easynav::InflationFilter`
 - **Description:**  
   Expands obstacle information in the costmap by assigning graded costs around `LETHAL_OBSTACLE` cells based on distance. Uses a breadth-first wavefront propagation algorithm (distance bins) to efficiently inflate obstacles up to `inflation_radius`.  
-  The filter reads both the static map and the dynamic filtered map, applies inflation to each, and merges results. If `ObstacleBounds` is available in `NavState`, inflation is restricted to the updated region for performance.
+  The filter reads both the base map and the dynamic filtered map, applies inflation to each, and merges results. If `ObstacleBounds` is available in `NavState`, inflation is restricted to the updated region for performance.
 
 **Parameters:**
 
@@ -95,7 +95,7 @@ Each entry in `<plugin>.filters` defines a sub-namespace `<plugin>.<filter>` wit
 
 | Key | Type | Access | Description |
 |---|---|---|---|
-| `map.static` | `Costmap2D` | **Read** | Static costmap to inflate. |
+| `map.base` | `Costmap2D` | **Read** | Base costmap to inflate. |
 | `map.dynamic.filtered` | `Costmap2D` | **Read/Write** | Dynamic costmap input and output after inflation. |
 | `map.dynamic.obstacle_bounds` | `ObstacleBounds` | **Read** (optional) | Restricts inflation to updated region for performance. |
 
@@ -130,7 +130,7 @@ maps_manager_node:
 | Direction | Topic | Type | Purpose | QoS |
 |---|---|---|---|---|
 | Subscription | `<node_fqn>/<plugin>/incoming_map` | `nav_msgs/msg/OccupancyGrid` | Input occupancy map used to update the dynamic map. | `depth=1, transient_local, reliable` |
-| Publisher | `<node_fqn>/<plugin>/map` | `nav_msgs/msg/OccupancyGrid` | Publishes the static costmap. | `depth=1` |
+| Publisher | `<node_fqn>/<plugin>/map` | `nav_msgs/msg/OccupancyGrid` | Publishes the base costmap. | `depth=1` |
 | Publisher | `<node_fqn>/<plugin>/dynamic_map` | `nav_msgs/msg/OccupancyGrid` | Publishes the dynamic (live) costmap. | `depth=100` |
 
 ### Services
@@ -145,7 +145,7 @@ maps_manager_node:
 
 | Key | Type | Access | Notes |
 |---|---|---|---|
-| `map.static` | `Costmap2D` | **Write** | Static map loaded from YAML. |
+| `map.base` | `Costmap2D` | **Write** | Base map loaded from YAML. |
 | `map.dynamic` | `Costmap2D` | **Write** | Dynamic map after applying filters. |
 | `map.dynamic.filtered` | `Costmap2D` | **Read** | Previously filtered map used as input if available. |
 | `map.dynamic.obstacle_bounds` | `ObstacleBounds` | **Read** | Bounding box of updated obstacles (used to limit inflation region). |