[SMAC planner] Start and goal are moved to closest cell

[BriceRenaudeau]

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.

[BriceRenaudeau]

A quick test and it works:

[SteveMacenski]

This is definitely true and is detailed in this ticket: https://github.com/ros-planning/navigation2/issues/3955

I even highlight a solution in this comment: https://github.com/ros-planning/navigation2/issues/3955#issuecomment-1808461774 if but it seem that you're already so inclined and implemented it. Open a PR and you can quash another ticket too :-)

[BriceRenaudeau]

I saw that issue last year but the title confused me and I didn't follow your reply. When I have some time, I will make a PR. I will go for what you described;

• Create a worldToMapContinous to compute the decimals for the world to map conversion.
• Change the setGoal to use floats as inputs and static_cast to unsigned int's for the getIndex method.
• Do the same with setStart as well