Topo planner crash when planning short distance.

[SwiftGust]

Please see log below.
When you set goal near by start point, planner is crashed as below.
Why this crash happen?

process[fast_planner_node-1]: started with pid [15354]
process[traj_server-2]: started with pid [15355]
process[waypoint_generator-3]: started with pid [15356]
process[random_forest-4]: started with pid [15357]
process[quadrotor_simulator_so3-5]: started with pid [15367]
hit: 0.619039
miss: -0.619039
min log: -1.99243
max: 2.19722
thresh log: 1.38629
process[so3_control-6]: started with pid [15410]
process[so3_disturbance_generator-7]: started with pid [15411]
type is so3_control/SO3ControlNodelet
process[odom_visualization-8]: started with pid [15435]
process[pcl_render_node-9]: started with pid [15437]
[ WARN] [1587094882.514303067]: Finished generate random map 
[ WARN] [1587094882.747413023]: Global Pointcloud received..
global map has points: 123474.
[ WARN] [1587094882.984987842]: [Traj server]: ready.
state: INIT
no trigger_.
state: INIT
no trigger_.
state: INIT
no trigger_.
state: INIT
no trigger_.
Triggered!
manual: -18.6728 0.603114        1
[FSM]: from INIT to WAIT_TARGET
[FSM]: from WAIT_TARGET to GEN_NEW_TRAJ
fast_planner_node: /usr/local/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:367: Eigen::DenseCoeffsBase<Derived, 1>::Scalar& Eigen::DenseCoeffsBase<Derived, 1>::operator()(Eigen::Index, Eigen::Index) [with Derived = Eigen::Matrix<double, -1, -1>; Eigen::DenseCoeffsBase<Derived, 1>::Scalar = double; Eigen::Index = long int]: Assertion `row >= 0 && row < rows() && col >= 0 && col < cols()' failed.
Stack trace (most recent call last):
#17   Object "[0xffffffffffffffff]", at 0xffffffffffffffff, in 
#16   Object "/home/sjo/ws_fast/devel/lib/plan_manage/fast_planner_node", at 0x565555570c49, in _start
#15   Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7efbfc7d1b96, in __libc_start_main
#14   Object "/home/sjo/ws_fast/devel/lib/plan_manage/fast_planner_node", at 0x56555556e320, in main
#13   Object "/opt/ros/melodic/lib/libroscpp.so", at 0x7efbfe6fb87a, in ros::spin()
#12   Object "/opt/ros/melodic/lib/libroscpp.so", at 0x7efbfe712fe8, in ros::SingleThreadedSpinner::spin(ros::CallbackQueue*)
#11   Object "/opt/ros/melodic/lib/libroscpp.so", at 0x7efbfe6baf7a, in ros::CallbackQueue::callAvailable(ros::WallDuration)
#10   Object "/opt/ros/melodic/lib/libroscpp.so", at 0x7efbfe6b9b8b, in ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS*)
#9    Object "/opt/ros/melodic/lib/libroscpp.so", at 0x7efbfe694a86, in ros::TimerManager<ros::Time, ros::Duration, ros::TimerEvent>::TimerQueueCallback::call()
#8    Object "/home/sjo/ws_fast/devel/lib/plan_manage/fast_planner_node", at 0x565555588695, in fast_planner::TopoReplanFSM::execFSMCallback(ros::TimerEvent const&)
#7    Object "/home/sjo/ws_fast/devel/lib/plan_manage/fast_planner_node", at 0x56555558813b, in fast_planner::TopoReplanFSM::callTopologicalTraj(int)
#6    Object "/home/sjo/ws_fast/devel/lib/plan_manage/fast_planner_node", at 0x565555591e13, in fast_planner::FastPlannerManager::planGlobalTraj(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)
#5    Object "/home/sjo/ws_fast/devel/lib/libpoly_traj.so", at 0x7efbfe3a5bbd, in minSnapTraj(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)
#4    Object "/home/sjo/ws_fast/devel/lib/libpoly_traj.so", at 0x7efbfe3a8d9a, in Eigen::DenseCoeffsBase<Eigen::Matrix<double, -1, -1, 0, -1, -1>, 1>::operator()(long, long)
#3    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7efbfc7e0411, in __assert_fail
#2    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7efbfc7e0399, in 
#1    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7efbfc7f0800, in abort
#0    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7efbfc7eee97, in gsignal
Aborted (Signal sent by tkill() 15354 1000)
[fast_planner_node-1] process has died [pid 15354, exit code -6, cmd /home/sjo/ws_fast/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/sjo/.ros/log/4bd5985e-805d-11ea-a7eb-00d8617c19f3/fast_planner_node-1.log].
log file: /home/sjo/.ros/log/4bd5985e-805d-11ea-a7eb-00d8617c19f3/fast_planner_node-1*.log

[ZbyLGsc]

Yes this is a bug. Thanks for reporting it.

[SwiftGust]

As far as I debugged into, This only happens when inter_points are only two elements, argument Time size (seg_num) is one given to function minSnapTraj(...), accessing index out of size at Line 79-80 causes program to crash

Fast-Planner/fast_planner/poly_traj/src/polynomial_traj.cpp

Lines 74 to 80 in 733e880

	Ct = Eigen::MatrixXd::Zero(num_d, num_f + num_p);
	Ct(0, 0) = 1;
	Ct(2, 1) = 1;
	Ct(4, 2) = 1; // stack the start point
	Ct(1, 3) = 1;
	Ct(3, 2 * seg_num + 4) = 1;
	Ct(5, 2 * seg_num + 5) = 1;

with modified code to increased inter_points, minSnapTraj(...) works fine however, later parameterizeToBSpline(...) fails due to size of the given points are less than 3, seems points too close are truncated by getTrajByRadius(...) causes program to crash again.

Codes -

Fast-Planner/fast_planner/plan_manage/src/planner_manager.cpp

Line 272 in 733e880

PolynomialTraj gl_traj = minSnapTraj(pos, zero, zero, zero, zero, time);

Fast-Planner/fast_planner/plan_manage/src/planner_manager.cpp

Lines 479 to 496 in 733e880

	Eigen::MatrixXd FastPlannerManager::reparamLocalTraj(double start_t, double& dt, double& duration) {
	/* get the sample points local traj within radius */
	vector<Eigen::Vector3d> point_set;
	vector<Eigen::Vector3d> start_end_derivative;
	global_data_.getTrajByRadius(start_t, pp_.local_traj_len_, pp_.ctrl_pt_dist, point_set,
	start_end_derivative, dt, duration);
	/* parameterization of B-spline */
	Eigen::MatrixXd ctrl_pts;
	NonUniformBspline::parameterizeToBspline(dt, point_set, start_end_derivative, ctrl_pts);
	plan_data_.local_start_end_derivative_ = start_end_derivative;
	// cout << "ctrl pts:" << ctrl_pts.rows() << endl;
	return ctrl_pts;
	}

[ZbyLGsc]

@SwiftGust I have fixed this problem. You may test again.

[SwiftGust]

Now it's working regardless of goal distance.
Great! Thank you.