search

PX4 / PX4-Avoidance

PX4 avoidance ROS node for obstacle detection and avoidance.
http://px4.io
BSD 3-Clause "New" or "Revised" License
631 stars 330 forks source link

Can not fly with avoidance on a real drone #646

Closed Cristian-wp closed 2 years ago

Cristian-wp commented 3 years ago

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

julianoes commented 3 years ago

@Jaeyoung-Lim any idea what is going on here?

@Cristian-wp already said he would upload a log here.

Cristian-wp commented 3 years ago

My log is to big to be uploaded tomorrow I do another quick test and upload it there

Cristian-wp commented 3 years ago

@julianoes this are the logs of my tests, I have also a rosbag if is needed. I have see an old issue so I have set as first point the takeoff at 3m, but the topic /goal_position was always set to 0 0 0 .... maybe there is a comunication problem?

https://logs.px4.io/plot_app?log=e089088c-a5a0-4f25-81aa-f2beba52035c https://logs.px4.io/plot_app?log=f527bedc-2ba7-4fbf-90c8-e3aa3611c14e (I havent stop the log after stopping the avoidance)

Cristian-wp commented 3 years ago

@julianoes @Jaeyoung-Lim Sorry I close it for error

julianoes commented 3 years ago

So I don't see it go into takeoff mode. How are you commanding it to do something?

Jaeyoung-Lim commented 3 years ago

@Cristian-wp Are you sure you dont have issues with usb 3.0 interfering with GPS? You might want to try comparing with using a usb 2.0 cable

Cristian-wp commented 3 years ago

@julianoes I use QGC v.4.1.3, I program the mission starting with a takeoff, after I set some waypoints and at the end a return. If you need I also have a rosbag) Today as you see it did not takeoff, but yesterday was takeoff in that wrong way.... If I use mission mode without avoidance I can takeoff and perform a mission normaly.

Cristian-wp commented 3 years ago

@Jaeyoung-Lim Hi! In these test I was using Holybro RTK, because as you say USB3.0 can give noise to GPS. If I use an USB2 cable the realsense can not stream the pointcloud so I can not change the cable...I have already try to use a 2.0 and a C-C usb cable... But On the avoidance official page they use my same camera, how they do? Tomorrow I try to reduce the fps, maybe in this way I can use the 2.0

Cristian-wp commented 3 years ago

If can help this is the output of my console:

SUMMARY

CLEAR PARAMETERS

PARAMETERS

NODES / local_planner_node (local_planner/local_planner_node) mavros (mavros/mavros_node) rqt_param_toggle (local_planner/rqt_param_toggle.sh) tf_depth_camera (tf/static_transform_publisher) /camera/ realsense2_camera (nodelet/nodelet) realsense2_camera_manager (nodelet/nodelet)

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

setting /run_id to 2cc2f90e-cf83-11eb-bc62-04f02148bb7e process[rosout-1]: started with pid [9037] started core service [/rosout] process[tf_depth_camera-2]: started with pid [9055] process[mavros-3]: started with pid [9056] process[camera/realsense2_camera_manager-4]: started with pid [9057] process[camera/realsense2_camera-5]: started with pid [9068] process[local_planner_node-6]: started with pid [9073] process[rqt_param_toggle-7]: started with pid [9082] [ INFO] [1623944787.001326735]: Initializing nodelet with 8 worker threads. [ INFO] [1623944787.012096847]: Initializing nodelet with 8 worker threads. [ INFO] [1623944787.042956786]: FCU URL: /dev/ttyACM0:57600 [ INFO] [1623944787.046231381]: serial0: device: /dev/ttyACM0 @ 57600 bps [ INFO] [1623944787.046621418]: GCS bridge disabled [ INFO] [1623944787.066224654]: Plugin 3dr_radio loaded [ INFO] [1623944787.068323256]: Plugin 3dr_radio initialized [ INFO] [1623944787.068380835]: Plugin actuator_control loaded [ INFO] [1623944787.080055196]: Plugin actuator_control initialized [ INFO] [1623944787.082233629]: Plugin adsb loaded [ INFO] [1623944787.089366185]: RealSense ROS v2.2.11 [ INFO] [1623944787.089388693]: Running with LibRealSense v2.45.0 [ INFO] [1623944787.089527040]: Plugin adsb initialized [ INFO] [1623944787.089611757]: Plugin altitude loaded [ INFO] [1623944787.093992097]: Plugin altitude initialized [ INFO] [1623944787.094045163]: Plugin cam_imu_sync loaded [ INFO] [1623944787.095015331]: Plugin cam_imu_sync initialized [ INFO] [1623944787.095079032]: Plugin command loaded [ INFO] [1623944787.108267842]: Plugin command initialized [ INFO] [1623944787.108350923]: Plugin companion_process_status loaded [ INFO] [1623944787.109407728]:
[ INFO] [1623944787.112669986]: Plugin companion_process_status initialized [ INFO] [1623944787.112740564]: Plugin debug_value loaded [ INFO] [1623944787.125395515]: Plugin debug_value initialized [ INFO] [1623944787.125441050]: Plugin distance_sensor blacklisted [ INFO] [1623944787.125525677]: Plugin esc_status loaded [ INFO] [1623944787.126898088]: Plugin esc_status initialized [ INFO] [1623944787.126983219]: Plugin fake_gps loaded [ INFO] [1623944787.154656240]: Plugin fake_gps initialized [ INFO] [1623944787.154761007]: Plugin ftp loaded [ INFO] [1623944787.164698100]: Plugin ftp initialized [ INFO] [1623944787.164851683]: Plugin geofence loaded [ INFO] [1623944787.172962500]: Plugin geofence initialized [ INFO] [1623944787.173083653]: Plugin global_position loaded [ INFO] [1623944787.194716663]: Plugin global_position initialized [ INFO] [1623944787.194790776]: Plugin gps_rtk loaded [ INFO] [1623944787.198693763]: Plugin gps_rtk initialized [ INFO] [1623944787.198761707]: Plugin gps_status loaded [ INFO] [1623944787.201692790]: Plugin gps_status initialized [ INFO] [1623944787.201763169]: Plugin hil loaded [ INFO] [1623944787.214032877]: Plugin hil initialized [ INFO] [1623944787.214135078]: Plugin home_position loaded [ INFO] [1623944787.216763210]: Plugin home_position initialized [ INFO] [1623944787.216834210]: Plugin imu loaded [ INFO] [1623944787.223103688]: Plugin imu initialized [ INFO] [1623944787.223186752]: Plugin landing_target loaded [ INFO] [1623944787.231380969]: Plugin landing_target initialized [ INFO] [1623944787.231473437]: Plugin local_position loaded [ INFO] [1623944787.236556987]: Plugin local_position initialized [ INFO] [1623944787.236630923]: Plugin log_transfer loaded [ INFO] [1623944787.239549805]: Plugin log_transfer initialized [ INFO] [1623944787.239638880]: Plugin manual_control loaded [ INFO] [1623944787.241895783]: Plugin manual_control initialized [ INFO] [1623944787.241980304]: Plugin mocap_pose_estimate loaded [ INFO] [1623944787.245181669]: Plugin mocap_pose_estimate initialized [ INFO] [1623944787.245246973]: Plugin mount_control loaded [ INFO] [1623944787.248099353]: Plugin mount_control initialized [ INFO] [1623944787.248157757]: Plugin obstacle_distance loaded [ INFO] [1623944787.250855196]: Plugin obstacle_distance initialized [ INFO] [1623944787.250927290]: Plugin odom loaded [ INFO] [1623944787.254838472]: Plugin odom initialized [ INFO] [1623944787.254905387]: Plugin onboard_computer_status loaded [ INFO] [1623944787.256790489]: Plugin onboard_computer_status initialized [ INFO] [1623944787.256850696]: Plugin param loaded [ INFO] [1623944787.258934165]: Plugin param initialized [ INFO] [1623944787.258985813]: Plugin play_tune loaded [ INFO] [1623944787.261013858]: Plugin play_tune initialized [ INFO] [1623944787.261067740]: Plugin px4flow loaded [ INFO] [1623944787.266826241]: Plugin px4flow initialized [ INFO] [1623944787.266905743]: Plugin rallypoint loaded [ INFO] [1623944787.269078955]: Plugin rallypoint initialized [ INFO] [1623944787.269098443]: Plugin rangefinder blacklisted [ INFO] [1623944787.269157857]: Plugin rc_io loaded [ INFO] [1623944787.271982126]: Plugin rc_io initialized [ INFO] [1623944787.272008577]: Plugin safety_area blacklisted [ INFO] [1623944787.272088798]: Plugin setpoint_accel loaded [ INFO] [1623944787.274691956]: Plugin setpoint_accel initialized [ INFO] [1623944787.274765295]: Plugin setpoint_attitude loaded [ INFO] [1623944787.281512130]: Plugin setpoint_attitude initialized [ INFO] [1623944787.281590785]: Plugin setpoint_position loaded [ INFO] [1623944787.295020017]: Plugin setpoint_position initialized [ INFO] [1623944787.295147157]: Plugin setpoint_raw loaded [ INFO] [1623944787.302545518]: Plugin setpoint_raw initialized [ INFO] [1623944787.302669989]: Plugin setpoint_trajectory loaded [ INFO] [1623944787.310676475]: Plugin setpoint_trajectory initialized [ INFO] [1623944787.310795478]: Plugin setpoint_velocity loaded [ INFO] [1623944787.315743071]: Plugin setpoint_velocity initialized [ INFO] [1623944787.315896105]: Plugin sys_status loaded [ INFO] [1623944787.320957138]: Device with serial number 021122070141 was found.

[ INFO] [1623944787.320987240]: Device with physical ID 2-3-6 was found. [ INFO] [1623944787.320998596]: Device with name Intel RealSense D435I was found. [ WARN] [1623944787.321241390]: Error extracting usb port from device with physical ID: 2-3-6 Please report on github issue at https://github.com/IntelRealSense/realsense-ros [ INFO] [1623944787.321274742]: Resetting device... [ INFO] [1623944787.323910328]: Plugin sys_status initialized [ INFO] [1623944787.324010593]: Plugin sys_time loaded [ INFO] [1623944787.329782349]: TM: Timesync mode: MAVLINK [ INFO] [1623944787.330650532]: Plugin sys_time initialized [ INFO] [1623944787.330749317]: Plugin trajectory loaded [ INFO] [1623944787.335213628]: Plugin trajectory initialized [ INFO] [1623944787.335299549]: Plugin vfr_hud loaded [ INFO] [1623944787.335707281]: Plugin vfr_hud initialized [ INFO] [1623944787.335723605]: Plugin vibration blacklisted [ INFO] [1623944787.335770171]: Plugin vision_pose_estimate loaded [ INFO] [1623944787.342093176]: Plugin vision_pose_estimate initialized [ INFO] [1623944787.342157681]: Plugin vision_speed_estimate loaded [ INFO] [1623944787.345246714]: Plugin vision_speed_estimate initialized [ INFO] [1623944787.345408229]: Plugin waypoint loaded [ INFO] [1623944787.348960508]: Plugin waypoint initialized [ INFO] [1623944787.349013170]: Plugin wheel_odometry blacklisted [ INFO] [1623944787.349106836]: Plugin wind_estimation loaded [ INFO] [1623944787.349591420]: Plugin wind_estimation initialized [ INFO] [1623944787.349618536]: Autostarting mavlink via USB on PX4 [ INFO] [1623944787.349649523]: Built-in SIMD instructions: SSE, SSE2 [ INFO] [1623944787.349665996]: Built-in MAVLink package version: 2021.3.3 [ INFO] [1623944787.349684644]: Known MAVLink dialects: common ardupilotmega ASLUAV all autoquad icarous matrixpilot paparazzi standard uAvionix ualberta [ INFO] [1623944787.349723615]: MAVROS started. MY ID 1.240, TARGET ID 1.1 [ WARN] [1623944787.366177587]: GP: No GPS fix [ INFO] [1623944787.624148171]: IMU: Scaled IMU message used. [ INFO] [1623944787.630091867]: IMU: Attitude quaternion IMU detected! [ INFO] [1623944787.630262259]: IMU: High resolution IMU detected! [ INFO] [1623944787.914547172]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot [ INFO] [1623944788.030352200]: IMU: Attitude quaternion IMU detected! [ INFO] [1623944788.030505910]: IMU: High resolution IMU detected! Waiting for service /sc_node/set_parameters... [ INFO] [1623944788.920281892]: GF: Using MISSION_ITEM_INT [ INFO] [1623944788.920318336]: RP: Using MISSION_ITEM_INT [ INFO] [1623944788.920339735]: WP: Using MISSION_ITEM_INT [ INFO] [1623944788.920361863]: VER: 1.1: Capabilities 0x000000000000e4ef [ INFO] [1623944788.920386985]: VER: 1.1: Flight software: 010b03ff (a6274bc5ed000000) [ INFO] [1623944788.920405769]: VER: 1.1: Middleware software: 010b03ff (a6274bc5ed000000) [ INFO] [1623944788.920423201]: VER: 1.1: OS software: 080200ff (ec20f2e6c5cc35b2) [ INFO] [1623944788.920440377]: VER: 1.1: Board hardware: 00000032 [ INFO] [1623944788.920458852]: VER: 1.1: VID/PID: 26ac:0032 [ INFO] [1623944788.920477143]: VER: 1.1: UID: 3438510636363539 [ WARN] [1623944788.921028932]: CMD: Unexpected command 520, result 0 [ERROR] [1623944792.170532245]: PR: Unknown parameter to get: MPC_ACC_HOR [ERROR] [1623944792.173382930]: PR: Unknown parameter to get: MPC_ACC_DOWN_MAX [ERROR] [1623944792.176357323]: PR: Unknown parameter to get: MPC_ACC_UP_MAX [ERROR] [1623944792.179199844]: PR: Unknown parameter to get: MPC_XY_CRUISE [ERROR] [1623944792.182308711]: PR: Unknown parameter to get: MPC_Z_VEL_MAX_DN [ERROR] [1623944792.185069013]: PR: Unknown parameter to get: MPC_Z_VEL_MAX_UP [ERROR] [1623944792.187849039]: PR: Unknown parameter to get: CP_DIST [ERROR] [1623944792.190627207]: PR: Unknown parameter to get: MPC_LAND_SPEED [ERROR] [1623944792.193389915]: PR: Unknown parameter to get: MPC_JERK_MAX [ERROR] [1623944792.195984903]: PR: Unknown parameter to get: NAV_ACC_RAD [ERROR] [1623944792.198554861]: PR: Unknown parameter to get: MPC_YAWRAUTO_MAX [ INFO] [1623944793.343909582]:
[ INFO] [1623944793.544961622]: Device with serial number 021122070141 was found.

[ INFO] [1623944793.545026376]: Device with physical ID 2-3-7 was found. [ INFO] [1623944793.545052323]: Device with name Intel RealSense D435I was found. [ WARN] [1623944793.545547162]: Error extracting usb port from device with physical ID: 2-3-7 Please report on github issue at https://github.com/IntelRealSense/realsense-ros

[ INFO] [1623944793.707373458]: JSON file is not provided [ INFO] [1623944793.707399349]: ROS Node Namespace: camera [ INFO] [1623944793.707423277]: Device Name: Intel RealSense D435I [ INFO] [1623944793.707443035]: Device Serial No: 021122070141 [ INFO] [1623944793.707462090]: Device physical port: 2-3-7 [ INFO] [1623944793.707483866]: Device FW version: 05.12.12.100 [ INFO] [1623944793.707507749]: Device Product ID: 0x0B3A [ INFO] [1623944793.707533156]: Enable PointCloud: On [ INFO] [1623944793.707554329]: Align Depth: Off [ INFO] [1623944793.707577151]: Sync Mode: On [ INFO] [1623944793.707671843]: Device Sensors: [ INFO] [1623944793.707758855]: Stereo Module was found. [ INFO] [1623944793.707809164]: RGB Camera was found. [ INFO] [1623944793.707834914]: Motion Module was found. [ INFO] [1623944793.707887244]: (Fisheye, 1) sensor isn't supported by current device! -- Skipping... [ INFO] [1623944793.707919431]: (Fisheye, 2) sensor isn't supported by current device! -- Skipping... [ INFO] [1623944793.707963069]: (Pose, 0) sensor isn't supported by current device! -- Skipping... [ INFO] [1623944793.708029589]: Add Filter: pointcloud [ INFO] [1623944793.709061560]: num_filters: 1 [ INFO] [1623944793.709099048]: Setting Dynamic reconfig parameters. [ INFO] [1623944794.273209073]: Done Setting Dynamic reconfig parameters. [ INFO] [1623944794.295066432]: depth stream is enabled - width: 640, height: 480, fps: 30, Format: Z16 [ INFO] [1623944794.295421563]: infra1 stream is enabled - width: 640, height: 480, fps: 30, Format: Y8 [ INFO] [1623944794.295740661]: infra2 stream is enabled - width: 640, height: 480, fps: 30, Format: Y8

[ INFO] [1623944794.301094941]: Expected frequency for depth = 30.00000 [ INFO] [1623944794.308333509]: Expected frequency for infra1 = 30.00000 [ INFO] [1623944794.312528091]: Expected frequency for infra2 = 30.00000

[ INFO] [1623944794.330300785]: insert Depth to Stereo Module [ INFO] [1623944794.330394344]: insert Infrared to Stereo Module [ INFO] [1623944794.330426432]: insert Infrared to Stereo Module 17/06 15:46:34,379 WARNING [140481686116096] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61 [ INFO] [1623944794.430302662]: SELECTED BASE:Depth, 0 [ INFO] [1623944794.451947709]: RealSense Node Is Up! 17/06 15:46:34,501 WARNING [140481686116096] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61 17/06 15:46:34,551 WARNING [140481686116096] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61 17/06 15:46:34,602 WARNING [140481686116096] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61 17/06 15:46:34,652 WARNING [140481686116096] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61 [ERROR] [1623944794.759911943]: TF Buffer: could not retrieve requested transform from buffer, unregistered [ WARN] [1623944794.761585049]: TF Buffer: could not retrieve requested transform from buffer, tf has already been dropped from buffer [ WARN] [1623944794.766673208]: TF Buffer: could not retrieve requested transform from buffer, tf has already been dropped from buffer [ WARN] [1623944794.771759756]: TF Buffer: could not retrieve requested transform from buffer, tf has already been dropped from buffer [ WARN] [1623944794.776909316]: TF Buffer: could not retrieve requested transform from buffer, tf has already been dropped from buffer [ WARN] [1623944794.781953697]: TF Buffer: could not retrieve requested transform from buffer, tf has already been dropped from buffer [ WARN] [1623944794.787082541]: TF Buffer: could not retrieve requested transform from buffer, tf has already been dropped from buffer [ WARN] [1623944794.792231794]: TF Buffer: could not retrieve requested transform from buffer, tf has already been dropped from buffer [ WARN] [1623944794.797281377]: TF Buffer: could not retrieve requested transform from buffer, tf has already been dropped from buffer [ WARN] [1623944794.802364532]: TF Buffer: could not retrieve requested transform from buffer, tf has already been dropped from buffer 17/06 15:46:34,805 WARNING [140481686116096] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61 [ WARN] [1623944794.807499632]: TF Buffer: could not retrieve requested transform from buffer, tf has already been dropped from buffer [ WARN] [1623944794.812577705]: TF Buffer: could not retrieve requested transform from buffer, tf has already been dropped from buffer [ WARN] [1623944794.817689422]: TF Buffer: could not retrieve requested transform from buffer, tf has already been dropped from buffer 17/06 15:46:35,560 WARNING [140481686116096] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61 17/06 15:46:35,610 WARNING [140481686116096] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61 [ERROR] [1623944797.200668458]: PR: Unknown parameter to get: MPC_ACC_HOR [ERROR] [1623944797.201498910]: PR: Unknown parameter to get: MPC_ACC_DOWN_MAX [ERROR] [1623944797.202177865]: PR: Unknown parameter to get: MPC_ACC_UP_MAX [ERROR] [1623944797.202755120]: PR: Unknown parameter to get: MPC_XY_CRUISE [ERROR] [1623944797.203275571]: PR: Unknown parameter to get: MPC_Z_VEL_MAX_DN [ERROR] [1623944797.203768263]: PR: Unknown parameter to get: MPC_Z_VEL_MAX_UP [ERROR] [1623944797.204385592]: PR: Unknown parameter to get: CP_DIST [ERROR] [1623944797.205021623]: PR: Unknown parameter to get: MPC_LAND_SPEED [ERROR] [1623944797.205586084]: PR: Unknown parameter to get: MPC_JERK_MAX [ERROR] [1623944797.206181053]: PR: Unknown parameter to get: NAV_ACC_RAD [ERROR] [1623944797.206662908]: PR: Unknown parameter to get: MPC_YAWRAUTO_MAX [ INFO] [1623944797.917221374]: HP: requesting home position [ INFO] [1623944798.222994803]: parameter MPC_ACC_DOWN_MAX is set from nan to 3.000000

[ INFO] [1623944798.223031942]: parameter MPC_ACC_HOR is set from nan to 3.000000

[ INFO] [1623944798.223740693]: parameter MPC_ACC_UP_MAX is set from nan to 4.000000

[ INFO] [1623944798.225607122]: parameter MPC_JERK_MAX is set from nan to 8.000000

[ INFO] [1623944798.232649657]: parameter MPC_LAND_SPEED is set from nan to 0.700000

[ INFO] [1623944798.236284620]: parameter MPC_TKO_SPEED is set from nan to 5.000000

[ INFO] [1623944798.251615851]: parameter MPC_YAWRAUTO_MAX is set from nan to 45.000000

[ INFO] [1623944798.252899880]: parameter MPC_Z_VEL_MAX_DN is set from nan to 1.000000

[ INFO] [1623944798.252933288]: parameter MPC_Z_VEL_MAX_UP is set from nan to 3.000000

[ INFO] [1623944798.253193949]: parameter NAV_ACC_RAD is set from nan to 2.000000

[ INFO] [1623944802.965159649]: GF: mission received [ INFO] [1623944802.965471595]: RP: mission received [ INFO] [1623944803.070178541]: WP: item #0* F: C: 22 p: 0 0 0 nan x: 451297635 y: 75946219 z: 3 [ INFO] [1623944803.077417271]: WP: item #1 F: C: 16 p: 0 0 0 nan x: 451300864 y: 75945767 z: 3 [ INFO] [1623944803.078506077]: WP: item #2 F: C: 16 p: 0 0 0 nan x: 451298257 y: 75941413 z: 3 [ INFO] [1623944803.148693233]: WP: item #3 F: C: 20 p: 0 0 0 0 x: 0 y: 0 z: 0 [ INFO] [1623944803.148830920]: WP: mission received [ INFO] [1623944807.917195550]: HP: requesting home position

I copy the output indoor, but I always do my test outside to there is no " HP: requesting home position"

julianoes commented 3 years ago

In your first log you're always in ALTCTL. Were you able to go into POSCTL? Because if not it probably means you don't have a good position estimate which is also required for mission.

In the second one it keeps saying that the connection dropped. Do you know why/when that happens? That's also not a good sign. (You can see it on the bottom under "Logged messages".)

And there is the warning "Obstacle avoidance system failed", that also doesn't look good.

Screenshot from 2021-06-17 18-27-58

Cristian-wp commented 3 years ago

Oh my god...My companion computer is connected to the Pixhawk by the standard USB A --> uUSB cable provided...can it be that the problem?

julianoes commented 3 years ago

I have no idea. It could be various things, that's what you'll have to figure out. One thing to look at is mavlink status on the PX4 side to see what is going on.

Cristian-wp commented 3 years ago

@julianoes I have do it in this moment, (Avoidance active) but I am indoor now, so no GPS signal.

instance #0: mavlink chan: #0 type: USB CDC flow control: OFF rates: tx: 23.457 kB/s txerr: 0.000 kB/s tx rate mult: 1.000 tx rate max: 800000 B/s rx: 3.548 kB/s FTP enabled: YES, TX enabled: YES mode: Config MAVLink version: 2 transport protocol: serial (/dev/ttyACM0 @2000000)

instance #1: GCS heartbeat: 846898 us ago mavlink chan: #1 type: RADIO Link rssi: 198 remote rssi: 193 txbuf: 98 noise: 72 remote noise: 66 rx errors: 0 fixed: 0 flow control: ON rates: tx: 1.261 kB/s txerr: 0.000 kB/s tx rate mult: 0.956 tx rate max: 1200 B/s rx: 0.041 kB/s FTP enabled: YES, TX enabled: YES mode: Normal MAVLink version: 2 transport protocol: serial (/dev/ttyS1 @57600) ping statistics: last: 143.74 ms mean: 212.00 ms max: 1150.99 ms min: 143.74 ms dropped packets: 0

Cristian-wp commented 3 years ago

I have also perform rostopic echo /diagnostics and this is the output:

header: seq: 124 stamp: secs: 1624011759 nsecs: 621780685 frame_id: '' status:

julianoes commented 3 years ago

Oh right, that all looks fine. Do you see anything else happening when it has this drops?

Cristian-wp commented 3 years ago

As suggested in a topic I have also change rosconsol to perform debug on the local planner but I do not get any error, this is the last part:

[ INFO] [1624024340.121066072]: [OA] Planning started, using 1 cameras

[ INFO] [1624024340.144858202]: [SP]Tree (114 nodes, 5 path nodes, 40 expanded) calculated in 12.23ms.
[DEBUG] [1624024340.144896978]: [SP] node 0 : [ -0.305969, -1.012261, 0.248882]
[DEBUG] [1624024340.144912561]: [SP] node 1 : [ 1.357684, -0.164588, -0.467854]
[DEBUG] [1624024340.144927612]: [SP] node 2 : [ -0.486489, -0.456676, 0.248882]
[DEBUG] [1624024340.144941076]: [SP] node 3 : [ 0.390553, -0.691679, -1.533131]
[DEBUG] [1624024340.144962292]: [SP] node 4 : [ -0.021662, 0.117338, 0.248882]
[DEBUG] [1624024340.169859801]: [WG] Generate Waypoint, current position: [-0.021669, 0.117347, 0.248709].
[ INFO] [1624024340.169900007]: [WG] Speed adapted WP: [0.000000 0.000000 0.000000].
[DEBUG] [1624024340.169911852]: [WG] Smoothed GoTo location: -0.000158, 0.000793, 0.004097, with dt=0.100085
[ INFO] [1624024340.169927015]: [WG] Final waypoint: [-0.000158 0.000793 0.004097]. nan nan nan 

[DEBUG] [1624024340.169943047]: [WG] Using calculated tree

[DEBUG] [1624024340.169959471]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024340.219626566]: [OA] Planning started, using 1 cameras

[ INFO] [1624024340.244878699]: [SP]Tree (114 nodes, 5 path nodes, 40 expanded) calculated in 12.68ms.
[DEBUG] [1624024340.244912705]: [SP] node 0 : [ 0.882278, -0.486908, 0.248701]
[DEBUG] [1624024340.244928881]: [SP] node 1 : [ -1.046926, -0.385802, -0.268937]
[DEBUG] [1624024340.244944073]: [SP] node 2 : [ 0.882278, -0.284697, 0.248701]
[DEBUG] [1624024340.244959192]: [SP] node 3 : [ 0.120781, -0.779219, -1.533312]
[DEBUG] [1624024340.244974669]: [SP] node 4 : [ -0.021259, 0.117583, 0.248701]
[DEBUG] [1624024340.269925460]: [WG] Generate Waypoint, current position: [-0.021121, 0.117651, 0.248617].
[ INFO] [1624024340.270049631]: [WG] Speed adapted WP: [-0.000000 0.000002 0.000004].
[DEBUG] [1624024340.270132959]: [WG] Smoothed GoTo location: -0.000129, 0.000650, 0.003677, with dt=0.100120
[ INFO] [1624024340.270203049]: [WG] Final waypoint: [-0.000129 0.000650 0.003677]. nan nan nan 

[DEBUG] [1624024340.270276469]: [WG] Using calculated tree

[DEBUG] [1624024340.270335589]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024340.325508529]: [OA] Planning started, using 1 cameras

[ INFO] [1624024340.357891766]: [SP]Tree (123 nodes, 5 path nodes, 40 expanded) calculated in 14.58ms.
[DEBUG] [1624024340.357926480]: [SP] node 0 : [ -0.305071, -1.011596, 0.248750]
[DEBUG] [1624024340.357941936]: [SP] node 1 : [ 1.358581, -0.163923, -0.467986]
[DEBUG] [1624024340.357955318]: [SP] node 2 : [ -0.485592, -0.456012, 0.248750]
[DEBUG] [1624024340.357982084]: [SP] node 3 : [ 0.391451, -0.691014, -1.533263]
[DEBUG] [1624024340.357996777]: [SP] node 4 : [ -0.020764, 0.118003, 0.248750]
[DEBUG] [1624024340.369859507]: [WG] Generate Waypoint, current position: [-0.020640, 0.118138, 0.248887].
[ INFO] [1624024340.369894854]: [WG] Speed adapted WP: [-0.000028 0.000161 0.000339].
[DEBUG] [1624024340.369903813]: [WG] Smoothed GoTo location: -0.000109, 0.000548, 0.003303, with dt=0.099876
[ INFO] [1624024340.369911061]: [WG] Final waypoint: [-0.000109 0.000548 0.003303]. nan nan nan 

[DEBUG] [1624024340.369919430]: [WG] Using calculated tree

[DEBUG] [1624024340.369927181]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024340.419722871]: [OA] Planning started, using 1 cameras

[ INFO] [1624024340.443835165]: [SP]Tree (119 nodes, 5 path nodes, 40 expanded) calculated in 11.96ms.
[DEBUG] [1624024340.443867252]: [SP] node 0 : [ -0.304573, -1.011133, 0.248425]
[DEBUG] [1624024340.443876827]: [SP] node 1 : [ 1.359080, -0.163460, -0.468311]
[DEBUG] [1624024340.443884668]: [SP] node 2 : [ -0.485093, -0.455549, 0.248425]
[DEBUG] [1624024340.443897342]: [SP] node 3 : [ 0.391949, -0.690551, -1.533589]
[DEBUG] [1624024340.443909564]: [SP] node 4 : [ -0.020266, 0.118466, 0.248425]
[DEBUG] [1624024340.469877459]: [WG] Generate Waypoint, current position: [-0.020169, 0.118588, 0.248251].
[ INFO] [1624024340.469979180]: [WG] Speed adapted WP: [0.000000 0.000000 0.000000].
[DEBUG] [1624024340.470045770]: [WG] Smoothed GoTo location: -0.000090, 0.000456, 0.002959, with dt=0.100068
[ INFO] [1624024340.470114413]: [WG] Final waypoint: [-0.000090 0.000456 0.002959]. nan nan nan 

[DEBUG] [1624024340.470179954]: [WG] Using calculated tree

[DEBUG] [1624024340.470259059]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024340.521321589]: [OA] Planning started, using 1 cameras

[ INFO] [1624024340.543524675]: [SP]Tree (121 nodes, 5 path nodes, 40 expanded) calculated in 10.29ms.
[DEBUG] [1624024340.543553747]: [SP] node 0 : [ -0.303970, -1.010832, 0.248307]
[DEBUG] [1624024340.543562268]: [SP] node 1 : [ 1.359682, -0.163159, -0.468429]
[DEBUG] [1624024340.543569733]: [SP] node 2 : [ -0.484491, -0.455247, 0.248307]
[DEBUG] [1624024340.543579687]: [SP] node 3 : [ 0.392552, -0.690250, -1.533706]
[DEBUG] [1624024340.543587871]: [SP] node 4 : [ -0.019663, 0.118767, 0.248307]
[DEBUG] [1624024340.569791384]: [WG] Generate Waypoint, current position: [-0.019523, 0.118834, 0.248245].
[ INFO] [1624024340.569822279]: [WG] Speed adapted WP: [-0.000003 0.000021 0.000044].
[DEBUG] [1624024340.569831201]: [WG] Smoothed GoTo location: -0.000075, 0.000379, 0.002647, with dt=0.099858
[ INFO] [1624024340.569838919]: [WG] Final waypoint: [-0.000075 0.000379 0.002647]. nan nan nan 

[DEBUG] [1624024340.569847090]: [WG] Using calculated tree

[DEBUG] [1624024340.569854626]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024340.619665938]: [OA] Planning started, using 1 cameras

[ INFO] [1624024340.642123677]: [SP]Tree (120 nodes, 5 path nodes, 40 expanded) calculated in 10.99ms.
[DEBUG] [1624024340.642156957]: [SP] node 0 : [ -0.303460, -1.010568, 0.248216]
[DEBUG] [1624024340.642166343]: [SP] node 1 : [ 1.360193, -0.162895, -0.468520]
[DEBUG] [1624024340.642176598]: [SP] node 2 : [ -0.483980, -0.454983, 0.248216]
[DEBUG] [1624024340.642186211]: [SP] node 3 : [ 0.393062, -0.689986, -1.533797]
[DEBUG] [1624024340.642196214]: [SP] node 4 : [ -0.019153, 0.119031, 0.248216]
[DEBUG] [1624024340.669858678]: [WG] Generate Waypoint, current position: [-0.019023, 0.119099, 0.248381].
[ INFO] [1624024340.669897929]: [WG] Speed adapted WP: [-0.000016 0.000097 0.000203].
[DEBUG] [1624024340.669908180]: [WG] Smoothed GoTo location: -0.000063, 0.000322, 0.002369, with dt=0.100075
[ INFO] [1624024340.669919036]: [WG] Final waypoint: [-0.000063 0.000322 0.002369]. nan nan nan 

[DEBUG] [1624024340.669928124]: [WG] Using calculated tree

[DEBUG] [1624024340.669937762]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024340.721468158]: [OA] Planning started, using 1 cameras

[ INFO] [1624024340.749191931]: [SP]Tree (99 nodes, 5 path nodes, 40 expanded) calculated in 13.90ms.
[DEBUG] [1624024340.749251715]: [SP] node 0 : [ -0.303062, -1.010362, 0.248716]
[DEBUG] [1624024340.749266446]: [SP] node 1 : [ 1.360591, -0.162689, -0.468020]
[DEBUG] [1624024340.749283700]: [SP] node 2 : [ -0.483582, -0.454778, 0.248716]
[DEBUG] [1624024340.749294156]: [SP] node 3 : [ 0.393460, -0.689780, -1.533297]
[DEBUG] [1624024340.749308711]: [SP] node 4 : [ -0.018755, 0.119237, 0.248716]
[DEBUG] [1624024340.769789353]: [WG] Generate Waypoint, current position: [-0.018735, 0.119264, 0.248969].
[ INFO] [1624024340.769828967]: [WG] Speed adapted WP: [-0.000049 0.000313 0.000653].
[DEBUG] [1624024340.769846036]: [WG] Smoothed GoTo location: -0.000057, 0.000300, 0.002137, with dt=0.099930
[ INFO] [1624024340.769861357]: [WG] Final waypoint: [-0.000057 0.000300 0.002137]. nan nan nan 

[DEBUG] [1624024340.769878339]: [WG] Using calculated tree

[DEBUG] [1624024340.769895081]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024340.821666756]: [OA] Planning started, using 1 cameras

[ INFO] [1624024340.844617062]: [SP]Tree (118 nodes, 5 path nodes, 40 expanded) calculated in 9.77ms.
[DEBUG] [1624024340.844647767]: [SP] node 0 : [ -0.302745, -1.010131, 0.249389]
[DEBUG] [1624024340.844655541]: [SP] node 1 : [ 1.360908, -0.162458, -0.467347]
[DEBUG] [1624024340.844662119]: [SP] node 2 : [ -0.483265, -0.454547, 0.249389]
[DEBUG] [1624024340.844672237]: [SP] node 3 : [ 0.393777, -0.689550, -1.532624]
[DEBUG] [1624024340.844682092]: [SP] node 4 : [ -0.018437, 0.119467, 0.249389]
[DEBUG] [1624024340.869759224]: [WG] Generate Waypoint, current position: [-0.018361, 0.119513, 0.249634].
[ INFO] [1624024340.869793574]: [WG] Speed adapted WP: [-0.000084 0.000545 0.001139].
[DEBUG] [1624024340.869802322]: [WG] Smoothed GoTo location: -0.000058, 0.000317, 0.001955, with dt=0.099966
[ INFO] [1624024340.869810547]: [WG] Final waypoint: [-0.000058 0.000317 0.001955]. nan nan nan 

[DEBUG] [1624024340.869821846]: [WG] Using calculated tree

[DEBUG] [1624024340.869833258]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024340.924824029]: [OA] Planning started, using 1 cameras

[ INFO] [1624024340.947168977]: [SP]Tree (98 nodes, 5 path nodes, 40 expanded) calculated in 9.72ms.
[DEBUG] [1624024340.947203103]: [SP] node 0 : [ 0.687500, -0.661012, 0.046048]
[DEBUG] [1624024340.947215477]: [SP] node 1 : [ -1.220567, -0.149746, -0.266821]
[DEBUG] [1624024340.947224367]: [SP] node 2 : [ 0.687500, -0.451955, 0.250817]
[DEBUG] [1624024340.947230363]: [SP] node 3 : [ -0.160173, -0.777346, -1.531196]
[DEBUG] [1624024340.947236356]: [SP] node 4 : [ -0.018134, 0.119456, 0.250817]
[DEBUG] [1624024340.969761712]: [WG] Generate Waypoint, current position: [-0.018177, 0.119409, 0.251340].
[ INFO] [1624024340.969794152]: [WG] Speed adapted WP: [-0.000161 0.001058 0.002227].
[DEBUG] [1624024340.969803417]: [WG] Smoothed GoTo location: -0.000069, 0.000397, 0.001844, with dt=0.100001
[ INFO] [1624024340.969815764]: [WG] Final waypoint: [-0.000069 0.000397 0.001844]. nan nan nan 

[DEBUG] [1624024340.969826478]: [WG] Using calculated tree

[DEBUG] [1624024340.969837495]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024341.020732227]: [OA] Planning started, using 1 cameras

[ INFO] [1624024341.047550862]: [SP]Tree (122 nodes, 5 path nodes, 40 expanded) calculated in 14.08ms.
[DEBUG] [1624024341.047623991]: [SP] node 0 : [ -0.302146, -1.010146, 0.252884]
[DEBUG] [1624024341.047642236]: [SP] node 1 : [ 1.361506, -0.162473, -0.463852]
[DEBUG] [1624024341.047658379]: [SP] node 2 : [ -0.482666, -0.454562, 0.252884]
[DEBUG] [1624024341.047672723]: [SP] node 3 : [ 0.394376, -0.689564, -1.529129]
[DEBUG] [1624024341.047689331]: [SP] node 4 : [ -0.017839, 0.119453, 0.252884]
[DEBUG] [1624024341.069864388]: [WG] Generate Waypoint, current position: [-0.017761, 0.119422, 0.253505].
[ INFO] [1624024341.069900698]: [WG] Speed adapted WP: [-0.000251 0.001685 0.003577].
[DEBUG] [1624024341.069909889]: [WG] Smoothed GoTo location: -0.000091, 0.000556, 0.001823, with dt=0.100106
[ INFO] [1624024341.069918885]: [WG] Final waypoint: [-0.000091 0.000556 0.001823]. nan nan nan 

[DEBUG] [1624024341.069931371]: [WG] Using calculated tree

[DEBUG] [1624024341.069944138]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024341.151856901]: [OA] Planning started, using 1 cameras

[DEBUG] [1624024341.169757106]: [WG] Generate Waypoint, current position: [-0.017583, 0.119395, 0.256273].
[ INFO] [1624024341.169790902]: [WG] Speed adapted WP: [-0.000360 0.002444 0.005245].
[DEBUG] [1624024341.169800537]: [WG] Smoothed GoTo location: -0.000126, 0.000802, 0.001912, with dt=0.099890
[ INFO] [1624024341.169810131]: [WG] Final waypoint: [-0.000126 0.000802 0.001912]. nan nan nan 

[DEBUG] [1624024341.169822049]: [WG] Using calculated tree

[DEBUG] [1624024341.169833596]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024341.177172621]: [SP]Tree (105 nodes, 5 path nodes, 40 expanded) calculated in 12.28ms.
[DEBUG] [1624024341.177189894]: [SP] node 0 : [ 0.688029, -0.661066, 0.050555]
[DEBUG] [1624024341.177198449]: [SP] node 1 : [ -1.220038, -0.149801, -0.262314]
[DEBUG] [1624024341.177205145]: [SP] node 2 : [ 0.688029, -0.452009, 0.255324]
[DEBUG] [1624024341.177211383]: [SP] node 3 : [ -0.159644, -0.777400, -1.526689]
[DEBUG] [1624024341.177218744]: [SP] node 4 : [ -0.017605, 0.119402, 0.255324]
[ INFO] [1624024341.220290828]: [OA] Planning started, using 1 cameras

[ INFO] [1624024341.241977899]: [SP]Tree (104 nodes, 5 path nodes, 40 expanded) calculated in 10.81ms.
[DEBUG] [1624024341.242009026]: [SP] node 0 : [ -0.301664, -1.010383, 0.258499]
[DEBUG] [1624024341.242018253]: [SP] node 1 : [ 1.361989, -0.162710, -0.458237]
[DEBUG] [1624024341.242029915]: [SP] node 2 : [ -0.482184, -0.454798, 0.258499]
[DEBUG] [1624024341.242038358]: [SP] node 3 : [ 0.394858, -0.689801, -1.523514]
[DEBUG] [1624024341.242045764]: [SP] node 4 : [ -0.017356, 0.119216, 0.258499]
[DEBUG] [1624024341.269899644]: [WG] Generate Waypoint, current position: [-0.017327, 0.119068, 0.259281].
[ INFO] [1624024341.270014238]: [WG] Speed adapted WP: [-0.000457 0.003137 0.006831].
[DEBUG] [1624024341.270085736]: [WG] Smoothed GoTo location: -0.000172, 0.001127, 0.002119, with dt=0.100198
[ INFO] [1624024341.270155894]: [WG] Final waypoint: [-0.000172 0.001127 0.002119]. nan nan nan 

[DEBUG] [1624024341.270217644]: [WG] Using calculated tree

[DEBUG] [1624024341.270291789]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024341.327350093]: [OA] Planning started, using 1 cameras

[ INFO] [1624024341.362243810]: [SP]Tree (118 nodes, 5 path nodes, 40 expanded) calculated in 17.55ms.
[DEBUG] [1624024341.362317895]: [SP] node 0 : [ -0.503786, -0.663019, 0.061588]
[DEBUG] [1624024341.362336538]: [SP] node 1 : [ 1.362239, -0.163019, -0.456050]
[DEBUG] [1624024341.362350084]: [SP] node 2 : [ -0.481934, -0.455107, 0.260686]
[DEBUG] [1624024341.362364753]: [SP] node 3 : [ 0.395108, -0.690110, -1.521327]
[DEBUG] [1624024341.362384365]: [SP] node 4 : [ -0.017106, 0.118907, 0.260686]
[DEBUG] [1624024341.369791746]: [WG] Generate Waypoint, current position: [-0.017021, 0.118838, 0.261117].
[ INFO] [1624024341.369824949]: [WG] Speed adapted WP: [-0.000484 0.003381 0.007430].
[DEBUG] [1624024341.369834090]: [WG] Smoothed GoTo location: -0.000220, 0.001471, 0.002412, with dt=0.099836
[ INFO] [1624024341.369842471]: [WG] Final waypoint: [-0.000220 0.001471 0.002412]. nan nan nan 

[DEBUG] [1624024341.369853589]: [WG] Using calculated tree

[DEBUG] [1624024341.369867537]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024341.420942054]: [OA] Planning started, using 1 cameras

[ INFO] [1624024341.449287422]: [SP]Tree (120 nodes, 5 path nodes, 40 expanded) calculated in 17.01ms.
[DEBUG] [1624024341.449321916]: [SP] node 0 : [ -0.300874, -1.011122, 0.262661]
[DEBUG] [1624024341.449333811]: [SP] node 1 : [ 1.362778, -0.163449, -0.454075]
[DEBUG] [1624024341.449343308]: [SP] node 2 : [ -0.481395, -0.455538, 0.262661]
[DEBUG] [1624024341.449352110]: [SP] node 3 : [ 0.395647, -0.690540, -1.519352]
[DEBUG] [1624024341.449360858]: [SP] node 4 : [ -0.016567, 0.118477, 0.262661]
[DEBUG] [1624024341.469770509]: [WG] Generate Waypoint, current position: [-0.016456, 0.118334, 0.263357].
[ INFO] [1624024341.469807210]: [WG] Speed adapted WP: [-0.000511 0.003673 0.008175].
[DEBUG] [1624024341.469818874]: [WG] Smoothed GoTo location: -0.000267, 0.001818, 0.002777, with dt=0.099982
[ INFO] [1624024341.469829811]: [WG] Final waypoint: [-0.000267 0.001818 0.002777]. nan nan nan 

[DEBUG] [1624024341.469840205]: [WG] Using calculated tree

[DEBUG] [1624024341.469851097]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024341.520123648]: [OA] Planning started, using 1 cameras

[ INFO] [1624024341.545988017]: [SP]Tree (104 nodes, 5 path nodes, 40 expanded) calculated in 12.65ms.
[DEBUG] [1624024341.546020424]: [SP] node 0 : [ -0.440453, -0.856042, 0.065623]
[DEBUG] [1624024341.546035218]: [SP] node 1 : [ 1.363086, -0.163729, -0.452015]
[DEBUG] [1624024341.546046829]: [SP] node 2 : [ -0.481087, -0.455817, 0.264721]
[DEBUG] [1624024341.546057502]: [SP] node 3 : [ 0.395955, -0.690820, -1.517292]
[DEBUG] [1624024341.546069601]: [SP] node 4 : [ -0.016260, 0.118197, 0.264721]
[DEBUG] [1624024341.569853325]: [WG] Generate Waypoint, current position: [-0.016196, 0.118075, 0.265111].
[ INFO] [1624024341.569893920]: [WG] Speed adapted WP: [-0.000524 0.003822 0.008582].
[DEBUG] [1624024341.569905471]: [WG] Smoothed GoTo location: -0.000310, 0.002146, 0.003190, with dt=0.100086
[ INFO] [1624024341.569916210]: [WG] Final waypoint: [-0.000310 0.002146 0.003190]. nan nan nan 

[DEBUG] [1624024341.569927524]: [WG] Using calculated tree

[DEBUG] [1624024341.569941478]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024341.622679786]: [OA] Planning started, using 1 cameras

[ INFO] [1624024341.645282594]: [SP]Tree (102 nodes, 5 path nodes, 40 expanded) calculated in 9.90ms.
[DEBUG] [1624024341.645309374]: [SP] node 0 : [ -0.440320, -0.856164, 0.066928]
[DEBUG] [1624024341.645317510]: [SP] node 1 : [ 1.363218, -0.163851, -0.450710]
[DEBUG] [1624024341.645324000]: [SP] node 2 : [ -0.480954, -0.455939, 0.266026]
[DEBUG] [1624024341.645330358]: [SP] node 3 : [ 0.396088, -0.690942, -1.515987]
[DEBUG] [1624024341.645338109]: [SP] node 4 : [ -0.016127, 0.118075, 0.266026]
[DEBUG] [1624024341.669863215]: [WG] Generate Waypoint, current position: [-0.016143, 0.118146, 0.266405].
[ INFO] [1624024341.669897892]: [WG] Speed adapted WP: [-0.000528 0.003867 0.008720].
[DEBUG] [1624024341.669906879]: [WG] Smoothed GoTo location: -0.000347, 0.002439, 0.003626, with dt=0.100006
[ INFO] [1624024341.669916310]: [WG] Final waypoint: [-0.000347 0.002439 0.003626]. nan nan nan 

[DEBUG] [1624024341.669924747]: [WG] Using calculated tree

[DEBUG] [1624024341.669934971]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024341.720184331]: [OA] Planning started, using 1 cameras

[ INFO] [1624024341.746782649]: [SP]Tree (96 nodes, 5 path nodes, 40 expanded) calculated in 13.09ms.
[DEBUG] [1624024341.746815566]: [SP] node 0 : [ -0.502500, -0.663640, 0.067594]
[DEBUG] [1624024341.746826596]: [SP] node 1 : [ 1.363525, -0.163640, -0.450044]
[DEBUG] [1624024341.746839483]: [SP] node 2 : [ -0.480648, -0.455728, 0.266691]
[DEBUG] [1624024341.746852182]: [SP] node 3 : [ 0.396394, -0.690731, -1.515322]
[DEBUG] [1624024341.746863616]: [SP] node 4 : [ -0.015820, 0.118286, 0.266691]
[DEBUG] [1624024341.769787079]: [WG] Generate Waypoint, current position: [-0.015775, 0.118289, 0.266801].
[ INFO] [1624024341.769826110]: [WG] Speed adapted WP: [-0.000484 0.003626 0.008178].
[DEBUG] [1624024341.769838790]: [WG] Smoothed GoTo location: -0.000375, 0.002665, 0.004047, with dt=0.099926
[ INFO] [1624024341.769851330]: [WG] Final waypoint: [-0.000375 0.002665 0.004047]. nan nan nan 

[DEBUG] [1624024341.769864022]: [WG] Using calculated tree

[DEBUG] [1624024341.769876630]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024341.821883281]: [OA] Planning started, using 1 cameras

[DEBUG] [1624024341.869728473]: [WG] Generate Waypoint, current position: [-0.016041, 0.118138, 0.266444].
[ INFO] [1624024341.869768949]: [WG] Speed adapted WP: [-0.000425 0.003128 0.007054].
[DEBUG] [1624024341.869798882]: [WG] Smoothed GoTo location: -0.000390, 0.002794, 0.004412, with dt=0.099942
[ INFO] [1624024341.869813882]: [WG] Final waypoint: [-0.000390 0.002794 0.004412]. nan nan nan 

[DEBUG] [1624024341.869827969]: [WG] Using calculated tree

[DEBUG] [1624024341.869841874]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024341.869888064]: [SP]Tree (101 nodes, 5 path nodes, 40 expanded) calculated in 17.79ms.
[DEBUG] [1624024341.869912201]: [SP] node 0 : [ -0.440089, -0.856008, 0.067488]
[DEBUG] [1624024341.869923150]: [SP] node 1 : [ 1.363450, -0.163694, -0.450150]
[DEBUG] [1624024341.869933220]: [SP] node 2 : [ -0.480723, -0.455783, 0.266586]
[DEBUG] [1624024341.869942326]: [SP] node 3 : [ 0.396320, -0.690785, -1.515427]
[DEBUG] [1624024341.869950163]: [SP] node 4 : [ -0.015895, 0.118232, 0.266586]
[ INFO] [1624024341.920565124]: [OA] Planning started, using 1 cameras

[ INFO] [1624024341.943573796]: [SP]Tree (119 nodes, 5 path nodes, 40 expanded) calculated in 11.54ms.
[DEBUG] [1624024341.943606476]: [SP] node 0 : [ -0.440145, -0.856229, 0.067235]
[DEBUG] [1624024341.943618415]: [SP] node 1 : [ 1.363394, -0.163915, -0.450403]
[DEBUG] [1624024341.943628942]: [SP] node 2 : [ -0.480779, -0.456003, 0.266333]
[DEBUG] [1624024341.943637452]: [SP] node 3 : [ 0.396264, -0.691006, -1.515680]
[DEBUG] [1624024341.943645913]: [SP] node 4 : [ -0.015951, 0.118011, 0.266333]
[DEBUG] [1624024341.969788050]: [WG] Generate Waypoint, current position: [-0.015991, 0.117964, 0.266395].
[ INFO] [1624024341.969823659]: [WG] Speed adapted WP: [-0.000375 0.002769 0.006253].
[DEBUG] [1624024341.969834203]: [WG] Smoothed GoTo location: -0.000394, 0.002839, 0.004707, with dt=0.100057
[ INFO] [1624024341.969843888]: [WG] Final waypoint: [-0.000394 0.002839 0.004707]. nan nan nan 

[DEBUG] [1624024341.969853039]: [WG] Using calculated tree

[DEBUG] [1624024341.969861981]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024342.021291358]: [OA] Planning started, using 1 cameras

[ INFO] [1624024342.046241552]: [SP]Tree (95 nodes, 5 path nodes, 40 expanded) calculated in 12.66ms.
[DEBUG] [1624024342.046284130]: [SP] node 0 : [ -0.561571, -0.938240, 0.266274]
[DEBUG] [1624024342.046295448]: [SP] node 1 : [ 1.181574, -0.269109, -0.450462]
[DEBUG] [1624024342.046304270]: [SP] node 2 : [ -0.683028, -0.366829, 0.266274]
[DEBUG] [1624024342.046312427]: [SP] node 3 : [ 0.125990, -0.779044, -1.515739]
[DEBUG] [1624024342.046320775]: [SP] node 4 : [ -0.016050, 0.117759, 0.266274]
[DEBUG] [1624024342.069781539]: [WG] Generate Waypoint, current position: [-0.016108, 0.117589, 0.266015].
[ INFO] [1624024342.069818320]: [WG] Speed adapted WP: [-0.000316 0.002310 0.005225].
[DEBUG] [1624024342.069830097]: [WG] Smoothed GoTo location: -0.000388, 0.002803, 0.004914, with dt=0.099994
[ INFO] [1624024342.069841889]: [WG] Final waypoint: [-0.000388 0.002803 0.004914]. nan nan nan 

[DEBUG] [1624024342.069854104]: [WG] Using calculated tree

[DEBUG] [1624024342.069871635]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024342.120471243]: [OA] Planning started, using 1 cameras

[ INFO] [1624024342.147287844]: [SP]Tree (119 nodes, 6 path nodes, 40 expanded) calculated in 16.07ms.
[DEBUG] [1624024342.147322898]: [SP] node 0 : [ -1.380337, -0.552442, -0.450080]
[DEBUG] [1624024342.147334673]: [SP] node 1 : [ 0.484265, -0.650161, 0.266656]
[DEBUG] [1624024342.147346598]: [SP] node 2 : [ -1.258880, 0.018969, -0.450080]
[DEBUG] [1624024342.147356864]: [SP] node 3 : [ 0.585293, -0.273119, 0.266656]
[DEBUG] [1624024342.147367030]: [SP] node 4 : [ -0.053324, -0.598510, -1.600505]
[DEBUG] [1624024342.147378074]: [SP] node 5 : [ -0.015812, 0.117243, 0.266656]
[DEBUG] [1624024342.169787476]: [WG] Generate Waypoint, current position: [-0.015814, 0.117068, 0.266926].
[ INFO] [1624024342.169823998]: [WG] Speed adapted WP: [-0.000309 0.002284 0.005208].
[DEBUG] [1624024342.169836852]: [WG] Smoothed GoTo location: -0.000377, 0.002738, 0.005059, with dt=0.100005
[ INFO] [1624024342.169850674]: [WG] Final waypoint: [-0.000377 0.002738 0.005059]. nan nan nan 

[DEBUG] [1624024342.169863653]: [WG] Using calculated tree

[DEBUG] [1624024342.169876569]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024342.220439534]: [OA] Planning started, using 1 cameras

[ INFO] [1624024342.247289872]: [SP]Tree (132 nodes, 6 path nodes, 40 expanded) calculated in 13.63ms.
[DEBUG] [1624024342.247328744]: [SP] node 0 : [ -1.311375, -0.227439, -0.250157]
[DEBUG] [1624024342.247341169]: [SP] node 1 : [ 0.554650, -0.727439, 0.267481]
[DEBUG] [1624024342.247353201]: [SP] node 2 : [ -1.248889, -0.244182, -0.449255]
[DEBUG] [1624024342.247369453]: [SP] node 3 : [ 0.615713, -0.341902, 0.267481]
[DEBUG] [1624024342.247388521]: [SP] node 4 : [ -0.053418, -0.598757, -1.599680]
[DEBUG] [1624024342.247405411]: [SP] node 5 : [ -0.015906, 0.116996, 0.267481]
[DEBUG] [1624024342.269760126]: [WG] Generate Waypoint, current position: [-0.015988, 0.116910, 0.267617].
[ INFO] [1624024342.269797782]: [WG] Speed adapted WP: [-0.000309 0.002257 0.005166].
[DEBUG] [1624024342.269812213]: [WG] Smoothed GoTo location: -0.000367, 0.002666, 0.005157, with dt=0.099974
[ INFO] [1624024342.269824909]: [WG] Final waypoint: [-0.000367 0.002666 0.005157]. nan nan nan 

[DEBUG] [1624024342.269836311]: [WG] Using calculated tree

[DEBUG] [1624024342.269848017]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024342.320502145]: [OA] Planning started, using 1 cameras

[ INFO] [1624024342.346883914]: [SP]Tree (130 nodes, 6 path nodes, 40 expanded) calculated in 16.02ms.
[DEBUG] [1624024342.346919231]: [SP] node 0 : [ -1.341928, -0.159175, -0.250025]
[DEBUG] [1624024342.346930401]: [SP] node 1 : [ 0.524097, -0.659175, 0.267614]
[DEBUG] [1624024342.346946384]: [SP] node 2 : [ -1.279442, -0.175919, -0.449122]
[DEBUG] [1624024342.346957867]: [SP] node 3 : [ 0.585160, -0.273638, 0.267614]
[DEBUG] [1624024342.346967083]: [SP] node 4 : [ -0.053456, -0.599030, -1.599547]
[DEBUG] [1624024342.346982271]: [SP] node 5 : [ -0.015945, 0.116724, 0.267614]
[DEBUG] [1624024342.369778343]: [WG] Generate Waypoint, current position: [-0.016016, 0.116580, 0.267742].
[ INFO] [1624024342.369819159]: [WG] Speed adapted WP: [-0.000277 0.002020 0.004639].
[DEBUG] [1624024342.369830984]: [WG] Smoothed GoTo location: -0.000354, 0.002575, 0.005205, with dt=0.100022
[ INFO] [1624024342.369842117]: [WG] Final waypoint: [-0.000354 0.002575 0.005205]. nan nan nan 

[DEBUG] [1624024342.369852250]: [WG] Using calculated tree

[DEBUG] [1624024342.369862805]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024342.421233530]: [OA] Planning started, using 1 cameras

[ INFO] [1624024342.443544395]: [SP]Tree (135 nodes, 6 path nodes, 40 expanded) calculated in 10.29ms.
[DEBUG] [1624024342.443574908]: [SP] node 0 : [ -1.339585, -0.152981, -0.249341]
[DEBUG] [1624024342.443588403]: [SP] node 1 : [ 0.463954, -0.845294, 0.268297]
[DEBUG] [1624024342.443599067]: [SP] node 2 : [ -1.279191, -0.176164, -0.448439]
[DEBUG] [1624024342.443609903]: [SP] node 3 : [ 0.585411, -0.273884, 0.268297]
[DEBUG] [1624024342.443620486]: [SP] node 4 : [ -0.053206, -0.599275, -1.598864]
[DEBUG] [1624024342.443631970]: [SP] node 5 : [ -0.015695, 0.116479, 0.268297]
[DEBUG] [1624024342.469868115]: [WG] Generate Waypoint, current position: [-0.015563, 0.116376, 0.268530].
[ INFO] [1624024342.469902329]: [WG] Speed adapted WP: [-0.000272 0.002031 0.004686].
[DEBUG] [1624024342.469910907]: [WG] Smoothed GoTo location: -0.000341, 0.002487, 0.005221, with dt=0.100085
[ INFO] [1624024342.469918297]: [WG] Final waypoint: [-0.000341 0.002487 0.005221]. nan nan nan 

[DEBUG] [1624024342.469930769]: [WG] Using calculated tree

[DEBUG] [1624024342.469941880]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024342.520550836]: [OA] Planning started, using 1 cameras

[ INFO] [1624024342.545701128]: [SP]Tree (127 nodes, 6 path nodes, 40 expanded) calculated in 12.62ms.
[DEBUG] [1624024342.545768150]: [SP] node 0 : [ -1.341487, -0.159794, -0.248715]
[DEBUG] [1624024342.545787987]: [SP] node 1 : [ 0.524539, -0.659794, 0.268923]
[DEBUG] [1624024342.545806081]: [SP] node 2 : [ -1.279000, -0.176537, -0.447813]
[DEBUG] [1624024342.545822823]: [SP] node 3 : [ 0.585602, -0.274257, 0.268923]
[DEBUG] [1624024342.545839323]: [SP] node 4 : [ -0.053015, -0.599648, -1.598238]
[DEBUG] [1624024342.545854271]: [SP] node 5 : [ -0.015504, 0.116106, 0.268923]
[DEBUG] [1624024342.569785111]: [WG] Generate Waypoint, current position: [-0.015556, 0.115992, 0.269148].
[ INFO] [1624024342.569821063]: [WG] Speed adapted WP: [-0.000264 0.001967 0.004563].
[DEBUG] [1624024342.569835249]: [WG] Smoothed GoTo location: -0.000329, 0.002402, 0.005212, with dt=0.099917
[ INFO] [1624024342.569852187]: [WG] Final waypoint: [-0.000329 0.002402 0.005212]. nan nan nan 

[DEBUG] [1624024342.569867574]: [WG] Using calculated tree

[DEBUG] [1624024342.569881066]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024342.621038708]: [OA] Planning started, using 1 cameras

[ INFO] [1624024342.646373303]: [SP]Tree (129 nodes, 6 path nodes, 40 expanded) calculated in 12.82ms.
[DEBUG] [1624024342.646405520]: [SP] node 0 : [ -1.310772, -0.228607, -0.247633]
[DEBUG] [1624024342.646426323]: [SP] node 1 : [ 0.555254, -0.728607, 0.270005]
[DEBUG] [1624024342.646444532]: [SP] node 2 : [ -1.248285, -0.245351, -0.446731]
[DEBUG] [1624024342.646458027]: [SP] node 3 : [ 0.616317, -0.343070, 0.270005]
[DEBUG] [1624024342.646475488]: [SP] node 4 : [ -0.052814, -0.599926, -1.597156]
[DEBUG] [1624024342.646490533]: [SP] node 5 : [ -0.015303, 0.115828, 0.270005]
[DEBUG] [1624024342.669865760]: [WG] Generate Waypoint, current position: [-0.015158, 0.115797, 0.270211].
[ INFO] [1624024342.669906492]: [WG] Speed adapted WP: [-0.000271 0.002072 0.004835].
[DEBUG] [1624024342.669918928]: [WG] Smoothed GoTo location: -0.000318, 0.002338, 0.005194, with dt=0.100086
[ INFO] [1624024342.669934685]: [WG] Final waypoint: [-0.000318 0.002338 0.005194]. nan nan nan 

[DEBUG] [1624024342.669952858]: [WG] Using calculated tree

[DEBUG] [1624024342.669967191]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024342.720751635]: [OA] Planning started, using 1 cameras

[ INFO] [1624024342.746514489]: [SP]Tree (127 nodes, 6 path nodes, 40 expanded) calculated in 13.23ms.
[DEBUG] [1624024342.746555612]: [SP] node 0 : [ -1.310285, -0.228520, -0.247417]
[DEBUG] [1624024342.746582046]: [SP] node 1 : [ 0.555740, -0.728520, 0.270221]
[DEBUG] [1624024342.746599769]: [SP] node 2 : [ -1.247799, -0.245263, -0.446515]
[DEBUG] [1624024342.746627539]: [SP] node 3 : [ 0.616803, -0.342983, 0.270221]
[DEBUG] [1624024342.746646116]: [SP] node 4 : [ -0.052327, -0.599838, -1.596940]
[DEBUG] [1624024342.746664814]: [SP] node 5 : [ -0.014816, 0.115915, 0.270221]
[DEBUG] [1624024342.769762258]: [WG] Generate Waypoint, current position: [-0.014763, 0.115917, 0.269994].
[ INFO] [1624024342.769802064]: [WG] Speed adapted WP: [-0.000230 0.001807 0.004209].
[DEBUG] [1624024342.769815391]: [WG] Smoothed GoTo location: -0.000306, 0.002262, 0.005154, with dt=0.099895
[ INFO] [1624024342.769831890]: [WG] Final waypoint: [-0.000306 0.002262 0.005154]. nan nan nan 

[DEBUG] [1624024342.769850684]: [WG] Using calculated tree

[DEBUG] [1624024342.769868261]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024342.821637386]: [OA] Planning started, using 1 cameras

[ INFO] [1624024342.843057078]: [SP]Tree (134 nodes, 6 path nodes, 40 expanded) calculated in 9.56ms.
[DEBUG] [1624024342.843088681]: [SP] node 0 : [ -1.340429, -0.160191, -0.247866]
[DEBUG] [1624024342.843101263]: [SP] node 1 : [ 0.525597, -0.660191, 0.269772]
[DEBUG] [1624024342.843115066]: [SP] node 2 : [ -1.277942, -0.176934, -0.446964]
[DEBUG] [1624024342.843126740]: [SP] node 3 : [ 0.586660, -0.274654, 0.269772]
[DEBUG] [1624024342.843138319]: [SP] node 4 : [ -0.051957, -0.600045, -1.597389]
[DEBUG] [1624024342.843150109]: [SP] node 5 : [ -0.014446, 0.115708, 0.269772]
[DEBUG] [1624024342.869871177]: [WG] Generate Waypoint, current position: [-0.014349, 0.115621, 0.269564].
[ INFO] [1624024342.869907281]: [WG] Speed adapted WP: [-0.000178 0.001437 0.003349].
[DEBUG] [1624024342.869916882]: [WG] Smoothed GoTo location: -0.000288, 0.002151, 0.005073, with dt=0.100107
[ INFO] [1624024342.869926603]: [WG] Final waypoint: [-0.000288 0.002151 0.005073]. nan nan nan 

[DEBUG] [1624024342.869935449]: [WG] Using calculated tree

[DEBUG] [1624024342.869945621]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024342.920917299]: [OA] Planning started, using 1 cameras

[ INFO] [1624024342.946804555]: [SP]Tree (126 nodes, 6 path nodes, 40 expanded) calculated in 14.88ms.
[DEBUG] [1624024342.946845894]: [SP] node 0 : [ -1.340064, -0.160340, -0.248958]
[DEBUG] [1624024342.946866576]: [SP] node 1 : [ 0.525961, -0.660340, 0.268681]
[DEBUG] [1624024342.946885522]: [SP] node 2 : [ -1.277578, -0.177084, -0.448055]
[DEBUG] [1624024342.946902003]: [SP] node 3 : [ 0.587024, -0.274803, 0.268681]
[DEBUG] [1624024342.946917293]: [SP] node 4 : [ -0.051592, -0.600195, -1.598480]
[DEBUG] [1624024342.946932750]: [SP] node 5 : [ -0.014081, 0.115559, 0.268681]
[DEBUG] [1624024342.969786601]: [WG] Generate Waypoint, current position: [-0.014038, 0.115541, 0.268496].
[ INFO] [1624024342.969823019]: [WG] Speed adapted WP: [-0.000113 0.000932 0.002165].
[DEBUG] [1624024342.969834654]: [WG] Smoothed GoTo location: -0.000264, 0.001989, 0.004933, with dt=0.099915
[ INFO] [1624024342.969851449]: [WG] Final waypoint: [-0.000264 0.001989 0.004933]. nan nan nan 

[DEBUG] [1624024342.969869089]: [WG] Using calculated tree

[DEBUG] [1624024342.969887364]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024343.021403711]: [OA] Planning started, using 1 cameras

[ INFO] [1624024343.047056762]: [SP]Tree (133 nodes, 6 path nodes, 40 expanded) calculated in 12.34ms.
[DEBUG] [1624024343.047087704]: [SP] node 0 : [ -1.337629, -0.153952, -0.249543]
[DEBUG] [1624024343.047109660]: [SP] node 1 : [ 0.465910, -0.846266, 0.268095]
[DEBUG] [1624024343.047123360]: [SP] node 2 : [ -1.277235, -0.177135, -0.448641]
[DEBUG] [1624024343.047138834]: [SP] node 3 : [ 0.587367, -0.274855, 0.268095]
[DEBUG] [1624024343.047153367]: [SP] node 4 : [ -0.051250, -0.600246, -1.599066]
[DEBUG] [1624024343.047173904]: [SP] node 5 : [ -0.013738, 0.115507, 0.268095]
[DEBUG] [1624024343.069736476]: [WG] Generate Waypoint, current position: [-0.013692, 0.115435, 0.267970].
[ INFO] [1624024343.069769185]: [WG] Speed adapted WP: [-0.000077 0.000647 0.001502].
[DEBUG] [1624024343.069778912]: [WG] Smoothed GoTo location: -0.000237, 0.001795, 0.004738, with dt=0.099947
[ INFO] [1624024343.069786907]: [WG] Final waypoint: [-0.000237 0.001795 0.004738]. nan nan nan 

[DEBUG] [1624024343.069796076]: [WG] Using calculated tree

[DEBUG] [1624024343.069806088]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024343.121447020]: [OA] Planning started, using 1 cameras

[ INFO] [1624024343.145557942]: [SP]Tree (130 nodes, 6 path nodes, 40 expanded) calculated in 11.88ms.
[DEBUG] [1624024343.145602378]: [SP] node 0 : [ -1.337083, -0.154133, -0.249387]
[DEBUG] [1624024343.145616998]: [SP] node 1 : [ 0.466456, -0.846447, 0.268251]
[DEBUG] [1624024343.145630318]: [SP] node 2 : [ -1.276689, -0.177316, -0.448485]
[DEBUG] [1624024343.145651063]: [SP] node 3 : [ 0.587913, -0.275036, 0.268251]
[DEBUG] [1624024343.145668070]: [SP] node 4 : [ -0.050703, -0.600427, -1.598910]
[DEBUG] [1624024343.145685238]: [SP] node 5 : [ -0.013192, 0.115326, 0.268251]
[DEBUG] [1624024343.169874627]: [WG] Generate Waypoint, current position: [-0.013072, 0.115245, 0.268401].
[ INFO] [1624024343.169914672]: [WG] Speed adapted WP: [-0.000078 0.000685 0.001594].
[DEBUG] [1624024343.169925749]: [WG] Smoothed GoTo location: -0.000211, 0.001612, 0.004517, with dt=0.100145
[ INFO] [1624024343.169936520]: [WG] Final waypoint: [-0.000211 0.001612 0.004517]. nan nan nan 

[DEBUG] [1624024343.169946655]: [WG] Using calculated tree

[DEBUG] [1624024343.169956072]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024343.221240871]: [OA] Planning started, using 1 cameras

[ INFO] [1624024343.244830000]: [SP]Tree (132 nodes, 6 path nodes, 40 expanded) calculated in 11.90ms.
[DEBUG] [1624024343.244859094]: [SP] node 0 : [ -1.336792, -0.153780, -0.248705]
[DEBUG] [1624024343.244869138]: [SP] node 1 : [ 0.466747, -0.846094, 0.268933]
[DEBUG] [1624024343.244877578]: [SP] node 2 : [ -1.276398, -0.176963, -0.447803]
[DEBUG] [1624024343.244886112]: [SP] node 3 : [ 0.588204, -0.274683, 0.268933]
[DEBUG] [1624024343.244904523]: [SP] node 4 : [ -0.050413, -0.600074, -1.598228]
[DEBUG] [1624024343.244913869]: [SP] node 5 : [ -0.012902, 0.115679, 0.268933]
[DEBUG] [1624024343.269861121]: [WG] Generate Waypoint, current position: [-0.012937, 0.115819, 0.269106].
[ INFO] [1624024343.269901067]: [WG] Speed adapted WP: [-0.000103 0.000926 0.002151].
[DEBUG] [1624024343.269911905]: [WG] Smoothed GoTo location: -0.000190, 0.001477, 0.004301, with dt=0.099986
[ INFO] [1624024343.269921892]: [WG] Final waypoint: [-0.000190 0.001477 0.004301]. nan nan nan 

[DEBUG] [1624024343.269931752]: [WG] Using calculated tree

[DEBUG] [1624024343.269940704]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024343.320968232]: [OA] Planning started, using 1 cameras

[ INFO] [1624024343.346708075]: [SP]Tree (126 nodes, 6 path nodes, 40 expanded) calculated in 15.58ms.
[DEBUG] [1624024343.346747504]: [SP] node 0 : [ -1.338544, -0.160430, -0.248231]
[DEBUG] [1624024343.346759536]: [SP] node 1 : [ 0.527481, -0.660430, 0.269407]
[DEBUG] [1624024343.346769158]: [SP] node 2 : [ -1.276058, -0.177173, -0.447329]
[DEBUG] [1624024343.346778855]: [SP] node 3 : [ 0.588544, -0.274893, 0.269407]
[DEBUG] [1624024343.346788237]: [SP] node 4 : [ -0.050073, -0.600284, -1.597754]
[DEBUG] [1624024343.346796645]: [SP] node 5 : [ -0.012561, 0.115470, 0.269407]
[DEBUG] [1624024343.369787762]: [WG] Generate Waypoint, current position: [-0.012401, 0.115376, 0.269633].
[ INFO] [1624024343.369829356]: [WG] Speed adapted WP: [-0.000100 0.000930 0.002174].
[DEBUG] [1624024343.369841443]: [WG] Smoothed GoTo location: -0.000174, 0.001372, 0.004096, with dt=0.099928
[ INFO] [1624024343.369852845]: [WG] Final waypoint: [-0.000174 0.001372 0.004096]. nan nan nan 

[DEBUG] [1624024343.369863595]: [WG] Using calculated tree

[DEBUG] [1624024343.369873933]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024343.421246316]: [OA] Planning started, using 1 cameras

[ INFO] [1624024343.447003415]: [SP]Tree (136 nodes, 6 path nodes, 40 expanded) calculated in 13.17ms.
[DEBUG] [1624024343.447034809]: [SP] node 0 : [ 1.378687, -0.479200, -0.247523]
[DEBUG] [1624024343.447047459]: [SP] node 1 : [ -0.529380, -0.781408, 0.270115]
[DEBUG] [1624024343.447057558]: [SP] node 2 : [ 1.213764, -0.112278, -0.446621]
[DEBUG] [1624024343.447066195]: [SP] node 3 : [ -0.650837, -0.209997, 0.270115]
[DEBUG] [1624024343.447076933]: [SP] node 4 : [ -0.049732, -0.600360, -1.597046]
[DEBUG] [1624024343.447100064]: [SP] node 5 : [ -0.012221, 0.115394, 0.270115]
[DEBUG] [1624024343.469862213]: [WG] Generate Waypoint, current position: [-0.012246, 0.115234, 0.270309].
[ INFO] [1624024343.469904486]: [WG] Speed adapted WP: [-0.000110 0.001032 0.002420].
[DEBUG] [1624024343.469915882]: [WG] Smoothed GoTo location: -0.000161, 0.001300, 0.003912, with dt=0.100075
[ INFO] [1624024343.469927428]: [WG] Final waypoint: [-0.000161 0.001300 0.003912]. nan nan nan 

[DEBUG] [1624024343.469937945]: [WG] Using calculated tree

[DEBUG] [1624024343.469947980]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024343.521299392]: [OA] Planning started, using 1 cameras

[ INFO] [1624024343.549083471]: [SP]Tree (133 nodes, 6 path nodes, 40 expanded) calculated in 12.85ms.
[DEBUG] [1624024343.549106614]: [SP] node 0 : [ 1.298954, -0.498541, -0.247161]
[DEBUG] [1624024343.549114748]: [SP] node 1 : [ -0.630250, -0.599647, 0.270477]
[DEBUG] [1624024343.549121615]: [SP] node 2 : [ 1.213923, -0.307559, -0.446259]
[DEBUG] [1624024343.549129671]: [SP] node 3 : [ -0.650679, -0.209839, 0.270477]
[DEBUG] [1624024343.549137472]: [SP] node 4 : [ -0.049573, -0.600201, -1.596684]
[DEBUG] [1624024343.549146013]: [SP] node 5 : [ -0.012062, 0.115552, 0.270477]
[DEBUG] [1624024343.569867154]: [WG] Generate Waypoint, current position: [-0.012090, 0.115757, 0.270587].
[ INFO] [1624024343.569902333]: [WG] Speed adapted WP: [-0.000114 0.001092 0.002552].
[DEBUG] [1624024343.569910697]: [WG] Smoothed GoTo location: -0.000152, 0.001252, 0.003751, with dt=0.100000
[ INFO] [1624024343.569917657]: [WG] Final waypoint: [-0.000152 0.001252 0.003751]. nan nan nan 

[DEBUG] [1624024343.569926362]: [WG] Using calculated tree

[DEBUG] [1624024343.569934152]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024343.621287201]: [OA] Planning started, using 1 cameras

[ INFO] [1624024343.649287954]: [SP]Tree (137 nodes, 6 path nodes, 40 expanded) calculated in 10.96ms.
[DEBUG] [1624024343.649326273]: [SP] node 0 : [ -1.347356, -0.220079, -0.246816]
[DEBUG] [1624024343.649334918]: [SP] node 1 : [ 0.518669, -0.720079, 0.270822]
[DEBUG] [1624024343.649342937]: [SP] node 2 : [ -1.224476, -0.050949, -0.445914]
[DEBUG] [1624024343.649350694]: [SP] node 3 : [ 0.619697, -0.343037, 0.270822]
[DEBUG] [1624024343.649357624]: [SP] node 4 : [ -0.049433, -0.599892, -1.596339]
[DEBUG] [1624024343.649364206]: [SP] node 5 : [ -0.011922, 0.115861, 0.270822]
[DEBUG] [1624024343.669755495]: [WG] Generate Waypoint, current position: [-0.011805, 0.115854, 0.270790].
[ INFO] [1624024343.669785489]: [WG] Speed adapted WP: [-0.000108 0.001058 0.002474].
[DEBUG] [1624024343.669794394]: [WG] Smoothed GoTo location: -0.000144, 0.001216, 0.003607, with dt=0.099883
[ INFO] [1624024343.669802810]: [WG] Final waypoint: [-0.000144 0.001216 0.003607]. nan nan nan 

[DEBUG] [1624024343.669810687]: [WG] Using calculated tree

[DEBUG] [1624024343.669817807]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024343.721502263]: [OA] Planning started, using 1 cameras

[ INFO] [1624024343.751048924]: [SP]Tree (141 nodes, 6 path nodes, 40 expanded) calculated in 17.80ms.
[DEBUG] [1624024343.751087357]: [SP] node 0 : [ -1.955111, 0.575808, -0.247431]
[DEBUG] [1624024343.751104123]: [SP] node 1 : [ -0.589086, -0.790218, 0.270207]
[DEBUG] [1624024343.751118663]: [SP] node 2 : [ 1.214453, -0.306961, -0.446529]
[DEBUG] [1624024343.751133618]: [SP] node 3 : [ -0.650149, -0.209241, 0.270207]
[DEBUG] [1624024343.751150358]: [SP] node 4 : [ -0.049044, -0.599604, -1.596954]
[DEBUG] [1624024343.751166590]: [SP] node 5 : [ -0.011532, 0.116150, 0.270207]
[DEBUG] [1624024343.769780768]: [WG] Generate Waypoint, current position: [-0.011624, 0.116203, 0.269775].
[ INFO] [1624024343.769815548]: [WG] Speed adapted WP: [-0.000067 0.000673 0.001562].
[DEBUG] [1624024343.769827217]: [WG] Smoothed GoTo location: -0.000134, 0.001148, 0.003452, with dt=0.100028
[ INFO] [1624024343.769838306]: [WG] Final waypoint: [-0.000134 0.001148 0.003452]. nan nan nan 

[DEBUG] [1624024343.769848691]: [WG] Using calculated tree

[DEBUG] [1624024343.769861089]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024343.821996380]: [OA] Planning started, using 1 cameras

[ INFO] [1624024343.843451890]: [SP]Tree (141 nodes, 6 path nodes, 40 expanded) calculated in 9.51ms.
[DEBUG] [1624024343.843485290]: [SP] node 0 : [ -1.306760, -0.228009, -0.249193]
[DEBUG] [1624024343.843497759]: [SP] node 1 : [ 0.559265, -0.728009, 0.268445]
[DEBUG] [1624024343.843508833]: [SP] node 2 : [ -1.244274, -0.244752, -0.448291]
[DEBUG] [1624024343.843518332]: [SP] node 3 : [ 0.620328, -0.342472, 0.268445]
[DEBUG] [1624024343.843534311]: [SP] node 4 : [ -0.048802, -0.599327, -1.598716]
[DEBUG] [1624024343.843548807]: [SP] node 5 : [ -0.011291, 0.116427, 0.268445]
[DEBUG] [1624024343.869782870]: [WG] Generate Waypoint, current position: [-0.011086, 0.116502, 0.267872].
[ INFO] [1624024343.869813120]: [WG] Speed adapted WP: [-0.000002 0.000020 0.000045].
[DEBUG] [1624024343.869822270]: [WG] Smoothed GoTo location: -0.000117, 0.001010, 0.003249, with dt=0.099999
[ INFO] [1624024343.869834541]: [WG] Final waypoint: [-0.000117 0.001010 0.003249]. nan nan nan 

[DEBUG] [1624024343.869843486]: [WG] Using calculated tree

[DEBUG] [1624024343.869853058]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024343.921750881]: [OA] Planning started, using 1 cameras

[ INFO] [1624024343.955225790]: [SP]Tree (139 nodes, 6 path nodes, 40 expanded) calculated in 19.68ms.
[DEBUG] [1624024343.955259270]: [SP] node 0 : [ -1.954409, 0.575932, -0.250969]
[DEBUG] [1624024343.955270256]: [SP] node 1 : [ -0.588383, -0.790093, 0.266669]
[DEBUG] [1624024343.955284198]: [SP] node 2 : [ 1.215155, -0.306836, -0.450067]
[DEBUG] [1624024343.955297828]: [SP] node 3 : [ -0.649446, -0.209117, 0.266669]
[DEBUG] [1624024343.955317413]: [SP] node 4 : [ -0.048341, -0.599479, -1.600492]
[DEBUG] [1624024343.955330200]: [SP] node 5 : [ -0.010830, 0.116275, 0.266669]
[DEBUG] [1624024343.969868654]: [WG] Generate Waypoint, current position: [-0.010842, 0.116180, 0.266258].
[ INFO] [1624024343.969912047]: [WG] Speed adapted WP: [0.000000 0.000000 0.000000].
[DEBUG] [1624024343.969936618]: [WG] Smoothed GoTo location: -0.000099, 0.000858, 0.003018, with dt=0.100097
[ INFO] [1624024343.969958533]: [WG] Final waypoint: [-0.000099 0.000858 0.003018]. nan nan nan 

[DEBUG] [1624024343.969980351]: [WG] Using calculated tree

[DEBUG] [1624024343.970003266]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024344.021749008]: [OA] Planning started, using 1 cameras

[ INFO] [1624024344.046775408]: [SP]Tree (134 nodes, 6 path nodes, 40 expanded) calculated in 12.69ms.
[DEBUG] [1624024344.046807410]: [SP] node 0 : [ 1.380363, -0.478421, -0.252549]
[DEBUG] [1624024344.046818929]: [SP] node 1 : [ -0.527704, -0.780629, 0.265089]
[DEBUG] [1624024344.046831546]: [SP] node 2 : [ 1.215441, -0.111499, -0.451647]
[DEBUG] [1624024344.046841150]: [SP] node 3 : [ -0.649161, -0.209218, 0.265089]
[DEBUG] [1624024344.046851137]: [SP] node 4 : [ -0.048056, -0.599581, -1.602072]
[DEBUG] [1624024344.046861057]: [SP] node 5 : [ -0.010545, 0.116173, 0.265089]
[DEBUG] [1624024344.069787807]: [WG] Generate Waypoint, current position: [-0.010481, 0.116100, 0.264476].
[ INFO] [1624024344.069826314]: [WG] Speed adapted WP: [0.000000 0.000000 0.000000].
[DEBUG] [1624024344.069839085]: [WG] Smoothed GoTo location: -0.000082, 0.000717, 0.002777, with dt=0.099915
[ INFO] [1624024344.069855351]: [WG] Final waypoint: [-0.000082 0.000717 0.002777]. nan nan nan 

[DEBUG] [1624024344.069872306]: [WG] Using calculated tree

[DEBUG] [1624024344.069887776]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024344.122096118]: [OA] Planning started, using 1 cameras

[ INFO] [1624024344.147103867]: [SP]Tree (137 nodes, 6 path nodes, 40 expanded) calculated in 13.57ms.
[DEBUG] [1624024344.147171006]: [SP] node 0 : [ 1.300734, -0.497851, -0.254300]
[DEBUG] [1624024344.147183049]: [SP] node 1 : [ -0.628470, -0.598956, 0.263338]
[DEBUG] [1624024344.147192659]: [SP] node 2 : [ 1.215703, -0.306868, -0.453398]
[DEBUG] [1624024344.147200993]: [SP] node 3 : [ -0.648899, -0.209148, 0.263338]
[DEBUG] [1624024344.147209447]: [SP] node 4 : [ -0.047793, -0.599511, -1.603823]
[DEBUG] [1624024344.147219884]: [SP] node 5 : [ -0.010282, 0.116243, 0.263338]
[DEBUG] [1624024344.169783926]: [WG] Generate Waypoint, current position: [-0.010203, 0.116328, 0.262896].
[ INFO] [1624024344.169818088]: [WG] Speed adapted WP: [0.000000 0.000000 0.000000].
[DEBUG] [1624024344.169830597]: [WG] Smoothed GoTo location: -0.000068, 0.000593, 0.002536, with dt=0.099992
[ INFO] [1624024344.169845901]: [WG] Final waypoint: [-0.000068 0.000593 0.002536]. nan nan nan 

[DEBUG] [1624024344.169859441]: [WG] Using calculated tree

[DEBUG] [1624024344.169873365]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024344.221918132]: [OA] Planning started, using 1 cameras

[ INFO] [1624024344.247002235]: [SP]Tree (106 nodes, 5 path nodes, 40 expanded) calculated in 12.67ms.
[DEBUG] [1624024344.247040163]: [SP] node 0 : [ -0.496686, -0.665499, 0.062437]
[DEBUG] [1624024344.247053475]: [SP] node 1 : [ 1.369339, -0.165499, -0.455201]
[DEBUG] [1624024344.247069096]: [SP] node 2 : [ -0.474834, -0.457588, 0.261535]
[DEBUG] [1624024344.247085843]: [SP] node 3 : [ 0.402208, -0.692590, -1.520478]
[DEBUG] [1624024344.247102023]: [SP] node 4 : [ -0.010006, 0.116427, 0.261535]
[DEBUG] [1624024344.269786850]: [WG] Generate Waypoint, current position: [-0.010039, 0.116427, 0.260968].
[ INFO] [1624024344.269824935]: [WG] Speed adapted WP: [0.000000 0.000000 0.000000].
[DEBUG] [1624024344.269836884]: [WG] Smoothed GoTo location: -0.000056, 0.000488, 0.002302, with dt=0.100007
[ INFO] [1624024344.269852956]: [WG] Final waypoint: [-0.000056 0.000488 0.002302]. nan nan nan 

[DEBUG] [1624024344.269868850]: [WG] Using calculated tree

[DEBUG] [1624024344.269884444]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024344.322155401]: [OA] Planning started, using 1 cameras

[ INFO] [1624024344.352499544]: [SP]Tree (96 nodes, 5 path nodes, 40 expanded) calculated in 18.08ms.
[DEBUG] [1624024344.352539406]: [SP] node 0 : [ -0.496345, -0.665498, 0.060837]
[DEBUG] [1624024344.352551803]: [SP] node 1 : [ 1.369680, -0.165498, -0.456801]
[DEBUG] [1624024344.352562505]: [SP] node 2 : [ -0.474493, -0.457586, 0.259935]
[DEBUG] [1624024344.352582474]: [SP] node 3 : [ 0.402549, -0.692589, -1.522078]
[DEBUG] [1624024344.352596682]: [SP] node 4 : [ -0.009665, 0.116428, 0.259935]
[DEBUG] [1624024344.369799046]: [WG] Generate Waypoint, current position: [-0.009536, 0.116338, 0.259373].
[ INFO] [1624024344.369838618]: [WG] Speed adapted WP: [0.000000 0.000000 0.000000].
[DEBUG] [1624024344.369850008]: [WG] Smoothed GoTo location: -0.000046, 0.000401, 0.002080, with dt=0.100014
[ INFO] [1624024344.369860313]: [WG] Final waypoint: [-0.000046 0.000401 0.002080]. nan nan nan 

[DEBUG] [1624024344.369869954]: [WG] Using calculated tree

[DEBUG] [1624024344.369881931]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024344.422252990]: [OA] Planning started, using 1 cameras

[ INFO] [1624024344.446005892]: [SP]Tree (101 nodes, 5 path nodes, 40 expanded) calculated in 12.27ms.
[DEBUG] [1624024344.446041050]: [SP] node 0 : [ -0.293605, -1.013228, 0.257138]
[DEBUG] [1624024344.446057345]: [SP] node 1 : [ 1.370048, -0.165555, -0.459598]
[DEBUG] [1624024344.446066827]: [SP] node 2 : [ -0.474125, -0.457643, 0.257138]
[DEBUG] [1624024344.446075558]: [SP] node 3 : [ 0.402917, -0.692646, -1.524875]
[DEBUG] [1624024344.446085722]: [SP] node 4 : [ -0.009298, 0.116371, 0.257138]
[DEBUG] [1624024344.469782914]: [WG] Generate Waypoint, current position: [-0.009251, 0.116389, 0.256271].
[ INFO] [1624024344.469820611]: [WG] Speed adapted WP: [0.000000 0.000000 0.000000].
[DEBUG] [1624024344.469831773]: [WG] Smoothed GoTo location: -0.000038, 0.000329, 0.001873, with dt=0.099982
[ INFO] [1624024344.469842560]: [WG] Final waypoint: [-0.000038 0.000329 0.001873]. nan nan nan 

[DEBUG] [1624024344.469852006]: [WG] Using calculated tree

[DEBUG] [1624024344.469861547]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024344.522352477]: [OA] Planning started, using 1 cameras

[ INFO] [1624024344.547188067]: [SP]Tree (112 nodes, 6 path nodes, 40 expanded) calculated in 13.12ms.
[DEBUG] [1624024344.547227910]: [SP] node 0 : [ -1.794754, 0.202303, -0.262913]
[DEBUG] [1624024344.547239307]: [SP] node 1 : [ -0.293423, -1.013450, 0.254725]
[DEBUG] [1624024344.547249718]: [SP] node 2 : [ 1.370229, -0.165777, -0.462011]
[DEBUG] [1624024344.547258574]: [SP] node 3 : [ -0.473944, -0.457866, 0.254725]
[DEBUG] [1624024344.547266519]: [SP] node 4 : [ 0.403099, -0.692868, -1.527288]
[DEBUG] [1624024344.547274455]: [SP] node 5 : [ -0.009116, 0.116149, 0.254725]
[DEBUG] [1624024344.569782705]: [WG] Generate Waypoint, current position: [-0.009093, 0.115966, 0.254111].
[ INFO] [1624024344.569817708]: [WG] Speed adapted WP: [0.000000 0.000000 0.000000].
[DEBUG] [1624024344.569829407]: [WG] Smoothed GoTo location: -0.000031, 0.000269, 0.001681, with dt=0.099997
[ INFO] [1624024344.569841143]: [WG] Final waypoint: [-0.000031 0.000269 0.001681]. nan nan nan 

[DEBUG] [1624024344.569851383]: [WG] Using calculated tree

[DEBUG] [1624024344.569860597]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024344.622236649]: [OA] Planning started, using 1 cameras

[ INFO] [1624024344.664833214]: [SP]Tree (108 nodes, 5 path nodes, 40 expanded) calculated in 24.31ms.
[DEBUG] [1624024344.664876380]: [SP] node 0 : [ -0.293253, -1.013784, 0.252630]
[DEBUG] [1624024344.664887179]: [SP] node 1 : [ 1.370399, -0.166111, -0.464105]
[DEBUG] [1624024344.664897993]: [SP] node 2 : [ -0.473774, -0.458200, 0.252630]
[DEBUG] [1624024344.664907350]: [SP] node 3 : [ 0.403269, -0.693202, -1.529383]
[DEBUG] [1624024344.664916557]: [SP] node 4 : [ -0.008946, 0.115815, 0.252630]
[DEBUG] [1624024344.669707906]: [WG] Generate Waypoint, current position: [-0.008977, 0.115753, 0.251963].
[ INFO] [1624024344.669731749]: [WG] Speed adapted WP: [0.000000 0.000000 0.000000].
[DEBUG] [1624024344.669741589]: [WG] Smoothed GoTo location: -0.000025, 0.000221, 0.001506, with dt=0.099915
[ INFO] [1624024344.669750044]: [WG] Final waypoint: [-0.000025 0.000221 0.001506]. nan nan nan 

[DEBUG] [1624024344.669757245]: [WG] Using calculated tree

[DEBUG] [1624024344.669765330]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024344.722478972]: [OA] Planning started, using 1 cameras

[ INFO] [1624024344.748776109]: [SP]Tree (100 nodes, 5 path nodes, 40 expanded) calculated in 13.20ms.
[DEBUG] [1624024344.748814186]: [SP] node 0 : [ -0.293002, -1.014105, 0.251128]
[DEBUG] [1624024344.748825670]: [SP] node 1 : [ 1.370651, -0.166432, -0.465608]
[DEBUG] [1624024344.748836850]: [SP] node 2 : [ -0.473522, -0.458520, 0.251128]
[DEBUG] [1624024344.748847563]: [SP] node 3 : [ 0.403520, -0.693523, -1.530885]
[DEBUG] [1624024344.748857177]: [SP] node 4 : [ -0.008695, 0.115494, 0.251128]
[DEBUG] [1624024344.769785753]: [WG] Generate Waypoint, current position: [-0.008612, 0.115389, 0.250898].
[ INFO] [1624024344.769821114]: [WG] Speed adapted WP: [0.000000 0.000000 0.000000].
[DEBUG] [1624024344.769832242]: [WG] Smoothed GoTo location: -0.000021, 0.000181, 0.001346, with dt=0.100089
[ INFO] [1624024344.769842669]: [WG] Final waypoint: [-0.000021 0.000181 0.001346]. nan nan nan 

[DEBUG] [1624024344.769851951]: [WG] Using calculated tree

[DEBUG] [1624024344.769861242]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024344.822540375]: [OA] Planning started, using 1 cameras

[ INFO] [1624024344.848046074]: [SP]Tree (104 nodes, 5 path nodes, 40 expanded) calculated in 12.81ms.
[DEBUG] [1624024344.848077236]: [SP] node 0 : [ -0.292635, -1.014680, 0.250275]
[DEBUG] [1624024344.848088247]: [SP] node 1 : [ 1.371018, -0.167007, -0.466461]
[DEBUG] [1624024344.848099541]: [SP] node 2 : [ -0.473155, -0.459095, 0.250275]
[DEBUG] [1624024344.848108681]: [SP] node 3 : [ 0.403887, -0.694098, -1.531738]
[DEBUG] [1624024344.848117436]: [SP] node 4 : [ -0.008328, 0.114919, 0.250275]
[DEBUG] [1624024344.869789653]: [WG] Generate Waypoint, current position: [-0.008292, 0.114674, 0.249983].
[ INFO] [1624024344.869828663]: [WG] Speed adapted WP: [0.000000 0.000000 0.000000].
[DEBUG] [1624024344.869840109]: [WG] Smoothed GoTo location: -0.000017, 0.000148, 0.001201, with dt=0.100007
[ INFO] [1624024344.869850774]: [WG] Final waypoint: [-0.000017 0.000148 0.001201]. nan nan nan 

[DEBUG] [1624024344.869864736]: [WG] Using calculated tree

[DEBUG] [1624024344.869876455]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024344.922817181]: [OA] Planning started, using 1 cameras

[ INFO] [1624024344.947699049]: [SP]Tree (112 nodes, 6 path nodes, 40 expanded) calculated in 13.26ms.
[DEBUG] [1624024344.947759023]: [SP] node 0 : [ -1.793489, 0.200464, -0.268525]
[DEBUG] [1624024344.947772720]: [SP] node 1 : [ -0.292158, -1.015290, 0.249113]
[DEBUG] [1624024344.947783226]: [SP] node 2 : [ 1.371495, -0.167617, -0.467623]
[DEBUG] [1624024344.947794360]: [SP] node 3 : [ -0.472678, -0.459705, 0.249113]
[DEBUG] [1624024344.947809082]: [SP] node 4 : [ 0.404364, -0.694708, -1.532900]
[DEBUG] [1624024344.947819878]: [SP] node 5 : [ -0.007851, 0.114309, 0.249113]
[DEBUG] [1624024344.969870967]: [WG] Generate Waypoint, current position: [-0.007753, 0.114161, 0.248845].
[ INFO] [1624024344.969912749]: [WG] Speed adapted WP: [0.000000 0.000000 0.000000].
[DEBUG] [1624024344.969925748]: [WG] Smoothed GoTo location: -0.000014, 0.000121, 0.001070, with dt=0.100084
[ INFO] [1624024344.969937853]: [WG] Final waypoint: [-0.000014 0.000121 0.001070]. nan nan nan 

[DEBUG] [1624024344.969951279]: [WG] Using calculated tree

[DEBUG] [1624024344.969962829]:  [WGN] Update to TRY_PATH state 

[ INFO] [1624024345.022684891]: [OA] Planning started, using 1 cameras

[ INFO] [1624024345.047617254]: [SP]Tree (109 nodes, 5 path nodes, 40 expanded) calculated in 12.76ms.
[DEBUG] [1624024345.047654403]: [SP] node 0 : [ -0.291912, -1.015656, 0.247915]
[DEBUG] [1624024345.047671906]: [SP] node 1 : [ 1.371740, -0.167983, -0.468821]
[DEBUG] [1624024345.047686794]: [SP] node 2 : [ -0.472433, -0.460072, 0.247915]
[DEBUG] [1624024345.047705518]: [SP] node 3 : [ 0.404610, -0.695074, -1.534098]
[DEBUG] [1624024345.047723977]: [SP] node 4 : [ -0.007605, 0.113943, 0.247915]
[DEBUG] [1624024345.069856864]: [WG] Generate Waypoint, current position: [-0.007555, 0.113837, 0.247434].
[ INFO] [1624024345.069898246]: [WG] Speed adapted WP: [0.000000 0.000000 0.000000].
[DEBUG] [1624024345.069910553]: [WG] Smoothed GoTo location: -0.000011, 0.000099, 0.000952, with dt=0.099986
[ INFO] [1624024345.069922296]: [WG] Final waypoint: [-0.000011 0.000099 0.000952]. nan nan nan 

[DEBUG] [1624024345.069933246]: [WG] Using calculated tree

[DEBUG] [1624024345.069944562]:  [WGN] Update to TRY_PATH state

This time I perform the test outdoor, I attach the log

https://logs.px4.io/plot_app?log=05484363-29e7-464d-aa6a-767e6c88e458

https://logs.px4.io/plot_app?log=912f7947-22c8-4675-93f1-c7b0da76373c

Cristian-wp commented 3 years ago

@jkflying do you have any ideas?

Jaeyoung-Lim commented 3 years ago

@Cristian-wp From the launchfile you have attached, the baudrate seems to be set at 57600

This is way below the minimum required data rate for the avoidance to work. Could you try with higher baudrates such as 921600?

Cristian-wp commented 3 years ago

Hi @Jaeyoung-Lim I have already try to increase it to 921600 and highter values but nothing change....

Jaeyoung-Lim commented 3 years ago

@Cristian-wp That means you have other issues, the baudrate should be set to 921600 in either case.

Could you share the log with baudrate set to 921600? I believe then the logs should look different

Cristian-wp commented 3 years ago

Hi @Jaeyoung-Lim yes, I just try to reduce the fps to avoid USB3.0 and use 2.0 or C. I do this and after I take the new test. (without propeller because I am in the campus and I can not fly now)

Cristian-wp commented 3 years ago

Hi @Jaeyoung-Lim , this is the log with 921600 baudrate and the camera connected to an USB 2.0.

https://logs.px4.io/plot_app?log=98fd4e87-a3e7-48fb-8490-b1df5b231ae5

This one have the same baudrate but the connection is on USB3.0

https://logs.px4.io/plot_app?log=6b6ebd7e-6c97-427e-9332-2cb0e72baaab

Cristian-wp commented 3 years ago

This time it seems to work, there is a swith between mission and position because I move the roll stick for error

https://logs.px4.io/plot_app?log=4043a0b0-2163-47a5-90dd-4bfe7d1af744

Cristian-wp commented 3 years ago

https://logs.px4.io/plot_app?log=c2e31995-b6ad-4a27-b6e8-6e79fcc9b9bd I have retry immediatly and the error reappear

Cristian-wp commented 3 years ago

I think I try to change fly controller

Cristian-wp commented 3 years ago

Ok I have found something really intresting, this is an extract from the log when the avoidance work: Screenshot from 2021-06-21 17-09-17

This is from a case when the avoidance fail:

Screenshot from 2021-06-21 17-09-32

As you can see in the correct case there is EKF aligned at 0:00:09, in the wrong you can see at 0:00:13 reset velocity to gps and starting gps fusion, after this messages the obstacle avoidance fails. I have just check the other logs and if there are these messages the avoidance does not start...

gajena commented 3 years ago

But in both logs, data shows similar GPS uncertainty and also similar magnetic interference, not sure why ekf is not getting converged. But maybe try in an open field away from buildings (recalibrate the Accel and Gyro) that might help (not sure though).

Cristian-wp commented 3 years ago

Hi @gajena I always do my test in an open field :( (same for calibration, no smartphone or other thing near the drone)

Cristian-wp commented 2 years ago

It was a comunication problem, I have not use the usb-serial adapter.

julianoes commented 2 years ago

Ok, thanks for reporting back. I'll close this issue then.