SmacHybrid planner can crash to segmentation fault #4141
Description
Bug report
Required Info:
- Operating System:
- Ubuntu 22.04
- ROS2 Version:
- Humble (latest ros:humble container)
- Version or commit hash:
- a3cdbbf + minimal changes to make the rolling version of nav2 to work on Humble (no changes to planner(s) code)
- DDS implementation:
- CycloneDDS
Steps to reproduce issue
This part I am still uncertain. The issue keep happening seemingly randomly during navigation. Have not been able to pinpoint the exact reason/case as I do not yet fully understand what is going on in the SmacHybridPlanner. Sometimes the segmentation fault happens quite soon after starting navigating. Sometimes it takes many many navigations. (Had my simulation giving consecutive goals for 95 minutes before I got the latest seg fault for the log to include in this issue.)
Expected behavior
Planner keeps running 😁
Actual behavior
Planner server crashes to a segmentation fault.
Log
// Logs I have added
[planner_server-2] D: 1.22173 is smaller than sqrt(2)
[planner_server-2] From x: 603.254 From y: 180.122 From yaw: 5.67232
[planner_server-2] To x: 604 To y: 180 To yaw: 0
// My logs end
[planner_server-2] Stack trace (most recent call last) in thread 1430:
[planner_server-2] #31 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7ffaaf4aaee7, in
[planner_server-2] #30 Source "/usr/include/c++/11/mutex", line 712, in _Prepare_execution<std::call_once<void (std::__future_base::_State_baseV2::)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter>()>, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter>()>, bool>(std::once_flag&, void (std::__future_base::_State_baseV2::&&)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter>()>, bool*), std::__future_base::_State_baseV2*&&, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter>()>&&, bool&&)::<lambda()> > [0x7ffaafe2eb2b]
[planner_server-2] 709: // Store address in thread-local pointer:
[planner_server-2] 710: __once_callable = std::__addressof(__c);
[planner_server-2] 711: // Trampoline function to invoke the closure via thread-local pointer:
[planner_server-2] > 712: __once_call = [] { (static_cast<_Callable>(__once_callable))(); };
[planner_server-2] 713: }
[planner_server-2] 714:
[planner_server-2] 715: ~_Prepare_execution()
[planner_server-2] #29 Source "/usr/include/c++/11/mutex", line 712, in operator() [0x7ffaafe2eb16]
[planner_server-2] 709: // Store address in thread-local pointer:
[planner_server-2] 710: __once_callable = std::__addressof(__c);
[planner_server-2] 711: // Trampoline function to invoke the closure via thread-local pointer:
[planner_server-2] > 712: __once_call = [] { (static_cast<_Callable>(__once_callable))(); };
[planner_server-2] 713: }
[planner_server-2] 714:
[planner_server-2] 715: ~_Prepare_execution()
[planner_server-2] #28 Source "/usr/include/c++/11/mutex", line 776, in operator() [0x7ffaafe22ddd]
[planner_server-2] 773: {
[planner_server-2] 774: // Closure type that runs the function
[planner_server-2] 775: auto __callable = [&] {
[planner_server-2] > 776: std::__invoke(std::forward<_Callable>(__f),
[planner_server-2] 777: std::forward<_Args>(__args)...);
[planner_server-2] 778: };
[planner_server-2] #27 Source "/usr/include/c++/11/bits/invoke.h", line 96, in __invoke<void (std::__future_base::_State_baseV2::)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter>()>, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter>()>, bool> [0x7ffaafe2eade]
[planner_server-2] 93: using __result = __invoke_result<_Callable, _Args...>;
[planner_server-2] 94: using __type = typename __result::type;
[planner_server-2] 95: using __tag = typename __result::__invoke_type;
[planner_server-2] > 96: return std::__invoke_impl<__type>(__tag{}, std::forward<_Callable>(__fn),
[planner_server-2] 97: std::forward<_Args>(__args)...);
[planner_server-2] 98: }
[planner_server-2] #26 Source "/usr/include/c++/11/bits/invoke.h", line 74, in __invoke_impl<void, void (std::__future_base::_State_baseV2::)(std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter>()>, bool*), std::__future_base::_State_baseV2*, std::function<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter>()>, bool> [0x7ffaafe3abcc]
[planner_server-2] 71: __invoke_impl(__invoke_memfun_deref, _MemFun&& __f, _Tp&& __t,
[planner_server-2] 72: _Args&&... __args)
[planner_server-2] 73: {
[planner_server-2] > 74: return ((std::forward<_Tp>(__t)).__f)(std::forward<_Args>(__args)...);
[planner_server-2] 75: }
[planner_server-2] 76:
[planner_server-2] 77: template<typename _Res, typename _MemPtr, typename _Tp>
[planner_server-2] #25 Source "/usr/include/c++/11/future", line 571, in _M_do_set [0x7ffaafe1bc15]
[planner_server-2] 568: void
[planner_server-2] 569: _M_do_set(function<_Ptr_type()>* __f, bool* __did_set)
[planner_server-2] 570: {
[planner_server-2] > 571: _Ptr_type __res = (__f)();
[planner_server-2] 572: // Notify the caller that we did try to set; if we do not throw an
[planner_server-2] 573: // exception, the caller will be aware that it did set (e.g., see
[planner_server-2] 574: // _M_set_result).
[planner_server-2] #24 Source "/usr/include/c++/11/bits/std_function.h", line 590, in operator() [0x7ffaafe23081]
[planner_server-2] 587: {
[planner_server-2] 588: if (_M_empty())
[planner_server-2] 589: __throw_bad_function_call();
[planner_server-2] > 590: return _M_invoker(_M_functor, std::forward<_ArgTypes>(__args)...);
[planner_server-2] 591: }
[planner_server-2] 592:
[planner_server-2] 593: #if __cpp_rtti
[planner_server-2] #23 Source "/usr/include/c++/11/bits/std_function.h", line 291, in _M_invoke [0x7ffaafea9ea1]
[planner_server-2] 288: _M_invoke(const _Any_data& __functor, _ArgTypes&&... __args)
[planner_server-2] 289: {
[planner_server-2] 290: return std::__invoke_r<_Res>(_Base::_M_get_pointer(__functor),
[planner_server-2] > 291: std::forward<_ArgTypes>(__args)...);
[planner_server-2] 292: }
[planner_server-2] 293:
[planner_server-2] 294: template
[planner_server-2] #22 Source "/usr/include/c++/11/bits/invoke.h", line 116, in __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, 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>&> [0x7ffaafeabd6a]
[planner_server-2] 113: else
[planner_server-2] 114: return std::__invoke_impl<__type>(__tag{},
[planner_server-2] 115: std::forward<_Callable>(__fn),
[planner_server-2] > 116: std::forward<_Args>(__args)...);
[planner_server-2] 117: }
[planner_server-2] 118: #else // C++11
[planner_server-2] 119: template<typename _Res, typename _Callable, typename... _Args>
[planner_server-2] #21 Source "/usr/include/c++/11/bits/invoke.h", line 61, in __invoke_impl<std::unique_ptr<std::__future_base::_Result, std::__future_base::_Result_base::_Deleter>, std::__future_base::_Task_setter<std::unique_ptr<std::__future_base::_Result, 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>&> [0x7ffaafead04a]
[planner_server-2] 58: template<typename _Res, typename _Fn, typename... _Args>
[planner_server-2] 59: constexpr _Res
[planner_server-2] 60: __invoke_impl(__invoke_other, _Fn&& __f, _Args&&... __args)
[planner_server-2] > 61: { return std::forward<_Fn>(__f)(std::forward<_Args>(__args)...); }
[planner_server-2] 62:
[planner_server-2] 63: template<typename _Res, typename _MemFun, typename _Tp, typename... _Args>
[planner_server-2] 64: constexpr _Res
[planner_server-2] #20 Source "/usr/include/c++/11/future", line 1409, in operator() [0x7ffaafeae77d]
[planner_server-2] 1406: {
[planner_server-2] 1407: __try
[planner_server-2] 1408: {
[planner_server-2] >1409: (_M_fn)();
[planner_server-2] 1410: }
[planner_server-2] 1411: __catch(const __cxxabiv1::__forced_unwind&)
[planner_server-2] 1412: {
[planner_server-2] #19 Source "/usr/include/c++/11/bits/std_thread.h", line 266, in operator() [0x7ffaafeafe23]
[planner_server-2] 263: {
[planner_server-2] 264: using _Indices
[planner_server-2] 265: = typename _Build_index_tuple<tuple_size<_Tuple>::value>::__type;
[planner_server-2] > 266: return _M_invoke(_Indices());
[planner_server-2] 267: }
[planner_server-2] 268: };
[planner_server-2] #18 Source "/usr/include/c++/11/bits/std_thread.h", line 259, in _M_invoke<0> [0x7ffaafeb0ce3]
[planner_server-2] 256: template<size_t... _Ind>
[planner_server-2] 257: typename __result<_Tuple>::type
[planner_server-2] 258: _M_invoke(_Index_tuple<_Ind...>)
[planner_server-2] > 259: { return std::__invoke(std::get<_Ind>(std::move(_M_t))...); }
[planner_server-2] 260:
[planner_server-2] 261: typename __result<_Tuple>::type
[planner_server-2] 262: operator()()
[planner_server-2] #17 Source "/usr/include/c++/11/bits/invoke.h", line 96, in __invoke<nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose> >)::<lambda()> > [0x7ffaafeb1b04]
[planner_server-2] 93: using __result = __invoke_result<_Callable, _Args...>;
[planner_server-2] 94: using __type = typename __result::type;
[planner_server-2] 95: using __tag = typename __result::__invoke_type;
[planner_server-2] > 96: return std::__invoke_impl<__type>(__tag{}, std::forward<_Callable>(__fn),
[planner_server-2] 97: std::forward<_Args>(__args)...);
[planner_server-2] 98: }
[planner_server-2] #16 Source "/usr/include/c++/11/bits/invoke.h", line 61, in __invoke_impl<void, nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose> >)::<lambda()> > [0x7ffaafeb2c76]
[planner_server-2] 58: template<typename _Res, typename _Fn, typename... _Args>
[planner_server-2] 59: constexpr _Res
[planner_server-2] 60: __invoke_impl(__invoke_other, _Fn&& __f, _Args&&... __args)
[planner_server-2] > 61: { return std::forward<_Fn>(__f)(std::forward<_Args>(__args)...); }
[planner_server-2] 62:
[planner_server-2] 63: template<typename Res, typename MemFun, typename Tp, typename... Args>
[planner_server-2] 64: constexpr Res
[planner_server-2] #15 Source "/nav2_ws/install/nav2_util/include/nav2_util/simple_action_server.hpp", line 228, in handle_accepted [0x7ffaafe4d64d]
[planner_server-2] 225: execution_future = std::async(
[planner_server-2] 226: std::launch::async, this {
[planner_server-2] 227: setSoftRealTimePriority();
[planner_server-2] > 228: work();
[planner_server-2] 229: });
[planner_server-2] 230: }
[planner_server-2] 231: }
[planner_server-2] #14 Source "/nav2_ws/install/nav2_util/include/nav2_util/simple_action_server.hpp", line 241, in work [0x7ffaafe59d73]
[planner_server-2] 238: while (rclcpp::ok() && !stop_execution && is_active(current_handle)) {
[planner_server-2] 239: debug_msg("Executing the goal...");
[planner_server-2] 240: try {
[planner_server-2] > 241: execute_callback();
[planner_server-2] 242: } catch (std::exception & ex) {
[planner_server-2] 243: RCLCPP_ERROR(
[planner_server-2] 244: node_logging_interface->get_logger(),
[planner_server-2] #13 Source "/usr/include/c++/11/bits/std_function.h", line 590, in operator() [0x7ffaafe258df]
[planner_server-2] 587: {
[planner_server-2] 588: if (_M_empty())
[planner_server-2] 589: __throw_bad_function_call();
[planner_server-2] > 590: return _M_invoker(_M_functor, std::forward<_ArgTypes>(__args)...);
[planner_server-2] 591: }
[planner_server-2] 592:
[planner_server-2] 593: #if __cpp_rtti
[planner_server-2] #12 Source "/usr/include/c++/11/bits/std_function.h", line 290, in _M_invoke [0x7ffaafe415ce]
[planner_server-2] 287: static _Res
[planner_server-2] 288: _M_invoke(const _Any_data& __functor, _ArgTypes&&... __args)
[planner_server-2] 289: {
[planner_server-2] > 290: return std::__invoke_r<_Res>(_Base::_M_get_pointer(__functor),
[planner_server-2] 291: std::forward<_ArgTypes>(__args)...);
[planner_server-2] 292: }
[planner_server-2] #11 Source "/usr/include/c++/11/bits/invoke.h", line 111, in __invoke_r<void, std::_Bind<void (nav2_planner::PlannerServer::(nav2_planner::PlannerServer))()>&> [0x7ffaafe4d10b]
[planner_server-2] 108: using __type = typename __result::type;
[planner_server-2] 109: using __tag = typename __result::__invoke_type;
[planner_server-2] 110: if constexpr (is_void_v<_Res>)
[planner_server-2] > 111: std::__invoke_impl<__type>(__tag{}, std::forward<_Callable>(__fn),
[planner_server-2] 112: std::forward<_Args>(__args)...);
[planner_server-2] 113: else
[planner_server-2] 114: return std::__invoke_impl<__type>(__tag{},
[planner_server-2] #10 Source "/usr/include/c++/11/bits/invoke.h", line 61, in __invoke_impl<void, std::_Bind<void (nav2_planner::PlannerServer::(nav2_planner::PlannerServer))()>&> [0x7ffaafe5995d]
[planner_server-2] 58: template<typename _Res, typename _Fn, typename... _Args>
[planner_server-2] 59: constexpr _Res
[planner_server-2] 60: __invoke_impl(__invoke_other, _Fn&& __f, _Args&&... __args)
[planner_server-2] > 61: { return std::forward<_Fn>(__f)(std::forward<_Args>(__args)...); }
[planner_server-2] 62:
[planner_server-2] 63: template<typename _Res, typename _MemFun, typename _Tp, typename... _Args>
[planner_server-2] 64: constexpr _Res
[planner_server-2] #9 Source "/usr/include/c++/11/functional", line 503, in operator()<> [0x7ffaafe657d2]
[planner_server-2] 500: _Result
[planner_server-2] 501: operator()(_Args&&... __args)
[planner_server-2] 502: {
[planner_server-2] > 503: return this->__call<_Result>(
[planner_server-2] 504: std::forward_as_tuple(std::forward<_Args>(__args)...),
[planner_server-2] 505: _Bound_indexes());
[planner_server-2] 506: }
[planner_server-2] #8 Source "/usr/include/c++/11/functional", line 420, in __call<void, 0> [0x7ffaafe6dff8]
[planner_server-2] 417: _Result
[planner_server-2] 418: __call(tuple<_Args...>&& __args, _Index_tuple<_Indexes...>)
[planner_server-2] 419: {
[planner_server-2] > 420: return std::__invoke(_M_f,
[planner_server-2] 421: _Mu<_Bound_args>()(std::get<_Indexes>(_M_bound_args), __args)...
[planner_server-2] 422: );
[planner_server-2] 423: }
[planner_server-2] #7 Source "/usr/include/c++/11/bits/invoke.h", line 96, in __invoke<void (nav2_planner::PlannerServer::&)(), nav2_planner::PlannerServer&> [0x7ffaafe7695a]
[planner_server-2] 93: using __result = __invoke_result<_Callable, _Args...>;
[planner_server-2] 94: using __type = typename __result::type;
[planner_server-2] 95: using __tag = typename __result::__invoke_type;
[planner_server-2] > 96: return std::__invoke_impl<__type>(__tag{}, std::forward<_Callable>(__fn),
[planner_server-2] 97: std::forward<_Args>(__args)...);
[planner_server-2] 98: }
[planner_server-2] #6 Source "/usr/include/c++/11/bits/invoke.h", line 74, in __invoke_impl<void, void (nav2_planner::PlannerServer::&)(), nav2_planner::PlannerServer&> [0x7ffaafe80046]
[planner_server-2] 71: __invoke_impl(__invoke_memfun_deref, _MemFun&& __f, _Tp&& __t,
[planner_server-2] 72: _Args&&... __args)
[planner_server-2] 73: {
[planner_server-2] > 74: return ((std::forward<_Tp>(__t)).__f)(std::forward<_Args>(_args)...);
[planner_server-2] 75: }
[planner_server-2] 76:
[planner_server-2] 77: template<typename Res, typename MemPtr, typename Tp>
[planner_server-2] #5 Source "/nav2_ws/src/navigation2/nav2_planner/src/planner_server.cpp", line 519, in computePlan [0x7ffaafe178dc]
[planner_server-2] 516: throw nav2_core::PlannerTFError("Unable to transform poses to global frame");
[planner_server-2] 517: }
[planner_server-2] 518:
[planner_server-2] > 519: result->path = getPlan(start, goal_pose, goal->planner_id);
[planner_server-2] 520:
[planner_server-2] 521: if (!validatePath(goal_pose, result->path, goal->planner_id)) {
[planner_server-2] 522: throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path");
[planner_server-2] #4 Source "/karelics_workspace/karelics_brain/nav2_ws/src/navigation2/nav2_planner/src/planner_server.cpp", line 589, in getPlan [0x7ffaafe18b8f]
[planner_server-2] 586: goal.pose.position.x, goal.pose.position.y);
[planner_server-2] 587:
[planner_server-2] 588: if (planners.find(planner_id) != planners.end()) {
[planner_server-2] > 589: return planners[planner_id]->createPlan(start, goal);
[planner_server-2] 590: } else {
[planner_server-2] 591: if (planners.size() == 1 && planner_id.empty()) {
[planner_server-2] 592: RCLCPP_WARN_ONCE(
[planner_server-2] #3 Source "/nav2_ws/src/navigation2/nav2_smac_planner/src/smac_planner_hybrid.cpp", line 395, in createPlan [0x7ffa79bc36a4]
[planner_server-2] 392: expansions = std::make_unique<std::vector<std::tuple<float, float, float>>>();
[planner_server-2] 393: }
[planner_server-2] 394: // Note: All exceptions thrown are handled by the planner server and returned to the action
[planner_server-2] > 395: if (!_a_star->createPath(
[planner_server-2] 396: path, num_iterations,
[planner_server-2] 397: _tolerance / static_cast(costmap->getResolution()), expansions.get()))
[planner_server-2] 398: {
[planner_server-2] #2 Source "/karelics_workspace/karelics_brain/nav2_ws/src/navigation2/nav2_smac_planner/src/a_star.cpp", line 318, in createPath [0x7ffa79be4fba]
[planner_server-2] 316: // 2.1) Use an analytic expansion (if available) to generate a path
[planner_server-2] 317: expansion_result = nullptr;
[planner_server-2] > 318: expansion_result = _expander->tryAnalyticExpansion(
[planner_server-2] 319: current_node, getGoal(), neighborGetter, analytic_iterations, closest_distance);
[planner_server-2] 320: if (expansion_result != nullptr) {
[planner_server-2] 321: current_node = expansion_result;
[planner_server-2] #1 Source "/nav2_ws/src/navigation2/nav2_smac_planner/src/analytic_expansion.cpp", line 83, in tryAnalyticExpansion [0x7ffa79bf9627]
[planner_server-2] 80: if (analytic_iterations <= 0) {
[planner_server-2] 81: // Reset the counter and try the analytic path expansion
[planner_server-2] 82: analytic_iterations = desired_iterations;
[planner_server-2] > 83: AnalyticExpansionNodes analytic_nodes =
[planner_server-2] 84: getAnalyticPath(current_node, goal_node, getter, current_node->motion_table.state_space);
[planner_server-2] 85: if (!analytic_nodes.empty()) {
[planner_server-2] 86: // If we have a valid path, attempt to refine it
[planner_server-2] #0 Source "/nav2_ws/src/navigation2/nav2_smac_planner/src/analytic_expansion.cpp", line 259, in getAnalyticPath [0x7ffa79bf8915]
[planner_server-2] 256: const float max_cost = _search_info.analytic_expansion_max_cost;
[planner_server-2] 257: if (*std::max_element(node_costs.begin(), node_costs.end()) > max_cost) {
[planner_server-2] 258: // If any element is above the comfortable cost limit, check edge cases:
[planner_server-2] > 259: // (1) Check if goal is in greater than max_cost space requiring
[planner_server-2] 260: // entering it, but only entering it on final approach, not in-and-out
[planner_server-2] 261: // (2) Checks if goal is in normal space, but enters costed space unnecessarily
[planner_server-2] 262: // mid-way through, skirting obstacle or in non-globally confined space
[planner_server-2] Segmentation fault (Address not mapped to object [(nil)])
[ERROR] [planner_server-2]: process has died [pid 205, exit code -11, cmd '/karelics_workspace/karelics_brain/nav2_ws/install/nav2_planner/lib/nav2_planner/planner_server --ros-args --log-level info --ros-args -r __node:=planner_server -p use_sim_time:=True --params-file /tmp/tmp3_qleyov --params-file /tmp/tmp6ufp_bh7 -r /tf:=tf -r /tf_static:=tf_static'].
Note: On the last part of the trace the line with > seems to point to wrong line. On earlier logs it was the line 257 the if clause with dereferencing the max_element. Not sure why. Could be backward_ros and changing the file on runtime combination or something.
Additional information
I have been investigating this now for a bit. According to StackOverflow comment dereferencing the iterator returned by std::max_element when the input array is empty, can lead into segmentation fault. Running std::max_element(EmptyArray) returns EmptyArray.end(). I believe that is what happens in this case.
To have the node_costs array to be empty the num_iterations has to be 0. There is a comment on line 196 about the possibility but I fail to notice any action taken for that. Basically if distance between from and to poses is less than sqrt(2) we get the zero.
For myself I fixed this by returning early in similar fashion as on line 186. Simple if (d < sqrt_2) return AnalyticExpansionNodes(); and that seems to be working fine for me.