Bug report

When creating a plan, the start and the goal are moved to the middle of the closest cell.

• start = base_footprint
• goal = red arrow
• grid = 5cm/5cm = costmap resolution

Required Info:

• Operating System:
  • Ubuntu
• ROS2 Version:
  • Iron -->
• Version or commit hash:
  • source
• DDS implementation:
  • cyclon

Steps to reproduce issue

Plan a path not in the middle of a cell.

Expected behavior

The start and the goal are at the requested place.

Actual behavior

The start and the goal are moved to the middle of the closest cell

Additional information

This is due to the process used to add start and goal poses:
(smac_planner_hybrid.cpp)

unsigned int mx, my;
costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)
_a_star->setStart(mx, my, orientation_bin_id);

But setStart turns it back to float:
https://github.com/ros-planning/navigation2/blob/f815e8bd8e3826603e97749bb31a3145268de023/nav2_smac_planner/src/a_star.cpp#L145-L157

We can just compute the worldToMap manually to keep it as a float and modify setStart input types.