planner_server segmentation fault #3528
Description
Bug report
Required Info:
- Operating System:
- Ubuntu 22.04
- ROS2 Version:
- Humble
- Version or commit hash:
- For the smac planner I built it separately to debug. I used the latest state of the humble branch: 4b9bd86
- The other packages were on 1.1.6-1jammy.20230327
- DDS implementation:
- Fast DDS
Steps to reproduce issue
I wanted to experiment with the Smac State Lattice planner in simulation because it could not generate plans for trivial cases on my real robot (a forklift).
So for testing, I used the turtlebot3 and a smallish warehouse environment as seen in the picture below. I changed the footprint, the controller and planner of the turtlebot config to match my real robot. During testing, I had segmentation faults before so I used gdb to catch it if it comes up again.
The backtrace:
#0 0x00007ffff041d152 in nav2_smac_planner::GridCollisionChecker::inCollision (this=this@entry=0x555556207800, x=@0x7fffba7f64b8: 180, y=@0x7fffba7f64bc: 350, angle_bin=@0x7fffba7f64b4: 72,
traverse_unknown=@0x555556251504: false) at /home/levente/tmp/sandbox_ws/src/turtlebot3/src/nav2_smac_planner/src/collision_checker.cpp:128
#1 0x00007ffff0427ecf in nav2_smac_planner::NodeLattice::isNodeValid (this=0x7fffac1e2380, traverse_unknown=@0x555556251504: false, collision_checker=collision_checker@entry=0x555556207800,
motion_primitive=0x7fffac066da0, is_backwards=<optimized out>) at /home/levente/tmp/sandbox_ws/src/turtlebot3/src/nav2_smac_planner/src/node_lattice.cpp:253
#2 0x00007ffff04293ee in nav2_smac_planner::NodeLattice::getNeighbors(std::function<bool (unsigned int const&, nav2_smac_planner::NodeLattice*&)>&, nav2_smac_planner::GridCollisionChecker*, bool const&, std::vector<nav2_smac_planner::NodeLattice*, std::allocator<nav2_smac_planner::NodeLattice*> >&) (this=0x7fffac02dcb0, NeighborGetter=..., collision_checker=0x555556207800,
traverse_unknown=@0x555556251504: false, neighbors=std::vector of length 12, capacity 16 = {...}) at /usr/include/c++/11/bits/stl_vector.h:1043
#3 0x00007ffff04109e2 in nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeLattice>::createPath (this=this@entry=0x555556251500, path=std::vector of length 0, capacity 0,
iterations=@0x7fffba7f682c: 155, tolerance=@0x7fffba7f6838: 5) at /home/levente/tmp/sandbox_ws/src/turtlebot3/src/nav2_smac_planner/src/a_star.cpp:289
#4 0x00007ffff03f8f06 in nav2_smac_planner::SmacPlannerLattice::createPlan (this=0x5555562077f0, start=..., goal=...)
at /home/levente/tmp/sandbox_ws/src/turtlebot3/src/nav2_smac_planner/src/smac_planner_lattice.cpp:278
#5 0x00007ffff7f5e063 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 /opt/ros/humble/lib/libplanner_server_core.so
#6 0x00007ffff7f62da3 in nav2_planner::PlannerServer::computePlan() () from /opt/ros/humble/lib/libplanner_server_core.so
#7 0x00007ffff7f739dc in ?? () from /opt/ros/humble/lib/libplanner_server_core.so
#8 0x00007ffff7f8611c 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 /opt/ros/humble/lib/libplanner_server_core.so
#9 0x00007ffff7f54f9d 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 /opt/ros/humble/lib/libplanner_server_core.so
#10 0x00007ffff7699f68 in __pthread_once_slow (once_control=0x7fffb4001d18, init_routine=0x7ffff7adadb0 <__once_proxy>) at ./nptl/pthread_once.c:116
#11 0x00007ffff7f88d6a in 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>::_M_run() () from /opt/ros/humble/lib/libplanner_server_core.so
#12 0x00007ffff7adc2b3 in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#13 0x00007ffff7694b43 in start_thread (arg=<optimized out>) at ./nptl/pthread_create.c:442
#14 0x00007ffff7726a00 in clone3 () at ../sysdeps/unix/sysv/linux/x86_64/clone3.S:81
Relevant config:
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_link
use_sim_time: True
robot_radius: 0.22
footprint: "[ [-0.521, -0.3775], [1.731, -0.3775], [ 1.731, 0.7725], [-0.521, 0.7725] ]"
resolution: 0.05
track_unknown_space: true
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 3.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 2.0 # 3.0
inflation_radius: 3.5 # 0.55
always_send_full_costmap: True
planner_server:
ros__parameters:
expected_planner_frequency: 0.2
use_sim_time: True
planner_plugins: ["GridBased"]
GridBased:
downsample_costmap: false
downsampling_factor: 1
plugin: "nav2_smac_planner/SmacPlannerLattice"
allow_unknown: false
tolerance: 0.25
max_iterations: 1000000
max_on_approach_iterations: 1000
max_planning_time: 5.0
analytic_expansion_ratio: 3.5
analytic_expansion_max_length: 3.0
reverse_penalty: 2.0
change_penalty: 0.05
non_straight_penalty: 1.05
cost_penalty: 2.0
rotation_penalty: 5.0
retrospective_penalty: 0.015
lattice_filepath: "/home/levente/tmp/sandbox_ws/src/turtlebot3/src/nav2_smac_planner/lattice_primitives/output.json"
cache_obstacle_heuristic: True
allow_reverse_expansion: True
smooth_path: True
smoother:
max_iterations: 1000
w_smooth: 0.3
w_data: 0.2
tolerance: 0.0000000001 # 1.0e-10
do_refinement: true
refinement_num: 2
{
"motion_model": "diff",
"turning_radius": 1.0,
"grid_resolution": 0.05,
"stopping_threshold": 5,
"num_of_headings": 16
}
Picture of the situation when it happened:
I wanted to send the robot where my cursor was. (I visualized both the local and global footprint.)
I don't know if it is related or not but when it could not produce a plan in similar circumstances I get the following exception:
[planner_server-10] [WARN] [1680885924.574350816] [planner_server]: GridBased plugin failed to plan calculation to (2.75, 7.60): "vector::reserve"
[planner_server-10] [WARN] [1680885924.574454251] [planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
Expected behavior
Create a plan to the goal.
Actual behavior
Segmentation fault or exception with vector::reserve.