Triggered!
[TRIG]: from WAIT_TARGET to GEN_NEW_TRAJ
[kino replan]: -----------------------
start: -0.0191255 -0.0323767 4.04205, -0.0399715 0.00672028 -0.0561732, 0 0 0
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 705
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655459006.816304544, 272.488000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655459006.820134092, 272.492000000]: iter num: 120
[kino replan]: Reallocate ratio: 1.28737
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655459006.820254923, 272.492000000]: plan yaw
[ INFO] [1655459006.820307108, 272.492000000]: cost func: smooth | waypt |
[ INFO] [1655459006.820971181, 272.496000000]: iter num: 234
plan heading: 0.004
[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]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 1.40765 0.0394853 3.59481, 0.41261 0.00623065 -0.0158883, 0.0362706 -0.00040877 0.00603766
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 467
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655459017.585687169, 283.240000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655459017.586174745, 283.244000000]: iter num: 40
[kino replan]: Reallocate ratio: 1.0115
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655459017.586251370, 283.244000000]: plan yaw
[ INFO] [1655459017.586294626, 283.244000000]: cost func: smooth | waypt |
[ INFO] [1655459017.586634855, 283.244000000]: iter num: 151
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[FSM]: state: EXEC_TRAJ
[ WARN] [1655459018.681873454, 284.340000000]: current traj in collision.
[SAFETY]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 1.88163 0.0461124 3.58068, 0.447572 0.00583653 -0.0100677, 0.0272811 -0.00031031 0.00456337
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 530
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655459018.683864701, 284.340000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655459018.685038036, 284.344000000]: iter num: 116
[kino replan]: Reallocate ratio: 1.25632
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655459018.685140868, 284.344000000]: plan yaw
[ INFO] [1655459018.685234527, 284.344000000]: cost func: smooth | waypt |
[ INFO] [1655459018.685803182, 284.344000000]: iter num: 258
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ WARN] [1655459018.733032303, 284.392000000]: current traj in collision.
[SAFETY]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 1.86087 0.0458449 3.58112, 0.397186 0.00520054 -0.00912284, -0.000483186 -0.000591008 0.00363273
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 604
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655459018.735736356, 284.392000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655459018.740816473, 284.400000000]: iter num: 168
[kino replan]: Reallocate ratio: 1.04861
[kino replan]: time: 0.008, search: 0, optimize: 0.008, adjust time:0
[ INFO] [1655459018.740918791, 284.400000000]: plan yaw
[ INFO] [1655459018.740960322, 284.400000000]: cost func: smooth | waypt |
[ INFO] [1655459018.741648179, 284.400000000]: iter num: 250
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ WARN] [1655459018.784895358, 284.444000000]: current traj in collision.
[SAFETY]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 1.88152 0.0461139 3.58065, 0.397142 0.00520269 -0.00892991, -0.0012035 0.000710441 0.00359976
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 665
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655459018.788411056, 284.444000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655459018.789475275, 284.448000000]: iter num: 73
[kino replan]: Reallocate ratio: 1.23563
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655459018.789558398, 284.448000000]: plan yaw
[ INFO] [1655459018.789604228, 284.448000000]: cost func: smooth | waypt |
[ INFO] [1655459018.790247125, 284.448000000]: iter num: 306
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ WARN] [1655459018.831545371, 284.488000000]: current traj in collision.
[SAFETY]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 1.89899 0.0463451 3.58027, 0.397097 0.00526507 -0.00873671, -0.000742111 0.00208262 0.00496816
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 715
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655459018.835706411, 284.492000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655459018.840809090, 284.500000000]: iter num: 173
[kino replan]: Reallocate ratio: 1.0802
[kino replan]: time: 0.012, search: 0.004, optimize: 0.008, adjust time:0
[ INFO] [1655459018.840894331, 284.500000000]: plan yaw
[ INFO] [1655459018.840930944, 284.500000000]: cost func: smooth | waypt |
[ INFO] [1655459018.841456757, 284.500000000]: iter num: 268
plan heading: 0
[FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ WARN] [1655459018.883764757, 284.540000000]: current traj in collision.
[SAFETY]: from EXEC_TRAJ to REPLAN_TRAJ
[kino replan]: -----------------------
start: 1.91964 0.0466257 3.57983, 0.397036 0.00540234 -0.00847273, -0.00157232 0.00309269 0.00493065
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 717
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655459018.886756073, 284.544000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655459018.891881772, 284.548000000]: iter num: 141
[kino replan]: Reallocate ratio: 1.06128
[kino replan]: time: 0.008, search: 0.004, optimize: 0.004, adjust time:0
[ INFO] [1655459018.891979452, 284.548000000]: plan yaw
[ INFO] [1655459018.892027189, 284.548000000]: cost func: smooth | waypt |
[ INFO] [1655459018.892803215, 284.552000000]: iter num: 276
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: 3.41424 0.251419 3.55613, 0.271813 0.112985 -0.000474014, -0.0409195 0.0364564 0.00378483
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 748
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655459023.183044652, 288.832000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655459023.188195099, 288.836000000]: iter num: 143
[kino replan]: Reallocate ratio: 1.04333
[kino replan]: time: 0.008, search: 0.004, optimize: 0.004, adjust time:0
[ INFO] [1655459023.188331364, 288.836000000]: plan yaw
[ INFO] [1655459023.188386832, 288.836000000]: cost func: smooth | waypt |
[ INFO] [1655459023.189071370, 288.840000000]: iter num: 281
plan heading: 0.004
[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.47471 1.32252 3.55869, 0.223586 0.314999 -0.00717532, 0.0244653 0.0186174 -0.00585941
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 545
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655459027.997938160, 293.640000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655459028.000583479, 293.644000000]: iter num: 101
[kino replan]: Reallocate ratio: 1.08532
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655459028.000690862, 293.644000000]: plan yaw
[ INFO] [1655459028.000739877, 293.644000000]: cost func: smooth | waypt |
[ INFO] [1655459028.001295672, 293.644000000]: iter num: 225
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.44127 2.47881 3.50352, 0.312844 0.300073 -0.0211757, 0.025101 -0.0236946 -0.00218686
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 653
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655459031.612115055, 297.248000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655459031.615656516, 297.252000000]: iter num: 105
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655459031.615756699, 297.252000000]: plan yaw
[ INFO] [1655459031.615797633, 297.252000000]: cost func: smooth | waypt |
[ INFO] [1655459031.616329153, 297.252000000]: iter num: 229
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: 6.65837 3.37761 3.42268, 0.375515 0.212372 -0.0233487, 0.0116187 -0.0263757 0.000751398
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 625
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655459035.114823750, 300.744000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655459035.117739470, 300.748000000]: iter num: 111
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655459035.117829293, 300.748000000]: plan yaw
[ INFO] [1655459035.117858563, 300.748000000]: cost func: smooth | waypt |
[ INFO] [1655459035.118331540, 300.748000000]: iter num: 198
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.04934 3.96274 3.34654, 0.404802 0.116543 -0.0190725, 0.00530267 -0.0276346 0.00160206
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 551
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655459038.772878346, 304.296000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655459038.773724661, 304.296000000]: iter num: 53
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.004, search: 0.004, optimize: 0, adjust time:0
[ INFO] [1655459038.773789861, 304.296000000]: plan yaw
[ INFO] [1655459038.773832982, 304.296000000]: cost func: smooth | waypt |
[ INFO] [1655459038.774342048, 304.296000000]: iter num: 204
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.53238 4.2383 3.28217, 0.418229 0.0437319 -0.0176983, 0.00233234 -0.0160258 -2.13293e-05
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 544
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655459042.375026529, 307.892000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655459042.375981013, 307.892000000]: iter num: 62
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.004, search: 0.004, optimize: 0, adjust time:0
[ INFO] [1655459042.376078917, 307.892000000]: plan yaw
[ INFO] [1655459042.376124564, 307.892000000]: cost func: smooth | waypt |
[ INFO] [1655459042.376527321, 307.892000000]: iter num: 194
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.0362 4.30267 3.22682, 0.423894 -0.00524593 -0.0113647, 0.000964974 -0.0120383 0.00327396
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 597
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655459045.947195792, 311.456000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655459045.949942893, 311.460000000]: iter num: 95
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655459045.950063033, 311.460000000]: plan yaw
[ INFO] [1655459045.950108344, 311.460000000]: cost func: smooth | waypt |
[ INFO] [1655459045.950595666, 311.460000000]: iter num: 217
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: 12.5381 4.21551 3.20679, 0.426272 -0.0424144 -6.82524e-05, 0.000421513 -0.0092107 0.00313065
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 517
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655459049.485351374, 314.988000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655459049.486283231, 314.992000000]: iter num: 52
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655459049.486339508, 314.992000000]: plan yaw
[ INFO] [1655459049.486365433, 314.992000000]: cost func: smooth | waypt |
[ INFO] [1655459049.486715077, 314.992000000]: iter num: 213
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: 14.0304 4.03481 3.21552, 0.427295 -0.0570885 0.00352209, 0.000186722 -0.00262965 0.000447073
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 465
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655459052.989292101, 318.488000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655459052.991079130, 318.488000000]: iter num: 94
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0.004, search: 0.004, optimize: 0, adjust time:0
[ INFO] [1655459052.991168854, 318.488000000]: plan yaw
[ INFO] [1655459052.991210253, 318.488000000]: cost func: smooth | waypt |
[ INFO] [1655459052.991636891, 318.492000000]: iter num: 217
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: 15.5286 3.79619 3.23649, 0.427753 -0.0848904 0.00995665, 8.22551e-05 -0.0125021 0.00303199
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 445
- - - - - -
reach horizon
[kino replan]: kinodynamic search success.
[ INFO] [1655459056.495978127, 321.988000000]: cost func: smooth | dist | feasi | endpt |
[ INFO] [1655459056.497255734, 321.988000000]: iter num: 86
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0, search: 0, optimize: 0, adjust time:0
[ INFO] [1655459056.497317055, 321.988000000]: plan yaw
[ INFO] [1655459056.497344443, 321.988000000]: cost func: smooth | waypt |
[ INFO] [1655459056.497621834, 321.988000000]: iter num: 180
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.9867 3.43267 3.28935, 0.427948 -0.128827 0.0214044, 3.58853e-05 -0.0132129 0.00363376
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 361
- - - - - -
reach end
[kino replan]: kinodynamic search success.
[ INFO] [1655459059.906744739, 325.396000000]: cost func: smooth | dist | feasi |
[ INFO] [1655459059.907325903, 325.396000000]: iter num: 55
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0, search: 0, optimize: 0, adjust time:0
[ INFO] [1655459059.907375241, 325.396000000]: plan yaw
[ INFO] [1655459059.907418197, 325.396000000]: cost func: smooth | waypt |
[ INFO] [1655459059.907744678, 325.396000000]: 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: 18.384 2.8685 3.3958, 0.425593 -0.228707 0.04663, 0.000855706 -0.0372742 0.00923081
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 276
- - - - - -
reach end
[kino replan]: kinodynamic search success.
[ INFO] [1655459063.191634667, 328.672000000]: cost func: smooth | dist | feasi |
[ INFO] [1655459063.192023481, 328.676000000]: iter num: 32
[kino replan]: Reallocate ratio: 1.04409
[kino replan]: time: 0.004, search: 0, optimize: 0.004, adjust time:0
[ INFO] [1655459063.192074876, 328.676000000]: plan yaw
[ INFO] [1655459063.192109807, 328.676000000]: cost func: smooth | waypt |
[ INFO] [1655459063.192344977, 328.676000000]: iter num: 181
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.6692 2.09758 3.5316, 0.356554 -0.21495 0.0269545, -0.0141805 0.0180099 -0.00925406
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 240
- - - - - -
reach end
[kino replan]: kinodynamic search success.
[ INFO] [1655459066.456674858, 331.928000000]: cost func: smooth | dist | feasi |
[ INFO] [1655459066.456922506, 331.928000000]: iter num: 34
[kino replan]: Reallocate ratio: 1
[kino replan]: time: 0, search: 0, optimize: 0, adjust time:0
[ INFO] [1655459066.456974446, 331.928000000]: plan yaw
[ INFO] [1655459066.457006145, 331.928000000]: cost func: smooth | waypt |
[ INFO] [1655459066.457214227, 331.928000000]: iter num: 149
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: 20.9895 1.38788 3.64261, 0.292856 -0.139361 0.0387135, 6.15765e-05 0.0034437 0.00826029
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 152
- - - - - -
reach end
[kino replan]: kinodynamic search success.
[ INFO] [1655459070.603718455, 336.068000000]: cost func: smooth | dist | feasi |
[ INFO] [1655459070.603838770, 336.068000000]: iter num: 22
[kino replan]: Reallocate ratio: 1.44377
[kino replan]: time: 0, search: 0, optimize: 0, adjust time:0
[ INFO] [1655459070.603888314, 336.068000000]: plan yaw
[ INFO] [1655459070.603923543, 336.068000000]: cost func: smooth | waypt |
[ INFO] [1655459070.604036235, 336.068000000]: iter num: 111
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: 21.377 0.213032 4.50116, 0.0767201 -0.44078 0.328465, 0.110494 0.108165 -0.125324
goal:23.5 0 4, 0 0 0
- - - - - -
USE_NODE_NUM_: 106
- - - - - -
reach end
[kino replan]: kinodynamic search success.
[ INFO] [1655459073.965494533, 339.428000000]: cost func: smooth | dist | feasi |
[ INFO] [1655459073.965638476, 339.428000000]: iter num: 22
[kino replan]: Reallocate ratio: 1.04011
[kino replan]: time: 0, search: 0, optimize: 0, adjust time:0
[ INFO] [1655459073.965706794, 339.428000000]: plan yaw
[ INFO] [1655459073.965746984, 339.428000000]: cost func: smooth | waypt |
[ INFO] [1655459073.965873609, 339.428000000]: iter num: 91
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 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
[flight_node-9] killing on exit
[mavros-8] killing on exit
[geometric_controller-12] 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