Dynamic base costmap by fmrico · Pull Request #63 · EasyNavigation/easynav_plugins

fmrico · 2026-04-24T06:15:29Z

Context

When running CostmapMapsManager::update(), there were two potential sources for the base map: the internal state (map_base_) and the one stored in NavState under "map.base". This PR keeps both in sync, always selecting the most recent one.

This matters when updated maps arrive via the incoming_map topic (e.g., from SLAM Toolbox): the newly received map should be treated as the latest base map and propagated consistently.

Changes

Costmap2D now tracks a last-modified timestamp (last_modified_) and exposes getLastModifiedStamp() (used as the source of truth for map recency).
CostmapMapsManager::update() compares map_base_ vs nav_state["map.base"] (if present) and syncs to the newest one using Costmap2D::getLastModifiedStamp().nanoseconds().
Added/updated gtests to cover:
- Prefer the newest map when NavState provides a newer "map.base".
- Prefer the internal map when map_base_ is newer.
- incoming_map case: when an OccupancyGrid arrives on the incoming_map topic, after one update() cycle the map is reflected in map_base_ (checked via map.dynamic.filtered) and in nav_state["map.base"].

Checklist

Timestamp-based sync logic implemented
Tests cover both directions + incoming_map
Package colcon test passes (linters + gtest)

Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>

Copilot

Pull request overview

This PR introduces timestamp-based synchronization for the base costmap between CostmapMapsManager’s internal state and NavState["map.base"], aiming to ensure the most recent map is consistently used (including updates arriving via the incoming_map topic).

Changes:

Added a timestamp field to Costmap2D and exposed it via getLastModifiedStamp(); propagated it through toOccupancyGridMsg().
Updated CostmapMapsManager::update() to sync map_base_ and nav_state["map.base"] based on timestamp recency, and renamed “static” map concepts to “base” in relevant modules.
Added/updated gtests covering bidirectional base-map preference and the incoming_map update path.

Reviewed changes

Copilot reviewed 14 out of 14 changed files in this pull request and generated 7 comments.

Show a summary per file

File	Description
maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp	Adds tests validating timestamp-based base-map sync and incoming-map propagation.
maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp	Switches `map.static` → `map.base` and renames internal members/functions accordingly.
maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/InflationFilter.hpp	Updates filter member naming/signatures from static→base.
maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp	Implements timestamp-based sync between internal base map and `NavState["map.base"]`; adapts YAML/incoming-map handling to “base”.
maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp	Renames API surface from `set_static_map` → `set_base_map` and updates member names.
maps_managers/easynav_costmap_maps_manager/README.md	Documentation updates from static→base terminology and NavState key changes.
localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp	Updates NavState key usage from `map.static` → `map.base`.
localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp	Switches lookup from `map.static` → `map.base` and updates warning string.
localizers/easynav_costmap_localizer/README.md	Documentation updates for the new `map.base` key.
common/easynav_costmap_common/tests/costmap_2d_tests.cpp	Adds tests for timestamp propagation and behavior across constructors/copies/mutations.
common/easynav_costmap_common/src/easynav_costmap_common/costmap_2d.cpp	Implements `last_modified_` storage, copying, and stamp output in OccupancyGrid conversion.
common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp	Declares timestamp API/member and documents semantics.
common/easynav_costmap_common/package.xml	Adds `rclcpp` dependency required by `rclcpp::Time` usage.
common/easynav_costmap_common/CMakeLists.txt	Finds/links/exports `rclcpp` for the common costmap library.

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

Copilot · 2026-04-24T06:21:49Z

+  if (nav_state.has("map.base")) {
+    const auto & external_base = nav_state.get<Costmap2D>("map.base");
+
+    const int64_t internal_ns = map_base_.getLastModifiedStamp().nanoseconds();
+    const int64_t external_ns = external_base.getLastModifiedStamp().nanoseconds();
+
+    if (external_ns > internal_ns) {
+      map_base_ = external_base;
+    } else if (internal_ns > external_ns) {
+      nav_state.set("map.base", map_base_);
+    }


When the external NavState "map.base" is newer, update() copies it into map_base_, but base_grid_msg_ (used by savemap) and the base map publisher are not updated. This leaves the /.../<plugin>/map topic and saved map potentially stale even though the internal base map/dynamic map now use the newer NavState map. Consider updating base_grid_msg_ from map_base_ and republishing whenever the chosen base map changes (e.g., when external_ns > internal_ns, and also when set_base_map() is called).

Copilot · 2026-04-24T06:21:49Z

+InflationFilter::needs_recompute_base_(const Costmap2D & base_map) const
 {
-  if (!has_static_inflated_) {
+  if (!has_base_inflated_) {
    return true;
  }

-  if (static_map.getSizeInCellsX() != static_sig_.size_x ||
-    static_map.getSizeInCellsY() != static_sig_.size_y ||
-    static_map.getResolution() != static_sig_.resolution ||
-    static_map.getOriginX() != static_sig_.origin_x ||
-    static_map.getOriginY() != static_sig_.origin_y)
+  if (base_map.getSizeInCellsX() != base_sig_.size_x ||
+    base_map.getSizeInCellsY() != base_sig_.size_y ||
+    base_map.getResolution() != base_sig_.resolution ||
+    base_map.getOriginX() != base_sig_.origin_x ||
+    base_map.getOriginY() != base_sig_.origin_y)


needs_recompute_base_ only checks geometry (size/resolution/origin). With base maps now able to change dynamically (incoming_map/NavState sync) without changing geometry, base_inflated_ can become stale and merging it into the dynamic map can reintroduce outdated inflated obstacles or miss newly-added ones (especially when obstacle_bounds restricts inflation). Include base_map's last-modified stamp (or another content-change indicator) in the signature so inflation is recomputed when the base map data changes.

Copilot · 2026-04-24T06:21:50Z

  /**
   * @brief Replaces the current static map.
   *
   * @param new_map Shared pointer to a new map object. Must be of type SimpleMap.
   */
-  void set_static_map(const Costmap2D & new_map);
+  void set_base_map(const Costmap2D & new_map);


The set_base_map() doxygen still refers to a "static map" and a "Shared pointer ... SimpleMap", which no longer matches the signature (Costmap2D by const ref) and the new base-map terminology. Please update the brief/param docs to avoid misleading users of this public API.

Copilot · 2026-04-24T06:21:50Z

+    * 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.
+   *
+   * Currently only set by constructors and copy/assignment; other updates do not change it.


The new last_modified_ / getLastModifiedStamp() is documented as a "last costmap modification" timestamp, but it is not updated by mutating operations (setCost/resizeMap/reset/etc.). Since other code now uses this stamp as a recency source of truth, the current semantics are easy to misinterpret and can produce incorrect ordering if a map is modified after construction. Either update last_modified_ on mutations (or provide an explicit 'touch/setLastModifiedStamp' API) or rename/document it as an immutable "source stamp" taken from the input OccupancyGrid.

Suggested change

* 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.

*

* Currently only set by constructors and copy/assignment; other updates do not change it.

* The output message header stamp is set to the internal stored stamp associated with this

* costmap. This stamp reflects the source/construction timestamp carried into the costmap

* and is not automatically updated by every mutating operation.

*/

void toOccupancyGridMsg(nav_msgs::msg::OccupancyGrid & msg) const;

/**

* @brief Get the internal stored timestamp associated with this costmap.

*

* This value is initialized by constructors and preserved by copy/assignment. It should be

* treated as a source/construction stamp, not as a guaranteed "last mutation" timestamp,

* because mutating operations do not automatically update it.

Copilot · 2026-04-24T06:21:50Z

+TEST_F(Costmap2DTest, TimestampNotUpdatedByChanges)
+{
+  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);
+  map.resizeMap(4u, 4u, 1.0, 1.0, 1.0);
+
+  EXPECT_EQ(map.getLastModifiedStamp().nanoseconds(), expected_ns);
+


This test codifies that the "last modified" timestamp remains unchanged after mutating the map (setCost/resizeMap). Since CostmapMapsManager now relies on this stamp to decide which base map is newer, this can lead to incorrect sync decisions if a Costmap2D is modified after construction. Either update the timestamp on mutations (and adjust the expectation here) or rename/reframe the API so it's clearly an immutable "source stamp" rather than a true modification time.

Copilot · 2026-04-24T06:21:51Z

-  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");


The warning text is grammatically incorrect ("There is yet no a map.base map"). Please rephrase to something like "There is not yet a map.base" / "No map.base available yet" so logs are clearer.

Suggested change

RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.base map");

RCLCPP_WARN(get_node()->get_logger(), "No map.base available yet");

Copilot · 2026-04-24T06:21:51Z

  }

-  const auto & map_static = nav_state.get<Costmap2D>("map.static");
+  const auto & map_static = nav_state.get<Costmap2D>("map.base");


The variable name map_static no longer matches the data being read from NavState (it now comes from "map.base"). Renaming it (e.g., base_map) would reduce confusion in future edits and debugging.

Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>

fmrico added 3 commits April 24, 2026 06:38

Adding timestamp to costmap

ebf7405

Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>

Change map static to base

d60661c

Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>

Costamp base map sincronized with nav_state

6ac95ae

Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>

Copilot AI review requested due to automatic review settings April 24, 2026 06:15

fmrico changed the title ~~Dynamic base costmap john~~ Dynamic base costmap Apr 24, 2026

Copilot started reviewing on behalf of fmrico April 24, 2026 06:15 View session

Copilot AI reviewed Apr 24, 2026

View reviewed changes

fmrico added 2 commits April 24, 2026 08:25

Publish changes in base map and be ready to save updated map

91c7e1b

Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>

Any change in the costmap set it as modified

7ff4876

Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Dynamic base costmap #63

Dynamic base costmap #63
fmrico wants to merge 5 commits intorollingfrom
dynamic_base_costmap_john

fmrico commented Apr 24, 2026

Uh oh!

Copilot AI left a comment

Uh oh!

Copilot AI Apr 24, 2026

Uh oh!

Copilot AI Apr 24, 2026

Uh oh!

Copilot AI Apr 24, 2026

Uh oh!

Copilot AI Apr 24, 2026

Uh oh!

Copilot AI Apr 24, 2026

Uh oh!

Copilot AI Apr 24, 2026

Uh oh!

Copilot AI Apr 24, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

-    * 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.
-   *
-   * Currently only set by constructors and copy/assignment; other updates do not change it.
+    * The output message header stamp is set to the internal stored stamp associated with this
+    * costmap. This stamp reflects the source/construction timestamp carried into the costmap
+    * and is not automatically updated by every mutating operation.
+   */
+  void toOccupancyGridMsg(nav_msgs::msg::OccupancyGrid & msg) const;
+  /**
+   * @brief Get the internal stored timestamp associated with this costmap.
+   *
+   * This value is initialized by constructors and preserved by copy/assignment. It should be
+   * treated as a source/construction stamp, not as a guaranteed "last mutation" timestamp,
+   * because mutating operations do not automatically update it.

	RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.base map");
	RCLCPP_WARN(get_node()->get_logger(), "No map.base available yet");

Conversation

fmrico commented Apr 24, 2026

Context

Changes

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Copilot AI Apr 24, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 24, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 24, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 24, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 24, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 24, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 24, 2026

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants