[ WARN] [1655457473.021495790, 241.460000000]: [Traj server]: ready.
[ WARN] [1655457473.022190356, 241.460000000]: GF: timeout, retries left 2
[ WARN] [1655457473.022258615, 241.460000000]: WP: timeout, retries left 2
WARN [mavlink] IGN REQUEST LIST: Busy
[ INFO] [1655457473.022496910, 241.460000000]: GF: mission received
[ERROR] [1655457473.025596128, 241.464000000]: FCU: IGN REQUEST LIST: Busy
[FSM]: from INIT to WAIT_TARGET
[ INFO] [1655457473.522388798, 241.960000000]: Got pose! Drone Ready to be armed.
[ WARN] [1655457474.022503829, 242.460000000]: WP: timeout, retries left 1
[ INFO] [1655457474.022681743, 242.460000000]: WP: mission received
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
INFO [ecl/EKF] 5192000: EKF commencing GPS fusion
[FSM]: state: WAIT_TARGET
[ INFO] [1655457478.540158904, 246.960000000]: Offboard enabled
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[ INFO] [1655457484.562086658, 252.972000000]: FCU: ARMED by Arm/Disarm component command
[ INFO] [1655457484.562535802, 252.972000000]: Vehicle armed
INFO [commander] Takeoff detected
[ INFO] [1655457484.613710182, 253.024000000]: FCU: Takeoff detected
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
Triggered!
[TRIG]: from WAIT_TARGET to GEN_NEW_TRAJ
[kino replan]: -----------------------
start: -0.0242555 -0.0386071 4.02858, -0.0149883 0.0319388 -0.0250007, 0 0 0
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 702
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655457495.915027904, 264.292000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655457495.917779285, 264.296000000]: iter num: 91
[kino replan]: Reallocate ratio: 1.2733
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655457495.917905085, 264.296000000]: plan yaw
[ INFO] [1655457495.917973678, 264.296000000]: cost func: smooth | waypt |
[ INFO] [1655457495.918449943, 264.296000000]: iter num: 257
plan heading: 0
[FSM]: from GEN_NEW_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 1.42619 0.269858 3.78712, 0.413183 0.0301684 -0.0236149, 0.0417989 -0.0018509 0.00144883
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 558
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655457505.724610525, 273.988000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655457505.725145916, 273.992000000]: iter num: 35
[kino replan]: Reallocate ratio: 1.00348
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655457505.725240030, 273.992000000]: plan yaw
[ INFO] [1655457505.725274603, 273.992000000]: cost func: smooth | waypt |
[ INFO] [1655457505.725647992, 273.992000000]: iter num: 158
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[ WARN] [1655457506.673270774, 274.936000000]: current traj in collision.
[SAFETY]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 1.83437 0.297727 3.76531, 0.447022 0.0286699 -0.022442, 0.0301958 -0.0013371 0.00104664
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 545
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655457506.675435408, 274.940000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655457506.676282361, 274.940000000]: iter num: 68
[kino replan]: Reallocate ratio: 1.22692
[kino replan]: time: 0.004, search: 0.004, optimize: 0, adjust time:0
[ INFO] [1655457506.676383051, 274.940000000]: plan yaw
[ INFO] [1655457506.676430931, 274.940000000]: cost func: smooth | waypt |
[ INFO] [1655457506.677096416, 274.940000000]: iter num: 258
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ WARN] [1655457506.723327656, 274.988000000]: current traj in collision.
[SAFETY]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 1.8575 0.299223 3.76414, 0.4484 0.0285591 -0.0223896, 0.027811 -0.00314809 0.00114043
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 495
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655457506.724985339, 274.988000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655457506.725447625, 274.988000000]: iter num: 66
[kino replan]: Reallocate ratio: 1.04196
[kino replan]: time: 0, search: 0, optimize: 0, adjust time:0
[ INFO] [1655457506.725508103, 274.988000000]: plan yaw
[ INFO] [1655457506.725542595, 274.988000000]: cost func: smooth | waypt |
[ INFO] [1655457506.725819806, 274.988000000]: iter num: 191
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ WARN] [1655457506.773364574, 275.036000000]: current traj in collision.
[SAFETY]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 1.87858 0.300596 3.76307, 0.44879 0.0283809 -0.0223041, 0.0261258 -0.0031871 0.00120136
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 587
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655457506.776066397, 275.040000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655457506.778204415, 275.040000000]: iter num: 151
[kino replan]: Reallocate ratio: 1.22865
[kino replan]: time: 0.004, search: 0.004, optimize: 0, adjust time:0
[ INFO] [1655457506.778346996, 275.040000000]: plan yaw
[ INFO] [1655457506.778408119, 275.040000000]: cost func: smooth | waypt |
[ INFO] [1655457506.779296635, 275.044000000]: iter num: 270
plan heading: 0.004
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ WARN] [1655457506.824901525, 275.088000000]: current traj in collision.
[SAFETY]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 1.90175 0.302097 3.7619, 0.449878 0.0283633 -0.0222497, 0.021753 0.00166384 0.00117006
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 430
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655457506.826847350, 275.088000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655457506.830984124, 275.096000000]: iter num: 181
[kino replan]: Reallocate ratio: 1.08346
[kino replan]: time: 0.008, search: 0, optimize: 0.008, adjust time:0
[ INFO] [1655457506.831079393, 275.096000000]: plan yaw
[ INFO] [1655457506.831120755, 275.096000000]: cost func: smooth | waypt |
[ INFO] [1655457506.831500917, 275.096000000]: iter num: 229
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[ WARN] [1655457508.922324023, 277.184000000]: current traj in collision.
[SAFETY]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 2.83678 0.42384 3.7128, 0.420957 0.110034 -0.0265459, -0.0390856 0.0646812 -0.00433811
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 606
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655457508.924570563, 277.184000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655457508.929705490, 277.192000000]: iter num: 178
[kino replan]: Reallocate ratio: 1.16704
[kino replan]: time: 0.008, search: 0, optimize: 0.008, adjust time:0
[ INFO] [1655457508.929783863, 277.192000000]: plan yaw
[ INFO] [1655457508.929812863, 277.192000000]: cost func: smooth | waypt |
[ INFO] [1655457508.930163921, 277.192000000]: iter num: 223
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 4.08812 1.24383 3.59529, 0.264189 0.31171 -0.0343326, -0.0265633 0.0314247 -9.2039e-05
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 662
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655457512.629169661, 280.884000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655457512.632074864, 280.888000000]: iter num: 114
[kino replan]: Reallocate ratio: 1.08351
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655457512.632152829, 280.888000000]: plan yaw
[ INFO] [1655457512.632191038, 280.888000000]: cost func: smooth | waypt |
[ INFO] [1655457512.632637468, 280.888000000]: iter num: 248
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 4.94936 2.48089 3.47405, 0.226958 0.338772 -0.0309922, 0.00414802 -0.0136354 0.00178419
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 570
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655457516.286249879, 284.536000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655457516.288844126, 284.540000000]: iter num: 110
[kino replan]: Reallocate ratio: 1.10932
[kino replan]: time: 0.008, search: 0.004, optimize: 0.004, adjust time:0
[ INFO] [1655457516.288931132, 284.540000000]: plan yaw
[ INFO] [1655457516.288980333, 284.540000000]: cost func: smooth | waypt |
[ INFO] [1655457516.289576897, 284.540000000]: iter num: 250
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 5.89303 3.64668 3.36853, 0.277595 0.263602 -0.0242131, 0.0210718 -0.0248592 0.00176573
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 721
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655457520.120357602, 288.360000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655457520.124084442, 288.364000000]: iter num: 122
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.008, search: 0.004, optimize: 0.004, adjust time:0
[ INFO] [1655457520.124166399, 288.364000000]: plan yaw
[ INFO] [1655457520.124203316, 288.364000000]: cost func: smooth | waypt |
[ INFO] [1655457520.124690838, 288.364000000]: iter num: 249
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[FSM]: state: REPLAN_TRAJ
[kino replan]: -----------------------
start: 7.13123 4.49867 3.28492, 0.335937 0.16359 -0.01803, 0.00899524 -0.0253767 0.00136414
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 616
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655457524.108589705, 292.340000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655457524.110173581, 292.344000000]: iter num: 75
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655457524.110232041, 292.344000000]: plan yaw
[ INFO] [1655457524.110262557, 292.344000000]: cost func: smooth | waypt |
[ INFO] [1655457524.110862945, 292.344000000]: iter num: 237
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 8.58333 4.91587 3.21804, 0.360209 0.0304951 -0.0148081, 0.00324768 -0.0354363 0.00055755
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 657
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655457528.271479194, 296.492000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655457528.272873231, 296.492000000]: iter num: 61
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.004, search: 0.004, optimize: 0, adjust time:0
[ INFO] [1655457528.272966147, 296.492000000]: plan yaw
[ INFO] [1655457528.273011572, 296.492000000]: cost func: smooth | waypt |
[ INFO] [1655457528.273549495, 296.496000000]: iter num: 207
plan heading: 0.004
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 10.081 4.81431 3.15843, 0.368924 -0.0645948 -0.0143198, 0.0011839 -0.0129182 0.000455185
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 619
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655457532.378946746, 300.592000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655457532.380077281, 300.592000000]: iter num: 65
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.004, search: 0.004, optimize: 0, adjust time:0
[ INFO] [1655457532.380146711, 300.592000000]: plan yaw
[ INFO] [1655457532.380180820, 300.592000000]: cost func: smooth | waypt |
[ INFO] [1655457532.380503978, 300.592000000]: iter num: 231
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 11.5461 4.48143 3.11766, 0.372039 -0.0985865 -0.00384267, 0.000451888 -0.00493106 0.00422149
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 634
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655457536.428537838, 304.544000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655457536.429125037, 304.544000000]: iter num: 40
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.004, search: 0.004, optimize: 0, adjust time:0
[ INFO] [1655457536.429191937, 304.544000000]: plan yaw
[ INFO] [1655457536.429230627, 304.544000000]: cost func: smooth | waypt |
[ INFO] [1655457536.429582893, 304.544000000]: iter num: 231
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 13 4.06443 3.13291, 0.373365 -0.113051 0.00950305, 0.000154774 -0.00171793 0.00119892
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 653
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655457540.335805334, 308.440000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655457540.336821023, 308.444000000]: iter num: 63
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655457540.336882589, 308.444000000]: plan yaw
[ INFO] [1655457540.336929304, 308.444000000]: cost func: smooth | waypt |
[ INFO] [1655457540.337415520, 308.444000000]: iter num: 193
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 14.4376 3.61955 3.17237, 0.373766 -0.117502 0.0100995, 6.10195e-05 -0.000676944 -0.000231795
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 667
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655457544.191450509, 312.288000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655457544.193047385, 312.292000000]: iter num: 64
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655457544.193124346, 312.292000000]: plan yaw
[ INFO] [1655457544.193173101, 312.292000000]: cost func: smooth | waypt |
[ INFO] [1655457544.193554222, 312.292000000]: iter num: 179
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 15.8762 3.16352 3.21466, 0.373924 -0.119256 0.0132307, 2.40834e-05 -0.000267224 0.00186805
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 663
- - - - - -
reach end
[kino replan]: kinodynamic search success.
[ INFO] [1655457548.044491216, 316.140000000]: cost func: smooth | dist | feasi |
[ INFO] [1655457548.045825468, 316.140000000]: iter num: 76
[kino replan]: Reallocate ratio: 1.10245
[kino replan]: time: 0.004, search: 0.004, optimize: 0, adjust time:0
[ INFO] [1655457548.045898206, 316.140000000]: plan yaw
[ INFO] [1655457548.046245688, 316.140000000]: cost func: smooth | waypt |
[ INFO] [1655457548.046770698, 316.140000000]: iter num: 279
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 17.3814 3.15908 3.23018, 0.191487 0.159154 -0.0117964, -0.0332438 0.0528009 -0.00418433
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 560
- - - - - -
reach end
[kino replan]: kinodynamic search success.
[ INFO] [1655457553.144619734, 321.232000000]: cost func: smooth | dist | feasi |
[ INFO] [1655457553.146171231, 321.232000000]: iter num: 66
[kino replan]: Reallocate ratio: 1.27092
[kino replan]: time: 0.004, search: 0.004, optimize: 0, adjust time:0
[ INFO] [1655457553.146276771, 321.232000000]: plan yaw
[ INFO] [1655457553.146310918, 321.232000000]: cost func: smooth | waypt |
[ INFO] [1655457553.147053768, 321.232000000]: iter num: 288
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 17.7539 4.61886 3.16475, -0.0260784 0.428746 -0.00769904, -0.0256919 0.0187745 0.00596518
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 428
- - - - - -
reach end
[kino replan]: kinodynamic search success.
[ INFO] [1655457557.873423946, 325.956000000]: cost func: smooth | dist | feasi |
[ INFO] [1655457557.873927617, 325.956000000]: iter num: 49
[kino replan]: Reallocate ratio: 1.30473
[kino replan]: time: 0, search: 0, optimize: 0, adjust time:0
[ INFO] [1655457557.873982430, 325.956000000]: plan yaw
[ INFO] [1655457557.874045789, 325.956000000]: cost func: smooth | waypt |
[ INFO] [1655457557.874514468, 325.956000000]: iter num: 246
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 19.2238 4.89602 2.96288, 0.413813 -0.425087 0.0233726, 0.00873593 -0.034549 0.0210533
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 195
- - - - - -
reach end
[kino replan]: kinodynamic search success.
[ INFO] [1655457566.200639274, 334.272000000]: cost func: smooth | dist | feasi |
[ INFO] [1655457566.200940880, 334.272000000]: iter num: 34
[kino replan]: Reallocate ratio: 1.04166
[kino replan]: time: 0.004, search: 0.004, optimize: 0, adjust time:0
[ INFO] [1655457566.201014508, 334.272000000]: plan yaw
[ INFO] [1655457566.201059498, 334.272000000]: cost func: smooth | waypt |
[ INFO] [1655457566.201250169, 334.272000000]: iter num: 119
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 20.2423 3.79344 3.08779, 0.42068 -0.4716 0.0786167, 0.00169947 -0.0104589 0.0202966
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 145
- - - - - -
reach end
[kino replan]: kinodynamic search success.
[ INFO] [1655457568.648805395, 336.716000000]: cost func: smooth | dist | feasi |
[ INFO] [1655457568.648908765, 336.716000000]: iter num: 18
[kino replan]: Reallocate ratio: 1.06264
[kino replan]: time: 0, search: 0, optimize: 0, adjust time:0
[ INFO] [1655457568.648957909, 336.716000000]: plan yaw
[ INFO] [1655457568.648995236, 336.716000000]: cost func: smooth | waypt |
[ INFO] [1655457568.649121669, 336.716000000]: iter num: 104
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 21.2184 2.676 3.33655, 0.408424 -0.476083 0.130468, 0.00424264 -0.0103007 0.0175907
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 76
- - - - - -
reach end
[kino replan]: kinodynamic search success.
[ INFO] [1655457571.063158214, 339.124000000]: cost func: smooth | dist | feasi |
[ INFO] [1655457571.063302195, 339.124000000]: iter num: 21
[kino replan]: Reallocate ratio: 1.03836
[kino replan]: time: 0, search: 0, optimize: 0, adjust time:0
[ INFO] [1655457571.063364500, 339.124000000]: plan yaw
[ INFO] [1655457571.063403543, 339.124000000]: cost func: smooth | waypt |
[ INFO] [1655457571.063515935, 339.124000000]: iter num: 71
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 22.1696 1.54746 3.63303, 0.411849 -0.494837 0.115069, 0.0074545 -0.00966001 -0.00946999
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 63
- - - - - -
reach end
[kino replan]: kinodynamic search success.
[ INFO] [1655457573.435971080, 341.496000000]: cost func: smooth | dist | feasi |
[ INFO] [1655457573.436104474, 341.496000000]: iter num: 23
[kino replan]: Reallocate ratio: 1.14987
[kino replan]: time: 0, search: 0, optimize: 0, adjust time:0
[ INFO] [1655457573.436152292, 341.496000000]: plan yaw
[ INFO] [1655457573.436180490, 341.496000000]: cost func: smooth | waypt |
[ INFO] [1655457573.436225136, 341.496000000]: iter num: 37
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[rvizvisualisation-16] killing on exit
[traj_msg_converter-15] killing on exit
[camera_pose_publisher-14] killing on exit
[rqt_reconfigure-13] killing on exit
[waypoint_generator-11] killing on exit
[flight_traj-10] killing on exit
[geometric_controller-12] killing on exit
[flight_node-9] killing on exit
[mavros-8] killing on exit
[gazebo_gui-6] killing on exit
[gazebo-5] killing on exit
Traceback (most recent call last):
File "/home/dieptran/catkin_ws/src/px4_fast_planner/scripts/camera_pose_publisher.py", line 41, in <module>
rate.sleep()
File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/timer.py", line 103, in sleep
sleep(self._remaining(curr_time))
File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/timer.py", line 165, in sleep
raise rospy.exceptions.ROSInterruptException("ROS shutdown request")
rospy.exceptions.ROSInterruptException: ROS shutdown request
[sitl-4] killing on exit