SmacPlanner backtrace failure when goal is close to the start pose

[DylanDeCoeyer-Quimesis]

Bug report

Required Info:

• Operating System:
  • Ubuntu 22.04
• ROS2 Version:
  • Iron source
• Version or commit hash:
  • 1.2.9
• DDS implementation:
  • CycloneDDS

Steps to reproduce issue

Request a path computation with the goal located on the same costmap cell as the robot.

Expected behavior

The SmacPlanner successfuly returns a path containing either the start pose only, or the start pose and the goal.

Actual behavior

The SmacPlanner fails due to a backtrace error originating from https://github.com/ros-navigation/navigation2/blob/1.2.9/nav2_smac_planner/src/node_hybrid.cpp#L704

Additional information

In this case, the start pose is considered as "being" the goal by the A* planner (https://github.com/ros-navigation/navigation2/blob/1.2.9/nav2_smac_planner/src/a_star.cpp#L308), so the backtrace is triggered but the first pose has no parent, leading to the error. This use case is not common, but I wouldn't expect the planner to fail.

[SteveMacenski]

I think it would be reasonable to bypass all of this around here (https://github.com/ros-navigation/navigation2/blob/1.2.9/nav2_smac_planner/src/smac_planner_hybrid.cpp#L348) by checking if the start cell is the same as the goal cell and returning the single pointed path. There's no real reason to call A* to then run into this.

Can submit a PR to implement for the 3 smac planners?