[Smac planner hybrid] turning radius not updated with new costmap

[LinusTxtonomy]

Bug report

Required Info:

• Operating System:
  • Debian12
• ROS2 Version:
  • humble
• Version or commit hash:
  • 7657f2f37659808884ae6a708f3d51054af6355c

Steps to reproduce issue

Send a first grid map to initialize the planner, then send an updated grid map with a different resolution

Expected behavior

The grid map is updated and the planner generates a path with the new grid map

Actual behavior

The grid map is updated but the turning radius that depends on grid map resolution is not updated. The path looks valid at first since the start and goal poses are transformed back and forth based on the new resolution but the turning radius only updates if the parameter for it is changed.

Additional information

I think the turning radius should be set when the createPlan function is called and not just in the parameter callback.

[SteveMacenski]

I'm not sure what you mean by "sending" grid maps to the planner, that is all handled internally to the planner/controller servers that you shouldn't have control over when using the task servers. How are you changing the resolution, specifically?

The State Lattice Planner for instance cannot be used with dynamically changing resolutions regardless since the minimum control set it computed relative to a given resolution.

Looking at Hybrid-A*, we use the minimum turning radius and update the primitives in initMotionModel which is called https://github.com/ros-planning/navigation2/blob/6d2278e42dafb7c421f9c6255a3856882a9dea82/nav2_smac_planner/src/a_star.cpp#L99. That is called in the main plugin on a per-request createPlan basis so that should be squared away.

The only gaps I could see is that we need to readjust the search_info.minimum_turning_radius & lookup_table_dim & analytic_expansion_max_length since we adjust those into grid coordinates in the planner plugin's configure and dynamic parameter update callbacks. Probably the _collision_checker.setFootprint() & _smoother also since the input for finding the inscribed radius cost will definitely not be the same with a new resolution. Possibly the costmap downsampler as well, but I'm not sure.

Have a potential plan of attack? It seems like alot needs to be updated if the resolution changes. If using dynamic parameters, perhaps we can acquire the resolution change in the Hybrid-A's callback as well to [trigger all of the `reinit_` variables to reinitialize the system's modules](https://github.com/ros-planning/navigation2/blob/6d2278e42dafb7c421f9c6255a3856882a9dea82/nav2_smac_planner/src/smac_planner_hybrid.cpp#L628-L666). That seems like the most straight forward and least invasive way if that works. Otherwise, that's going to be some interesting surgery.

[LinusTxtonomy]

Yes I shouldve been more clear about the use case. We are only using the hybrid-A* planner. The bug we had was that when we change the resolution of our grid map and then compute a new path, the turning radius value is not updated, which also influences the length of the segments. Currently we enforce a static resolution for the grid maps to avoid this. A reinit function that takes care of all affected values if a change in resolution is detected would be my first idea to tackle this problem.

[SteveMacenski]

Can you check if you can obtain the resolution parameter change in the Hybrid-A* dynamic parameters callback? If so, this is easy-peasy. Just capture that if changing and flag all those bools as true and the end of the function will take care of the rest. If we can't get a signal to the planner about a resolution change, this will get a bit more complicated

[LinusTxtonomy]

I found the core issue now. We had a mismatch between the costmap resolution in the settings for our global costmap and the resolution of the costmap we publish every second. If we dont receive a costmap before the planner is configured it uses the resolution from the settings and does not reinitialize when the actual costmap is sent. I will see if I can get your suggested approach of getting the resolution as a parameter change to work.

[SteveMacenski]

Wouldn't it be best just to change the resolution of the global costmap to your map's resolution?

[WaliaRohan]

Hi @SteveMacenski @LinusTxtonomy. I'd like to own this issue. Do we have enough information to get the ball rolling on this issue?

[SteveMacenski]

Yes! I think everything is in the thread above:

• Capture the resolution change in the dynamic parameter callback of the smac planner in the global namespace (make sure we get it?)
• If change, set all the reinitialize variable to true so that we basically reconstruct the key planner information using the new resolution
• Test changing resolutions and that we can successfully navigate

The test is probably where you'll spend the majority of the project's time to validate that it works, but it'll be well worth it :-)

[SteveMacenski]

https://github.com/ros-planning/navigation2/pull/4223 addresses