[SmacPlanner] The footprint is not dynamic

[BriceRenaudeau]

Bug report

When the footprint of the robot is changed using the costmap topic, the planner doesn't update the collision_checker so it can plan in obstacles. It can be seen in the /planned_footprints (they are using the costmap footprint)

The footprint is not a dynamic parameter of the planner, and it's a good point, It must retrieve it from the costmap.

Required Info:

• Operating System:
  • Ubuntu22.04
• ROS2 Version:
  • Iron
• Version or commit hash:
  • source - main
• DDS implementation:
  • cyclone

Steps to reproduce issue

1. Start the navigation
2. Change the footprint of the costmap using the topic
3. The costmap is updated
4. The collision_checker of the planner is not updated

Expected behavior

The planner updates the collision_checker and the path doesn't go into obstacles

Actual behavior

The planner doesn't update the collision_checker and the path can go into obstacles

Additional information

Note : If you change the value of the parameter angle_quantization_bins, the collision_checker is updated and the path is good.

Implementation possibilities:

• Easy and dirty: on every createPlan(), update collision_checker before setting the a_start. (tested ✔️ )
  • Strangely to work perfectly, you need to recreate the collision_checker, not just set the footprint;
• Hard and clean : Make collision_checker aware of the costmap changes.

[SteveMacenski]

We set it on initialization https://github.com/ros-planning/navigation2/blob/09ba08b7f45004df92f77fa683ea33d8afa2acce/nav2_smac_planner/src/smac_planner_lattice.cpp#L188

I suppose we could check if its changed in the main planning callback -- if so send it the updated footprint. I wouldn't want to generically do it every iteration since we precompute orientations that would slow down the planner to do when not necessary. That should be a 10 line PR, can you open it :-)

[BriceRenaudeau]

The footprint change check is already done in the setFootprint.

I added it to the main create_plan juste before setting it to the _a_star->setCollisionChecker(&_collision_checker);
Strangely, It doesn't work well. I had to create a new _collision_checker before.

[SteveMacenski]

You shouldn't need to -- perhaps try removing the pointer reference https://github.com/ros-planning/navigation2/blob/09ba08b7f45004df92f77fa683ea33d8afa2acce/nav2_smac_planner/src/analytic_expansion.cpp#L44?

It seems to me that the setFootprint should be sufficient. Reconstructing the entire object shouldn't be necessary and should be avoided (but wouldn't be the end of the world)

[BriceRenaudeau]

I found the issue.
I will create the PR today.