Planner Server Crash When Using ThetaStarPlanner Near Map Edges #4694
Description
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
