Fails to build a trajectory everytime...

[egorfolley]

Hey @deepak-1530,

Thanks a lot for this great repo, it's highly useful and valuable. I am facing an issue every time I am trying to build a trajectory. I am running it on Jetson Ubuntu 20 with ROS Noetic (ROS 1) and the sensor is ZED Mini. (I also tried using laptop - same issue).

I changed the MappingDrone.launch file:

<launch>
  <!-- transform from local_origin_ned to map-->
  <node pkg="tf" type="static_transform_publisher" name="mapTF_broadcaster" args="0 0 0 0 0 0 1 local_origin_ned map 100" />

  <!-- conversion from camera frame to base link frame (align camera X-Y-Z axes with that of Mavros)-->
  <node pkg="tf" type="static_transform_publisher" name="camTF_broadcaster" args="0.1 0 0 0 0 0  base_link zed_base_link 100" />

  <!-- Publish camera pose in world frame (read from EKF pose estimates and published over TF for octomap generation)-->
  <node pkg="FastPlannerOctomap" type="pubCamPose" name="pubCamPose"/>

  <!--mention the octomap launch commands-->
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
    <param name="resolution" value="0.10" />

    <!-- fixed map frame (set to 'map' if SLAM or localization running!) -->
    <param name="frame_id" type="string" value="map" />

    <!-- maximum range to integrate (speedup!) -->
    <param name="sensor_model/max_range" value="15" />

    <!-- data source to integrate (PointCloud2) -->
    <remap from="cloud_in" to="zed_node/point_cloud/cloud_registered" />

</node>
</launch>

I changed camera_link to zedm_base_link because I use camera as a visual localization (link). I also changed the localization in my PX4 (link). Also, I made changes to Planner to subscribe to zed_node/pose for pos, instead of mavros node. I am moving camera and my odometry position is changing and 3D map accordingly. Am I doing something wrong?

Next steps:

1. launch zed-camera node
2. launch mapping node
3. launch px4 node
4. run Planner

I have next outcome (last few lines of code):

(3.85 3.05 3.65)
(3.95 3.15 3.75)
(3.85 3.25 3.45)
(3.85 3.35 3.55)
(3.85 3.25 3.65)
(3.95 3.35 3.65)
(3.85 3.35 3.75)
(3.95 3.45 3.75)
(4.05 3.25 3.75)
(4.05 3.45 3.85)
Here
Here
Here 63
Collision (0.5032 0.0032 0.0032) -- 0.640312
Collision (0.505 0.005 0.005) -- 0.640312
Collision (0.5072 0.0072 0.0072) -- 0.640312
Collision (0.5098 0.0098 0.0098) -- 0.640312
Collision (0.5128 0.0128 0.0128) -- 0.640312
open set empty, no path!
use node num: 1
iter num: 1
Planner output status is >>>>> 3

No trajectory found ...
Trying again ...

[deepak-1530]

What is the initial height of your drone? Can you try changing the collision distance? You can change the margin in kinodynamic_astar.cpp file line no. 363. Here is the code snippet

void fast_planner::KinodynamicAstar::setParam(ros::NodeHandle& nh) { nh.param("search/max_tau", maxtau, 0.6); nh.param("search/init_max_tau", init_maxtau, 0.8); nh.param("search/max_vel", maxvel, 2.0); nh.param("search/max_acc", maxacc, 3.0); nh.param("search/w_time", wtime, 10.0); nh.param("search/horizon", horizon_, 6.0); nh.param("search/resolutionastar", resolution, 0.05); nh.param("search/time_resolution", timeresolution, 0.8); nh.param("search/lambda_heu", lambdaheu, 1.0); nh.param("search/margin", margin_, 1.0); CHANGE THIS THRESHOLD AND CHECK nh.param("search/allocate_num", allocatenum, 100000); nh.param("search/check_num", checknum, 5);

[egorfolley]

@deepak-1530 Yes, just checked these parameters, and it works, thanks!

Do you have a description of each param or I can understand them from a code?

Multiple thanks!