Not able to fly real system with ego-planner

[ta-jetson]

Hey, (@ZJU-jxlin, @bigsuperZZZX )

Firstly - Congrats on this amazing planner and thanks for this contribution.

I am trying to replicate this setup using a jetson orin nano, FC with PX4 1.14, D435i and T265. I get the odometry from T265 and depth (& pointcloud) from D435.

This is my launch file

<launch>
  <!-- size of map, change the size inflate 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=" 3.0"/>

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

  <!-- main algorithm params -->
  <include file="$(find ego_planner)/launch/advanced_param.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"/>
    <arg name="depth_topic" value="/pcl_render_node/depth"/>

    <!-- 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/cloud"/> -->

    <arg name="cloud_topic" value="/camera_d435/depth/color/points"/> 

    <!-- intrinsic params of the depth camera -->
    <arg name="cx" value="321.735473632812"/>
    <arg name="cy" value="243.194213867188"/>
    <arg name="fx" value="384.127471923828"/>
    <arg name="fy" value="384.127471923828"/>

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

    <!--always set to 1.5 times grater than sensing horizen-->
    <arg name="planning_horizon" value="7.5" /> 

    <!-- 1: use 2D Nav Goal to select goal  -->
    <!-- 2: use global waypoints below  -->
    <arg name="flight_type" value="1" />
    
    <!-- global waypoints -->
    <!-- It generates a piecewise min-snap traj passing all waypoints -->
    <arg name="point_num" value="5" />

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

    <arg name="point1_x" value="2.0" />
    <arg name="point1_y" value="5.0" />
    <arg name="point1_z" value="1.0" />

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

    <arg name="point3_x" value="-2.0" />
    <arg name="point3_y" value="-5.0" />
    <arg name="point3_z" value="1.0" />

    <arg name="point4_x" value="0.0" />
    <arg name="point4_y" value="0.0" />
    <arg name="point4_z" value="1.0" />
    
  </include>

  <!-- trajectory server -->
  <node pkg="ego_planner" 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.0" 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>

  <!-- use simulator -->
  <include file="$(find ego_planner)/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="c_num" value="200"/>
    <arg name="p_num" value="200"/>
    <arg name="min_dist" value="1.2"/>

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

  <!-- Static Transform between world and camera_link -->
  <node pkg="tf2_ros" type="static_transform_publisher" name="base_to_t265" args="0 0 0 0 0 0 base camera_odom_frame" />

  <!-- Static Transform between world and camera_d435_link -->
  <node pkg="tf2_ros" type="static_transform_publisher" name="base_to_d435" args="0 0 0 -1.57 1.57 0 base camera_d435_link" />

  <!-- Static Transform between camera_link and camera_d435_link -->
  <node pkg="tf2_ros" type="static_transform_publisher" name="t265_to_d435" args="0 0 0 0 0 0 camera_odom_frame camera_d435_link" />

There are issues. I don't know if these issues are interrelated.

1.The pointcloud is being published in only direction. It doesn't move with the camera. The point cloud does change as I move the system, but it doesn't move with the system.

As you can see, I have enable odometry and pointcloud. Pointcloud stays in the same direction, even though i yawed the system -90 degrees

2. When the planner starts, the body's visualization moves much faster than the actual system. It also follows weird trajectories and keep going in to random point in random directions before reaching the target node. (No obstacles, open environment)
   

As you can see from the above picture, both the body the depth don't follow the actual system. (Here I enabled Odometetry in Rviz).
Since the the camera is d435 itself (placed in a similar orientation), I haven't changed ant transformation in grid_map.cpp.

3. There is discrepancy between /grid_map/occupancy, /grid_map/occupancy_inflate & /camera_d435/depth/color/points in the same position.

4.I read that the actual experiments were carried out with d435i only, (Odometry was estimated using VINS (Using the IR cameras)). I am using two differnt cameras for pose and depth. Do I need to add some code to get it working or is changing the topic and static transforms enough?

[have tried enabling CUDA and using depth instead of cloud, makes no difference]
When giving depth, I don't have a topic that publishes pose (camera) ; so i decided to give cloud.
I am actively testing this system. Any suggestions would be helpful.

(P.S : I know this repo is out of maintainence, but the same issue occurs in ego-planner-swarm)

Thank you

[bigsuperZZZX]

1. (a)It seems that you haven't modified the topic names to fit the topics you provide with your hardware. (b) Make sure to change the "“ to 1.
2. The moving trajectory shown in rviz is the control commands from the planner, not the current drone position.
3. They are different.
4. You have to change the topic names. If changing the tf is required depends on the frame of your odometry and depth topics.
5. Refer to the launch files in Fast-drone-250 to see how real-world flight is configured.

[ta-jetson]

Thank you for the reply @bigsuperZZZX , I referred to the Fast-drone repo and tested it, when I am using those launch files I don't want to use the px4ctrl what is defined there, I tried removing it but the topics for trajectory are not being remapped. How to proceed without px4ctrl?

[bigsuperZZZX]

What is you hardware setting?

[ta-jetson]

I am using D435 for depth, T265 for odometry, jetson orin nano, and an fmuv6x flight controller

And when I modify the single_run_in_exp to my odom and depth topics, I am not getting /drone_0_planning/pos_cmd topic

[bigsuperZZZX]

Maybe trying the whole set of FAST-drone-250 repo to and gradually replacing components with yours would be easier.

[ta-jetson]

It would be helpful if you would let me know what the messages you are passing to mavros, like planning/pos_cmd or bspline, which are the commands that needs to be sent to mavros?

[bigsuperZZZX]

Only the "/mavros/setpoint_raw/attitude" topic.
Based on your current progress, it is estimated that there are tons of future problems to solve. Therefore FAST-drone-250 is more recommended.