From d703a824ce3307928bd400f446eaa9b1e4b2c282 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 3 May 2026 09:51:45 +0200 Subject: [PATCH 1/6] Cells cost have no real impact in path creation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_costmap_planner/CostmapPlanner.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp index f6bfdf20..702f5be8 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -140,7 +140,7 @@ void CostmapPlanner::on_initialize() void CostmapPlanner::update(NavState & nav_state) { - if (!nav_state.has("goals") || !nav_state.has("robot_pose") || !nav_state.has("map.dynamic")) { + if (!nav_state.has("goals") || !nav_state.has("robot_pose") || !nav_state.has("map")) { return; } @@ -150,7 +150,7 @@ void CostmapPlanner::update(NavState & nav_state) return; } - const auto & map = nav_state.get("map.dynamic"); + const auto & map = nav_state.get("map"); const auto & robot_pose = nav_state.get("robot_pose"); const auto & goal = goals.goals.front().pose; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); @@ -264,7 +264,6 @@ std::vector CostmapPlanner::a_star_path( int width = map.getSizeInCellsX(); // Precompute constants used inside the neighbor loop - const double lethal_norm = 1.0 / static_cast(LETHAL_OBSTACLE); const double axial_cost = 1.0; const double diagonal_cost = std::sqrt(2.0); auto idx = [&](int x, int y) {return y * width + x;}; @@ -296,8 +295,8 @@ std::vector CostmapPlanner::a_star_path( // Reject cells that would cause collision (>= INSCRIBED_INFLATED_OBSTACLE = 253) if (cell_cost >= INSCRIBED_INFLATED_OBSTACLE) {continue;} - // Calculate traversal cost for free and lightly inflated cells - double traversal_cost = 1.0 + cost_factor_ * static_cast(cell_cost) * lethal_norm; + // Calculate traversal cost: cost_factor_ acts as a direct multiplier on cell cost + double traversal_cost = 1.0 + cost_factor_ * static_cast(cell_cost); double step_cost = (dx == 0 || dy == 0) ? axial_cost : diagonal_cost; double new_cost = cost_so_far[idx(current.x, current.y)] + traversal_cost * step_cost; From 997294507bc51f39138468001db1e4245be3296d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 3 May 2026 09:53:30 +0200 Subject: [PATCH 2/6] Navstate key among filters are always map, not an arbitrary key MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../SerestController.cpp | 2 +- localizers/easynav_simple_localizer/README.md | 2 +- .../easynav_simple_localizer/AMCLLocalizer.cpp | 6 +++--- .../easynav_costmap_maps_manager/README.md | 14 +++++++------- .../CostmapMapsManager.cpp | 4 ++-- .../filters/InflationFilter.cpp | 8 ++++---- .../filters/ObstacleFilter.cpp | 4 ++-- .../tests/costmap_mapsmanager_tests.cpp | 16 ++++++++-------- .../tests/navmap_mapsmanager_tests.cpp | 8 ++++---- .../easynav_routes_maps_manager/README.md | 4 ++-- .../filters/RoutesCostmapFilter.hpp | 4 ++-- .../filters/RoutesCostmapFilter.cpp | 4 ++-- .../tests/routes_costmap_filter_tests.cpp | 10 +++++----- .../easynav_simple_maps_manager/README.md | 4 ++-- .../SimpleMapsManager.cpp | 8 ++++---- .../tests/simple_mapsmanager_tests.cpp | 8 ++++---- planners/easynav_costmap_planner/README.md | 2 +- planners/easynav_simple_planner/README.md | 2 +- .../src/easynav_simple_planner/SimplePlanner.cpp | 10 +++++----- 19 files changed, 60 insertions(+), 60 deletions(-) diff --git a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp index 9a32f1ff..52272e5f 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -375,7 +375,7 @@ SerestController::fetch_required_inputs( { const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has("map.dynamic")) { + if (!nav_state.has("path") || !nav_state.has("robot_pose") || !nav_state.has("map")) { publish_stop(nav_state, tf_info.robot_footprint_frame); return false; } diff --git a/localizers/easynav_simple_localizer/README.md b/localizers/easynav_simple_localizer/README.md index 8a06b1be..428f59cf 100644 --- a/localizers/easynav_simple_localizer/README.md +++ b/localizers/easynav_simple_localizer/README.md @@ -65,7 +65,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` | `SimpleMap` | **Read** | Static map for likelihood evaluation. | +| `map.base` | `SimpleMap` | **Read** | Static map for likelihood evaluation. | | `robot_pose` | `nav_msgs::msg::Odometry` | **Write** | Estimated robot pose. | ## TF Frames diff --git a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp index d391cb4b..723eb5cf 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -527,12 +527,12 @@ void 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("map.static"); + const auto & map_static = nav_state.get("map.base"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) diff --git a/maps_managers/easynav_costmap_maps_manager/README.md b/maps_managers/easynav_costmap_maps_manager/README.md index 3de9cd38..67e6c9a0 100644 --- a/maps_managers/easynav_costmap_maps_manager/README.md +++ b/maps_managers/easynav_costmap_maps_manager/README.md @@ -72,8 +72,8 @@ Each entry in `.filters` defines a sub-namespace `.` wit | Key | Type | Access | Description | |---|---|---|---| | `points` | `sensor_msgs::msg::PointCloud2` | **Read** | Input point clouds to detect obstacles. | -| `map.dynamic.filtered` | `Costmap2D` | **Write** | Marks cells as `LETHAL_OBSTACLE` (254). | -| `map.dynamic.obstacle_bounds` | `ObstacleBounds` | **Write** | Bounding box of updated obstacles for incremental inflation. | +| `map` | `Costmap2D` | **Write** | Marks cells as `LETHAL_OBSTACLE` (254). | +| `map.obstacle_bounds` | `ObstacleBounds` | **Write** | Bounding box of updated obstacles for incremental inflation. | #### **InflationFilter** @@ -96,8 +96,8 @@ Each entry in `.filters` defines a sub-namespace `.` wit | Key | Type | Access | Description | |---|---|---|---| | `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. | +| `map` | `Costmap2D` | **Read/Write** | Dynamic costmap input and output after inflation. | +| `map.obstacle_bounds` | `ObstacleBounds` | **Read** (optional) | Restricts inflation to updated region for performance. | **Cost Model:** Uses exponential decay: `cost = exp(-cost_scaling_factor * (distance - inscribed_radius)) * 253` for distances beyond inscribed radius. @@ -146,9 +146,9 @@ maps_manager_node: | Key | Type | Access | Notes | |---|---|---|---| | `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). | +| `map` | `Costmap2D` | **Write** | Dynamic map after applying filters. | +| `map` | `Costmap2D` | **Read** | Previously filtered map used as input if available. | +| `map.obstacle_bounds` | `ObstacleBounds` | **Read** | Bounding box of updated obstacles (used to limit inflation region). | --- diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp index 66be2125..0a4bb5bc 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp @@ -218,7 +218,7 @@ CostmapMapsManager::update(NavState & nav_state) *dynamic_map_ = map_base_; } - nav_state.set("map.dynamic.filtered", dynamic_map_); + nav_state.set("map", dynamic_map_); if (!nav_state.has("map_time")) { nav_state.set("map_time", get_node()->now()); @@ -228,7 +228,7 @@ CostmapMapsManager::update(NavState & nav_state) filter->update(nav_state); } - nav_state.set("map.dynamic", dynamic_map_); + nav_state.set("map", dynamic_map_); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp index 4df61c69..f03c72d1 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp @@ -99,7 +99,7 @@ InflationFilter::on_initialize() void InflationFilter::update(NavState & nav_state) { - auto dynamic_map_ptr = nav_state.get_ptr("map.dynamic.filtered"); + auto dynamic_map_ptr = nav_state.get_ptr("map"); Costmap2D & dynamic_map = *dynamic_map_ptr; const auto & base_map = nav_state.get("map.base"); @@ -121,8 +121,8 @@ InflationFilter::update(NavState & nav_state) int max_i = size_x; int max_j = size_y; - if (nav_state.has("map.dynamic.obstacle_bounds")) { - const auto & bb = nav_state.get("map.dynamic.obstacle_bounds"); + if (nav_state.has("map.obstacle_bounds")) { + const auto & bb = nav_state.get("map.obstacle_bounds"); unsigned int cmin_i, cmin_j, cmax_i, cmax_j; if (dynamic_map.worldToMap(bb.min_x, bb.min_y, cmin_i, cmin_j) && @@ -154,7 +154,7 @@ InflationFilter::update(NavState & nav_state) } } - nav_state.set("map.dynamic.filtered", dynamic_map_ptr); + nav_state.set("map", dynamic_map_ptr); } void diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp index 69e41672..d29fab15 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp @@ -47,7 +47,7 @@ ObstacleFilter::update(NavState & nav_state) return; } - auto dynamic_map_ptr = nav_state.get_ptr("map.dynamic.filtered"); + auto dynamic_map_ptr = nav_state.get_ptr("map"); Costmap2D & dynamic_map = *dynamic_map_ptr; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); @@ -81,7 +81,7 @@ ObstacleFilter::update(NavState & nav_state) if (!fused.empty()) { ObstacleBounds bb{min_x, min_y, max_x, max_y}; - nav_state.set("map.dynamic.obstacle_bounds", bb); + nav_state.set("map.obstacle_bounds", bb); } } diff --git a/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp b/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp index 19cf79db..d962bfd4 100644 --- a/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp +++ b/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp @@ -103,8 +103,8 @@ TEST_F(CostmapMapsManagerTest, SyncBaseMapPrefersNewerNavStateMap) const auto & base_after = navstate.get("map.base"); EXPECT_EQ(base_after.getLastModifiedStamp().nanoseconds(), newer_ns); - ASSERT_TRUE(navstate.has("map.dynamic.filtered")); - const auto dyn_ptr = navstate.get_ptr("map.dynamic.filtered"); + ASSERT_TRUE(navstate.has("map")); + const auto dyn_ptr = navstate.get_ptr("map"); ASSERT_TRUE(dyn_ptr != nullptr); EXPECT_EQ(dyn_ptr->getLastModifiedStamp().nanoseconds(), newer_ns); EXPECT_EQ(dyn_ptr->getCost(2, 2), easynav::LETHAL_OBSTACLE); @@ -142,8 +142,8 @@ TEST_F(CostmapMapsManagerTest, SyncBaseMapPrefersNewerInternalMap) const auto & base_after = navstate.get("map.base"); EXPECT_EQ(base_after.getLastModifiedStamp().nanoseconds(), newer_ns); - ASSERT_TRUE(navstate.has("map.dynamic.filtered")); - const auto dyn_ptr = navstate.get_ptr("map.dynamic.filtered"); + ASSERT_TRUE(navstate.has("map")); + const auto dyn_ptr = navstate.get_ptr("map"); ASSERT_TRUE(dyn_ptr != nullptr); EXPECT_EQ(dyn_ptr->getLastModifiedStamp().nanoseconds(), newer_ns); EXPECT_EQ(dyn_ptr->getCost(3, 3), easynav::LETHAL_OBSTACLE); @@ -187,8 +187,8 @@ TEST_F(CostmapMapsManagerTest, IncomingMapTopicUpdatesInternalAndNavState) EXPECT_EQ(base_after.getCost(5, 5), easynav::LETHAL_OBSTACLE); EXPECT_GT(base_after.getLastModifiedStamp().nanoseconds(), 0); - ASSERT_TRUE(navstate.has("map.dynamic.filtered")); - const auto dyn_ptr = navstate.get_ptr("map.dynamic.filtered"); + ASSERT_TRUE(navstate.has("map")); + const auto dyn_ptr = navstate.get_ptr("map"); ASSERT_TRUE(dyn_ptr != nullptr); EXPECT_EQ(dyn_ptr->getCost(5, 5), easynav::LETHAL_OBSTACLE); EXPECT_EQ(dyn_ptr->getLastModifiedStamp().nanoseconds(), @@ -224,8 +224,8 @@ TEST_F(CostmapMapsManagerTest, IncomingMapTopicUpdatesInternalAndNavState) // // manager->update(navstate); // -// ASSERT_TRUE(navstate.has("map.dynamic")); -// const auto & map = navstate.get("map.dynamic"); +// ASSERT_TRUE(navstate.has("map")); +// const auto & map = navstate.get("map"); // // unsigned int cx, cy; // ASSERT_TRUE(map.worldToMap(1.0, 1.0, cx, cy)); diff --git a/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp b/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp index 4c55b8fe..913234a8 100644 --- a/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp +++ b/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp @@ -68,8 +68,8 @@ class NavMapMapsManagerTest : public ::testing::Test // // manager->update(::easynav::NavState); // -// ASSERT_TRUE(::easynav::NavState.has("map.dynamic")); -// const auto & map = ::easynav::NavState.get("map.dynamic"); +// ASSERT_TRUE(::easynav::NavState.has("map")); +// const auto & map = ::easynav::NavState.get("map"); // // unsigned int cx, cy; // ASSERT_TRUE(map.worldToMap(1.0, 1.0, cx, cy)); @@ -111,8 +111,8 @@ class NavMapMapsManagerTest : public ::testing::Test // ::easynav::NavState ::easynav::NavState; // manager->update(::easynav::NavState); // -// ASSERT_TRUE(::easynav::NavState.has("map.static")); -// const auto & map = ::easynav::NavState.get("map.static"); +// ASSERT_TRUE(::easynav::NavState.has("map.base")); +// const auto & map = ::easynav::NavState.get("map.base"); // // EXPECT_EQ(map.getCost(5, 5), easynav::LETHAL_OBSTACLE); // EXPECT_EQ(map.getCost(1, 1), easynav::FREE_SPACE); diff --git a/maps_managers/easynav_routes_maps_manager/README.md b/maps_managers/easynav_routes_maps_manager/README.md index 1bf0af58..3e1ba4f2 100644 --- a/maps_managers/easynav_routes_maps_manager/README.md +++ b/maps_managers/easynav_routes_maps_manager/README.md @@ -97,7 +97,7 @@ has been written to the NavState on each update cycle. - **Plugin Name:** `easynav_routes_maps_manager/RoutesCostmapFilter` - **Type:** `easynav::RoutesCostmapFilter` - **Description:** - Reads the current `RoutesMap` and a dynamic costmap (`map.dynamic.filtered`) + Reads the current `RoutesMap` and a dynamic costmap (`map`) from the NavState and raises the cost of all cells that do not lie within a corridor around any route segment. This effectively constrains path planners to stay close to the defined routes. @@ -114,7 +114,7 @@ has been written to the NavState on each update cycle. | Key | Type | Access | Description | |---|---|---|---| | `routes` | `RoutesMap` | **Read** | In-memory set of route segments written by `RoutesMapsManager` on each update. | -| `map.dynamic.filtered` | `Costmap2D` | **Read/Write** | Dynamic costmap that is modified in place: cells outside the corridor are raised to at least `min_cost`. | +| `map` | `Costmap2D` | **Read/Write** | Dynamic costmap that is modified in place: cells outside the corridor are raised to at least `min_cost`. | In addition, the filter publishes a debug `nav_msgs/msg/OccupancyGrid` topic with the filtered costmap for visualization. Other routes filters may use diff --git a/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp index 7ea65635..975883ba 100644 --- a/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp +++ b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp @@ -28,7 +28,7 @@ namespace easynav /// @brief Costmap filter that raises costs outside navigation routes. /// /// This filter reads the current RoutesMap and a dynamic costmap -/// ("map.dynamic.filtered") from the NavState. All cells that do not +/// ("map") from the NavState. All cells that do not /// lie within a configurable corridor around any route segment have /// their cost raised to at least @c min_cost_. A debug occupancy grid /// representing the filtered costmap is also published. @@ -56,7 +56,7 @@ class RoutesCostmapFilter : public RoutesFilter /// @brief Apply the routes-based filtering to the costmap. /// - /// If both "routes" and "map.dynamic.filtered" entries are present + /// If both "routes" and "map" entries are present /// in the NavState, the costmap is modified in place and the debug /// occupancy grid is published. If either entry is missing, the /// function returns without making changes. diff --git a/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/filters/RoutesCostmapFilter.cpp b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/filters/RoutesCostmapFilter.cpp index 10a2a897..e5fb7b2d 100644 --- a/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/filters/RoutesCostmapFilter.cpp +++ b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/filters/RoutesCostmapFilter.cpp @@ -73,13 +73,13 @@ RoutesCostmapFilter::update(NavState & nav_state) return; } - if (!nav_state.has("map.dynamic.filtered")) { + if (!nav_state.has("map")) { return; } const auto & routes = nav_state.get("routes"); - auto costmap_ptr = nav_state.get_ptr("map.dynamic.filtered"); + auto costmap_ptr = nav_state.get_ptr("map"); if (!costmap_ptr) { return; } diff --git a/maps_managers/easynav_routes_maps_manager/tests/routes_costmap_filter_tests.cpp b/maps_managers/easynav_routes_maps_manager/tests/routes_costmap_filter_tests.cpp index a6f574ca..011e462e 100644 --- a/maps_managers/easynav_routes_maps_manager/tests/routes_costmap_filter_tests.cpp +++ b/maps_managers/easynav_routes_maps_manager/tests/routes_costmap_filter_tests.cpp @@ -58,7 +58,7 @@ TEST_F(RoutesCostmapFilterTest, DoesNothingWhenNavStateMissingKeys) ASSERT_NO_THROW(filter.initialize(node, "routes.routes_costmap")); NavState nav_state; - // No 'routes' and no 'map.dynamic.filtered' keys -> update should not crash + // No 'routes' and no 'map' keys -> update should not crash EXPECT_NO_THROW(filter.update(nav_state)); } @@ -93,11 +93,11 @@ TEST_F(RoutesCostmapFilterTest, RaisesCostOutsideRoutes) NavState nav_state; nav_state.set("routes", routes); - nav_state.set("map.dynamic.filtered", map); + nav_state.set("map", map); filter.update(nav_state); - auto map_after = nav_state.get("map.dynamic.filtered"); + auto map_after = nav_state.get("map"); // Cells clearly along the interior of the route should remain 0, // others should be >= 50 (default min_cost). The last endpoint @@ -142,11 +142,11 @@ TEST_F(RoutesCostmapFilterTest, IgnoresRoutePointsOutsideMap) NavState nav_state; nav_state.set("routes", routes); - nav_state.set("map.dynamic.filtered", map); + nav_state.set("map", map); filter.update(nav_state); - auto map_after = nav_state.get("map.dynamic.filtered"); + auto map_after = nav_state.get("map"); // No cell should be considered "on route"; all should be raised to >= 50 for (unsigned int x = 0; x < 5; ++x) { diff --git a/maps_managers/easynav_simple_maps_manager/README.md b/maps_managers/easynav_simple_maps_manager/README.md index 10f59ab4..00755612 100644 --- a/maps_managers/easynav_simple_maps_manager/README.md +++ b/maps_managers/easynav_simple_maps_manager/README.md @@ -58,8 +58,8 @@ At the heart of this stack is the SimpleMap data structure. It represents the en | Key | Type | Access | Notes | |---|---|---|---| | `points` | `PointPerceptions` | **Read** | Optional perception points bundle (not strictly required for this no-op manager). | -| `map.static` | `SimpleMap` | **Write** | Static map loaded from file / parameter configuration. | -| `map.dynamic` | `SimpleMap` | **Write** | Dynamic map after applying incoming updates. | +| `map.base` | `SimpleMap` | **Write** | Static map loaded from file / parameter configuration. | +| `map` | `SimpleMap` | **Write** | Dynamic map after applying incoming updates. | ## TF Frames diff --git a/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp b/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp index 76751621..a711d4cf 100644 --- a/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp +++ b/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp @@ -144,8 +144,8 @@ SimpleMapsManager::update(NavState & nav_state) } if (perceptions.empty()) { - nav_state.set("map.static", static_map_); - nav_state.set("map.dynamic", dynamic_map_); + nav_state.set("map.base", static_map_); + nav_state.set("map", dynamic_map_); return; } @@ -166,8 +166,8 @@ SimpleMapsManager::update(NavState & nav_state) } } - nav_state.set("map.static", static_map_); - nav_state.set("map.dynamic", dynamic_map_); + nav_state.set("map.base", static_map_); + nav_state.set("map", dynamic_map_); dynamic_map_.to_occupancy_grid(dynamic_grid_msg_); dynamic_grid_msg_.header.frame_id = tf_info.map_frame; diff --git a/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp b/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp index e6adb415..449dee29 100644 --- a/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp +++ b/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp @@ -99,8 +99,8 @@ TEST_F(SimpleMapsManagerTest, BasicDynamicUpdate) manager->update(navstate); - ASSERT_TRUE(navstate.has("map.dynamic")); - auto map = navstate.get("map.dynamic"); + ASSERT_TRUE(navstate.has("map")); + auto map = navstate.get("map"); auto cell1 = map.metric_to_cell(1.0, 1.0); EXPECT_TRUE(map.at(cell1.first, cell1.second)); @@ -148,8 +148,8 @@ TEST_F(SimpleMapsManagerTest, IncomingOccupancyGridUpdatesMaps) manager->update(navstate); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - ASSERT_TRUE(navstate.has("map.static")); - const auto & map = navstate.get("map.static"); + ASSERT_TRUE(navstate.has("map.base")); + const auto & map = navstate.get("map.base"); EXPECT_EQ(map.at(5, 5), 1); EXPECT_EQ(map.at(1, 1), 0); diff --git a/planners/easynav_costmap_planner/README.md b/planners/easynav_costmap_planner/README.md index 6750fca3..df142bd3 100644 --- a/planners/easynav_costmap_planner/README.md +++ b/planners/easynav_costmap_planner/README.md @@ -96,7 +96,7 @@ This plugin does not create subscriptions or services directly; it reads inputs | Key | Type | Access | Notes | |---|---|---|---| | `goals` | `nav_msgs::msg::Goals` | **Read** | Goal list used as planner targets. | -| `map.dynamic` | `Costmap2D` | **Read** | Dynamic costmap used for A* search. | +| `map` | `Costmap2D` | **Read** | Dynamic costmap used for A* search. | | `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Current robot pose used as the start position. | | `path` | `nav_msgs::msg::Path` | **Write** | Output path to follow. | diff --git a/planners/easynav_simple_planner/README.md b/planners/easynav_simple_planner/README.md index 324dc632..efe694cc 100644 --- a/planners/easynav_simple_planner/README.md +++ b/planners/easynav_simple_planner/README.md @@ -47,7 +47,7 @@ This plugin does not create subscriptions or services directly; it reads inputs | Key | Type | Access | Notes | |---|---|---|---| | `goals` | `nav_msgs::msg::Goals` | **Read** | Planner targets. | -| `map.dynamic` | `SimpleMap` | **Read** | Dynamic `SimpleMap` grid used for search. | +| `map` | `SimpleMap` | **Read** | Dynamic `SimpleMap` grid used for search. | | `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Start pose for path planning. | | `path` | `nav_msgs::msg::Path` | **Write** | Output path to follow. | diff --git a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp index b869c68c..aa8f707d 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -113,16 +113,16 @@ SimplePlanner::update(NavState & nav_state) return; } - if (!nav_state.has("map.dynamic")) { - RCLCPP_WARN(get_node()->get_logger(), "SimplePlanner::update map.dynamic map not found"); + if (!nav_state.has("map")) { + RCLCPP_WARN(get_node()->get_logger(), "SimplePlanner::update map map not found"); return; } SimpleMap map_typed; - if (nav_state.has("map.dynamic")) { - map_typed = nav_state.get("map.dynamic"); + if (nav_state.has("map")) { + map_typed = nav_state.get("map"); } else { - RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map.dynamic map"); + RCLCPP_WARN(get_node()->get_logger(), "There is yet no a map"); return; } From 0be0003c266e98413c2e59d08602b03fcc596a15 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 4 May 2026 11:24:29 +0200 Subject: [PATCH 3/6] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- maps_managers/easynav_costmap_maps_manager/README.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/maps_managers/easynav_costmap_maps_manager/README.md b/maps_managers/easynav_costmap_maps_manager/README.md index 67e6c9a0..7acf6015 100644 --- a/maps_managers/easynav_costmap_maps_manager/README.md +++ b/maps_managers/easynav_costmap_maps_manager/README.md @@ -146,8 +146,7 @@ maps_manager_node: | Key | Type | Access | Notes | |---|---|---|---| | `map.base` | `Costmap2D` | **Write** | Base map loaded from YAML. | -| `map` | `Costmap2D` | **Write** | Dynamic map after applying filters. | -| `map` | `Costmap2D` | **Read** | Previously filtered map used as input if available. | +| `map` | `Costmap2D` | **Read/Write** | Dynamic map after applying filters; if an existing filtered `map` is available, the manager may use it as input. | | `map.obstacle_bounds` | `ObstacleBounds` | **Read** | Bounding box of updated obstacles (used to limit inflation region). | --- From 54f155abadba845821c969ad0effaf59018620f5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 4 May 2026 11:24:56 +0200 Subject: [PATCH 4/6] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- .../easynav_costmap_maps_manager/filters/ObstacleFilter.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp index d29fab15..dfce79fa 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp @@ -48,6 +48,10 @@ ObstacleFilter::update(NavState & nav_state) } auto dynamic_map_ptr = nav_state.get_ptr("map"); + if (!dynamic_map_ptr) { + RCLCPP_WARN(get_node()->get_logger(), "There is no map in NavState"); + return; + } Costmap2D & dynamic_map = *dynamic_map_ptr; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); From b3c84262a0fb450367d4bdf4f1e82038818dbef6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 4 May 2026 11:29:14 +0200 Subject: [PATCH 5/6] Avoid rewrite map with dynamic_map MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_costmap_maps_manager/CostmapMapsManager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp index 0a4bb5bc..6e3231f3 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp @@ -228,7 +228,7 @@ CostmapMapsManager::update(NavState & nav_state) filter->update(nav_state); } - nav_state.set("map", dynamic_map_); + dynamic_map_ = nav_state.get>("map"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); From e9428a3dce1a087a08eb146492bf288c5329cf5c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 5 May 2026 07:40:33 +0200 Subject: [PATCH 6/6] Update dynamic_map_ with map key at navstate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_costmap_maps_manager/CostmapMapsManager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp index 6e3231f3..f763f0a5 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp @@ -228,7 +228,7 @@ CostmapMapsManager::update(NavState & nav_state) filter->update(nav_state); } - dynamic_map_ = nav_state.get>("map"); + *dynamic_map_ = nav_state.get("map"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();