Fast planner crahes when trying to change altitude only

[mzahana]

Hi @ZbyLGsc

When I try to send a goal point to just fly up ( from (0,0,0) to (0,0,1) ) and similarly fly down, the fast planner node crashes and I get the following output

Triggered!
manual: 0 0 1
[TRIG]: from WAIT_TARGET to GEN_NEW_TRAJ
[B-spline]:point set have only 1 points.
fast_planner_node: /usr/include/eigen3/Eigen/src/Core/PlainObjectBase.h:285: void Eigen::PlainObjectBase<Derived>::resize(Eigen::Index, Eigen::Index) [with Derived = Eigen::Matrix<double, -1, -1>; Eigen::Index = long int]: Assertion `(!(RowsAtCompileTime!=Dynamic) || (rows==RowsAtCompileTime)) && (!(ColsAtCompileTime!=Dynamic) || (cols==ColsAtCompileTime)) && (!(RowsAtCompileTime==Dynamic && MaxRowsAtCompileTime!=Dynamic) || (rows<=MaxRowsAtCompileTime)) && (!(ColsAtCompileTime==Dynamic && MaxColsAtCompileTime!=Dynamic) || (cols<=MaxColsAtCompileTime)) && rows>=0 && cols>=0 && "Invalid sizes when resizing a matrix or array."' failed.
Stack trace (most recent call last):
#17   Object "[0xffffffffffffffff]", at 0xffffffffffffffff, in 
#16   Object "/home/arrow/catkin_ws/devel/lib/plan_manage/fast_planner_node", at 0x55f199690b69, in _start
#15   Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7f2d3dce6b96, in __libc_start_main
#14   Object "/home/arrow/catkin_ws/devel/lib/plan_manage/fast_planner_node", at 0x55f19968e240, in main
#13   Object "/opt/ros/melodic/lib/libroscpp.so", at 0x7f2d3fc0c88a, in ros::spin()
#12   Object "/opt/ros/melodic/lib/libroscpp.so", at 0x7f2d3fc23ff8, in ros::SingleThreadedSpinner::spin(ros::CallbackQueue*)
#11   Object "/opt/ros/melodic/lib/libroscpp.so", at 0x7f2d3fbcbf8a, in ros::CallbackQueue::callAvailable(ros::WallDuration)
#10   Object "/opt/ros/melodic/lib/libroscpp.so", at 0x7f2d3fbcab9b, in ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS*)
#9    Object "/opt/ros/melodic/lib/libroscpp.so", at 0x7f2d3fba5a96, in ros::TimerManager<ros::Time, ros::Duration, ros::TimerEvent>::TimerQueueCallback::call()
#8    Object "/home/arrow/catkin_ws/devel/lib/plan_manage/fast_planner_node", at 0x55f1996a8605, in fast_planner::TopoReplanFSM::execFSMCallback(ros::TimerEvent const&)
#7    Object "/home/arrow/catkin_ws/devel/lib/plan_manage/fast_planner_node", at 0x55f1996a80ab, in fast_planner::TopoReplanFSM::callTopologicalTraj(int)
#6    Object "/home/arrow/catkin_ws/devel/lib/plan_manage/fast_planner_node", at 0x55f1996b1f33, in fast_planner::FastPlannerManager::planGlobalTraj(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)
#5    Object "/home/arrow/catkin_ws/devel/lib/plan_manage/fast_planner_node", at 0x55f1996b7065, in fast_planner::GlobalTrajData::setLocalTraj(fast_planner::NonUniformBspline, double, double, double)
#4    Object "/home/arrow/catkin_ws/devel/lib/plan_manage/fast_planner_node", at 0x55f1996b4b31, in fast_planner::NonUniformBspline::operator=(fast_planner::NonUniformBspline const&)
#3    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7f2d3dcf5411, in __assert_fail
#2    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7f2d3dcf5399, in 
#1    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7f2d3dd05800, in abort
#0    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7f2d3dd03e97, in gsignal
Aborted (Signal sent by tkill() 17028 1000)
[fast_planner_node-1] process has died [pid 17028, exit code -6, cmd /home/arrow/catkin_ws/devel/lib/plan_manage/fast_planner_node /odom_world:=/mavros/local_position/odom /sdf_map/odom:=/mavros/local_position/odom /sdf_map/cloud:=/camera/depth /sdf_map/pose:=/camera/pose /sdf_map/depth:=/camera/depth/image_raw __name:=fast_planner_node __log:=/home/arrow/.ros/log/0938d10e-b4b1-11ea-9acd-0242ac110003/fast_planner_node-1.log].
log file: /home/arrow/.ros/log/0938d10e-b4b1-11ea-9acd-0242ac110003/fast_planner_node-1*.log

I noted the following message

[B-spline]:point set have only 1 points.

Any idea on what the issue is?

[mzahana]

I should mention that I change the altitude by publishing to /move_base_simple/goal topic. I noticed no matter what the target altitude is it stays at 1 meter. Is there a way to allow setting arbitrary altitude?

[ZbyLGsc]

@mzahana After checking I found this is a bug in the yaw angle planning. If the (x,y) coordinates of the start and end points are the same there will be infinite number in yaw planning which crashes the planner. The most straightforward solution is to set a (x,y) different from the start point. For example you can try (1,0,1) to takeoff.