segfault in fast_planner_node on jetson orin Linux 35.5.0

[lida2003]

Latest code on github and segfault when I click in simulator.

Any idea or suggestion?

[FSM]: state: INIT
wait for goal.
[FSM]: state: INIT
wait for goal.
[FSM]: state: INIT
wait for goal.
[FSM]: state: INIT
wait for goal.
[FSM]: state: INIT
wait for goal.
Triggered!
[FSM]: from INIT to WAIT_TARGET
[FSM]: from WAIT_TARGET to GEN_NEW_TRAJ
[kino replan]: -----------------------
start:         -19           0 -2.1684e-19, 0 0 0, 0 0 0
goal:-8.71761  1.11666        1, 0 0 0
Stack trace (most recent call last):
#13   Object "/home/daniel/HKUST/devel/lib/plan_manage/fast_planner_node", at 0xaaaad834a41b, in 
#12   Object "/lib/aarch64-linux-gnu/libc.so.6", at 0xffff967afe0f, in __libc_start_main
#11   Object "/home/daniel/HKUST/devel/lib/plan_manage/fast_planner_node", at 0xaaaad8347f7f, in main
#10   Object "/opt/ros/noetic/lib/libroscpp.so", at 0xffff97178a07, in ros::spin()
#9    Object "/opt/ros/noetic/lib/libroscpp.so", at 0xffff971906c7, in ros::SingleThreadedSpinner::spin(ros::CallbackQueue*)
#8    Object "/opt/ros/noetic/lib/libroscpp.so", at 0xffff9713a193, in ros::CallbackQueue::callAvailable(ros::WallDuration)
#7    Object "/opt/ros/noetic/lib/libroscpp.so", at 0xffff97138a6b, in ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS*)
#6    Object "/opt/ros/noetic/lib/libroscpp.so", at 0xffff97114bbf, in ros::TimerManager<ros::Time, ros::Duration, ros::TimerEvent>::TimerQueueCallback::call()
#5    Object "/home/daniel/HKUST/devel/lib/plan_manage/fast_planner_node", at 0xaaaad8350b8f, in fast_planner::KinoReplanFSM::execFSMCallback(ros::TimerEvent const&)
#4    Object "/home/daniel/HKUST/devel/lib/plan_manage/fast_planner_node", at 0xaaaad835045b, in fast_planner::KinoReplanFSM::callKinodynamicReplan()
#3    Object "/home/daniel/HKUST/devel/lib/plan_manage/fast_planner_node", at 0xaaaad8367723, in fast_planner::FastPlannerManager::kinodynamicReplan(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)
#2    Object "/home/daniel/HKUST/devel/lib/libpath_searching.so", at 0xffff9734bce3, in fast_planner::KinodynamicAstar::search(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, bool, bool, double)
#1    Object "/home/daniel/HKUST/devel/lib/libpath_searching.so", at 0xffff973453db, in 
#0    Object "/lib/aarch64-linux-gnu/libc.so.6", at 0xffff96815690, in 
Segmentation fault (Address not mapped to object [0xa])
[fast_planner_node-1] process has died [pid 9919, exit code -11, cmd /home/daniel/HKUST/devel/lib/plan_manage/fast_planner_node /odom_world:=/state_ukf/odom /sdf_map/odom:=/state_ukf/odom /sdf_map/cloud:=/pcl_render_node/cloud /sdf_map/pose:=/pcl_render_node/camera_pose /sdf_map/depth:=/pcl_render_node/depth __name:=fast_planner_node __log:=/home/daniel/.ros/log/b044125c-22de-11ef-9440-200b7460c5b7/fast_planner_node-1.log].
log file: /home/daniel/.ros/log/b044125c-22de-11ef-9440-200b7460c5b7/fast_planner_node-1*.log
^C[pcl_render_node-9] killing on exit
[odom_visualization-8] killing on exit
[so3_disturbance_generator-7] killing on exit
[so3_control-6] killing on exit
[quadrotor_simulator_so3-5] killing on exit
[random_forest-4] killing on exit
[waypoint_generator-3] killing on exit
[traj_server-2] killing on exit
terminate called after throwing an instance of 'boost::wrapexcept<boost::lock_error>'
  what():  boost: mutex lock failed in pthread_mutex_lock: Invalid argument
shutting down processing monitor...
... shutting down processing monitor complete
done

detailed log: b044125c-22de-11ef-9440-200b7460c5b7.zip

[IngaVS]

timeToIndex function doesn't return the value. you can fix it by return idx

int KinodynamicAstar::timeToIndex(double time) { int idx = floor((time - timeorigin) * inv_timeresolution); return idx; }

[lida2003]

@IngaVS Thanks, great job. Can you make a PR?

--- EDIT, still no chance

wait for goal.
Triggered!
[FSM]: from INIT to WAIT_TARGET
[FSM]: from WAIT_TARGET to GEN_NEW_TRAJ
[kino replan]: -----------------------
start:          -19            0 -1.73472e-18, 0 0 0, 0 0 0
goal:10.0461 3.33384       1, 0 0 0
open set empty, no path!
use node num: 1
iter num: 1
[kino replan]: kinodynamic search fail!
reach horizon
[kino replan]: retry search success.
[ INFO] [1726125196.863977978]: cost func: smooth | dist  | feasi | endpt |
Stack trace (most recent call last):
#14   Object "/home/daniel/catkin_ws/devel/lib/plan_manage/fast_planner_node", at 0xaaaabe50a42b, in
#13   Object "/lib/aarch64-linux-gnu/libc.so.6", at 0xffff9899de0f, in __libc_start_main
#12   Object "/home/daniel/catkin_ws/devel/lib/plan_manage/fast_planner_node", at 0xaaaabe507f8f, in main
#11   Object "/opt/ros/noetic/lib/libroscpp.so", at 0xffff99364a07, in ros::spin()
#10   Object "/opt/ros/noetic/lib/libroscpp.so", at 0xffff9937c6c7, in ros::SingleThreadedSpinner::spin(ros::CallbackQueue*)
#9    Object "/opt/ros/noetic/lib/libroscpp.so", at 0xffff99326193, in ros::CallbackQueue::callAvailable(ros::WallDuration)
#8    Object "/opt/ros/noetic/lib/libroscpp.so", at 0xffff99324a6b, in ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS*)
#7    Object "/opt/ros/noetic/lib/libroscpp.so", at 0xffff99300bbf, in ros::TimerManager<ros::Time, ros::Duration, ros::TimerEvent>::TimerQueueCallback::call()
#6    Object "/home/daniel/catkin_ws/devel/lib/plan_manage/fast_planner_node", at 0xaaaabe510b9f, in fast_planner::KinoReplanFSM::execFSMCallback(ros::TimerEvent const&)
#5    Object "/home/daniel/catkin_ws/devel/lib/plan_manage/fast_planner_node", at 0xaaaabe51046b, in fast_planner::KinoReplanFSM::callKinodynamicReplan()
#4    Object "/home/daniel/catkin_ws/devel/lib/plan_manage/fast_planner_node", at 0xaaaabe5278b7, in fast_planner::FastPlannerManager::kinodynamicReplan(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)
#3    Object "/home/daniel/catkin_ws/devel/lib/libbspline_opt.so", at 0xffff994dea7b, in fast_planner::BsplineOptimizer::BsplineOptimizeTraj(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, double const&, int const&, int, int)
#2    Object "/home/daniel/catkin_ws/devel/lib/libbspline_opt.so", at 0xffff994dd2ab, in fast_planner::BsplineOptimizer::optimize()
#1    Object "/opt/ros/noetic/lib/libnlopt_cxx.so.0", at 0xffff9874b30b, in nlopt_set_min_objective
#0    Object "/opt/ros/noetic/lib/libnlopt_cxx.so.0", at 0xffff9874b328, in nlopt_set_precond_max_objective
Segmentation fault (Address not mapped to object [0x59])
[fast_planner_node-1] process has died [pid 17157, exit code -11, cmd /home/daniel/catkin_ws/devel/lib/plan_manage/fast_planner_node /odom_world:=/state_ukf/odom /sdf_map/odom:=/state_ukf/odom /sdf_map/cloud:=/pcl_render_node/cloud /sdf_map/pose:=/pcl_render_node/camera_pose /sdf_map/depth:=/pcl_render_node/depth __name:=fast_planner_node __log:=/home/daniel/.ros/log/70376740-70d6-11ef-bb25-200b7460c5b7/fast_planner_node-1.log].
log file: /home/daniel/.ros/log/70376740-70d6-11ef-bb25-200b7460c5b7/fast_planner_node-1*.log