NullPointer Accessed during findCircumscribedCost calculation of nav2_smac_planner #4468
Description
Bug report
Required Info:
- Operating System:
- ubuntu 22.04
- ROS2 Version:
- humble OR Iron
- Version or commit hash:
- the latest
- DDS implementation:
- the defaulted
Steps to reproduce issue
Launch the navigation2 as following steps:
#!/bin/bash
export ASAN_OPTIONS=halt_on_error=0:new_delete_type_mismatch=0:detect_leaks=0:log_pah=asan
source install/setup.bash
export TURTLEBOT3_MODEL=waffle
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/humble/share/turtlebot3_gazebo/models
ros2 launch nav2_bringup tb3_simulation_launch.py headless:=True use_rviz:=False use_composition:=False using smac_planner as plugin within following configuration:
planner_server:
ros__parameters:
GridBased:
allow_reverse_expansion: false
allow_unknown: true
analytic_expansion_max_cost: 200.0
analytic_expansion_max_cost_override: false
analytic_expansion_max_length: 3.0
analytic_expansion_ratio: 3.5
cache_obstacle_heuristic: false
change_penalty: 0.05
cost_penalty: 2.0
lattice_filepath: /*****/sample_primitives/5cm_resolution/1m_turning_radius/diff/output.json
lookup_table_size: 20.0
max_iterations: 1000000
max_on_approach_iterations: 1000
max_planning_time: 5.0
non_straight_penalty: 1.05
plugin: nav2_smac_planner/SmacPlannerLattice
retrospective_penalty: 0.015
reverse_penalty: 2.0
rotation_penalty: 5.0
smooth_path: true
smoother:
do_refinement: true
max_iterations: 1000
refinement_num: 2
tolerance: 1.0e-10
w_data: 0.2
w_smooth: 0.3
tolerance: 0.25
expected_planner_frequency: 20.0
planner_plugins:
- GridBased
use_sim_time: true
Expected behavior
no bug occured.
Actual behavior
the Asan report of this bug is as following:
=================================================================
==1121394==ERROR: AddressSanitizer: SEGV on unknown address 0x0000000000d8 (pc 0x7da5d89a626b bp 0x61300003f540 sp 0x7da5cf1e5c80 T34)
==1121394==The signal is caused by a READ memory access.
==1121394==Hint: address points to the zero page.
#0 0x7da5d89a626b in nav2_smac_planner::findCircumscribedCost(std::shared_ptr<nav2_costmap_2d::Costmap2DROS>) (/home/*****/nav2_humble/install/nav2_smac_planner/lib/libnav2_smac_planner_lattice.so+0x4226b) (BuildId: ef827f19cc7664c3943561e2a6a07083767758f4)
#1 0x7da5d89a0b0b in nav2_smac_planner::SmacPlannerLattice::createPlan(geometry_msgs::msg::PoseStamped_<std::allocator<void> > const&, geometry_msgs::msg::PoseStamped_<std::allocator<void> > const&) (/home/*****/nav2_humble/install/nav2_smac_planner/lib/libnav2_smac_planner_lattice.so+0x3cb0b) (BuildId: ef827f19cc7664c3943561e2a6a07083767758f4)
#2 0x7da5dfb78cf4 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&) (/home/*****/nav2_humble/install/nav2_planner/lib/libplanner_server_core.so+0x178cf4) (BuildId: 1b8bf26805500b9cf783c2dd56285836f05b374f)
#3 0x7da5dfb6a10d in nav2_planner::PlannerServer::computePlan() (/home/*****/nav2_humble/install/nav2_planner/lib/libplanner_server_core.so+0x16a10d) (BuildId: 1b8bf26805500b9cf783c2dd56285836f05b374f)
#4 0x7da5dfc49963 in nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::work() (/home/*****/nav2_humble/install/nav2_planner/lib/libplanner_server_core.so+0x249963) (BuildId: 1b8bf26805500b9cf783c2dd56285836f05b374f)
#5 0x7da5dfc48cd4 in 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'()> >, void>::operator()() const (/home/*****/nav2_humble/install/nav2_planner/lib/libplanner_server_core.so+0x248cd4) (BuildId: 1b8bf26805500b9cf783c2dd56285836f05b374f)
#6 0x7da5dfc489e7 in std::enable_if<is_invocable_r_v<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'()> >, void>&>, std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> >::type std::__invoke_r<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'()> >, void>&>(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'()> >, void>&) (/home/*****/nav2_humble/install/nav2_planner/lib/libplanner_server_core.so+0x2489e7) (BuildId: 1b8bf26805500b9cf783c2dd56285836f05b374f)
#7 0x7da5dfc48828 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'()> >, void> >::_M_invoke(std::_Any_data const&) (/home/*****/nav2_humble/install/nav2_planner/lib/libplanner_server_core.so+0x248828) (BuildId: 1b8bf26805500b9cf783c2dd56285836f05b374f)
#8 0x7da5dfc483ef 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*) (/home/*****/nav2_humble/install/nav2_planner/lib/libplanner_server_core.so+0x2483ef) (BuildId: 1b8bf26805500b9cf783c2dd56285836f05b374f)
#9 0x7da5dd899ee7 in __pthread_once_slow nptl/./nptl/pthread_once.c:116:7
#10 0x7da5dfc463f1 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'()> >, void>::_M_run() (/home/*****/nav2_humble/install/nav2_planner/lib/libplanner_server_core.so+0x2463f1) (BuildId: 1b8bf26805500b9cf783c2dd56285836f05b374f)
#11 0x7da5ddcdc252 (/lib/x86_64-linux-gnu/libstdc++.so.6+0xdc252) (BuildId: e37fe1a879783838de78cbc8c80621fa685d58a2)
#12 0x7da5dd894ac2 in start_thread nptl/./nptl/pthread_create.c:442:8
#13 0x7da5dd92684f misc/../sysdeps/unix/sysv/linux/x86_64/clone3.S:81
AddressSanitizer can not provide additional info.
SUMMARY: AddressSanitizer: SEGV (/home/*****/nav2_humble/install/nav2_smac_planner/lib/libnav2_smac_planner_lattice.so+0x4226b) (BuildId: ef827f19cc7664c3943561e2a6a07083767758f4) in nav2_smac_planner::findCircumscribedCost(std::shared_ptr<nav2_costmap_2d::Costmap2DROS>)
Thread T34 created by T15 here:
#0 0x572ec3c5e87c in __interceptor_pthread_create (/home/*****/nav2_humble/install/nav2_planner/lib/nav2_planner/planner_server+0x9387c) (BuildId: 191f253724b34c41ec9522f9202cc91f782cabef)
#1 0x7da5ddcdc328 in std::thread::_M_start_thread(std::unique_ptr<std::thread::_State, std::default_delete<std::thread::_State> >, void (*)()) (/lib/x86_64-linux-gnu/libstdc++.so.6+0xdc328) (BuildId: e37fe1a879783838de78cbc8c80621fa685d58a2)
#2 0x7da5dfc45d52 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'()> >, void>::_Async_state_impl<nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose> >)::'lambda'()>(nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose> >)::'lambda'()&&) (/home/*****/nav2_humble/install/nav2_planner/lib/libplanner_server_core.so+0x245d52) (BuildId: 1b8bf26805500b9cf783c2dd56285836f05b374f)
#3 0x7da5dfc454f8 in std::__shared_count<(__gnu_cxx::_Lock_policy)2>::__shared_count<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'()> >, void>, std::allocator<void>, nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose> >)::'lambda'()>(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'()> >, void>*&, std::_Sp_alloc_shared_tag<std::allocator<void> >, nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose> >)::'lambda'()&&) (/home/*****/nav2_humble/install/nav2_planner/lib/libplanner_server_core.so+0x2454f8) (BuildId: 1b8bf26805500b9cf783c2dd56285836f05b374f)
#4 0x7da5dfc436a3 in std::future<std::__invoke_result<std::decay<nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose> >)::'lambda'()>::type>::type> std::async<nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose> >)::'lambda'()>(std::launch, nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose> >)::'lambda'()&&) (/home/*****/nav2_humble/install/nav2_planner/lib/libplanner_server_core.so+0x2436a3) (BuildId: 1b8bf26805500b9cf783c2dd56285836f05b374f)
#5 0x7da5dfc2c3e9 in nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose> >) (/home/*****/nav2_humble/install/nav2_planner/lib/libplanner_server_core.so+0x22c3e9) (BuildId: 1b8bf26805500b9cf783c2dd56285836f05b374f)
#6 0x7da5dfc4e9f7 in void std::__invoke_impl<void, void (nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::*&)(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose> >), nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>*&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose> > >(std::__invoke_memfun_deref, void (nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::*&)(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose> >), nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>*&, std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose> >&&) (/home/*****/nav2_humble/install/nav2_planner/lib/libplanner_server_core.so+0x24e9f7) (BuildId: 1b8bf26805500b9cf783c2dd56285836f05b374f)
#7 0x7da5dfc33f46 in rclcpp_action::Server<nav2_msgs::action::ComputePathToPose>::call_goal_accepted_callback(std::shared_ptr<rcl_action_goal_handle_s>, std::array<unsigned char, 16ul>, std::shared_ptr<void>) (/home/*****/nav2_humble/install/nav2_planner/lib/libplanner_server_core.so+0x233f46) (BuildId: 1b8bf26805500b9cf783c2dd56285836f05b374f)
#8 0x7da5df12e246 in rclcpp_action::ServerBase::execute_goal_request_received(std::shared_ptr<void>&) (/opt/ros/humble/lib/librclcpp_action.so+0x13246) (BuildId: 4dfcc4cee7010878193255b3a622d5194654caa8)
Thread T15 created by T0 here:
#0 0x572ec3c5e87c in __interceptor_pthread_create (/home/*****/nav2_humble/install/nav2_planner/lib/nav2_planner/planner_server+0x9387c) (BuildId: 191f253724b34c41ec9522f9202cc91f782cabef)
#1 0x7da5ddcdc328 in std::thread::_M_start_thread(std::unique_ptr<std::thread::_State, std::default_delete<std::thread::_State> >, void (*)()) (/lib/x86_64-linux-gnu/libstdc++.so.6+0xdc328) (BuildId: e37fe1a879783838de78cbc8c80621fa685d58a2)
#2 0x7da5dfc28004 in nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::SimpleActionServer(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>, std::shared_ptr<rclcpp::node_interfaces::NodeClockInterface>, std::shared_ptr<rclcpp::node_interfaces::NodeLoggingInterface>, std::shared_ptr<rclcpp::node_interfaces::NodeWaitablesInterface>, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::function<void ()>, std::function<void ()>, std::chrono::duration<long, std::ratio<1l, 1000l> >, bool, rcl_action_server_options_s const&) (/home/*****/nav2_humble/install/nav2_planner/lib/libplanner_server_core.so+0x228004) (BuildId: 1b8bf26805500b9cf783c2dd56285836f05b374f)
#3 0x7da5dfc25865 in nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::SimpleActionServer<std::shared_ptr<nav2_util::LifecycleNode> >(std::shared_ptr<nav2_util::LifecycleNode>, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::function<void ()>, std::function<void ()>, std::chrono::duration<long, std::ratio<1l, 1000l> >, bool, rcl_action_server_options_s const&) (/home/*****/nav2_humble/install/nav2_planner/lib/libplanner_server_core.so+0x225865) (BuildId: 1b8bf26805500b9cf783c2dd56285836f05b374f)
#4 0x7da5dfb69408 in std::__detail::_MakeUniq<nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose> >::__single_object std::make_unique<nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>, std::shared_ptr<nav2_util::LifecycleNode>, char const (&) [21], std::_Bind<void (nav2_planner::PlannerServer::* (nav2_planner::PlannerServer*))()>, std::nullptr_t, std::chrono::duration<long, std::ratio<1l, 1000l> >, bool>(std::shared_ptr<nav2_util::LifecycleNode>&&, char const (&) [21], std::_Bind<void (nav2_planner::PlannerServer::* (nav2_planner::PlannerServer*))()>&&, std::nullptr_t&&, std::chrono::duration<long, std::ratio<1l, 1000l> >&&, bool&&) (/home/*****/nav2_humble/install/nav2_planner/lib/libplanner_server_core.so+0x169408) (BuildId: 1b8bf26805500b9cf783c2dd56285836f05b374f)
#5 0x7da5dfb63ee4 in nav2_planner::PlannerServer::on_configure(rclcpp_lifecycle::State const&) (/home/*****/nav2_humble/install/nav2_planner/lib/libplanner_server_core.so+0x163ee4) (BuildId: 1b8bf26805500b9cf783c2dd56285836f05b374f)
#6 0x7da5def2b8ec (/opt/ros/humble/lib/librclcpp_lifecycle.so+0x288ec) (BuildId: 97f6428dc1ee45fd402b522b3b8e6b4fcfeabe76)
==1121394==ABORTING
Additional information
It's a shutdown-issue
First, based on my execution logs, I can confirm this is a shutdown issue.
It's additional tickets related to #4463, which behaviors in nav2_planner
Below is an analysis of the cause of this bug:
The action_server_ binds the nav2_planner::PlannerServer::computePlan() function as a callback function,
and using the nav2_smac_planner::findCircumscribedCost , which may access the early shutdown costmap_ros_: