Conversation
Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>
Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>
There was a problem hiding this comment.
Pull request overview
This PR standardizes NavState map key usage across the navigation stack and adjusts the CostmapPlanner A* traversal cost so high-cost cells influence route selection as intended.
Changes:
- Rename NavState keys to use
mapfor the dynamic/working map andmap.basefor the base/static map (plusmap.obstacle_boundsfor incremental inflation). - Update planners, localizers, map managers, filters, and tests/docs to match the new key convention.
- Modify CostmapPlanner A* traversal cost to apply
cost_factordirectly to raw costmap cell costs.
Reviewed changes
Copilot reviewed 20 out of 20 changed files in this pull request and generated 7 comments.
Show a summary per file
| File | Description |
|---|---|
| planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp | Switches planner input key from map.dynamic to map. |
| planners/easynav_simple_planner/README.md | Updates documented NavState key from map.dynamic to map. |
| planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp | Uses map key and changes A* traversal cost calculation. |
| planners/easynav_costmap_planner/README.md | Updates documented NavState key to map. |
| maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp | Updates tests to assert map / map.base keys. |
| maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp | Writes map and map.base instead of map.dynamic / map.static. |
| maps_managers/easynav_simple_maps_manager/README.md | Documents map / map.base outputs. |
| maps_managers/easynav_routes_maps_manager/tests/routes_costmap_filter_tests.cpp | Updates tests to use map for filter input/output. |
| maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/filters/RoutesCostmapFilter.cpp | Reads/modifies costmap under map key. |
| maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp | Updates header docs to reference map. |
| maps_managers/easynav_routes_maps_manager/README.md | Documents map as the costmap key for the routes filter. |
| maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp | Updates commented test examples to map / map.base. |
| maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp | Updates tests to use map key for the dynamic costmap. |
| maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp | Switches dynamic costmap pointer key to map; obstacle bounds key to map.obstacle_bounds. |
| maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp | Switches dynamic costmap pointer key to map; reads bounds from map.obstacle_bounds; writes back to map. |
| maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp | Writes dynamic costmap under unified map key before/after filters. |
| maps_managers/easynav_costmap_maps_manager/README.md | Documents new keys (map, map.obstacle_bounds) in filter/manager tables. |
| localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp | Switches static map key from map.static to map.base. |
| localizers/easynav_simple_localizer/README.md | Updates documented static map key to map.base. |
| controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp | Updates required-input check to use map key. |
Comments suppressed due to low confidence (1)
maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp:106
dynamic_map_ptris dereferenced without checking for null. IfNavStatedoesn't contain amapentry (or it isn't stored as a pointer), this will crash. Add a guard (and consider also guarding access tomap.base) before dereferencing/reading.
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");
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| } | ||
|
|
||
| nav_state.set("map.dynamic", dynamic_map_); | ||
| nav_state.set("map", dynamic_map_); |
| 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"); |
Comment on lines
+116
to
127
| 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<SimpleMap>("map.dynamic"); | ||
| if (nav_state.has("map")) { | ||
| map_typed = nav_state.get<SimpleMap>("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; | ||
| } |
| map_typed = nav_state.get<SimpleMap>("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"); |
Comment on lines
96
to
101
| | 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. | |
Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>
Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>
Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>
Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Hi,
This PR contains two changes:
I hope it helps.