hkust-aerial-robotics / fast-planner Goto Github PK

When I run: roslaunch plan_manage topo_replan.launch, and give a target point, it appears an error that:

Stack trace (most recent call last):
#13   Object "[0xffffffffffffffff]", at 0xffffffffffffffff, in 
#12   Object "/home/daihn666/catkin_ws/devel/lib/plan_manage/fast_planner_node", at 0x41c148, in _start
#11   Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7fb24fd2083f, in __libc_start_main
#10   Object "/home/daihn666/catkin_ws/devel/lib/plan_manage/fast_planner_node", at 0x4197db, in main
#9    Object "/opt/ros/kinetic/lib/libroscpp.so", at 0x7fb252688e9a, in ros::spin()
#8    Object "/opt/ros/kinetic/lib/libroscpp.so", at 0x7fb2526a3e38, in ros::SingleThreadedSpinner::spin(ros::CallbackQueue*)
#7    Object "/opt/ros/kinetic/lib/libroscpp.so", at 0x7fb25264723a, in ros::CallbackQueue::callAvailable(ros::WallDuration)
#6    Object "/opt/ros/kinetic/lib/libroscpp.so", at 0x7fb252645837, in ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS*)
#5    Object "/opt/ros/kinetic/lib/libroscpp.so", at 0x7fb25261f2dc, in ros::TimerManager<ros::Time, ros::Duration, ros::TimerEvent>::TimerQueueCallback::call()
#4    Object "/home/daihn666/catkin_ws/devel/lib/plan_manage/fast_planner_node", at 0x433927, in fast_planner::TopoReplanFSM::execFSMCallback(ros::TimerEvent const&)
#3    Object "/home/daihn666/catkin_ws/devel/lib/plan_manage/fast_planner_node", at 0x43343e, in fast_planner::TopoReplanFSM::callTopologicalTraj(int)
#2    Object "/home/daihn666/catkin_ws/devel/lib/plan_manage/fast_planner_node", at 0x43bdff, in fast_planner::FastPlannerManager::planGlobalTraj(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)
#1    Object "/home/daihn666/catkin_ws/devel/lib/libpoly_traj.so", at 0x7fb25233349e, 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&)
#0    Object "/home/daihn666/catkin_ws/devel/lib/libpath_searching.so", at 0x7fb253036dd0, in void Eigen::internal::call_assignment_no_alias<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >, Eigen::internal::assign_op<double> >(Eigen::Matrix<double, -1, 1, 0, -1, 1>&, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::internal::assign_op<double> const&)
Segmentation fault (Signal sent by the kernel [(nil)])
[fast_planner_node-1] process has died [pid 21125, exit code -11, cmd /home/daihn666/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/daihn666/.ros/log/2003cfbe-d645-11ea-8617-485f9981cbdb/fast_planner_node-1.log].
log file: /home/daihn666/.ros/log/2003cfbe-d645-11ea-8617-485f9981cbdb/fast_planner_node-1*.log

I am not sure whether it is an Eigen problem or a stack problem. My Eigen version is 3.2.92. My college has also this version Eigen, but he can run this demo without problem.

Hello

I try to subscribe /planning/pos_cmd topic to control the drone in gazebo, but I found that the planning has stopped and the topic also stopped publishing before the drone reaches the destination.

Can you tell me what I should do if I want to control my drone to fly along the planned path？

thanks

my gpu is :GeForce MX150 how to configure nvcc_ARCH

能否介绍一下这个函数？
Eigen::VectorXd prow(3), vrow(3), arow(3);
prow << 1, 4, 1;
vrow << -1, 0, 1;
arow << 1, -2, 1;
以上数值是根据什么取的？或者有公式吗？

Hi @ZbyLGsc
I am trying to use topo_replan in Gazebo simulation setup where I have some large obstacles like building and warehouses. With the default parameters of topo_replan, the drone doesn't find a good path around, e.g. large building (see attached picture) and ends up crashing into it. I would like to know what parameters can be tuned to be able to avoid such large obstacles (not necessarily flat objects).

Thanks

Thank you for this great package for simulating MAV. I am running Ubuntu 16.04 with ROS kinetic and Gazebo9.
I would like to mention that I tried to compile the code but it giving many errors in the fast_planner package mainly in
polynomial_traj.cpp ,astar.cpp , kinodynamic_astar.cpp program files, and many more.

I look forward to hearing from you. Thank you for your valuable time.

Hello,

Just reporting this error. The code is running though.

dyn_planner_node: /usr/include/eigen3/Eigen/src/Core/DenseStorage.h:128: Eigen::internal::plain_array<T, Size, MatrixOrArrayOptions, 32>::plain_array() [with T = double; int Size = 36; int MatrixOrArrayOptions = 0]: Assertion `(reinterpret_cast<size_t>(eigen_unaligned_array_assert_workaround_gcc47(array)) & (31)) == 0 && "this assertion is explained here: " "http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html" " **** READ THIS WEB PAGE !!! ***"' failed.
Stack trace (most recent call last):
#8 Object "[0xffffffffffffffff]", at 0xffffffffffffffff, in
#7 Object "/home/michael/catkin_ws/devel/lib/plan_manage/dyn_planner_node", at 0x412978, in _start
#6 Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7ff76ad3d82f, in __libc_start_main
#5 Object "/home/michael/catkin_ws/devel/lib/plan_manage/dyn_planner_node", at 0x41248f, in main
#4 Object "/home/michael/catkin_ws/devel/lib/plan_manage/dyn_planner_node", at 0x42055e, in dyn_planner::PlanningFSM::init(ros::NodeHandle&)
#3 Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7ff76ad4ac81, in __assert_fail
#2 Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7ff76ad4abd6, in
#1 Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7ff76ad54029, in abort
#0 Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7ff76ad52428, in gsignal
Aborted (Signal sent by tkill() 27357 1000)
[ WARN] [1563280908.273078848]: Finished generate random map
[dyn_planner_node-1] process has died [pid 27357, exit code -6, cmd /home/michael/catkin_ws/devel/lib/plan_manage/dyn_planner_node /laser_cloud_surround:=/random_forest/all_map /odom_world:=/state_ukf/odom __name:=dyn_planner_node __log:=/home/michael/.ros/log/bdfb3aa8-a7c6-11e9-8a55-f894c28abe6b/dyn_planner_node-1.log].
log file: /home/michael/.ros/log/bdfb3aa8-a7c6-11e9-8a55-f894c28abe6b/dyn_planner_node-1.log
[ WARN] [1563280944.450820443]: [waypoint_generator] invalid goal in manual-lonely-waypoint mode.
[ WARN] [1563281031.431866653]: [waypoint_generator] invalid goal in manual-lonely-waypoint mode.
[ WARN] [1563281075.151515836]: [waypoint_generator] invalid goal in manual-lonely-waypoint mode.
[ WARN] [1563281085.073110464]: [waypoint_generator] invalid goal in manual-lonely-waypoint mode.

This is absolute great work!
But Is it possible to limit or fix the flying height? because it plans to fly over a wall.

能回答一下，这个函数是从哪里来的吗？
const Vector3d dp = x2.head(3) - x1.head(3);//获取位置差
const Vector3d v0 = x1.segment(3, 3);//获取x1向量中第3个元素开始的3个元素；也就是起点速度
const Vector3d v1 = x2.segment(3, 3);

double c1 = -36 * dp.dot(dp);
double c2 = 24 * (v0 + v1).dot(dp);
double c3 = -4 * (v0.dot(v0) + v0.dot(v1) + v1.dot(v1));
double c4 = 0;
double c5 = w_time_;

std::vector ts = quartic(c5, c4, c3, c2, c1);

double v_max = max_vel_;
double t_bar = (x1.head(3) - x2.head(3)).lpNorm() / v_max;
ts.push_back(t_bar);

double cost = 100000000;
double t_d = t_bar;

for (auto t : ts)
{
if (t < t_bar)
continue;
double c = -c1 / (3 * t * t * t) - c2 / (2 * t * t) - c3 / t + w_time_ * t;
if (c < cost)
{
cost = c;
t_d = t;
}
}

optimal_time = t_d;

return 1.0 * (1 + tie_breaker_) * cost;

First of all, Fast planner's kino & topo both algorithms are works exceptionally.

However, I found topo planner cannot plan through the flat wall-shaped object,
I think the planner wouldn't have much problem finding a bypass path through wall.
I don't know problem maybe a simple issue behind or limitation of local map strategy.

1. Problem in sdf map couldn't update depth properly as picture below,

2. Another problem is path twists then vehicle moves like uncontrolled then program crashes as follows

crash log

[ INFO] [1587540352.802406181]: iter num: 82
plan heading: 0.00013925
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[ERROR] [1587540353.298135604]: max vel: 18.056680, max acc: 134.117603.
ratio: 11.5809
[ INFO] [1587540353.298351864]: cost func: smooth | dist  | feasi |
[ INFO] [1587540353.303459633]: iter num: 118
[ WARN] [1587540353.303508063]: [Refine]: cost 0.00540908 seconds, time change is: 49.9948
[ INFO] [1587540353.303544483]: plan yaw
[ INFO] [1587540353.303825133]: cost func: smooth | waypt |
[ INFO] [1587540353.304844822]: iter num: 96
plan heading: 0.00134053
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
state: EXEC_TRAJ
[ WARN] [1587540353.411409102]: current traj 2.786250 m to collision
[SAFETY]: from EXEC_TRAJ to REPLAN_TRAJ
[ INFO] [1587540353.419981248]: [Topo]: ---------
[Topo]: sample num: 384, raw path num: 1, 1, pruned path num: 1, select path num: 1, pruned path num: 1
[Topo]: total time: 0.00546091, graph: 0.00504886, search: 4.75e-06, short: 0.00019535, prune: 1.1e-06, select: 0.00021085
[ INFO] [1587540353.425513227]: [Optimize]: ---------
[ INFO] [1587540353.425726917]: cost func: smooth | guide |
[ INFO] [1587540353.425825637]: cost func: smooth | dist  | feasi |
[ INFO] [1587540353.430891275]: iter num: 155
[ INFO] [1587540353.430929115]: optimization 0 cost 0.000125, 0.000126, 0.005095 seconds.
[planner]: optimization time: 0.00544329
ratio: 0.82771
[ INFO] [1587540353.431084045]: cost func: smooth | dist  | feasi |
[ WARN] [1587540353.433768794]: [Optimization]: nlopt exception
nlopt failure
[ INFO] [1587540353.433793784]: iter num: 196
[ WARN] [1587540353.433808124]: [Refine]: cost 0.0028037 seconds, time change is: -7.62869
[ INFO] [1587540353.433830364]: plan yaw
[ INFO] [1587540353.433959994]: cost func: smooth | waypt |
[ INFO] [1587540353.434395264]: iter num: 90
plan heading: 0.00057517
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ WARN] [1587540353.461693427]: current traj 4.518979 m to collision
[SAFETY]: from EXEC_TRAJ to REPLAN_TRAJ
[ INFO] [1587540353.464955076]: [Topo]: ---------
[Topo]: sample num: 377, raw path num: 4, 4, pruned path num: 1, select path num: 1, pruned path num: 1
[Topo]: total time: 0.00562946, graph: 0.00506682, search: 8.01e-06, short: 0.00024741, prune: 6.357e-05, select: 0.00024365
[ INFO] [1587540353.470637283]: [Optimize]: ---------
[ INFO] [1587540353.470903393]: cost func: smooth | guide |
[ INFO] [1587540353.471002653]: cost func: smooth | dist  | feasi |
[ INFO] [1587540353.475683792]: iter num: 230
[ INFO] [1587540353.475716072]: optimization 0 cost 0.000167, 0.000127, 0.004708 seconds.
[planner]: optimization time: 0.00510211
ratio: 1.24659
[ INFO] [1587540353.475899912]: cost func: smooth | dist  | feasi |
[ INFO] [1587540353.476831152]: iter num: 65
[ WARN] [1587540353.476859862]: [Refine]: cost 0.00108291 seconds, time change is: 1.33538
[ INFO] [1587540353.476894742]: plan yaw
[ INFO] [1587540353.476950172]: cost func: smooth | waypt |
[ INFO] [1587540353.477057692]: iter num: 70
plan heading: 0.00016601
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ WARN] [1587540353.511636152]: current traj 5.667316 m to collision
[SAFETY]: from EXEC_TRAJ to REPLAN_TRAJ
[ INFO] [1587540353.514968071]: [Topo]: ---------
[Topo]: sample num: 402, raw path num: 2, 2, pruned path num: 1, select path num: 1, pruned path num: 1
[Topo]: total time: 0.00539363, graph: 0.00505092, search: 5.5e-06, short: 0.00018369, prune: 1.691e-05, select: 0.00013661
[ INFO] [1587540353.520402800]: [Optimize]: ---------
[ INFO] [1587540353.520609510]: cost func: smooth | guide |
[ INFO] [1587540353.520718130]: cost func: smooth | dist  | feasi |
[ INFO] [1587540353.523657598]: iter num: 184
[ INFO] [1587540353.523692988]: optimization 0 cost 0.000151, 0.000116, 0.002974 seconds.
[planner]: optimization time: 0.00331785
ratio: 1.19515
[ INFO] [1587540353.523854558]: cost func: smooth | dist  | feasi |
[ INFO] [1587540353.526090387]: iter num: 190
[ WARN] [1587540353.526111657]: [Refine]: cost 0.00236315 seconds, time change is: 1.24586
[ INFO] [1587540353.526134047]: plan yaw
[ INFO] [1587540353.526183077]: cost func: smooth | waypt |
[ INFO] [1587540353.526313267]: iter num: 82
plan heading: 0.00018699
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ WARN] [1587540353.563552466]: current traj 5.646009 m to collision
[SAFETY]: from EXEC_TRAJ to REPLAN_TRAJ
terminate called after throwing an instance of 'std::bad_alloc'
  what():  std::bad_alloc
Stack trace (most recent call last):
#21   Object "[0xffffffffffffffff]", at 0xffffffffffffffff, in
#20   Object "/home/sjo/ws_fast/devel/lib/plan_manage/fast_planner_node", at 0x565555570d19, in _start
#19   Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7efbfc7f7b96, in __libc_start_main
#18   Object "/home/sjo/ws_fast/devel/lib/plan_manage/fast_planner_node", at 0x56555556e3f0, in main
#17   Object "/opt/ros/melodic/lib/libroscpp.so", at 0x7efbfe72187a, in ros::spin()
#16   Object "/opt/ros/melodic/lib/libroscpp.so", at 0x7efbfe738fe8, in ros::SingleThreadedSpinner::spin(ros::CallbackQueue*)
#15   Object "/opt/ros/melodic/lib/libroscpp.so", at 0x7efbfe6e0f7a, in ros::CallbackQueue::callAvailable(ros::WallDuration)
#14   Object "/opt/ros/melodic/lib/libroscpp.so", at 0x7efbfe6dfb8b, in ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS*)
#13   Object "/opt/ros/melodic/lib/libroscpp.so", at 0x7efbfe6baa86, in ros::TimerManager<ros::Time, ros::Duration, ros::TimerEvent>::TimerQueueCallback::call()
#12   Object "/home/sjo/ws_fast/devel/lib/plan_manage/fast_planner_node", at 0x565555588955, in fast_planner::TopoReplanFSM::execFSMCallback(ros::TimerEvent const&)
#11   Object "/home/sjo/ws_fast/devel/lib/plan_manage/fast_planner_node", at 0x565555587ab4, in fast_planner::TopoReplanFSM::callTopologicalTraj(int)
#10   Object "/home/sjo/ws_fast/devel/lib/plan_manage/fast_planner_node", at 0x5655555931f7, in fast_planner::FastPlannerManager::topoReplan(bool)
#9    Object "/home/sjo/ws_fast/devel/lib/plan_manage/fast_planner_node", at 0x565555591342, in fast_planner::FastPlannerManager::reparamLocalTraj(double, double&, double&)
#8    Object "/home/sjo/ws_fast/devel/lib/libbspline.so", at 0x7efbfee6ee48, in fast_planner::NonUniformBspline::parameterizeToBspline(double const&, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > const&, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&)
#7    Object "/home/sjo/ws_fast/devel/lib/plan_manage/fast_planner_node", at 0x565555597c0f, in Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >::resize(long, long)
#6    Object "/home/sjo/ws_fast/devel/lib/plan_manage/fast_planner_node", at 0x56555559459d, in Eigen::internal::throw_std_bad_alloc()
#5    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7efbfd20fd53, in __cxa_throw
#4    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7efbfd20fb20, in std::terminate()
#3    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7efbfd20fae5, in
#2    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7efbfd209956, in
#1    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7efbfc816800, in abort
#0    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7efbfc814e97, in gsignal
Aborted (Signal sent by tkill() 23295 1000)
[fast_planner_node-2] process has died [pid 23295, 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:=/map_generator/global_cloud_no_use /sdf_map/pose:=/pcl_render_node/camera_pose /sdf_map/depth:=/pcl_render_node/depth __name:=fast_planner_node __log:=/home/sjo/.ros/log/735868d2-846a-11ea-99e5-00d8617c19f3/fast_planner_node-2.log].
log file: /home/sjo/.ros/log/735868d2-846a-11ea-99e5-00d8617c19f3/fast_planner_node-2*.log

To Reproduce

I made wall object with tiny modification on polar object generation code in map_generator package as follow

original

modifed for wall object

    for (int r = -5; r < 5; r++)

For reproduction, use launch files attached.
topo-wall.zip

There are a number of libraries (.so) in the project, what do I need to rebuild from for this to work under jetson nano?

Hi @ZbyLGsc ,

I would like to test planners in this project with an actual flight controller. What are the control commands (i.e. position/velocity/acceleration) that are sent to the flight controller?

Thanks.

Thanks for your work,but I'm trouble in something like this
can u help to solve this Problem

the 5th formular in the paper perhaps should be this one

but in the paper it look like this

Hi.
First of all thank you for sharing great work!

I found an issue during the simulation.
That it is often observed that quadrotor model follows trajectory undesirable way
that the vehicle flip towards wrong direction then return & starts to follow trajectory (See GIF below)

This easily can be observed especially when transitioning from stopped state(WAIT_TARGET) to executing new trajectory(EXEC_TRAJ) or rarely seen while following trajectory.

I'd like to know why and is this can easily be solved?

Thanks in advance.

P.S. Launch file & parameters are left untouched.

In file kinodynamic_astar.cpp, the code to judge whether a node has been in the closed set is:
Eigen::Vector3i pro_id = posToIndex(pro_state.head(3));
PathNodePtr pro_node = expanded_nodes_.find(pro_id);
The code only consider about the positon of the node while ignore the vecility.
Is it possible that, a node in the most optimal path can not be push into the open set because there has been another node with same position and different vecility?

I see there is a series of parameters named "heading_planner" in topo_algorithm.xml. But I can not change them to reduce the yaw rate and find where they are in codes. Could you tell me what these parameters mean ?

When I try

catkin_make

I got results like this

/so3_control_nodelet.cpp:218:24: error: expected constructor, destructor, or type conversion before ‘(’ token
 PLUGINLIB_DECLARE_CLASS(so3_control, SO3ControlNodelet, SO3ControlNodelet,
                        ^
uav_simulator/so3_control/

Hi @ZbyLGsc ,
Do depth image or cloud messages have to be in particular frame?
Usually they are in the sensor frame.
Is there any required transformation?

Thanks.

Hi, I have some questions about the yaw plan function.

1. How is this derived？
   states2pts << 1.0, -dt_yaw, (1 / 3.0) * dt_yaw * dt_yaw, 1.0, 0.0, -(1 / 6.0) * dt_yaw * dt_yaw, 1.0,
   dt_yaw, (1 / 3.0) * dt_yaw * dt_yaw;
2. the size of control points is seg_num + 3, why ? I find that In calcWaypointsCost function there are only seg_num+2 control points being used to calc cost. Can I undestand that the extra point is used to contrain end_yaw? But I dont know how (states2pts * end_yaw and states2pts * start_yaw) works, as mentioned in ques 1.
   Thanks!

Hi,

Great work with the repo and papers, very impressive!
Huge thanks for making the code public, not only it's the best solution I've seen but it's also probably the only usable open code in this field.
Just wanted to ask if you are going to publish the code for RAPTOR paper as well (which you've added to readme), and if so do you have a time estimate?

Thanks!

Hi @ZbyLGsc

How to change obstacle inflation to account for different drone sizes?
Is it this one

Thank you very much for this great open source project.
I want to use it in 2D navigation of mobile robot. Do you think it is suitable? Any suggestions？Thanks.

There is a problem that the process of map_generator. Does anyone know how to deal with it? Thnaks

[ WARN] [1597058385.626132476]: Finished generate random map 
[random_forest-5] process has died [pid 32441, exit code -11, cmd /home/ziniu/Documents/hkust_ws/devel/lib/map_generator/random_forest ~odometry:=/state_ukf/odom __name:=random_forest __log:=/home/ziniu/.ros/log/6435ec34-dafb-11ea-bc80-0024d6f1973f/random_forest-5.log].
log file: /home/ziniu/.ros/log/6435ec34-dafb-11ea-bc80-0024d6f1973f/random_forest-5*.log
[ WARN] [1597058385.994766179]: [Traj server]: ready.

Guys, any pointers on how to install it in Ubuntu 18.04?

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?

Hi @ZbyLGsc
When I try to publish a new goal waypoint to /move_base_simple/goal while the drone is already on its way to a previous one, it does not change its trajectory to the new one and just continues to complete flying to the current goal. Is there a way to make the drone abort the current goal and change to the new one?

Now your algorithm sets a fixed height (1m) independent of the target height. can I fix this?

In topo-planner, I know "obstacles_inflation" can make paths further than obstacles. But this way will burden processor. From paper, the base of B-spline paths is topo-graph. Is there any way to increase the distance between planned paths and obstacles, particularly in the situation of single obstacle?

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

Hi.
I am trying to compile this package using catkin build. Environment is Ubuntu 18 with ROS melodic. I am getting the following error. Could you please give some hints on how to solve this?
Thanks

Errors     << so3_disturbance_generator:make /home/aaal/catkin_ws/logs/so3_disturbance_generator/build.make.000.log                                                                                                                   
/usr/bin/ld: cannot find -lpose_utils
collect2: error: ld returned 1 exit status
make[2]: *** [/home/aaal/catkin_ws/devel/lib/so3_disturbance_generator/so3_disturbance_generator] Error 1
make[1]: *** [CMakeFiles/so3_disturbance_generator.dir/all] Error 2
make: *** [all] Error 2

First of all, thank you for sharing this great piece of work!

I am testing the kino dynamic part and I have few questions for you:

• I do not have cuda available so I use CPU only and I saw in this case that this is the pcl_render_node.cpp which is used to create point clouds.

In kino_replan.launch you mentioned that we need to choose between depth image or point cloud but actually in sdf_map.cpp, the /sdf_map/cloud topic is subscribed anyway so comment <arg name="cloud_topic" value="/pcl_render_node/cloud"/> in the kino_replan.launch file leads to an error at start.

• When I run the demo, one thing I observe is that very often the target / traj to be track by the drone is really far ahead, see for instance the pic below:

and sometimes the drone is flying thru obstacles:

Do you know how can I handle these issues (or is it because my computer is too slow)? How to provide a new traj which is within a known small range from the current drone pose? fsm/thresh_replan is a parameter to tell when to replan and has no effect on the trajectory tracking.

• the parameter traj_server/time_forward (kino_replan.launch) is not used in the code so far, do you plan to use it later on?

• in the demo it is not possible to provide different z values unless we change the planning mode (2) and provide fixed waypoints?
  Thanks again for the great work

I'm playing with the simulator and found that increasing map size tremendously consumes memory.
On my 64GB RAM desktop maximum available size is roughly 100 x 100 x 100 m with 0.1m resolution (removed uav simulator from launch) and increasing size further throws std::bad_alloc error

I think memory usage should be optimized much further if planner to be used at wider space or outdoor.

Only plan_manage/launch/simulation.launch is launched in this test.

Reducing map resolution reduces memory usage significantly but I'm worrying about the bad impact on planner solution.

Memory usage
Memory usage is 52.5GB

Before launch - using 6.8GB

After launch - using 59.3 GB

Simulation.launch file diff from master is as following

$ git diff simulation.launch
diff --git a/fast_planner/plan_manage/launch/simulation.launch b/fast_planner/plan_manage/launch/simulation.launch
index 42ba205..467ff11 100644
--- a/fast_planner/plan_manage/launch/simulation.launch
+++ b/fast_planner/plan_manage/launch/simulation.launch
@@ -1,9 +1,9 @@
 <launch>
 
   <!-- size of map, change the size in x, y, z according to your application -->
-  <arg name="map_size_x" value="40.0"/>
-  <arg name="map_size_y" value="40.0"/>
-  <arg name="map_size_z" value=" 5.0"/>
+  <arg name="map_size_x" value="100.0"/>
+  <arg name="map_size_y" value="100.0"/>
+  <arg name="map_size_z" value="100.0"/>
 
   <!-- topic of your odometry such as VIO or LIO -->
   <arg name="odom_topic" value="/state_ukf/odom" />
@@ -39,7 +39,7 @@
     <!-- 1: use 2D Nav Goal to select goal  -->
     <!-- 2: use global waypoints below  -->
     <arg name="flight_type" value="1" />
-    
+
     <!-- global waypoints -->
     <!-- If flight_type is set to 2, the drone will travel these waypoints one by one -->
     <arg name="point_num" value="3" />
@@ -61,13 +61,13 @@
   </include>
 
   <!-- use simulator. It should be disabled if a real drone is used -->
-  <include file="$(find plan_manage)/launch/simulator.xml">
+  <!-- <include file="$(find plan_manage)/launch/simulator.xml">
     <arg name="map_size_x_" value="$(arg map_size_x)"/>
     <arg name="map_size_y_" value="$(arg map_size_y)"/>
     <arg name="map_size_z_" value="$(arg map_size_z)"/>
 
      <arg name="odometry_topic" value="$(arg odom_topic)" />
-  </include>
+  </include> -->
 
   <!-- trajectory server and trigger, you don't need to change them -->
   <node pkg="plan_manage" name="traj_server" type="traj_server" output="screen">
@@ -78,10 +78,10 @@
   </node>
 
   <node pkg="waypoint_generator" name="waypoint_generator" type="waypoint_generator" output="screen">
-    <remap from="~odom" to="$(arg odom_topic)"/>        
+    <remap from="~odom" to="$(arg odom_topic)"/>
     <remap from="~goal" to="/move_base_simple/goal"/>
     <remap from="~traj_start_trigger" to="/traj_start_trigger" />
-    <param name="waypoint_type" value="manual-lonely-waypoint"/>    
+    <param name="waypoint_type" value="manual-lonely-waypoint"/>
   </node>

Console output

$ roslaunch plan_manage simulation.launch 
SUMMARY
========

PARAMETERS
 * /fast_planner_node/bspline/limit_acc: 1.0
 * /fast_planner_node/bspline/limit_ratio: 1.1
 * /fast_planner_node/bspline/limit_vel: 2.1
 * /fast_planner_node/fsm/flight_type: 1
 * /fast_planner_node/fsm/thresh_no_replan: 2.0
 * /fast_planner_node/fsm/thresh_replan: 1.5
 * /fast_planner_node/fsm/waypoint0_x: 19.0
 * /fast_planner_node/fsm/waypoint0_y: 0.0
 * /fast_planner_node/fsm/waypoint0_z: 1.0
 * /fast_planner_node/fsm/waypoint1_x: 0.0
 * /fast_planner_node/fsm/waypoint1_y: -19.0
 * /fast_planner_node/fsm/waypoint1_z: 1.0
 * /fast_planner_node/fsm/waypoint2_x: 0.0
 * /fast_planner_node/fsm/waypoint2_y: 19.0
 * /fast_planner_node/fsm/waypoint2_z: 1.0
 * /fast_planner_node/fsm/waypoint_num: 3
 * /fast_planner_node/manager/clearance_threshold: 0.2
 * /fast_planner_node/manager/control_points_distance: 0.4
 * /fast_planner_node/manager/dynamic_environment: 0
 * /fast_planner_node/manager/local_segment_length: 6.0
 * /fast_planner_node/manager/max_acc: 1.0
 * /fast_planner_node/manager/max_jerk: 4.0
 * /fast_planner_node/manager/max_vel: 2.1
 * /fast_planner_node/manager/use_active_perception: False
 * /fast_planner_node/manager/use_geometric_path: False
 * /fast_planner_node/manager/use_kinodynamic_path: True
 * /fast_planner_node/manager/use_optimization: True
 * /fast_planner_node/manager/use_topo_path: False
 * /fast_planner_node/optimization/algorithm1: 15
 * /fast_planner_node/optimization/algorithm2: 11
 * /fast_planner_node/optimization/dist0: 0.7
 * /fast_planner_node/optimization/lambda1: 10.0
 * /fast_planner_node/optimization/lambda2: 0.8
 * /fast_planner_node/optimization/lambda3: 1e-05
 * /fast_planner_node/optimization/lambda4: 0.01
 * /fast_planner_node/optimization/max_acc: 1.0
 * /fast_planner_node/optimization/max_iteration_num1: 2
 * /fast_planner_node/optimization/max_iteration_num2: 300
 * /fast_planner_node/optimization/max_iteration_num3: 200
 * /fast_planner_node/optimization/max_vel: 2.1
 * /fast_planner_node/optimization/order: 3
 * /fast_planner_node/planner_node/planner: 1
 * /fast_planner_node/sdf_map/ceil_height: 2.5
 * /fast_planner_node/sdf_map/cx: 385.754
 * /fast_planner_node/sdf_map/cy: 236.743
 * /fast_planner_node/sdf_map/depth_filter_margin: 2
 * /fast_planner_node/sdf_map/depth_filter_maxdist: 5.0
 * /fast_planner_node/sdf_map/depth_filter_mindist: 0.2
 * /fast_planner_node/sdf_map/depth_filter_tolerance: 0.15
 * /fast_planner_node/sdf_map/esdf_slice_height: 0.3
 * /fast_planner_node/sdf_map/frame_id: world
 * /fast_planner_node/sdf_map/fx: 385.754
 * /fast_planner_node/sdf_map/fy: 257.296
 * /fast_planner_node/sdf_map/ground_height: -1.0
 * /fast_planner_node/sdf_map/k_depth_scaling_factor: 1000.0
 * /fast_planner_node/sdf_map/local_bound_inflate: 0.0
 * /fast_planner_node/sdf_map/local_map_margin: 50
 * /fast_planner_node/sdf_map/local_update_range_x: 5.5
 * /fast_planner_node/sdf_map/local_update_range_y: 5.5
 * /fast_planner_node/sdf_map/local_update_range_z: 4.5
 * /fast_planner_node/sdf_map/map_size_x: 100.0
 * /fast_planner_node/sdf_map/map_size_y: 100.0
 * /fast_planner_node/sdf_map/map_size_z: 100.0
 * /fast_planner_node/sdf_map/max_ray_length: 4.5
 * /fast_planner_node/sdf_map/min_ray_length: 0.5
 * /fast_planner_node/sdf_map/obstacles_inflation: 0.099
 * /fast_planner_node/sdf_map/p_hit: 0.65
 * /fast_planner_node/sdf_map/p_max: 0.9
 * /fast_planner_node/sdf_map/p_min: 0.12
 * /fast_planner_node/sdf_map/p_miss: 0.35
 * /fast_planner_node/sdf_map/p_occ: 0.8
 * /fast_planner_node/sdf_map/pose_type: 1
 * /fast_planner_node/sdf_map/resolution: 0.1
 * /fast_planner_node/sdf_map/show_esdf_time: False
 * /fast_planner_node/sdf_map/show_occ_time: False
 * /fast_planner_node/sdf_map/skip_pixel: 2
 * /fast_planner_node/sdf_map/use_depth_filter: True
 * /fast_planner_node/sdf_map/visualization_truncate_height: 2.49
 * /fast_planner_node/search/allocate_num: 100000
 * /fast_planner_node/search/check_num: 5
 * /fast_planner_node/search/horizon: 7.0
 * /fast_planner_node/search/init_max_tau: 0.8
 * /fast_planner_node/search/lambda_heu: 10.0
 * /fast_planner_node/search/margin: 0.2
 * /fast_planner_node/search/max_acc: 1.0
 * /fast_planner_node/search/max_tau: 0.8
 * /fast_planner_node/search/max_vel: 2.1
 * /fast_planner_node/search/resolution_astar: 0.1
 * /fast_planner_node/search/time_resolution: 0.8
 * /fast_planner_node/search/w_time: 10.0
 * /rosdistro: melodic
 * /rosversion: 1.14.3
 * /traj_server/traj_server/time_forward: 1.5
 * /waypoint_generator/waypoint_type: manual-lonely-way...

NODES
  /
    fast_planner_node (plan_manage/fast_planner_node)
    traj_server (plan_manage/traj_server)
    waypoint_generator (waypoint_generator/waypoint_generator)

auto-starting new master
process[master]: started with pid [27068]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to c6aeab74-38fa-11ea-a451-00d8617c19f3
process[rosout-1]: started with pid [27079]
started core service [/rosout]
process[fast_planner_node-2]: started with pid [27086]
process[traj_server-3]: started with pid [27087]
process[waypoint_generator-4]: started with pid [27088]
hit: 0.619039
miss: -0.619039
min log: -1.99243
max: 2.19722
thresh log: 1.38629
[ WARN] [1579246033.185737302]: [Traj server]: ready.
margin:0.2
origin_: -50 -50  -1
map size: 100 100 100
[FSM]: state: INIT
no odom.
[FSM]: state: INIT
no odom.

In the function "needConnection", if the checking for a shorter path is true, should the neighbor for both Graph node g1 and g2 need to be changed.
[OLD]
if (pathLength(path1) < pathLength(path2)) {
g1->neighbors_[i]->pos_ = pt;
// ROS_WARN("shorter!");
}
---->
[NEW]
if (pathLength(path1) < pathLength(path2)) {
g1->neighbors_[i]->pos_ = pt;
g2->neighbors_[j]->pos_ = pt;
// ROS_WARN("shorter!");
}
Or maybe I miss understanding some parts of code.

Please tell me which package you use for estimation of position and flight indoors on dji m100?

@ZbyLGsc Thank you for this great work~
I would like to know the different applicability (or the different trajectory style) between these two Path Searching methods, based on your flight tests. What i can see is that there are a lot of circular obstacles in the Topological simulation but not in the Kinodynamic one.
Also, will there be a third method in the future~~?

I'm sorry to bother you but I can't solve this issue by myself.I can't install libarmadillo-dev but I have installed libarmadillo6.I try sudo apt install -f but it failed.

[sudo] wbzhang233 的密码： 
正在读取软件包列表... 完成
正在分析软件包的依赖关系树       
正在读取状态信息... 完成       
没有可用的软件包 libarmadillo-dev，但是它被其它的软件包引用了。
这可能意味着这个缺失的软件包可能已被废弃，
或者只能在其他发布源中找到

E: 软件包 libarmadillo-dev 没有可安装候选

pair<double, Eigen::Vector3d> EDTEnvironment::interpolateTrilinear(double values[2][2][2],......)in line 59
and
pair<double, Eigen::Vector3d> EDTEnvironment::evaluateEDTWithGrad(const Eigen::Vector3d& pos,...,...)in line 82

it seems like these function do not return any thing

by the way, what does the abbreviate EDT means?
i only find Euclid Distance Field in the paper, but nothing with the abbreviate EDT.

After completed all the installation while launching the launch from the terminal got the error given below

hello, I see that in map update thread , data was written to distance buffer. In other thread, data in distance was read. Does this exist thread safety issues?

The default param only saves a little map and disappears quickly. If you increase the deth scaling factor, does it store more maps?

I'm trying to use pointcloud topic fusion method in the planner simulation.

However, I've found the critical issue when using pointcloud + odometry for SDF map fusion. Previously updated local map out of FOV doesn't maintained at all, only local map is updated and causes planner to fail often.

Using depth image - local map out of FOV is well maintained

Using pointcloud - local map out of FOV disappears

As far as I understood
Difference between image + camera pose fusion & pointcloud + odometry fusion is that

For Pointcloud fusion,
subscriber(indep_cloud_sub_) handles pointcloud topic with cloudCallback(...) which directly updates inflated map from pointclouds(md_.occupancy_buffer_inflate_ ) without occupancy updates.

while the depth image fusion methods
updateOccupancyCallback() calls

projectDepthImage()
raycastProcess()
clearAndInflateLocalMap()

These 3 functions do reproject depth image to points, updates occupancy then inflates.

It seems these functions fills the same md_.occupancy_buffer_inflate_ similary with cloudCallback
but issue occurs.

So I have tested with pointcloud to update md_.proj_points_ instead of md_.occupancy_buffer_inflate_ updating directly by modifying sdf_map.cpp as follows but issue still persists but cannot find out why.

Can you guide me what is causing the issue?

Modified followings

sdf_map.cpp

In updateOccupancyCallback, commented projectDepthImage() for sure and prevent counter reset.

void SDFMap::updateOccupancyCallback(const ros::TimerEvent& /*event*/) {
  if (!md_.occ_need_update_) return;

  /* update occupancy */
  ros::Time t1, t2;
  t1 = ros::Time::now();

  // projectDepthImage();
  raycastProcess();

  if (md_.need_clear_local_map_) {
    clearAndInflateLocalMap();
  }

  t2 = ros::Time::now();

  md_.fuse_time_ += (t2 - t1).toSec();
  md_.max_fuse_time_ = max(md_.max_fuse_time_, (t2 - t1).toSec());

  if (mp_.show_occ_time_)
    ROS_WARN("Fusion: cur t = %lf, avg t = %lf, max t = %lf", (t2 - t1).toSec(),
             md_.fuse_time_ / md_.update_num_, md_.max_fuse_time_);

  md_.occ_need_update_ = false;
  if (md_.need_clear_local_map_) md_.esdf_need_update_ = true;
  md_.need_clear_local_map_ = false;
}

Modified cloudCallback(...) to update md_.proj_points_ same as proejctDepthImage()

void SDFMap::cloudCallback(const sensor_msgs::PointCloud2ConstPtr& img) {

  pcl::PointCloud<pcl::PointXYZ> latest_cloud;
  pcl::fromROSMsg(*img, latest_cloud);

  md_.has_cloud_ = true;

  if (!md_.has_odom_) {
    // std::cout << "no odom!" << std::endl;
    return;
  }

  if (latest_cloud.points.size() == 0) return;

  if (isnan(md_.camera_pos_(0)) || isnan(md_.camera_pos_(1)) || isnan(md_.camera_pos_(2))) return;

  this->resetBuffer(md_.camera_pos_ - mp_.local_update_range_,
                    md_.camera_pos_ + mp_.local_update_range_);

  pcl::PointXYZ pt;
  Eigen::Vector3d p3d, p3d_inf;

  int inf_step = ceil(mp_.obstacles_inflation_ / mp_.resolution_);
  int inf_step_z = 1;

  md_.proj_points_cnt = 0;
  for (size_t i = 0; i < latest_cloud.points.size(); ++i) {
    pt = latest_cloud.points[i];
    p3d(0) = pt.x, p3d(1) = pt.y, p3d(2) = pt.z;
    md_.proj_points_[md_.proj_points_cnt++] = p3d;
  }
  md_.occ_need_update_ = true;
  md_.update_num_ +=1;
}

Modified launch file - cloud_simulation.launch

<launch>

  <!-- size of map, change the size in x, y, z according to your application -->
  <arg name="map_size_x" value="40.0"/>
  <arg name="map_size_y" value="40.0"/>
  <arg name="map_size_z" value=" 5.0"/>

  <!-- topic of your odometry such as VIO or LIO -->
  <arg name="odom_topic" value="/state_ukf/odom" />

  <node pkg="tf" type="static_transform_publisher" name="world_map_linker" args="0 0 0 0 0 0 world map 10" />
  <!-- main algorithm params -->
  <include file="$(find plan_manage)/launch/kino_algorithm.xml">

    <arg name="map_size_x_" value="$(arg map_size_x)"/>
    <arg name="map_size_y_" value="$(arg map_size_y)"/>
    <arg name="map_size_z_" value="$(arg map_size_z)"/>
    <arg name="odometry_topic" value="$(arg odom_topic)"/>

    <!-- camera pose: transform of camera frame in the world frame -->
    <!-- depth topic: depth image, 640x480 by default -->
    <!-- don't set cloud_topic if you already set these ones! -->
    <arg name="camera_pose_topic" value="/pcl_render_node/camera_pose_no_use"/>
    <arg name="depth_topic" value="/pcl_render_node/depth_no_use"/>

    <!-- topic of point cloud measurement, such as from LIDAR  -->
    <!-- don't set camera pose and depth, if you already set this one! -->
    <arg name="cloud_topic" value="/pcl_render_node/rendered_pcl"/>

    <!-- intrinsic params of the depth camera -->
    <arg name="cx" value="385.754"/>
    <arg name="cy" value="236.743"/>
    <arg name="fx" value="385.754"/>
    <arg name="fy" value="257.296"/>

    <!-- maximum velocity and acceleration the drone will reach -->
    <arg name="max_vel" value="2.1" />
    <arg name="max_acc" value="1.0" />

    <!-- 1: use 2D Nav Goal to select goal  -->
    <!-- 2: use global waypoints below  -->
    <arg name="flight_type" value="1" />
    
    <!-- global waypoints -->
    <!-- If flight_type is set to 2, the drone will travel these waypoints one by one -->
    <arg name="point_num" value="3" />

    <arg name="point0_x" value="19.0" />
    <arg name="point0_y" value="0.0" />
    <arg name="point0_z" value="1.0" />

    <!-- set more waypoints if you need -->
    <arg name="point1_x" value="0.0" />
    <arg name="point1_y" value="-19.0" />
    <arg name="point1_z" value="1.0" />

    <arg name="point2_x" value="0.0" />
    <arg name="point2_y" value="19.0" />
    <arg name="point2_z" value="1.0" />


  </include>

  <!-- use simulator. It should be disabled if a real drone is used -->
  <include file="$(find plan_manage)/launch/simulator.xml">
    <arg name="map_size_x_" value="$(arg map_size_x)"/>
    <arg name="map_size_y_" value="$(arg map_size_y)"/>
    <arg name="map_size_z_" value="$(arg map_size_z)"/>

     <arg name="odometry_topic" value="$(arg odom_topic)" />
  </include>

  <!-- trajectory server and trigger, you don't need to change them -->
  <node pkg="plan_manage" name="traj_server" type="traj_server" output="screen">
    <remap from="/position_cmd" to="planning/pos_cmd"/>

    <remap from="/odom_world" to="$(arg odom_topic)"/>
    <param name="traj_server/time_forward" value="1.5" type="double"/>
  </node>

  <node pkg="waypoint_generator" name="waypoint_generator" type="waypoint_generator" output="screen">
    <remap from="~odom" to="$(arg odom_topic)"/>        
    <remap from="~goal" to="/move_base_simple/goal"/>
    <remap from="~traj_start_trigger" to="/traj_start_trigger" />
    <param name="waypoint_type" value="manual-lonely-waypoint"/>    
  </node>



</launch>

swarmtal_msgs/drone_onboard_command.h and traj_generator/polynomial_traj.hpp can not be found in the project.
while the traj_generator.cpp does not be compiled in CMakeLists.txt
So the traj_generator.cpp is useless and should be deleted?

Hi guys, you have a very interesting project.
I want to try flying in the woods with your algorithm. However, your package descriptions are sorely lacking. Can we expect a description of the project in the near future?

The change in the pr #4 has been done to build it under Ubuntu 18.04. The segmentation fault error occurs when I set the 2D nav goal in rviz:

Stack trace (most recent call last):
#4 Object "[0x7ffe7d4eb93f]", at 0x7ffe7d4eb93f, in
#3 Object "/home/colin/Repos/catkin_fast_planner/devel/lib/libpath_searching.so", at 0x7ff6fd00cf82, in
#2 Object "/lib/x86_64-linux-gnu/libgcc_s.so.1", at 0x7ff6fab22d06, in _Unwind_Resume
#1 Object "/lib/x86_64-linux-gnu/libgcc_s.so.1", at 0x7ff6fab223e5, in
#0 Object "/lib/x86_64-linux-gnu/libgcc_s.so.1", at 0x7ff6fab21f34, in
Segmentation fault (Signal sent by the kernel [(nil)])
[fast_planner_node-1] process has died [pid 19276, exit code -11, cmd /home/colin/Repos/catkin_fast_planner/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/colin/.ros/log/5440e0a2-c9e1-11ea-980d-802bf9e83091/fast_planner_node-1.log].
log file: /home/colin/.ros/log/5440e0a2-c9e1-11ea-980d-802bf9e83091/fast_planner_node-1*.log

Does anyone has this problem? Thanks a lot