Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

SmacHybrid planner can crash to segmentation fault #4141

Closed
jonipol opened this issue Feb 23, 2024 · 3 comments
Closed

SmacHybrid planner can crash to segmentation fault #4141

jonipol opened this issue Feb 23, 2024 · 3 comments

Comments

@jonipol
Copy link
Contributor

jonipol commented Feb 23, 2024

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.


Feature request

Feature description

Implementation considerations

@SteveMacenski
Copy link
Member

SteveMacenski commented Feb 23, 2024

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.

The C++ docs that I usually reference have some remark if that would be a problem that I see aren't listed there. But just being a sane person knowing STL, yeah your expectation there would align with mine. I think the documentation just had that case missing.

Can you adjust that to:

auto max_cost_it = std::max_element(node_costs.begin(), node_costs.end());
if (max_cost_it != node_costs.end() && *node_costs > max_cost) {

Does it go away? If so, submit a PR to main and I'll merge + backport it to all the distributions. Thanks for the note, I agree with your analysis that this is the culprit - but as you point out - we shouldn't have been able to get here with that empty if all's well above.

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.

You can add that too! Why not both, I practice defensive programming.

@jonipol
Copy link
Contributor Author

jonipol commented Feb 23, 2024

Thanks for quick response and the feedback! I will do this once I am back from my trip on Monday.

jonipol added a commit to jonipol/navigation2 that referenced this issue Feb 26, 2024
jonipol added a commit to jonipol/navigation2 that referenced this issue Feb 26, 2024
@SteveMacenski
Copy link
Member

Merging imminent

SteveMacenski pushed a commit that referenced this issue Feb 27, 2024
* Prevent a possible segmentation fault

#4141
Signed-off-by: Joni Pöllänen <[email protected]>

* Cleanup

Signed-off-by: Joni Pöllänen <[email protected]>

---------

Signed-off-by: Joni Pöllänen <[email protected]>
mergify bot pushed a commit that referenced this issue Feb 27, 2024
* Prevent a possible segmentation fault

#4141
Signed-off-by: Joni Pöllänen <[email protected]>

* Cleanup

Signed-off-by: Joni Pöllänen <[email protected]>

---------

Signed-off-by: Joni Pöllänen <[email protected]>
(cherry picked from commit 654e7e0)

# Conflicts:
#	nav2_smac_planner/src/analytic_expansion.cpp
mergify bot pushed a commit that referenced this issue Feb 27, 2024
* Prevent a possible segmentation fault

#4141
Signed-off-by: Joni Pöllänen <[email protected]>

* Cleanup

Signed-off-by: Joni Pöllänen <[email protected]>

---------

Signed-off-by: Joni Pöllänen <[email protected]>
(cherry picked from commit 654e7e0)

# Conflicts:
#	nav2_smac_planner/src/analytic_expansion.cpp
stevedanomodolor pushed a commit to stevedanomodolor/navigation2 that referenced this issue Mar 25, 2024
…gation#4147)

* Prevent a possible segmentation fault

ros-navigation#4141
Signed-off-by: Joni Pöllänen <[email protected]>

* Cleanup

Signed-off-by: Joni Pöllänen <[email protected]>

---------

Signed-off-by: Joni Pöllänen <[email protected]>
Signed-off-by: stevedan <[email protected]>
stevedanomodolor added a commit to stevedanomodolor/navigation2 that referenced this issue Mar 25, 2024
Signed-off-by: stevedan <[email protected]>

New MPPI Cost Critic (Contrib: Brice Renaudeau) (ros-navigation#4090)

* Share code

Signed-off-by: Brice <[email protected]>

* Update inflation_cost_critic.hpp

- copyright
- ifndef

Signed-off-by: Brice <[email protected]>

* fix lint cpp

- extra space

Signed-off-by: Brice <[email protected]>

* Fix Smac Planner confined collision checker  (ros-navigation#4055)

* Update collision_checker.cpp

Signed-off-by: Steve Macenski <[email protected]>

* Fix tests

Signed-off-by: Steve Macenski <[email protected]>

* Update test_a_star.cpp

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: Brice <[email protected]>

* Prevent analytic expansions from shortcutting Smac Planner feasible paths (ros-navigation#3962)

* a potential solution to smac shortcutting

* costmap reoslution

* some fixes

* completed prototype

* some fixes for collision detection and performance

* completing shortcutting fix

* updating tests

* adding readme

---------

Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: Brice <[email protected]>

* change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070)

Signed-off-by: GoesM <[email protected]>
Co-authored-by: GoesM <[email protected]>
Signed-off-by: Brice <[email protected]>

* [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (ros-navigation#4067)

* prototype to test SE2 footprint H improvements

* some fixes

* fixed

* invert logic

* Working final prototype to be tested

* complete unit test conversions

* Update inflation_layer.hpp

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: Brice <[email protected]>

* Adding new Smac paper to readme

Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: Brice <[email protected]>

* Update README.md

Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: Brice <[email protected]>

* [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074)

* don't repeat yourself: templates in tests

Signed-off-by: Davide Faconti <[email protected]>

* misse change

Signed-off-by: Davide Faconti <[email protected]>

---------

Signed-off-by: Davide Faconti <[email protected]>
Signed-off-by: Brice <[email protected]>

* Allow path end pose deviation revive (ros-navigation#4065)

* Support stitching paths in compute path to poses

* Update nav2_planner/src/planner_server.cpp

Co-authored-by: Steve Macenski <[email protected]>

* Rename parameter to allow_path_through_poses_goal_deviation

* Fix description

* restore nav2_params

* missing whitespace

* lint fix

* removed parameter

Signed-off-by: gg <[email protected]>

* Update planner_server.hpp

* Update planner_server.cpp

---------

Signed-off-by: gg <[email protected]>
Co-authored-by: pepisg <[email protected]>
Co-authored-by: Pedro Alejandro González <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>
Signed-off-by: Brice <[email protected]>

* Updated code to use getInflationLayer() method (ros-navigation#4076)

* updated code to use getInflationLayer method

Signed-off-by: Jose Faria <[email protected]>

* Fix linting

Signed-off-by: Jose Faria <[email protected]>

---------

Signed-off-by: Jose Faria <[email protected]>
Signed-off-by: Brice <[email protected]>

* 1594 twist stamped publisher (ros-navigation#4077)

* Add TwistStamped to controller_server via TwistPublisher util

* Add a new util class for publishing either Twist or TwistStamped
* Add a new parameter for selecting to stamp the twist data
* Consume TwistPublisher in nav2_controller

Signed-off-by: Ryan Friedman <[email protected]>

* Fix small issues

* Unused variable
* Incorrect doxygen

Signed-off-by: Ryan Friedman <[email protected]>

* Remove stored node and assert

Signed-off-by: Ryan Friedman <[email protected]>

* Add tests for node

* Facing timeout even though it does the same thing as velocity smoother test

Signed-off-by: Ryan Friedman <[email protected]>

* Add missing spin call to solve timeout

Signed-off-by: Ryan Friedman <[email protected]>

* Fix copyright (me instead of intel)

Signed-off-by: Ryan Friedman <[email protected]>

* Add full test coverage with subscriber

Signed-off-by: Ryan Friedman <[email protected]>

* Remove unused rclcpp fixture

* Can't use it due to needing to join the pub thread after rclcpp shuts down

Signed-off-by: Ryan Friedman <[email protected]>

* Use TwistStamped in nav2_behaviors

Signed-off-by: Ryan Friedman <[email protected]>

* Use TwistStamped in collision monitor node

Signed-off-by: Ryan Friedman <[email protected]>

* Add TwistStamped readme updates to velocity smoother

Signed-off-by: Ryan Friedman <[email protected]>

* Add TwistSubscriber implementation

Signed-off-by: Ryan Friedman <[email protected]>

* Fix syntax errors

Signed-off-by: Ryan Friedman <[email protected]>

* Use TwistSubscriber in test_velocity_smoother

Signed-off-by: Ryan Friedman <[email protected]>

* Use TwistSubscriber in assisted_teleop

Signed-off-by: Ryan Friedman <[email protected]>

* Use TwistSubscriber in collision monitor node

Signed-off-by: Ryan Friedman <[email protected]>

* Use TwistSubscriber in velocity smoother

Signed-off-by: Ryan Friedman <[email protected]>

* Remove unused code

Signed-off-by: Ryan Friedman <[email protected]>

* add timestamp and frame_id to TwistStamped message

* Add missing utility include

Signed-off-by: Ryan Friedman <[email protected]>

* Document TwistPublisher and TwistSubscriber usage

Signed-off-by: Ryan Friedman <[email protected]>

* Use pass-by-reference

* Instead of std::move(std::unique_ptr<TwistStamped>)

Signed-off-by: Ryan Friedman <[email protected]>

* Finish twist subscriber tests

Signed-off-by: Ryan Friedman <[email protected]>

* Add other constructor and docs

Signed-off-by: Ryan Friedman <[email protected]>

* Fix linter issues

Signed-off-by: Ryan Friedman <[email protected]>

* Manually fix paren alignment

Signed-off-by: Ryan Friedman <[email protected]>

* Remove GSoC reference

Signed-off-by: Ryan Friedman <[email protected]>

* Document twist bool param in README

Signed-off-by: Ryan Friedman <[email protected]>

* Handle twistPublisher in collision monitor

* Implement behavior in the stamped callback
* Unstamped callback calls the stamped callback
* Switch to unique pointer for publisher

Signed-off-by: Ryan Friedman <[email protected]>

* Convert to using TwistStamped interally

* Use incoming twistStamped timestamp if available
* Convert all internal representations to use TwistStamped

Signed-off-by: Ryan Friedman <[email protected]>

* Remove nav2_util usage instructions

Signed-off-by: Ryan Friedman <[email protected]>

* Remove unused Twist only subscriber

Signed-off-by: Ryan Friedman <[email protected]>

* More linter fixes

Signed-off-by: Ryan Friedman <[email protected]>

* Prefer working with unique_ptr for cmd_vel

* This makes it easier to switch to std::move instead of dereference on
  publish

Signed-off-by: Ryan Friedman <[email protected]>

* Completing twist stamped migration

* shared to unique ptr

Signed-off-by: Steve Macenski <[email protected]>

* twist add stamps and properly propogated

* nav2_util: fix for compiling with clang

- Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move]

Signed-off-by: Rhys Mainwaring <[email protected]>

---------

Signed-off-by: Ryan Friedman <[email protected]>
Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: Rhys Mainwaring <[email protected]>
Co-authored-by: pedro-fuoco <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>
Co-authored-by: Rhys Mainwaring <[email protected]>
Signed-off-by: Brice <[email protected]>

* Change costmap_queue to shared library (ros-navigation#4072)

Signed-off-by: cybaol <[email protected]>
Signed-off-by: Brice <[email protected]>

* fix include of hpp

Signed-off-by: Brice Renaudeau <[email protected]>

* inflation cost optmiizations and cleanu

* rename, add defaults, and docs

* smoke test addition

* lintg

* normalize weight

* update readme

* increment cache

* Update cost_critic.hpp

Signed-off-by: Steve Macenski <[email protected]>

* Update cost_critic.cpp

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: Brice <[email protected]>
Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: GoesM <[email protected]>
Signed-off-by: Davide Faconti <[email protected]>
Signed-off-by: gg <[email protected]>
Signed-off-by: Jose Faria <[email protected]>
Signed-off-by: Ryan Friedman <[email protected]>
Signed-off-by: Rhys Mainwaring <[email protected]>
Signed-off-by: cybaol <[email protected]>
Signed-off-by: Brice Renaudeau <[email protected]>
Co-authored-by: BriceRenaudeau <[email protected]>
Co-authored-by: GoesM <[email protected]>
Co-authored-by: GoesM <[email protected]>
Co-authored-by: Davide Faconti <[email protected]>
Co-authored-by: Joshua Wallace <[email protected]>
Co-authored-by: pepisg <[email protected]>
Co-authored-by: Pedro Alejandro González <[email protected]>
Co-authored-by: jncfa <[email protected]>
Co-authored-by: Ryan <[email protected]>
Co-authored-by: pedro-fuoco <[email protected]>
Co-authored-by: Rhys Mainwaring <[email protected]>
Co-authored-by: Kino <[email protected]>

[behavior_tree] support both ports and blackboard (ros-navigation#4060)

* check result of blackboard->get and use halTree

Signed-off-by: Davide Faconti <[email protected]>

* remove unused header

Signed-off-by: Davide Faconti <[email protected]>

* BT: add port inputs when missing and use getInputPortOrBlackboard

Signed-off-by: Davide Faconti <[email protected]>

* add description

Signed-off-by: Davide Faconti <[email protected]>

* change return value of getInputPortOrBlackboard

Signed-off-by: Davide Faconti <[email protected]>

* updated tree XML

Signed-off-by: Davide Faconti <[email protected]>

---------

Signed-off-by: Davide Faconti <[email protected]>

Use ament_export_targets for all targets (ros-navigation#4112)

* Matches new internal ALIAS targets
* Use ALIAS targets for all internal linkage
* Remove unnecessary calls to ament_target_dependencies in test code
* Export includes in proper folders for overlays in colcon

Signed-off-by: Ryan Friedman <[email protected]>

adding new Friends of Nav2 Sponsors!

Update README.md (ros-navigation#4114)

Signed-off-by: Steve Macenski <[email protected]>

CI green P3 (ros-navigation#4117)

Refactors `possible_inscribed_cost` to `possible_circumscribed_cost` in collision checker for smac_planner and mppi_controller (ros-navigation#4113). (ros-navigation#4118)

* Utilizes circumscribed radius to assess potential collision points on the robot.
* Ensures safety when the robot's center is outside the circumscribed radius, eliminating additional checks and optimizing CPU usage.
Signed-off-by: Alan Xue <[email protected]>

Added cast to float to getClosestAngularBin return for behavior consistency (ros-navigation#4123)

* Added cast to float to getClosestAngularBin return for behavior consistency.

Signed-off-by: Hunter Song <[email protected]>

* Revised test name

Signed-off-by: Hunter Song <[email protected]>

---------

Signed-off-by: Hunter Song <[email protected]>

Adding context to MPPI readme

Signed-off-by: Steve Macenski <[email protected]>

[behavior_tree] Add generate_nav2_tree_nodes_xml (ros-navigation#4073)

* Add generate_nav2_tree_nodes_xml

Signed-off-by: Davide Faconti <[email protected]>

* behavior_tree: removed list of plugins from yaml

Signed-off-by: Davide Faconti <[email protected]>

* check result of get_parameter(plugin_lib_names)

Signed-off-by: Davide Faconti <[email protected]>

* fix previous commit (uncrustify)

Signed-off-by: Davide Faconti <[email protected]>

* revert change

Signed-off-by: Davide Faconti <[email protected]>

---------

Signed-off-by: Davide Faconti <[email protected]>

Fix compilation with clang (ros-navigation#4131)

Signed-off-by: Ramon Wijnands <[email protected]>

code improvement based on feedback. Name changes and minor code improvement

Signed-off-by: stevedan <[email protected]>

Fix goal handle was not freed correctly (ros-navigation#4137)

* Fix goal handle was not freed correctly

* Update nav2_util/include/nav2_util/simple_action_server.hpp

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: Steve Macenski <[email protected]>
Co-authored-by: Ziohang <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>

Adding Nvidia as Gold Sponsor! (ros-navigation#4144)

Prevent a possible segmentation fault (ros-navigation#4141) (ros-navigation#4147)

* Prevent a possible segmentation fault

Signed-off-by: Joni Pöllänen <[email protected]>

* Cleanup

Signed-off-by: Joni Pöllänen <[email protected]>

---------

Signed-off-by: Joni Pöllänen <[email protected]>

Updating stereolabs logo (ros-navigation#4158)

Update README.md

Signed-off-by: Steve Macenski <[email protected]>

behavior_tree: migration to BT.CPP 4.5.x (ros-navigation#4128)

* behavior_tree: migration to BT.CPP 4.5.x

Signed-off-by: Davide Faconti <[email protected]>

* fix formatting

Signed-off-by: Davide Faconti <[email protected]>

* fix lint warning

Signed-off-by: Davide Faconti <[email protected]>

* remove setStatus(BT::NodeStatus::IDLE)

Signed-off-by: Davide Faconti <[email protected]>

* handle SKIPPED state

Signed-off-by: Davide Faconti <[email protected]>

* fix SequenceStar

Signed-off-by: Davide Faconti <[email protected]>

* add tests

Signed-off-by: Davide Faconti <[email protected]>

* fix

Signed-off-by: Davide Faconti <[email protected]>

* fix unit tests

Signed-off-by: Davide Faconti <[email protected]>

* add emitWakeUpSignal()

Signed-off-by: Davide Faconti <[email protected]>

* fit test

Signed-off-by: Davide Faconti <[email protected]>

* fix

Signed-off-by: Davide Faconti <[email protected]>

* remove SequenceStar

Signed-off-by: Davide Faconti <[email protected]>

* remove SequenceStar

Signed-off-by: Davide Faconti <[email protected]>

* keep resetStatus() in action_node

Signed-off-by: Davide Faconti <[email protected]>

* add suggestions from peer reviewer

Signed-off-by: Davide Faconti <[email protected]>

* fix compilation

Signed-off-by: Davide Faconti <[email protected]>

---------

Signed-off-by: Davide Faconti <[email protected]>

bug fix (ros-navigation#4160)

Signed-off-by: Tianchu <[email protected]>

Stop planner if the goal is cancelled (ros-navigation#4148)

* Add support for stopping planners when the action is cancelled

Signed-off-by: Kemal Bektas <[email protected]>

* Support cancel in Smac planner

Signed-off-by: Kemal Bektas <[email protected]>

* Support cancel in Theta* planner

Signed-off-by: Kemal Bektas <[email protected]>

* Support cancel in Navfn planner

Signed-off-by: Kemal Bektas <[email protected]>

* Update nav2_system_tests to use cancel checker

Signed-off-by: Kemal Bektas <[email protected]>

* Add terminal_checking_interval parameter

Signed-off-by: Kemal Bektas <[email protected]>

* Handle timeout and cancel check on the same branch and default to 5000 iterations

Signed-off-by: Kemal Bektas <[email protected]>

* Add system tests for cancel

Signed-off-by: Kemal Bektas <[email protected]>

---------

Signed-off-by: Kemal Bektas <[email protected]>
Co-authored-by: Kemal Bektas <[email protected]>

Improvement on the wat the distance heuristic is calculated

Signed-off-by: stevedan <[email protected]>

Fix BT.CPP import (ros-navigation#4165)

Signed-off-by: Tony Najjar <[email protected]>

Update default recommendation from Obstacles to Cost critic in MPPI (ros-navigation#4170)

Signed-off-by: Steve Macenski <[email protected]>

nav2_controller: add loop rate log (ros-navigation#4171)

* update smac_planner README

Signed-off-by: ARK3r <[email protected]>

* added current controller loop rate logging

Signed-off-by: ARK3r <[email protected]>

* linting

Signed-off-by: ARK3r <[email protected]>

* uncrustify lint

Signed-off-by: ARK3r <[email protected]>

* Update nav2_controller/src/controller_server.cpp

Signed-off-by: Steve Macenski <[email protected]>

* Update nav2_controller/src/controller_server.cpp

Signed-off-by: Steve Macenski <[email protected]>

* Update nav2_controller/src/controller_server.cpp

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: ARK3r <[email protected]>
Signed-off-by: Steve Macenski <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>

improvement based on feedback

Signed-off-by: stevedan <[email protected]>
ajtudela pushed a commit to grupo-avispa/navigation2 that referenced this issue Apr 19, 2024
…gation#4147)

* Prevent a possible segmentation fault

ros-navigation#4141
Signed-off-by: Joni Pöllänen <[email protected]>

* Cleanup

Signed-off-by: Joni Pöllänen <[email protected]>

---------

Signed-off-by: Joni Pöllänen <[email protected]>
enricosutera pushed a commit to enricosutera/navigation2 that referenced this issue May 19, 2024
…gation#4147)

* Prevent a possible segmentation fault

ros-navigation#4141
Signed-off-by: Joni Pöllänen <[email protected]>

* Cleanup

Signed-off-by: Joni Pöllänen <[email protected]>

---------

Signed-off-by: Joni Pöllänen <[email protected]>
Signed-off-by: enricosutera <[email protected]>
ajtudela pushed a commit to grupo-avispa/navigation2 that referenced this issue Jun 13, 2024
…gation#4147)

* Prevent a possible segmentation fault

ros-navigation#4141
Signed-off-by: Joni Pöllänen <[email protected]>

* Cleanup

Signed-off-by: Joni Pöllänen <[email protected]>

---------

Signed-off-by: Joni Pöllänen <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>
Marc-Morcos pushed a commit to Marc-Morcos/navigation2 that referenced this issue Jul 4, 2024
…gation#4147)

* Prevent a possible segmentation fault

ros-navigation#4141
Signed-off-by: Joni Pöllänen <[email protected]>

* Cleanup

Signed-off-by: Joni Pöllänen <[email protected]>

---------

Signed-off-by: Joni Pöllänen <[email protected]>
Manos-G pushed a commit to Manos-G/navigation2 that referenced this issue Aug 1, 2024
…gation#4147)

* Prevent a possible segmentation fault

ros-navigation#4141
Signed-off-by: Joni Pöllänen <[email protected]>

* Cleanup

Signed-off-by: Joni Pöllänen <[email protected]>

---------

Signed-off-by: Joni Pöllänen <[email protected]>
jplapp pushed a commit to logivations/navigation2 that referenced this issue Nov 11, 2024
…gation#4147)

* Prevent a possible segmentation fault

ros-navigation#4141
Signed-off-by: Joni Pöllänen <[email protected]>

* Cleanup

Signed-off-by: Joni Pöllänen <[email protected]>

---------

Signed-off-by: Joni Pöllänen <[email protected]>

(cherry picked from commit 654e7e0)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

2 participants