Planner Server Crash When Using ThetaStarPlanner Near Map Edges

[JeisonUR]

Hello I am launching nav2 on a small test map, with the default configuration; the only change is that I am using the ThetaStarPlanner. When using the action to try to navigate to points near the edge of the map, the planner server is crashing.

Bug report

Required Info:

• Operating System: Ubuntu 20.04.6 LTS

• ROS2 Version: humble

• Version or commit hash: ros-humble-navigation2 1.1.12-1focal.20231230.114340

• DDS implementation: CycloneDDS

Steps to reproduce issue

• Modify the nav2 configuration file to use ThetaStarPlanner

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    use_sim_time: False
    planner_plugins: ["GridBased", "LongFootprint"]
    GridBased:
      plugin: "nav2_theta_star_planner/ThetaStarPlanner"
      how_many_corners: 8
      w_euc_cost: 1.0
      w_traversal_cost: 2.0
      w_heuristic_cost: 1.0

• Launch nav2, set the attached map (the map.yaml is attached as map.txt and the map.pgm as map.png)
• Use the action navigate_to_pose

ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "{pose: {header: {frame_id: "map"}, pose: {position: {x: -5.69, y: 3.72}} }}"

• With the coordinate sent in the command, we have been able to replicate the problem 10% of the time; in other coordinates near the edge of the map, it occurs but with less frequency.
• The result of a debugging session is attached, where it is found that the error is in the implementation of the getCost and isSafe functions of Costmap2D https://github.com/ros-navigation/navigation2/blob/main/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp#L222, below is presented the backtrace of functions called before of getting the segmentation fault

  Thread 2 "planner_server" received signal SIGSEGV, Segmentation fault.
  [Switching to Thread 1034669.1196420]
  0x0000fffff7673194 in nav2_costmap_2d::Costmap2D::getCost (this=<optimized out>, mx=4098453552, my=<optimized out>) at ./include/nav2_costmap_2d/costmap_2d.hpp:214
  214 ./include/nav2_costmap_2d/costmap_2d.hpp: No such file or directory.
  (gdb) bt
  #0  0x0000fffff7673194 in nav2_costmap_2d::Costmap2D::getCost (this=<optimized out>, 
  mx=4098453552, my=<optimized out>) at ./include/nav2_costmap_2d/costmap_2d.hpp:214
  #1  0x0000fffff447f9c0 in theta_star::ThetaStar::isSafe (this=0xaaaaaae884c0, 
  this=0xaaaaaae884c0, cy=@0xaaaaaae884cc: 65535, cx=@0xaaaaaae884c8: -196513744)
  at ./include/nav2_theta_star_planner/theta_star.hpp:98
  #2  theta_star::ThetaStar::isUnsafeToPlan (this=0xaaaaaae884c0)
  at ./include/nav2_theta_star_planner/theta_star.hpp:116
  #3  nav2_theta_star_planner::ThetaStarPlanner::getPlan (this=this@entry=0xaaaaaae8cc10, 
  global_path=...) at ./src/theta_star_planner.cpp:160
  #4  0x0000fffff447fffc in nav2_theta_star_planner::ThetaStarPlanner::createPlan (
  --Type <RET> for more, q to quit, c to continue without paging--c
  aae8cc10, start=..., goal=...) at ./src/theta_star_planner.cpp:124
  #5  0x0000fffff7efc7b8 in nav2_planner::PlannerServer::getPlan(geometry_msgs::msg::PoseStamped_<std::allocator<void> > const&, geometry_msgs::msg::PoseStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) () from target:/opt/ros/humble/lib/libplanner_server_core.so
  #6  0x0000fffff7f026c8 in nav2_planner::PlannerServer::computePlan() () from target:/opt/ros/humble/lib/libplanner_server_core.so
  #7  0x0000fffff7f29058 in nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::work() () from target:/opt/ros/humble/lib/libplanner_server_core.so
  #8  0x0000fffff7f2a8a0 in std::_Function_handler<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> (), std::__future_base::_Task_setter<std::unique_ptr<std::__future_base::_Result<void>, std::__future_base::_Result_base::_Deleter>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose> >)::{lambda()#1}> >, void> >::_M_invoke(std::_Any_data const&) () from target:/opt/ros/humble/lib/libplanner_server_core.so
  #9  0x0000fffff7f06e1c in std::__future_base::_State_baseV2::_M_do_set(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> ()>*, bool*) () from target:/opt/ros/humble/lib/libplanner_server_core.so
  #10 0x0000fffff778b3b8 in __pthread_once_slow () from target:/lib/aarch64-linux-gnu/libpthread.so.0
  #11 0x0000fffff7f0cd8c in std::thread::_State_impl<std::thread::_Invoker<std::tuple<std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose> >)::{lambda()#1}> >, void>::_Async_state_impl(std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose> >)::{lambda()#1}>&&)::{lambda()#1}> > >::_M_run() () from target:/opt/ros/humble/lib/libplanner_server_core.so
  #12 0x0000fffff7a23f9c in ?? () from target:/lib/aarch64-linux-gnu/libstdc++.so.6
  #13 0x0000fffff7782624 in start_thread () from target:/lib/aarch64-linux-gnu/libpthread.so.0
  #14 0x0000fffff789149c in ?? () from target:/lib/aarch64-linux-gnu/libc.so.6

Expected behavior

• The planner is unable to compute a path to the goal, the planner server returns failure

  
  [bt_navigator-7] [INFO] [1727369934.201440751] [gary.nav.nav2.bt_navigator]: Begin navigating from current location (-1.77, 0.34) to (-5.69, 3.72)
  [planner_server-5] [ERROR] [1727369934.845930189] [gary.nav.nav2.planner_server]: Could not generate path between the given poses
  [WARN] [1727369934.846080943] [gary.nav.nav2.planner_server]: Planning algorithm GridBased failed to generate a valid path to (-5.69, 3.72)
  [planner_server-5] [WARN] [1727369934.846129680] [gary.nav.nav2.planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
  [bt_navigator-7] [WARN] [1727369934.861850871] [gary.nav.nav2.bt_navigator_navigate_to_pose_rclcpp_node]: COMPUTE PATH ABORTED
  [bt_navigator-7] [ERROR] [1727369934.872160633] [gary.nav.nav2.bt_navigator]: Goal failed

#### Actual behavior

planner server crash

[bt_navigator-7] [INFO] [1727370008.710462882] [gary.nav.nav2.bt_navigator]: Begin navigating from current location (-1.77, 0.34) to (-5.69, 3.72) [LAUNCH ERROR] [planner_server-5.launch]: process has died [pid 1631378, exit code -11, cmd '/opt/ros/humble/lib/nav2_planner/planner_server --ros-args -r __node:=planner_server -r __ns:=/gary/nav/nav2 --params-file /tmp/tmp4k0ocqsj -r /tf:=/tf -r /tf_static:=/tf_static -r /gary/nav/nav2/cmd_vel:=/gary/wheels/cmd_vel -r /gary/nav/nav2/odom:=/gary/odom/odom'].

#### Additional information

[map.txt](https://github.com/user-attachments/files/17155358/map.txt)

![map](https://github.com/user-attachments/assets/e473980a-15ce-48a1-8708-143d0e5a4847)

[SteveMacenski]

The map you uploaded is corrupted and I can't view it. Could that be related? Please fork https://github.com/botsandus/planner_playground and add your map + start/goal/map as a reproducible example with your configurations - then comment back once you have a reproducable example. Please also provide an image of the map/start/goal that creates this issue so I can see how close to the edge "close" means.

This is a weird error to see - is there something odd about your install or configuration of your computer? Does this occur using the global_costmap's default parameter / footprint set? Curious if this relates to your specific robot config as well.

./include/nav2_costmap_2d/costmap_2d.hpp: No such file or directory.

This isn't exactly an "I'm out of bounds" or other normal crash. This seems like a compilation issue. Does this work for other non-edge requests?

[brayanpa]

Hi Steve, sorry for the misunderstanding. We missed the initial part of the error message. The error referring to the missing costmap_2d.hpp is because gdb can't find the file during debugging, but that doesn't affect the library execution. The original error is a segmentation fault. We just updated the original issue to add the missing part of the error (first lines).

We have forked the planner_playground repo and have been able to replicate the error there. Here is our fork:

https://github.com/brayanpa/planner_playground

We launch it with:

ros2 launch planner_playground test_planner.launch.py

and set the start and goal points as:

ros2 topic pub --once /goal_pose geometry_msgs/msg/PoseStamped "{header: { frame_id: map}, pose: {position: {x: 1.22, y: -3.81}}}"

ros2 topic pub --once /goal_pose geometry_msgs/msg/PoseStamped "{header: { frame_id: map}, pose: {position: {x: -6.1, y: -4.67}}}"

As a result, the planner_server crashed again.

[lifecycle_manager-7] [INFO] [1727387190.917720018] [lifecycle_manager_navigation]: Managed nodes are active
[rviz2-9] [INFO] [1727387191.152310412] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-9] [INFO] [1727387191.152396343] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-9] [INFO] [1727387191.172072072] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-9] [INFO] [1727387191.271659887] [rviz2]: Trying to create a map of size 483 x 627 using 1 swatches
[rviz2-9] [INFO] [1727387192.100608204] [rviz2]: Trying to create a map of size 483 x 627 using 1 swatches
[planner_server-6] [WARN] [1727387208.421591443] [planner_server]: No planners specified in action call. Server will use only plugin GridBased  in server. This warning will appear once.
[ERROR] [planner_server-6]: process has died [pid 146773, exit code -11, cmd '/opt/ros/humble/lib/nav2_planner/planner_server --ros-args --log-level info --ros-args -r __node:=planner_server -p use_sim_time:=False --params-file /home/brayanur/nav2_tools/install/planner_playground/share/planner_playground/config/planner_server.yaml --params-file /home/brayanur/nav2_tools/install/planner_playground/share/planner_playground/config/global_costmap.yaml --params-file /home/brayanur/nav2_tools/install/planner_playground/share/planner_playground/config/global_costmap_test.yaml --params-file /home/brayanur/nav2_tools/install/planner_playground/share/planner_playground/config/costmaps_plugins.yaml --params-file /tmp/launch_params_ci5du8el -r /map:=/map_amcl'].

The same can be replicated by setting a goal in rviz outside of the map size.

[SteveMacenski]

Is the case that you were describing near the edge doing footprint collision checking where part of the footprint is outside of the costmap bounds? Or when you say "close" you mean just over the edge for point-checks that are done when using robot_radius rather than footprint

It seems like that may be the case, in which case we can do a check in the collision checker in Theta such that we reject plans off the map. We do this in other planning algorithms but perhaps we don't check the return value on worldToMap in Theta to handle it.

[SteveMacenski]

Ah, I see that we do check the worldToMap in main https://github.com/ros-navigation/navigation2/blob/main/nav2_theta_star_planner/src/theta_star_planner.cpp#L120-L126 but not in Humble's branch. Likely the PR that added that was not backward compatible with Humble so it wasn't backported. The throw is handled by Iron and newer when we added the contextual error codes for when algorithms fail.

I suppose we could just return an empty path in this case for humble and this condition should handle that situation properly

What do you think? (I didn't test your case yet, just looking into this from your descriptions, this seems like a very plausible explanation)

[brayanpa]

Reviewing the situation described at the beginning of the issue, the goal was slightly outside the map—by 1 pixel, to be exact. It seems that goals within the map are being handled correctly, even when part of the footprint extends beyond the map.

In our case, receiving an empty path would be sufficient to handle the situation.

[SteveMacenski]

Great! Can you open a PR to add in the worldToMap check on the output and return an empty path if that check fails - targeting the humble branch? Please also test that this resolves your issue completely :-)

[SteveMacenski]

@brayanpa any update? I can make the simple change, but I'd still need you to test in your cases to make sure that is the only issue you're running into

[brayanpa]

Yes, we are waiting to run the appropriate tests in our environments and ensure that the issue is fully resolved. You will definitely have the pull request by the end of this week at the latest.

[brayanpa]

@SteveMacenski We have opened PR #4706. The fix was simple, and we have tested it both in the planner playground and on our robot (Gary). The issue is completely resolved.

[SteveMacenski]

Thanks for the report and PR, merged!