search

PX4 / PX4-Avoidance

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

Avoidance always crashes #642

Open pirate-lofy opened 3 years ago

pirate-lofy commented 3 years ago

About the problem I used avoidance in simulation for a while and it worked very well, but when i used it with real hardware (jetson tx2/jetson xavier and realsense camera) it only worked for two separated times but most of the time it acts so weird, I thought it was the pixhawk controller is broken so i used a new one and got the same results. I tried HITL with RC and QGC and worked fine. I managed to make avoidance work in HITL mode then got the same results as the real test. I only use a script like https://docs.px4.io/master/en/ros/mavros_offboard.html to arm the vehicle and continuously publish the target point

Problem description first drone go up to 4-5 five meters then moves very slow to a position near from the origin then it suddenly moves very fast in a fixed direction (like there is no avoidance system working) and this scenario is repeated every time even if I changed the goal point.

Expected behavior the same as what happens in simulation

Log Files and Screenshots these are three logs from the HITL test

  1. https://review.px4.io/plot_app?log=2df68fc9-fa7f-4153-ae14-559bd71c55cb
  2. https://review.px4.io/plot_app?log=de0871d5-23af-405f-b7d6-b9f2dd2aec59
  3. https://review.px4.io/plot_app?log=4ab1f906-94f8-42d3-9f8b-00fd041b9f7e

logs this is the last part of local_planner logs from HITL test (this logs is not usually produced in real test nor simulation test)


[ INFO] [1619666857.264763582, 47.324000000]: [SP]Tree (59 nodes, 6 path nodes, 40 expanded) calculated in 18.15ms.
[ INFO] [1619666857.340889767, 47.400000000]: [WG] Speed adapted WP: [2.453394 -0.762474 5.806579].
[ INFO] [1619666857.340922993, 47.400000000]: [WG] Final waypoint: [2.467217 -0.729423 5.688729]. nan nan nan 

[ INFO] [1619666857.349254443, 47.408000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666857.349290361, 47.408000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666857.364797392, 47.420000000]: [SP]Tree (58 nodes, 7 path nodes, 40 expanded) calculated in 20.08ms.
[ INFO] [1619666857.450467071, 47.500000000]: [WG] Speed adapted WP: [2.448656 -0.757350 5.805224].
[ INFO] [1619666857.450508158, 47.500000000]: [WG] Final waypoint: [2.461490 -0.725807 5.687485]. nan nan nan 

[ INFO] [1619666857.467315297, 47.520000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666857.467354974, 47.520000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666857.484949968, 47.540000000]: [SP]Tree (60 nodes, 7 path nodes, 40 expanded) calculated in 11.07ms.
[ INFO] [1619666857.552979906, 47.600000000]: [WG] Speed adapted WP: [2.441467 -0.751906 5.806238].
[ INFO] [1619666857.553044311, 47.600000000]: [WG] Final waypoint: [2.455697 -0.721968 5.686333]. nan nan nan 

[ INFO] [1619666857.577281662, 47.628000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666857.577325428, 47.628000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666857.599544569, 47.652000000]: [SP]Tree (62 nodes, 7 path nodes, 40 expanded) calculated in 15.02ms.
[ INFO] [1619666857.645703893, 47.700000000]: [WG] Speed adapted WP: [2.436291 -0.745742 5.804735].
[ INFO] [1619666857.645744745, 47.700000000]: [WG] Final waypoint: [2.449895 -0.717796 5.685137]. nan nan nan 

[ INFO] [1619666857.657224368, 47.712000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666857.657271014, 47.712000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666857.668309861, 47.724000000]: [SP]Tree (58 nodes, 7 path nodes, 40 expanded) calculated in 16.88ms.
[ INFO] [1619666857.744814085, 47.800000000]: [WG] Speed adapted WP: [2.431895 -0.738867 5.801560].
[ INFO] [1619666857.744846912, 47.800000000]: [WG] Final waypoint: [2.444235 -0.713267 5.683808]. nan nan nan 

[ INFO] [1619666857.767466186, 47.824000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666857.767508161, 47.824000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666857.783216306, 47.840000000]: [SP]Tree (59 nodes, 7 path nodes, 40 expanded) calculated in 10.50ms.
[ INFO] [1619666857.843137490, 47.900000000]: [WG] Speed adapted WP: [2.426433 -0.734032 5.801185].
[ INFO] [1619666857.843185808, 47.900000000]: [WG] Final waypoint: [2.438580 -0.708535 5.682579]. nan nan nan 

[ INFO] [1619666857.877798956, 47.936000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666857.877835518, 47.936000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666857.893546547, 47.956000000]: [SP]Tree (58 nodes, 7 path nodes, 40 expanded) calculated in 10.46ms.
[ INFO] [1619666857.938991823, 48.000000000]: [WG] Speed adapted WP: [2.419084 -0.731422 5.803365].
[ INFO] [1619666857.939022804, 48.000000000]: [WG] Final waypoint: [2.432682 -0.703912 5.681536]. nan nan nan 

[ INFO] [1619666857.970248191, 48.020000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666857.970313082, 48.020000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666857.981803110, 48.044000000]: [SP]Tree (52 nodes, 7 path nodes, 40 expanded) calculated in 9.28ms.
[ INFO] [1619666858.037544491, 48.100000000]: [WG] Speed adapted WP: [2.414724 -0.728242 5.802900].
[ INFO] [1619666858.037573004, 48.100000000]: [WG] Final waypoint: [2.426902 -0.699398 5.680557]. nan nan nan 

[ INFO] [1619666858.058295539, 48.120000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666858.058331989, 48.120000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666858.078690796, 48.140000000]: [SP]Tree (54 nodes, 6 path nodes, 40 expanded) calculated in 23.91ms.
[ INFO] [1619666858.145320745, 48.204000000]: [WG] Speed adapted WP: [2.410045 -0.722491 5.799808].
[ INFO] [1619666858.145355539, 48.204000000]: [WG] Final waypoint: [2.421182 -0.694411 5.679443]. nan nan nan 

[ INFO] [1619666858.160469602, 48.220000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666858.160515877, 48.220000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666858.172356370, 48.228000000]: [SP]Tree (51 nodes, 7 path nodes, 40 expanded) calculated in 9.61ms.
[ INFO] [1619666858.239562218, 48.300000000]: [WG] Speed adapted WP: [2.406076 -0.714950 5.794497].
[ INFO] [1619666858.239601853, 48.300000000]: [WG] Final waypoint: [2.416198 -0.689229 5.678192]. nan nan nan 

[ INFO] [1619666858.260408620, 48.320000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666858.260442634, 48.320000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666858.274894108, 48.336000000]: [SP]Tree (58 nodes, 7 path nodes, 40 expanded) calculated in 10.59ms.
[ INFO] [1619666858.341468979, 48.400000000]: [WG] Speed adapted WP: [2.397912 -0.707644 5.795819].
[ INFO] [1619666858.341501049, 48.400000000]: [WG] Final waypoint: [2.410872 -0.683169 5.677034]. nan nan nan 

[ INFO] [1619666858.364488783, 48.420000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666858.364524748, 48.420000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666858.386518586, 48.440000000]: [SP]Tree (70 nodes, 7 path nodes, 40 expanded) calculated in 10.55ms.
[ INFO] [1619666858.446799042, 48.500000000]: [WG] Speed adapted WP: [2.392129 -0.699910 5.795157].
[ INFO] [1619666858.446849955, 48.500000000]: [WG] Final waypoint: [2.405534 -0.676573 5.675929]. nan nan nan 

[ INFO] [1619666858.471365447, 48.528000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666858.471398078, 48.528000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666858.481711472, 48.532000000]: [SP]Tree (65 nodes, 7 path nodes, 40 expanded) calculated in 9.34ms.
[ INFO] [1619666858.548655900, 48.600000000]: [WG] Speed adapted WP: [2.387396 -0.690582 5.792107].
[ INFO] [1619666858.548688340, 48.600000000]: [WG] Final waypoint: [2.400348 -0.669436 5.674780]. nan nan nan 

[ INFO] [1619666858.578701582, 48.628000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666858.578799625, 48.628000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666858.597600305, 48.648000000]: [SP]Tree (52 nodes, 6 path nodes, 40 expanded) calculated in 14.39ms.
[ INFO] [1619666858.651182634, 48.700000000]: [WG] Speed adapted WP: [2.383720 -0.680272 5.788048].
[ INFO] [1619666858.651229482, 48.700000000]: [WG] Final waypoint: [2.395647 -0.661734 5.673568]. nan nan nan 

[ INFO] [1619666858.677661627, 48.720000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666858.677701179, 48.720000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666858.694710509, 48.740000000]: [SP]Tree (53 nodes, 6 path nodes, 40 expanded) calculated in 10.07ms.
[ INFO] [1619666858.751605079, 48.800000000]: [WG] Speed adapted WP: [2.381448 -0.673022 5.791199].
[ INFO] [1619666858.751638064, 48.800000000]: [WG] Final waypoint: [2.391596 -0.653948 5.672499]. nan nan nan 

[ INFO] [1619666858.768762121, 48.816000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666858.768806570, 48.816000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666858.791925120, 48.840000000]: [SP]Tree (57 nodes, 7 path nodes, 40 expanded) calculated in 17.43ms.
[ INFO] [1619666858.850443103, 48.900000000]: [WG] Speed adapted WP: [2.379813 -0.664781 5.790388].
[ INFO] [1619666858.850520135, 48.900000000]: [WG] Final waypoint: [2.388128 -0.646068 5.671374]. nan nan nan 

[ INFO] [1619666858.876542093, 48.916000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666858.876617020, 48.916000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666858.889552554, 48.932000000]: [SP]Tree (58 nodes, 7 path nodes, 40 expanded) calculated in 10.20ms.
[ INFO] [1619666858.960149871, 49.004000000]: [WG] Speed adapted WP: [2.378528 -0.655975 5.787501].
[ INFO] [1619666858.960197088, 49.004000000]: [WG] Final waypoint: [2.384942 -0.637753 5.670004]. nan nan nan 

[ INFO] [1619666858.983616865, 49.028000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666858.983653924, 49.028000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666858.994300721, 49.040000000]: [SP]Tree (54 nodes, 7 path nodes, 40 expanded) calculated in 9.32ms.
[ INFO] [1619666859.051046663, 49.100000000]: [WG] Speed adapted WP: [2.377043 -0.647353 5.785780].
[ INFO] [1619666859.051081449, 49.100000000]: [WG] Final waypoint: [2.382126 -0.630079 5.668637]. nan nan nan 

[ INFO] [1619666859.067797331, 49.116000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666859.067829785, 49.116000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666859.088119695, 49.136000000]: [SP]Tree (49 nodes, 6 path nodes, 40 expanded) calculated in 22.14ms.
[ INFO] [1619666859.150131664, 49.200000000]: [WG] Speed adapted WP: [2.373208 -0.643168 5.789880].
[ INFO] [1619666859.150164925, 49.200000000]: [WG] Final waypoint: [2.379045 -0.622825 5.667405]. nan nan nan 

[ INFO] [1619666859.170123902, 49.220000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666859.170168452, 49.220000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666859.181250274, 49.232000000]: [SP]Tree (50 nodes, 7 path nodes, 40 expanded) calculated in 10.08ms.
[ INFO] [1619666859.254787489, 49.304000000]: [WG] Speed adapted WP: [2.370389 -0.637767 5.789748].
[ INFO] [1619666859.254826374, 49.304000000]: [WG] Final waypoint: [2.375779 -0.615958 5.666114]. nan nan nan 

[ INFO] [1619666859.267799709, 49.320000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666859.267845381, 49.320000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666859.291539359, 49.344000000]: [SP]Tree (56 nodes, 7 path nodes, 40 expanded) calculated in 10.84ms.
[ INFO] [1619666859.349071987, 49.400000000]: [WG] Speed adapted WP: [2.368102 -0.630975 5.787225].
[ INFO] [1619666859.349105641, 49.400000000]: [WG] Final waypoint: [2.372995 -0.609967 5.664923]. nan nan nan 

[ INFO] [1619666859.370959952, 49.420000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666859.370998414, 49.420000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666859.383844782, 49.436000000]: [SP]Tree (59 nodes, 7 path nodes, 40 expanded) calculated in 19.95ms.
[ INFO] [1619666859.443758394, 49.500000000]: [WG] Speed adapted WP: [2.366878 -0.626161 5.784709].
[ INFO] [1619666859.443791840, 49.500000000]: [WG] Final waypoint: [2.370440 -0.604096 5.663655]. nan nan nan 

[ INFO] [1619666859.461293025, 49.516000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666859.461336179, 49.516000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666859.491211285, 49.544000000]: [SP]Tree (61 nodes, 7 path nodes, 40 expanded) calculated in 12.40ms.
[ INFO] [1619666859.549328165, 49.600000000]: [WG] Speed adapted WP: [2.365623 -0.620082 5.790800].
[ INFO] [1619666859.549366087, 49.600000000]: [WG] Final waypoint: [2.368332 -0.598605 5.662721]. nan nan nan 

[ INFO] [1619666859.577041545, 49.628000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666859.577091929, 49.628000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666859.592310254, 49.644000000]: [SP]Tree (59 nodes, 7 path nodes, 40 expanded) calculated in 9.72ms.
[ INFO] [1619666859.654781352, 49.704000000]: [WG] Speed adapted WP: [2.365131 -0.614281 5.790449].
[ INFO] [1619666859.654814341, 49.704000000]: [WG] Final waypoint: [2.366657 -0.593021 5.661745]. nan nan nan 

[ INFO] [1619666859.678887684, 49.724000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666859.678916545, 49.724000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666859.691404975, 49.736000000]: [SP]Tree (55 nodes, 7 path nodes, 40 expanded) calculated in 10.33ms.
[ INFO] [1619666859.752391175, 49.800000000]: [WG] Speed adapted WP: [2.364964 -0.609183 5.788327].
[ INFO] [1619666859.752453560, 49.800000000]: [WG] Final waypoint: [2.365494 -0.587941 5.660641]. nan nan nan 

[ INFO] [1619666859.790668929, 49.840000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666859.790705387, 49.840000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666859.806207508, 49.856000000]: [SP]Tree (60 nodes, 9 path nodes, 40 expanded) calculated in 10.36ms.
[ INFO] [1619666859.852253841, 49.900000000]: [WG] Speed adapted WP: [2.364342 -0.602809 5.788445].
[ INFO] [1619666859.852289717, 49.900000000]: [WG] Final waypoint: [2.364486 -0.582501 5.659344]. nan nan nan 

[ INFO] [1619666859.869981973, 49.916000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666859.870021830, 49.916000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666859.882043377, 49.920000000]: [SP]Tree (60 nodes, 9 path nodes, 40 expanded) calculated in 20.94ms.
[ INFO] [1619666859.959255443, 50.000000000]: [WG] Speed adapted WP: [2.360806 -0.596300 5.790504].
[ INFO] [1619666859.959288797, 50.000000000]: [WG] Final waypoint: [2.363153 -0.576917 5.658029]. nan nan nan 

[ INFO] [1619666859.987041653, 50.024000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666859.987084180, 50.024000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666860.013277955, 50.052000000]: [SP]Tree (56 nodes, 9 path nodes, 40 expanded) calculated in 10.71ms.
[ INFO] [1619666860.058383203, 50.100000000]: [WG] Speed adapted WP: [2.359161 -0.591918 5.789662].
[ INFO] [1619666860.058420415, 50.100000000]: [WG] Final waypoint: [2.361643 -0.571403 5.656641]. nan nan nan 

[ INFO] [1619666860.077043764, 50.120000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666860.077100319, 50.120000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666860.089496039, 50.132000000]: [SP]Tree (56 nodes, 10 path nodes, 40 expanded) calculated in 10.27ms.
[ INFO] [1619666860.158787314, 50.204000000]: [WG] Speed adapted WP: [2.357022 -0.584842 5.787241].
[ INFO] [1619666860.158820613, 50.204000000]: [WG] Final waypoint: [2.359860 -0.565585 5.655044]. nan nan nan 

[ INFO] [1619666860.196249318, 50.244000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666860.196278046, 50.244000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666860.207746436, 50.256000000]: [SP]Tree (57 nodes, 10 path nodes, 40 expanded) calculated in 10.42ms.
[ INFO] [1619666860.260974017, 50.308000000]: [WG] Speed adapted WP: [2.355550 -0.578187 5.783541].
[ INFO] [1619666860.261002565, 50.308000000]: [WG] Final waypoint: [2.358041 -0.559748 5.653248]. nan nan nan 

[ INFO] [1619666860.281495607, 50.328000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666860.300957615, 50.348000000]: [SP]Tree (58 nodes, 10 path nodes, 40 expanded) calculated in 10.66ms.
[ INFO] [1619666860.354349189, 50.404000000]: [WG] Speed adapted WP: [2.350891 -0.573521 5.785841].
[ INFO] [1619666860.354380618, 50.404000000]: [WG] Final waypoint: [2.356039 -0.554675 5.651757]. nan nan nan 

[ INFO] [1619666860.372820646, 50.424000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666860.372855608, 50.424000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666860.383573663, 50.436000000]: [SP]Tree (63 nodes, 10 path nodes, 40 expanded) calculated in 9.04ms.
[ INFO] [1619666860.457817252, 50.500000000]: [WG] Speed adapted WP: [2.348363 -0.569528 5.784451].
[ INFO] [1619666860.457850567, 50.500000000]: [WG] Final waypoint: [2.353705 -0.549937 5.650297]. nan nan nan 

[ INFO] [1619666860.486880805, 50.524000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666860.486923305, 50.524000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666860.497513374, 50.536000000]: [SP]Tree (59 nodes, 10 path nodes, 40 expanded) calculated in 9.41ms.
[ INFO] [1619666860.566947149, 50.600000000]: [WG] Speed adapted WP: [2.344914 -0.562770 5.779796].
[ INFO] [1619666860.566985059, 50.600000000]: [WG] Final waypoint: [2.350917 -0.544958 5.648678]. nan nan nan 

[ INFO] [1619666860.598286381, 50.624000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666860.598331283, 50.624000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666860.618389795, 50.648000000]: [SP]Tree (50 nodes, 10 path nodes, 40 expanded) calculated in 10.68ms.
[ INFO] [1619666860.670827170, 50.700000000]: [WG] Speed adapted WP: [2.342324 -0.556955 5.776436].
[ INFO] [1619666860.670884113, 50.700000000]: [WG] Final waypoint: [2.348076 -0.539889 5.646956]. nan nan nan 

[ INFO] [1619666860.698016570, 50.724000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666860.698055834, 50.724000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666860.711500967, 50.740000000]: [SP]Tree (60 nodes, 9 path nodes, 40 expanded) calculated in 10.17ms.
[ INFO] [1619666860.771496409, 50.800000000]: [WG] Speed adapted WP: [2.337570 -0.552765 5.780385].
[ INFO] [1619666860.771527136, 50.800000000]: [WG] Final waypoint: [2.344932 -0.534967 5.645469]. nan nan nan 

[ INFO] [1619666860.800106935, 50.832000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666860.800157269, 50.832000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666860.812111004, 50.844000000]: [SP]Tree (55 nodes, 10 path nodes, 40 expanded) calculated in 10.55ms.
[ INFO] [1619666860.865063887, 50.900000000]: [WG] Speed adapted WP: [2.333486 -0.547032 5.779693].
[ INFO] [1619666860.865097847, 50.900000000]: [WG] Final waypoint: [2.341494 -0.529999 5.644061]. nan nan nan 

[ INFO] [1619666860.900642145, 50.936000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666860.900677881, 50.936000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666860.914677793, 50.952000000]: [SP]Tree (59 nodes, 7 path nodes, 40 expanded) calculated in 10.35ms.
[ INFO] [1619666860.962168942, 51.000000000]: [WG] Speed adapted WP: [2.329820 -0.540537 5.776784].
[ INFO] [1619666860.962227679, 51.000000000]: [WG] Final waypoint: [2.337867 -0.524743 5.642593]. nan nan nan 

[ INFO] [1619666860.987872581, 51.028000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666860.987919577, 51.028000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666860.999128678, 51.040000000]: [SP]Tree (65 nodes, 7 path nodes, 40 expanded) calculated in 9.85ms.
[ INFO] [1619666861.060530999, 51.100000000]: [WG] Speed adapted WP: [2.326684 -0.533340 5.774666].
[ INFO] [1619666861.060564023, 51.100000000]: [WG] Final waypoint: [2.334211 -0.519145 5.641095]. nan nan nan 

[ INFO] [1619666861.093671882, 51.128000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666861.093709803, 51.128000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666861.105842172, 51.140000000]: [SP]Tree (55 nodes, 6 path nodes, 40 expanded) calculated in 12.92ms.
[ INFO] [1619666861.163870287, 51.200000000]: [WG] Speed adapted WP: [2.324096 -0.525882 5.776165].
[ INFO] [1619666861.163901297, 51.200000000]: [WG] Final waypoint: [2.330709 -0.513198 5.639702]. nan nan nan 

[ INFO] [1619666861.199350916, 51.236000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666861.199437591, 51.236000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666861.211319407, 51.248000000]: [SP]Tree (57 nodes, 6 path nodes, 40 expanded) calculated in 10.11ms.
[ INFO] [1619666861.261903036, 51.300000000]: [WG] Speed adapted WP: [2.321712 -0.517569 5.776236].
[ INFO] [1619666861.261936740, 51.300000000]: [WG] Final waypoint: [2.327399 -0.506843 5.638445]. nan nan nan 

[ INFO] [1619666861.289978126, 51.328000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666861.290017972, 51.328000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666861.301807651, 51.336000000]: [SP]Tree (59 nodes, 6 path nodes, 40 expanded) calculated in 20.29ms.
[ INFO] [1619666861.365617528, 51.400000000]: [WG] Speed adapted WP: [2.319431 -0.509662 5.780595].
[ INFO] [1619666861.365654442, 51.400000000]: [WG] Final waypoint: [2.324248 -0.500078 5.637463]. nan nan nan 

[ INFO] [1619666861.389685948, 51.424000000]: ===== Set Goal ======: [3.000000, 0.000000, 3.386902].
[ INFO] [1619666861.389729716, 51.424000000]: [OA] Planning started, using 1 cameras

[ INFO] [1619666861.401730857, 51.440000000]: [SP]Tree (52 nodes, 6 path nodes, 40 expanded) calculated in 10.37ms.
[ INFO] [1619666861.460467186, 51.500000000]: [WG] Speed adapted WP: [2.317780 -0.503105 5.784381].
[ INFO] [1619666861.460497439, 51.500000000]: [WG] Final waypoint: [2.321339 -0.493185 5.636691]. nan nan nan 

[ INFO] [1619666861.558880526, 51.600000000]: [WG] Speed adapted WP: [2.316041 -0.495110 5.784485].
[ INFO] [1619666861.558923079, 51.600000000]: [WG] Final waypoint: [2.318732 -0.486092 5.635852]. nan nan nan 

[ INFO] [1619666861.663261952, 51.700000000]: [WG] Speed adapted WP: [2.314542 -0.486520 5.780970].
[ INFO] [1619666861.663292814, 51.700000000]: [WG] Final waypoint: [2.316545 -0.478652 5.634683]. nan nan nan 

[ INFO] [1619666861.761875067, 51.800000000]: [WG] Speed adapted WP: [2.312897 -0.479317 5.784791].
[ INFO] [1619666861.761909718, 51.800000000]: [WG] Final waypoint: [2.314726 -0.471107 5.633519]. nan nan nan 

[ INFO] [1619666861.862116634, 51.900000000]: [WG] Speed adapted WP: [2.311750 -0.471395 5.782978].
[ INFO] [1619666861.862150147, 51.900000000]: [WG] Final waypoint: [2.313239 -0.463391 5.632195]. nan nan nan 

[ WARN] [1619666876.602265871, 66.500000000]:  Planner abort: missing required data 

[ WARN] [1619666876.712058874, 66.600000000]:  Planner abort: missing required data 

[ WARN] [1619666876.816496793, 66.700000000]:  Planner abort: missing required data 

[ WARN] [1619666876.913689323, 66.800000000]:  Planner abort: missing required data 

[ WARN] [1619666877.016486520, 66.900000000]:  Planner abort: missing required data 

[ WARN] [1619666877.121670840, 67.000000000]:  Planner abort: missing required data 

[ WARN] [1619666877.218722972, 67.100000000]:  Planner abort: missing required data 

[ WARN] [1619666877.323198803, 67.200000000]:  Planner abort: missing required data 

[ WARN] [1619666877.423749841, 67.300000000]:  Planner abort: missing required data 

[ WARN] [1619666877.521893217, 67.400000000]:  Planner abort: missing required data 

^C[ WARN] [1619666877.619320282, 67.500000000]:  Planner abort: missing required data

Update

Diagnostics

header: 
  seq: 72
  stamp: 
    secs: 3868
    nsecs: 920000000
  frame_id: ''
status: 
  - 
    level: 0
    name: "mavros: FCU connection"
    message: "connected"
    hardware_id: "/dev/ttyUSB0:921600"
    values: 
      - 
        key: "Received packets:"
        value: "27990"
      - 
        key: "Dropped packets:"
        value: "0"
      - 
        key: "Buffer overruns:"
        value: "0"
      - 
        key: "Parse errors:"
        value: "0"
      - 
        key: "Rx sequence number:"
        value: "155"
      - 
        key: "Tx sequence number:"
        value: "0"
      - 
        key: "Rx total bytes:"
        value: "1712395"
      - 
        key: "Tx total bytes:"
        value: "43821"
      - 
        key: "Rx speed:"
        value: "25439.000000"
      - 
        key: "Tx speed:"
        value: "662.000000"
  - 
    level: 0
    name: "mavros: GPS"
    message: "3D fix"
    hardware_id: "/dev/ttyUSB0:921600"
    values: 
      - 
        key: "Satellites visible"
        value: "10"
      - 
        key: "Fix type"
        value: "3"
      - 
        key: "EPH (m)"
        value: "0.00"
      - 
        key: "EPV (m)"
        value: "0.00"
  - 
    level: 0
    name: "mavros: Heartbeat"
    message: "Normal"
    hardware_id: "/dev/ttyUSB0:921600"
    values: 
      - 
        key: "Heartbeats since startup"
        value: "66"
      - 
        key: "Frequency (Hz)"
        value: "1.000000"
      - 
        key: "Vehicle type"
        value: "Quadrotor"
      - 
        key: "Autopilot type"
        value: "PX4 Autopilot"
      - 
        key: "Mode"
        value: "AUTO.LOITER"
      - 
        key: "System status"
        value: "Standby"
  - 
    level: 2
    name: "mavros: System"
    message: "Sensor health"
    hardware_id: "/dev/ttyUSB0:921600"
    values: 
      - 
        key: "Sensor present"
        value: "0x32200020"
      - 
        key: "Sensor enabled"
        value: "0x32210020"
      - 
        key: "Sensor health"
        value: "0x20200020"
      - 
        key: "GPS"
        value: "Ok"
      - 
        key: "rc receiver"
        value: "Fail"
      - 
        key: "AHRS subsystem health"
        value: "Ok"
      - 
        key: "Battery"
        value: "Fail"
      - 
        key: "CPU Load (%)"
        value: "33.5"
      - 
        key: "Drop rate (%)"
        value: "0.0"
      - 
        key: "Errors comm"
        value: "0"
      - 
        key: "Errors count #1"
        value: "0"
      - 
        key: "Errors count #2"
        value: "0"
      - 
        key: "Errors count #3"
        value: "0"
      - 
        key: "Errors count #4"
        value: "0"
  - 
    level: 0
    name: "mavros: Battery"
    message: "Normal"
    hardware_id: "/dev/ttyUSB0:921600"
    values: 
      - 
        key: "Voltage"
        value: "65.54"
      - 
        key: "Current"
        value: "-0.0"
      - 
        key: "Remaining"
        value: "-1.0"
  - 
    level: 0
    name: "mavros: Time Sync"
    message: "Normal"
    hardware_id: "/dev/ttyUSB0:921600"
    values: 
      - 
        key: "Timesyncs since startup"
        value: "665"
      - 
        key: "Frequency (Hz)"
        value: "10.000000"
      - 
        key: "Last RTT (ms)"
        value: "0.000000"
      - 
        key: "Mean RTT (ms)"
        value: "0.685714"
      - 
        key: "Last remote time (s)"
        value: "3893.815561000"
      - 
        key: "Estimated time offset (s)"
        value: "9223372011.949357986"

Update 2

I can see that required data by planner (if I understand right this will be position and pointcloud) are always published as /local_postion/pose and /camera/depth/points always have data stream although I still get the Planner abort: missing required data error after the same amount of time every test

pirate-lofy commented 3 years ago

after other multiple tests I got this error TF_OLD_DATA ignoring data from the past for frame left_gripper at time 1415804777 I went to this page http://wiki.ros.org/tf/Errors%20explained and couldn't get a solution @mrivi can you help with this?

update 3

After many other trials, I sometimes had two more errors [ WARN] [1620012937.420610346, 80.292000000]: TF_OLD_DATA ignoring data from the past for frame fcu at time 80.292 according to authority unknown_publisher

TM : Time jump detected. Resetting time synchroniser.

kiprock commented 3 years ago

Hey @pirate-lofy I am getting similar results with the holybro px4 vision (up core + structure core). Simulation works great but as soon as I take off the drone and put it in offboard mode, it takes off in the same direction every time. I wonder if /local_postion/pose is too erratic in real life for the avoidance algorithm? May I ask how you are sending navigation points to the drone? If I send a 2d nav goal in rviz, it sets Z=0 and in a few tests the drone wanted to fly to the ground.

pirate-lofy commented 3 years ago

Sorry, I can't figure out whether you talk about simulation or real life test!

kiprock commented 3 years ago

I left another comment just now on another issue you opened. If I take off the real drone using QGC, set it in offboard it does not crash anymore, it just hovers, but it will not accept goals from the planner. It all works in simulation fine. i tried setting goals using 2d set goal in R-vis as well as publishing goals using /move_base_simple/goal. Have you had any progress?

eldewany commented 3 years ago

@kiprock The issue is that the local planner is never updated through the topic /mavros/trajectory/desired and thus it always goes to the predefined goal, i tried your method and the goal moves but i haven't tried in real flight so can you share more details about your setup?. @mrivi Can you help with that ? it works fine in sitl but other than that the topic is always empty.

kiprock commented 3 years ago

Hi @eldewany Thank you for responding. I was able to get it to work. it was my own mistake in the networking configuration as I did not have ROS_IP set on the second computer. I know this is a common problem and I thought I had checked it but no. As for the real flight, my drone was not behaving exactly like the simulation and resulted in several collisions when flying indoors. I would like to know if anyone else has tested Avoidance on real drone indoors. Thank you.

eldewany commented 3 years ago

@kiprock i don't think indoor navigation is supported for px4 now, at least not without vio or a a position estimator like a Vicon or something. Maybe try it outside in position mode to see if it works fine, then maybe try to run vio along with local planner.