Fix routes

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;
   }

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

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<SimpleMap>("map.static");
+  const auto & map_static = nav_state.get<SimpleMap>("map.base");
 
   const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
   const auto & filtered = PointPerceptionsOpsView(perceptions)

maps_managers/easynav_costmap_maps_manager/README.md

@@ -72,8 +72,8 @@ Each entry in `<plugin>.filters` defines a sub-namespace `<plugin>.<filter>` 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 `<plugin>.filters` defines a sub-namespace `<plugin>.<filter>` 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,8 @@ 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` | **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). |
 
 ---
 

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_);
+  *dynamic_map_ = nav_state.get<Costmap2D>("map");
 
   const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
 

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<Costmap2D>("map.dynamic.filtered");
+  auto dynamic_map_ptr = nav_state.get_ptr<Costmap2D>("map");
   Costmap2D & dynamic_map = *dynamic_map_ptr;
 
   const auto & base_map = nav_state.get<Costmap2D>("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<ObstacleBounds>("map.dynamic.obstacle_bounds");
+  if (nav_state.has("map.obstacle_bounds")) {
+    const auto & bb = nav_state.get<ObstacleBounds>("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

maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp

@@ -47,7 +47,11 @@ ObstacleFilter::update(NavState & nav_state)
     return;
   }
 
-  auto dynamic_map_ptr = nav_state.get_ptr<Costmap2D>("map.dynamic.filtered");
+  auto dynamic_map_ptr = nav_state.get_ptr<Costmap2D>("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();
 
@@ -81,7 +85,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);
   }
 }
 

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<easynav::Costmap2D>("map.base");
   EXPECT_EQ(base_after.getLastModifiedStamp().nanoseconds(), newer_ns);
 
-  ASSERT_TRUE(navstate.has("map.dynamic.filtered"));
-  const auto dyn_ptr = navstate.get_ptr<easynav::Costmap2D>("map.dynamic.filtered");
+  ASSERT_TRUE(navstate.has("map"));
+  const auto dyn_ptr = navstate.get_ptr<easynav::Costmap2D>("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<easynav::Costmap2D>("map.base");
   EXPECT_EQ(base_after.getLastModifiedStamp().nanoseconds(), newer_ns);
 
-  ASSERT_TRUE(navstate.has("map.dynamic.filtered"));
-  const auto dyn_ptr = navstate.get_ptr<easynav::Costmap2D>("map.dynamic.filtered");
+  ASSERT_TRUE(navstate.has("map"));
+  const auto dyn_ptr = navstate.get_ptr<easynav::Costmap2D>("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<easynav::Costmap2D>("map.dynamic.filtered");
+  ASSERT_TRUE(navstate.has("map"));
+  const auto dyn_ptr = navstate.get_ptr<easynav::Costmap2D>("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<easynav::Costmap2D>("map.dynamic");
+//  ASSERT_TRUE(navstate.has("map"));
+//  const auto & map = navstate.get<easynav::Costmap2D>("map");
 //
 //  unsigned int cx, cy;
 //  ASSERT_TRUE(map.worldToMap(1.0, 1.0, cx, cy));

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<easynav::NavMap2D>("map.dynamic");
+//  ASSERT_TRUE(::easynav::NavState.has("map"));
+//  const auto & map = ::easynav::NavState.get<easynav::NavMap2D>("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<easynav::NavMap2D>("map.static");
+//  ASSERT_TRUE(::easynav::NavState.has("map.base"));
+//  const auto & map = ::easynav::NavState.get<easynav::NavMap2D>("map.base");
 //
 //  EXPECT_EQ(map.getCost(5, 5), easynav::LETHAL_OBSTACLE);
 //  EXPECT_EQ(map.getCost(1, 1), easynav::FREE_SPACE);

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

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.

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<RoutesMap>("routes");
 
-  auto costmap_ptr = nav_state.get_ptr<Costmap2D>("map.dynamic.filtered");
+  auto costmap_ptr = nav_state.get_ptr<Costmap2D>("map");
   if (!costmap_ptr) {
     return;
   }

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<Costmap2D>("map.dynamic.filtered");
+  auto map_after = nav_state.get<Costmap2D>("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<Costmap2D>("map.dynamic.filtered");
+  auto map_after = nav_state.get<Costmap2D>("map");
 
   // No cell should be considered "on route"; all should be raised to >= 50
   for (unsigned int x = 0; x < 5; ++x) {

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
 

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;

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<easynav::SimpleMap>("map.dynamic");
+  ASSERT_TRUE(navstate.has("map"));
+  auto map = navstate.get<easynav::SimpleMap>("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<easynav::SimpleMap>("map.static");
+  ASSERT_TRUE(navstate.has("map.base"));
+  const auto & map = navstate.get<easynav::SimpleMap>("map.base");
 
   EXPECT_EQ(map.at(5, 5), 1);
   EXPECT_EQ(map.at(1, 1), 0);

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

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<Costmap2D>("map.dynamic");
+  const auto & map = nav_state.get<Costmap2D>("map");
   const auto & robot_pose = nav_state.get<nav_msgs::msg::Odometry>("robot_pose");
   const auto & goal = goals.goals.front().pose;
   const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
@@ -264,7 +264,6 @@ std::vector<geometry_msgs::msg::Pose> CostmapPlanner::a_star_path(
 
   int width = map.getSizeInCellsX();
   // Precompute constants used inside the neighbor loop
-  const double lethal_norm = 1.0 / static_cast<double>(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<geometry_msgs::msg::Pose> 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<double>(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<double>(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;