PX4-Avoidance: Can not fly with avoidance on a real drone

Hello, I am working with avoidance (master branch). In simulation all works, but now I am trying to make it work on a real UAV. The UAV is a DJI flamewheel 450 with a Holybro pixhawk and an Intel NUC as companion computer mounted on it. The camera is a IntelRealsense D450i mounted on it with a 3D print gimball (connected with an usbC->usbC cable to the NUC, in this way I can avoid the USB3.0 noise).

As distance sensor I use a Garmin lidar lite.

I have manage to generate the launch file (attached to this thread), and the topic are correctly published and in rviz I can see the pointcloud in /local_poincloud topic and I can see the tree paths generated. I have also compile in release mode, but I can not manage to make it work.

In QGC I set my mission (COM_OBST_AVOID=1) waypoint and start the mission, the problem is that sometime the UAV does not take off, but when it does, It hover and do not do anything…It can not even reach the takeoff height, since is fix at 3m but it fly at 1.5~2.5m and stop.

Can someone help me to solve this problem?

<launch>
    <arg name="ns" default="/"/>
    <arg name="fcu_url" default="/dev/ttyACM0:57600"/>
    <arg name="gcs_url" default="" />   <!-- GCS link is provided by SITL -->
    <arg name="tgt_system" default="1" />
    <arg name="tgt_component" default="1" />


 <!-- Launch static transform publishers -->
  <node pkg="tf" type="static_transform_publisher" name="tf_depth_camera"
          args="0.25 0 0.12 0 0 0 fcu camera_link 10"/>

    <!-- Launch MavROS -->
    <group ns="$(arg ns)">
        <include file="$(find mavros)/launch/node.launch">
            <arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
            <!-- Need to change the config file to get the tf topic and get local position in terms of local origin -->
            <arg name="config_yaml" value="$(find avoidance)/resource/px4_config.yaml" />
            <arg name="fcu_url" value="$(arg fcu_url)" />
            <arg name="gcs_url" value="$(arg gcs_url)" />
            <arg name="tgt_system" value="$(arg tgt_system)" />
            <arg name="tgt_component" value="$(arg tgt_component)" />
        </include>
    </group>

    <!-- Launch cameras -->
   <include file="$(find realsense2_camera)/launch/rs_d435_camera.launch" >
  </include>

    <!-- Launch avoidance -->

  <env name="ROSCONSOLE_CONFIG_FILE" value="$(find local_planner)/resource/custom_rosconsole.conf"/>
  <arg name="pointcloud_topics" default="[/camera/depth/color/points]"/>

    <!-- Launch local planner -->


    <node name="local_planner_node" pkg="local_planner" type="local_planner_node" output="screen" required="true" >
      <param name="goal_x_param" value="0" />
      <param name="goal_y_param" value="0"/>
      <param name="goal_z_param" value="4" />
      <rosparam param="pointcloud_topics" subst_value="True">$(arg pointcloud_topics)</rosparam>
    </node>
    
    <!-- set or toggle rqt parameters -->
    <node name="rqt_param_toggle" pkg="local_planner" type="rqt_param_toggle.sh" />

</launch>

frames.pdf