[ WARN] [1655456947.701833023, 241.460000000]: PR: request list timeout, retries left 2
[ WARN] [1655456947.701906770, 241.460000000]: GF: timeout, retries left 2
[ WARN] [1655456947.701997234, 241.460000000]: WP: timeout, retries left 2
[ WARN] [1655456947.702100300, 241.460000000]: RP: timeout, retries left 2
WARN [mavlink] IGN REQUEST LIST: Busy
WARN [mavlink] IGN REQUEST LIST: Busy
[ INFO] [1655456947.702765684, 241.460000000]: GF: mission received
[ERROR] [1655456947.705548941, 241.460000000]: FCU: IGN REQUEST LIST: Busy
[FSM]: from INIT to WAIT_TARGET
[ERROR] [1655456947.757618527, 241.516000000]: FCU: IGN REQUEST LIST: Busy
[ INFO] [1655456948.202748020, 241.960000000]: Got pose! Drone Ready to be armed.
[ WARN] [1655456948.704434320, 242.460000000]: RP: timeout, retries left 1
[ WARN] [1655456948.704528544, 242.460000000]: WP: timeout, retries left 1
WARN [mavlink] IGN REQUEST LIST: Busy
[ INFO] [1655456948.704660936, 242.460000000]: RP: mission received
[ERROR] [1655456948.707935772, 242.464000000]: FCU: IGN REQUEST LIST: Busy
[FSM]: state: WAIT_TARGET
[ WARN] [1655456949.705147279, 243.460000000]: WP: timeout, retries left 0
[ INFO] [1655456949.705307439, 243.460000000]: WP: mission received
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
[FSM]: state: WAIT_TARGET
INFO [ecl/EKF] 5192000: EKF commencing GPS fusion
[ INFO] [1655456952.710776082, 246.460000000]: 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] [1655456958.727733016, 252.464000000]: Vehicle armed
[ INFO] [1655456958.727850906, 252.464000000]: FCU: ARMED by Arm/Disarm component command
INFO [commander] Takeoff detected
[ INFO] [1655456958.779461760, 252.516000000]: 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.026823 -0.038794 4.02083, -0.00734018 0.0262008 -0.00741565, 0 0 0
goal:23.5 0 4, 0 0 0
open set empty, no path!
use node num: 1
iter num: 1
[kino replan]: kinodynamic search fail!
- - - - -
USE NODE NUM: 347
- - - - -
reach horizon
[kino replan]: retry search success.
[ INFO] [1655456969.886472406, 263.608000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655456969.890960473, 263.612000000]: iter num: 113
[kino replan]: Reallocate ratio: 1.30949
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655456969.891067763, 263.612000000]: plan yaw
[ INFO] [1655456969.891157155, 263.612000000]: cost func: smooth | waypt |
[ INFO] [1655456969.891326590, 263.612000000]: iter num: 54
plan heading: 0
[FSM]: from GEN_NEW_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 1.4905 0.00798738 4.00759, 0.944895 0.0169385 -0.00479412, 0.0481072 -0.00255894 0.000724258
goal:23.5 0 4, 0 0 0
- - - - -
USE NODE NUM: 306
- - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655456972.087131496, 265.804000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655456972.088066062, 265.804000000]: iter num: 72
[kino replan]: Reallocate ratio: 1.00129
[kino replan]: time: 0, search: 0, optimize: 0, adjust time:0
[ INFO] [1655456972.088123335, 265.804000000]: plan yaw
[ INFO] [1655456972.088170989, 265.804000000]: cost func: smooth | waypt |
[ INFO] [1655456972.088288331, 265.804000000]: iter num: 43
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[ WARN] [1655456972.829864958, 266.544000000]: current traj in collision.
[SAFETY]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 2.20124 0.0199088 4.00422, 0.974064 0.0153816 -0.00435348, 0.0307538 -0.00168734 0.000477561
goal:23.5 0 4, 0 0 0
- - - - -
USE NODE NUM: 260
- - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655456972.830887702, 266.544000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655456972.833581831, 266.548000000]: iter num: 158
[kino replan]: Reallocate ratio: 1.02
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655456972.833677024, 266.548000000]: plan yaw
[ INFO] [1655456972.833737128, 266.548000000]: cost func: smooth | waypt |
[ INFO] [1655456972.833880458, 266.548000000]: iter num: 49
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ WARN] [1655456972.971830226, 266.688000000]: current traj in collision.
[SAFETY]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 2.34173 0.0221754 4.00367, 0.97737 0.0163849 -0.00294509, 0.015395 0.0155704 0.0190475
goal:23.5 0 4, 0 0 0
- - - - -
USE NODE NUM: 327
- - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655456972.972740952, 266.688000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655456972.976133527, 266.692000000]: iter num: 156
[kino replan]: Reallocate ratio: 1.02299
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655456972.976215899, 266.692000000]: plan yaw
[ INFO] [1655456972.976281096, 266.692000000]: cost func: smooth | waypt |
[ INFO] [1655456972.976406777, 266.692000000]: iter num: 51
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 3.85388 0.18739 4.01874, 0.817325 0.230742 0.0180034, -0.149453 0.184704 0.00698227
goal:23.5 0 4, 0 0 0
- - - - -
USE NODE NUM: 236
- - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655456974.622356387, 268.336000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655456974.627439584, 268.340000000]: iter num: 168
[kino replan]: Reallocate ratio: 1.00265
[kino replan]: time: 0.008, search: 0.004, optimize: 0.004, adjust time:0
[ INFO] [1655456974.627542385, 268.340000000]: plan yaw
[ INFO] [1655456974.627615923, 268.340000000]: cost func: smooth | waypt |
[ INFO] [1655456974.627785799, 268.340000000]: iter num: 51
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: 5.1648 0.918468 4.06144, 0.586608 0.508055 0.0252939, -0.0745327 0.0884016 0.00152361
goal:23.5 0 4, 0 0 0
- - - - -
USE NODE NUM: 522
- - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655456976.523768053, 270.232000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655456976.528854876, 270.240000000]: iter num: 136
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.008, search: 0, optimize: 0.008, adjust time:0
[ INFO] [1655456976.528962678, 270.240000000]: plan yaw
[ INFO] [1655456976.529054107, 270.240000000]: cost func: smooth | waypt |
[ INFO] [1655456976.529282244, 270.240000000]: iter num: 49
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 6.20165 2.01693 4.11204, 0.509795 0.605866 0.0268982, -0.0111288 0.010829 0.000379915
goal:23.5 0 4, 0 0 0
- - - - -
USE NODE NUM: 579
- - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655456978.451954272, 272.160000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655456978.455706869, 272.164000000]: iter num: 79
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655456978.455802736, 272.164000000]: plan yaw
[ INFO] [1655456978.455917377, 272.164000000]: cost func: smooth | waypt |
[ INFO] [1655456978.456205041, 272.164000000]: iter num: 47
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: 7.19549 3.17023 4.16566, 0.499394 0.530404 0.0272536, -0.00109228 -0.0830061 3.44593e-05
goal:23.5 0 4, 0 0 0
- - - - -
USE NODE NUM: 273
- - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655456980.437232296, 274.136000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655456980.441692912, 274.140000000]: iter num: 121
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655456980.441782403, 274.140000000]: plan yaw
[ INFO] [1655456980.441875722, 274.140000000]: cost func: smooth | waypt |
[ INFO] [1655456980.442099765, 274.140000000]: iter num: 47
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: 8.31833 4.17214 4.22709, 0.498119 0.365491 0.0272937, -0.000184466 -0.0663881 5.84996e-06
goal:23.5 0 4, 0 0 0
- - - - -
USE NODE NUM: 252
- - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655456982.689216154, 276.388000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655456982.694313621, 276.392000000]: iter num: 119
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655456982.694387902, 276.392000000]: plan yaw
[ INFO] [1655456982.694495719, 276.392000000]: cost func: smooth | waypt |
[ INFO] [1655456982.694777177, 276.392000000]: iter num: 46
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: 9.61307 4.9503 4.29806, 0.497798 0.24628 0.02731, -4.84242e-05 -0.0352845 7.44063e-06
goal:23.5 0 4, 0 0 0
- - - - -
USE NODE NUM: 401
- - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655456985.300835047, 278.992000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655456985.304469219, 278.996000000]: iter num: 100
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.008, search: 0.004, optimize: 0.004, adjust time:0
[ INFO] [1655456985.304541960, 278.996000000]: plan yaw
[ INFO] [1655456985.304632597, 278.996000000]: cost func: smooth | waypt |
[ INFO] [1655456985.304791541, 278.996000000]: iter num: 46
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: 11.0048 5.535 4.37444, 0.497744 0.174909 0.0273187, 8.32285e-06 -0.0282262 -2.21992e-06
goal:23.5 0 4, 0 0 0
- - - - -
USE NODE NUM: 365
- - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655456988.099893477, 281.788000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655456988.102951061, 281.788000000]: iter num: 113
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.004, search: 0.004, optimize: 0, adjust time:0
[ INFO] [1655456988.103014940, 281.788000000]: plan yaw
[ INFO] [1655456988.103092105, 281.788000000]: cost func: smooth | waypt |
[ INFO] [1655456988.103265101, 281.788000000]: iter num: 57
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: 12.4762 5.8826 4.45518, 0.497756 0.0452265 0.0273165, -1.37489e-06 -0.0586061 -1.27355e-07
goal:23.5 0 4, 0 0 0
- - - - -
USE NODE NUM: 366
- - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655456991.056633888, 284.740000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655456991.061759559, 284.744000000]: iter num: 129
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655456991.061847527, 284.744000000]: plan yaw
[ INFO] [1655456991.061944423, 284.744000000]: cost func: smooth | waypt |
[ INFO] [1655456991.062081626, 284.744000000]: iter num: 38
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: 13.9734 5.76335 4.5373, 0.49766 -0.121771 0.0276248, -5.54324e-05 -0.0529439 7.77961e-05
goal:23.5 0 4, 0 0 0
- - - - -
USE NODE NUM: 347
- - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655456994.068456161, 287.748000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655456994.073507449, 287.752000000]: iter num: 145
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655456994.073571037, 287.752000000]: plan yaw
[ INFO] [1655456994.073636350, 287.752000000]: cost func: smooth | waypt |
[ INFO] [1655456994.073764007, 287.752000000]: iter num: 39
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: 15.3846 5.24182 4.61586, 0.49757 -0.234695 0.0277498, -1.14779e-05 -0.0288011 1.57255e-05
goal:23.5 0 4, 0 0 0
- - - - -
USE NODE NUM: 247
- - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655456996.910671760, 290.588000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655456996.912024191, 290.588000000]: iter num: 51
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.004, search: 0.004, optimize: 0, adjust time:0
[ INFO] [1655456996.912130362, 290.588000000]: plan yaw
[ INFO] [1655456996.912235501, 290.588000000]: cost func: smooth | waypt |
[ INFO] [1655456996.912440306, 290.588000000]: iter num: 38
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: 16.73 4.55524 4.69069, 0.497563 -0.260837 0.0279946, 8.78342e-06 0.000135391 0.000453598
goal:23.5 0 4, 0 0 0
- - - - -
USE NODE NUM: 232
- - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655456999.614284229, 293.288000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655456999.617174653, 293.288000000]: iter num: 82
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655456999.617253745, 293.292000000]: plan yaw
[ INFO] [1655456999.617326108, 293.292000000]: cost func: smooth | waypt |
[ INFO] [1655456999.617447198, 293.292000000]: iter num: 38
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: 18.0735 3.85124 4.76198, 0.497567 -0.260804 0.0251029, 6.21387e-06 1.96421e-05 -7.87885e-05
goal:23.5 0 4, 0 0 0
- - - - -
USE NODE NUM: 211
- - - - -
reach end
[kino replan]: kinodynamic search success.
[ INFO] [1655457002.323260708, 295.988000000]: cost func: smooth | dist | feasi |
[ INFO] [1655457002.328520595, 295.996000000]: iter num: 146
[kino replan]: Reallocate ratio: 1.02297
[kino replan]: time: 0.008, search: 0, optimize: 0.008, adjust time:0
[ INFO] [1655457002.328602462, 295.996000000]: plan yaw
[ INFO] [1655457002.328684271, 295.996000000]: cost func: smooth | waypt |
[ INFO] [1655457002.328849678, 295.996000000]: iter num: 50
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: 19.4884 3.55819 4.34551, 0.363158 0.145942 -0.330848, -0.0473886 0.134672 -0.095321
goal:23.5 0 4, 0 0 0
- - - - -
USE NODE NUM: 128
- - - - -
reach end
[kino replan]: kinodynamic search success.
[ INFO] [1655457005.526274636, 299.184000000]: cost func: smooth | dist | feasi |
[ INFO] [1655457005.528162535, 299.188000000]: iter num: 80
[kino replan]: Reallocate ratio: 1.05339
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655457005.528271107, 299.188000000]: plan yaw
[ INFO] [1655457005.528351870, 299.188000000]: cost func: smooth | waypt |
[ INFO] [1655457005.528481296, 299.188000000]: iter num: 47
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: 19.6556 4.13549 2.96375, -0.280761 0.0462309 -0.421693, -0.107927 -0.163846 0.0701095
goal:23.5 0 4, 0 0 0
- - - - -
USE NODE NUM: 144
- - - - -
reach end
[kino replan]: kinodynamic search success.
[ INFO] [1655457008.810452624, 302.448000000]: cost func: smooth | dist | feasi |
[ INFO] [1655457008.811580517, 302.448000000]: iter num: 69
[kino replan]: Reallocate ratio: 1.04703
[kino replan]: time: 0, search: 0, optimize: 0, adjust time:0
[ INFO] [1655457008.811646434, 302.448000000]: plan yaw
[ INFO] [1655457008.811688572, 302.448000000]: cost func: smooth | waypt |
[ INFO] [1655457008.811800178, 302.448000000]: iter num: 44
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: 19.2193 2.88986 2.24243, 0.412048 -0.632918 0.148275, 0.378164 -0.120234 0.183913
goal:23.5 0 4, 0 0 0
- - - - -
USE NODE NUM: 133
- - - - -
reach end
[kino replan]: kinodynamic search success.
[ INFO] [1655457012.873495867, 306.496000000]: cost func: smooth | dist | feasi |
[ INFO] [1655457012.873839767, 306.496000000]: iter num: 37
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0, search: 0, optimize: 0, adjust time:0
[ INFO] [1655457012.873897585, 306.496000000]: plan yaw
[ INFO] [1655457012.873966696, 306.496000000]: cost func: smooth | waypt |
[ INFO] [1655457012.874083644, 306.496000000]: iter num: 49
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.2637 1.88087 2.66358, 0.685609 -0.434424 0.286581, 0.0393759 0.18235 0.020433
goal:23.5 0 4, 0 0 0
- - - - -
USE NODE NUM: 55
- - - - -
reach end
[kino replan]: kinodynamic search success.
[ INFO] [1655457014.630168660, 308.244000000]: cost func: smooth | dist | feasi |
[ INFO] [1655457014.630569376, 308.244000000]: iter num: 31
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0, search: 0, optimize: 0, adjust time:0
[ INFO] [1655457014.630662052, 308.244000000]: plan yaw
[ INFO] [1655457014.630732387, 308.244000000]: cost func: smooth | waypt |
[ INFO] [1655457014.630880525, 308.244000000]: iter num: 44
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: 21.5787 1.34772 3.22217, 0.738827 -0.228225 0.315675, 0.0259306 -0.0722879 0.00668344
goal:23.5 0 4, 0 0 0
- - - - -
USE NODE NUM: 20
- - - - -
reach end
[kino replan]: kinodynamic search success.
[ INFO] [1655457016.477593340, 310.088000000]: cost func: smooth | dist | feasi |
[ INFO] [1655457016.477704865, 310.088000000]: iter num: 19
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0, search: 0, optimize: 0, adjust time:0
[ INFO] [1655457016.477748971, 310.088000000]: plan yaw
[ INFO] [1655457016.477876251, 310.088000000]: cost func: smooth | waypt |
[ INFO] [1655457016.477950192, 310.088000000]: iter num: 34
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 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
[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
[geometric_controller-12] killing on exit
[waypoint_generator-11] killing on exit
[rqt_reconfigure-13] killing on exit
[traj_server-10] killing on exit
[fast_planner_node-9] killing on exit
[gazebo_gui-6] killing on exit
[mavros-8] 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
des manager
[gazebo-5] killing on exit
[sitl-4] killing on exit
Exiting...
pxh> Shutting down
[tf_camera-3] killing on exit
[world_map_linker-2] killing on exit