[controller_server-1] [INFO] [1678976093.506972698] [controller_server]: Received a goal, begin computing control effort.
[controller_server-1] Stack trace (most recent call last) in thread 3098549:
[controller_server-1] #15   Object "", at 0xffffffffffffffff, in 
[controller_server-1] #14   Source "../sysdeps/unix/sysv/linux/x86_64/clone3.S", line 81, in __clone3 [0x7fef2a7269ff]
[controller_server-1] #13   Source "./nptl/pthread_create.c", line 442, in start_thread [0x7fef2a694b42]
[controller_server-1] #12   Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fef2aadc2b2, in 
[controller_server-1] #11 | Source "/usr/include/c++/11/future", line 1737, in _M_set_result
[controller_server-1]     |  1735: 	__try
[controller_server-1]     |  1736: 	  {
[controller_server-1]     | >1737: 	    _M_set_result(_S_task_setter(_M_result, _M_fn));
[controller_server-1]     |  1738: 	  }
[controller_server-1]     |  1739: 	__catch (const __cxxabiv1::__forced_unwind&)
[controller_server-1]     | Source "/usr/include/c++/11/future", line 411, in 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*>
[controller_server-1]     |   409:         // all calls to this function are serialized,
[controller_server-1]     |   410:         // side-effects of invoking __res only happen once
[controller_server-1]     | > 411: 	call_once(_M_once, &_State_baseV2::_M_do_set, this,
[controller_server-1]     |   412: 		  std::__addressof(__res), std::__addressof(__did_set));
[controller_server-1]     |   413: 	if (__did_set)
[controller_server-1]     | Source "/usr/include/c++/11/mutex", line 783, in __gthread_once
[controller_server-1]     |   782:       // XXX pthread_once does not reset the flag if an exception is thrown.
[controller_server-1]     | > 783:       if (int __e = __gthread_once(&__once._M_once, &__once_proxy))
[controller_server-1]     |   784: 	__throw_system_error(__e);
[controller_server-1]     |   785:     }
[controller_server-1]       Source "/usr/include/x86_64-linux-gnu/c++/11/bits/gthr-default.h", line 700, in _M_run [0x7fef2aecfec0]
[controller_server-1]         697: __gthread_once (__gthread_once_t *__once, void (*__func) (void))
[controller_server-1]         698: {
[controller_server-1]         699:   if (__gthread_active_p ())
[controller_server-1]       > 700:     return __gthrw_(pthread_once) (__once, __func);
[controller_server-1]         701:   else
[controller_server-1]         702:     return -1;
[controller_server-1]         703: }
[controller_server-1] #10   Source "./nptl/pthread_once.c", line 116, in __pthread_once_slow [0x7fef2a699f67]
[controller_server-1] #9    Object "/opt/ros/humble/lib/librclcpp.so", at 0x7fef2b0d001c, 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*)
[controller_server-1] #8  | Source "/usr/include/c++/11/bits/std_function.h", line 291, 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<void>, std::__future_base::_Result_base::_Deleter>, std::thread::_Invoker<std::tuple<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::<lambda()> > >, void>&>
[controller_server-1]     |   289:       {
[controller_server-1]     |   290: 	return std::__invoke_r<_Res>(*_Base::_M_get_pointer(__functor),
[controller_server-1]     | > 291: 				     std::forward<_ArgTypes>(__args)...);
[controller_server-1]     |   292:       }
[controller_server-1]     | Source "/usr/include/c++/11/bits/invoke.h", line 116, in __invoke_impl<std::unique_ptr<std::__future_base::_Result<void>, 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::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::<lambda()> > >, void>&>
[controller_server-1]     |   114: 	return std::__invoke_impl<__type>(__tag{},
[controller_server-1]     |   115: 					  std::forward<_Callable>(__fn),
[controller_server-1]     | > 116: 					  std::forward<_Args>(__args)...);
[controller_server-1]     |   117:     }
[controller_server-1]     |   118: #else // C++11
[controller_server-1]     | Source "/usr/include/c++/11/bits/invoke.h", line 61, in operator()
[controller_server-1]     |    59:     constexpr _Res
[controller_server-1]     |    60:     __invoke_impl(__invoke_other, _Fn&& __f, _Args&&... __args)
[controller_server-1]     | >  61:     { return std::forward<_Fn>(__f)(std::forward<_Args>(__args)...); }
[controller_server-1]     |    62: 
[controller_server-1]     |    63:   template<typename _Res, typename _MemFun, typename _Tp, typename... _Args>
[controller_server-1]     | Source "/usr/include/c++/11/future", line 1409, in operator()
[controller_server-1]     |  1407: 	__try
[controller_server-1]     |  1408: 	  {
[controller_server-1]     | >1409: 	    (*_M_fn)();
[controller_server-1]     |  1410: 	  }
[controller_server-1]     |  1411: 	__catch(const __cxxabiv1::__forced_unwind&)
[controller_server-1]     | Source "/usr/include/c++/11/bits/std_thread.h", line 260, in _M_invoke<0>
[controller_server-1]     |   258: 	  using _Indices
[controller_server-1]     |   259: 	    = typename _Build_index_tuple<tuple_size<_Tuple>::value>::__type;
[controller_server-1]     | > 260: 	  return _M_invoke(_Indices());
[controller_server-1]     |   261: 	}
[controller_server-1]     |   262:       };
[controller_server-1]     | Source "/usr/include/c++/11/bits/std_thread.h", line 253, in __invoke<nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::<lambda()> >
[controller_server-1]     |   251: 	  typename __result<_Tuple>::type
[controller_server-1]     |   252: 	  _M_invoke(_Index_tuple<_Ind...>)
[controller_server-1]     | > 253: 	  { return std::__invoke(std::get<_Ind>(std::move(_M_t))...); }
[controller_server-1]     |   254: 
[controller_server-1]     |   255: 	typename __result<_Tuple>::type
[controller_server-1]     | Source "/usr/include/c++/11/bits/invoke.h", line 96, in __invoke_impl<void, nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>::handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath> >)::<lambda()> >
[controller_server-1]     |    94:       using __type = typename __result::type;
[controller_server-1]     |    95:       using __tag = typename __result::__invoke_type;
[controller_server-1]     | >  96:       return std::__invoke_impl<__type>(__tag{}, std::forward<_Callable>(__fn),
[controller_server-1]     |    97: 					std::forward<_Args>(__args)...);
[controller_server-1]     |    98:     }
[controller_server-1]     | Source "/usr/include/c++/11/bits/invoke.h", line 61, in operator()
[controller_server-1]     |    59:     constexpr _Res
[controller_server-1]     |    60:     __invoke_impl(__invoke_other, _Fn&& __f, _Args&&... __args)
[controller_server-1]     | >  61:     { return std::forward<_Fn>(__f)(std::forward<_Args>(__args)...); }
[controller_server-1]     |    62: 
[controller_server-1]     |    63:   template<typename _Res, typename _MemFun, typename _Tp, typename... _Args>
[controller_server-1]       Source "/home/gd/dexory/dev_ws/install/nav2_util/include/nav2_util/simple_action_server.hpp", line 205, in _M_invoke [0x7fef2aef844b]
[controller_server-1]         203:       // Return quickly to avoid blocking the executor, so spin up a new thread
[controller_server-1]         204:       debug_msg("Executing goal asynchronously.");
[controller_server-1]       > 205:       execution_future_ = std::async(std::launch::async, [this]() {work();});
[controller_server-1]         206:     }
[controller_server-1]         207:   }
[controller_server-1] #7  | Source "/home/gd/dexory/dev_ws/install/nav2_util/include/nav2_util/simple_action_server.hpp", line 217, in operator()
[controller_server-1]     |   215:       debug_msg("Executing the goal...");
[controller_server-1]     |   216:       try {
[controller_server-1]     | > 217:         execute_callback_();
[controller_server-1]     |   218:       } catch (std::exception & ex) {
[controller_server-1]     |   219:         RCLCPP_ERROR(
[controller_server-1]       Source "/usr/include/c++/11/bits/std_function.h", line 590, in work [0x7fef2aef7624]
[controller_server-1]         587:       {
[controller_server-1]         588: 	if (_M_empty())
[controller_server-1]         589: 	  __throw_bad_function_call();
[controller_server-1]       > 590: 	return _M_invoker(_M_functor, std::forward<_ArgTypes>(__args)...);
[controller_server-1]         591:       }
[controller_server-1]         592: 
[controller_server-1]         593: #if __cpp_rtti
[controller_server-1] #6    Source "/home/gd/dexory/dev_ws/src/navigation2/nav2_controller/src/controller_server.cpp", line 393, in computeControl [0x7fef2aebbfdf]
[controller_server-1]         391:       updateGlobalPath();
[controller_server-1]         392: 
[controller_server-1]       > 393:       computeAndPublishVelocity();
[controller_server-1]         394: 
[controller_server-1]         395:       if (isGoalReached()) {
[controller_server-1]         396:         RCLCPP_INFO(get_logger(), "Reached the goal!");
[controller_server-1] #5    Source "/home/gd/dexory/dev_ws/src/navigation2/nav2_controller/src/controller_server.cpp", line 497, in computeAndPublishVelocity [0x7fef2aebb05a]
[controller_server-1]         495:   try {
[controller_server-1]         496:     cmd_vel_2d =
[controller_server-1]       > 497:       controllers_[current_controller_]->computeVelocityCommands(
[controller_server-1]         498:       pose,
[controller_server-1]         499:       nav_2d_utils::twist2Dto3D(twist),
[controller_server-1]         500:       goal_checkers_[current_goal_checker_].get());
[controller_server-1] #4    Source "/home/gd/dexory/dev_ws/src/navigation2/nav2_mppi_controller/src/controller.cpp", line 90, in computeVelocityCommands [0x7fef1c1551ed]
[controller_server-1]          87:   nav_msgs::msg::Path transformed_plan = path_handler_.transformPath(robot_pose);
[controller_server-1]          88: 
[controller_server-1]          89:   geometry_msgs::msg::TwistStamped cmd =
[controller_server-1]       >  90:     optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal_checker);
[controller_server-1]          91: 
[controller_server-1]          92: #ifdef BENCHMARK_TESTING
[controller_server-1]          93:   auto end = std::chrono::system_clock::now();
[controller_server-1] #3    Source "/home/gd/dexory/dev_ws/src/navigation2/nav2_mppi_controller/src/optimizer.cpp", line 140, in evalControl [0x7fef1c190e57]
[controller_server-1]         137:   prepare(robot_pose, robot_speed, plan, goal_checker);
[controller_server-1]         138: 
[controller_server-1]         139:   do {
[controller_server-1]       > 140:     optimize();
[controller_server-1]         141:   } while (fallback(critics_data_.fail_flag));
[controller_server-1]         142: 
[controller_server-1]         143:   utils::savitskyGolayFilter(control_sequence_, control_history_, settings_);
[controller_server-1] #2    Source "/home/gd/dexory/dev_ws/src/navigation2/nav2_mppi_controller/src/optimizer.cpp", line 157, in optimize [0x7fef1c18c252]
[controller_server-1]         154: {
[controller_server-1]         155:   for (size_t i = 0; i < settings_.iteration_count; ++i) {
[controller_server-1]         156:     generateNoisedTrajectories();
[controller_server-1]       > 157:     critic_manager_.evalTrajectoriesScores(critics_data_);
[controller_server-1]         158:     updateControlSequence();
[controller_server-1]         159:   }
[controller_server-1]         160: }
[controller_server-1] #1    Source "/home/gd/dexory/dev_ws/src/navigation2/nav2_mppi_controller/src/critic_manager.cpp", line 74, in evalTrajectoriesScores [0x7fef1c1ba6ef]
[controller_server-1]          71:     if (data.fail_flag) {
[controller_server-1]          72:       break;
[controller_server-1]          73:     }
[controller_server-1]       >  74:     critics_[q]->score(data);
[controller_server-1]          75:   }
[controller_server-1]          76: }
[controller_server-1] #0  | Source "/home/gd/dexory/dev_ws/src/navigation2/nav2_mppi_controller/src/critics/path_follow_critic.cpp", line 54, in operator bool
[controller_server-1]     |    52:   bool valid = false;
[controller_server-1]     |    53:   while (!valid && offseted_idx < path_size - 1) {
[controller_server-1]     | >  54:     valid = (*data.path_pts_valid)[offseted_idx];
[controller_server-1]     |    55:     if (!valid) {
[controller_server-1]     |    56:       offseted_idx++;
[controller_server-1]       Source "/usr/include/c++/11/bits/stl_bvector.h", line 87, in score [0x7feeeffc4917]
[controller_server-1]          84: #endif
[controller_server-1]          85: 
[controller_server-1]          86:     operator bool() const _GLIBCXX_NOEXCEPT
[controller_server-1]       >  87:     { return !!(*_M_p & _M_mask); }
[controller_server-1]          88: 
[controller_server-1]          89:     _Bit_reference&
[controller_server-1]          90:     operator=(bool __x) _GLIBCXX_NOEXCEPT
[controller_server-1] Segmentation fault (Address not mapped to object [(nil)])