search

rayvburn / humap_local_planner

Human-aware robot trajectory planner using a hybrid trajectory candidates generation and spatiotemporal cost functions
BSD 3-Clause "New" or "Revised" License
0 stars 1 forks source link

consider adding `ObstacleSeparationCostFunction` #110

Closed rayvburn closed 11 months ago

rayvburn commented 1 year ago

NOTE: different kernel was also tested but it makes not finding any valid trajectory easier. occdist_separation should be changed to 0.02 then.

diff --git a/src/obstacle_separation_cost_function.cpp b/src/obstacle_separation_cost_function.cpp
index e6f86aa..7f781d4 100644
--- a/src/obstacle_separation_cost_function.cpp
+++ b/src/obstacle_separation_cost_function.cpp
@@ -168,19 +168,19 @@ double ObstacleSeparationCostFunction::footprintCost(
                        return world_model->footprintCost(xk, yk, th, footprint_spec);
                };

-               // check 4 surrounding points
-               double f_sep_cost_left = footprintCostKernel(separation_dist, +M_PI_2);
-               double f_sep_cost_behind = footprintCostKernel(separation_dist, +M_PI);
-               double f_sep_cost_right = footprintCostKernel(separation_dist, -M_PI_2);
-               double f_sep_cost_front = footprintCostKernel(separation_dist, 0.0);
+               std::vector<double> costs;
+               // cost of the footprint at the center
+               costs.push_back(footprint_cost);
+               // check surrounding points
+               costs.push_back(footprintCostKernel(separation_dist, 0.0));
+               costs.push_back(footprintCostKernel(separation_dist, +M_PI_4));
+               costs.push_back(footprintCostKernel(separation_dist, +M_PI_2));
+               costs.push_back(footprintCostKernel(separation_dist, +3.0 * M_PI_4));
+               costs.push_back(footprintCostKernel(separation_dist, +M_PI));
+               costs.push_back(footprintCostKernel(separation_dist, -3.0 * M_PI_4));
+               costs.push_back(footprintCostKernel(separation_dist, -M_PI_2));
+               costs.push_back(footprintCostKernel(separation_dist, -M_PI_4));

-               std::vector<double> costs = {
-                       footprint_cost,
-                       f_sep_cost_left,
-                       f_sep_cost_behind,
-                       f_sep_cost_right,
-                       f_sep_cost_front
-               };
                double max_cost = *std::max_element(costs.cbegin(), costs.cend());
                double min_cost = *std::min_element(costs.cbegin(), costs.cend());
                // choosing min negative or max positive
rayvburn commented 1 year ago

Robot sometimes gets stuck with invalid trajectories due to the enlarged footprint.

image

image

[ERROR] [1694037768.256936616, 239.547000000]: Explored 126 trajectories, but couldn't find a valid one (cost -7.00)
  1) cost   -6.000, vel { 0.126,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   25.4 GFR    9.4 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.3 SMTH  0.8 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
  2) cost   -6.000, vel { 0.126,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   25.4 GFR    9.4 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.3 SMTH  0.8 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
  3) cost   -6.000, vel { 0.126,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   25.4 GFR    9.4 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.4 SMTH  0.6 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
  4) cost   -6.000, vel { 0.124,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.1 GL   25.4 GFR    9.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  5.6 SMTH  0.4 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
  5) cost   -6.000, vel { 0.123,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.4 GL   24.5 GFR   10.0 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  5.9 SMTH  0.8 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
  6) cost   -6.000, vel { 0.121,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.7 GL   23.6 GFR   10.4 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  6.4 SMTH  1.0 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
  7) cost   -6.000, vel { 0.118,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   23.2 GFR   10.8 ALGN    0.3 BWD  0.2 TTC  0.0 HSM  7.0 SMTH  1.2 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
  8) cost   -6.000, vel { 0.116,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    2.2 GL   23.6 GFR 5120.0 ALGN  960.0 BWD  0.2 TTC  0.0 HSM  7.5 SMTH  1.3 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
  9) cost   -6.000, vel { 0.113,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH 7040.3 GL 11200.4 GFR 5120.2 ALGN  960.0 BWD  0.2 TTC  0.0 HSM  7.9 SMTH  1.1 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 10) cost   -6.000, vel { 0.126,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   25.4 GFR    9.4 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.3 SMTH  0.8 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 11) cost   -6.000, vel { 0.126,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   25.4 GFR    9.4 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.3 SMTH  0.8 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 12) cost   -6.000, vel { 0.126,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   25.4 GFR    9.4 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.3 SMTH  0.8 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 13) cost   -6.000, vel { 0.126,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   25.4 GFR    9.4 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.3 SMTH  0.8 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 14) cost   -6.000, vel { 0.126,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   25.4 GFR    9.4 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.3 SMTH  0.8 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 15) cost   -6.000, vel { 0.126,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   25.4 GFR    9.4 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.3 SMTH  0.8 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 16) cost   -6.000, vel { 0.126,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   25.4 GFR    9.4 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.3 SMTH  0.8 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 17) cost   -6.000, vel { 0.126,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   25.4 GFR    9.4 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.3 SMTH  0.8 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 18) cost   -6.000, vel { 0.126,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   25.4 GFR    9.4 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.3 SMTH  0.8 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 19) cost   -6.000, vel { 0.126,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   25.4 GFR    9.4 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.3 SMTH  0.8 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 20) cost   -6.000, vel { 0.126,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   25.4 GFR    9.4 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.3 SMTH  0.8 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 21) cost   -6.000, vel { 0.126,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   25.4 GFR    9.4 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.3 SMTH  0.8 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 22) cost   -6.000, vel { 0.126,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   25.4 GFR    9.4 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.3 SMTH  0.8 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 23) cost   -6.000, vel { 0.126,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   25.4 GFR    9.4 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.3 SMTH  0.8 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 24) cost   -6.000, vel { 0.127,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   25.4 GFR    9.4 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.3 SMTH  0.8 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 25) cost   -6.000, vel { 0.127,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   25.4 GFR    9.4 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.4 SMTH  1.0 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 26) cost   -6.000, vel { 0.129,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.6 GL   25.8 GFR    9.2 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.5 SMTH  1.1 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 27) cost   -6.000, vel { 0.131,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.3 GL   26.2 GFR    8.8 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  5.8 SMTH  1.3 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 28) cost   -6.000, vel { 0.134,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.0 GL   27.1 GFR    8.4 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  5.3 SMTH  1.1 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 29) cost   -6.000, vel { 0.137,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.6 GL   28.0 GFR    9.0 ALGN    0.3 BWD  0.2 TTC  0.0 HSM  5.0 SMTH  1.0 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 30) cost   -6.000, vel { 0.141,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   28.4 GFR    9.4 ALGN    0.3 BWD  0.2 TTC  0.0 HSM  4.9 SMTH  1.0 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 31) cost   -6.000, vel { 0.145,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.4 GL   28.9 GFR    9.8 ALGN    0.4 BWD  0.2 TTC  0.0 HSM  5.2 SMTH  1.2 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 32) cost   -6.000, vel { 0.144,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   21.9 GFR   11.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.6 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 33) cost   -6.000, vel { 0.144,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   21.9 GFR   11.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.6 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 34) cost   -6.000, vel { 0.143,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   21.9 GFR   11.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.7 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 35) cost   -6.000, vel { 0.142,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.6 GL   21.4 GFR   11.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.7 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 36) cost   -6.000, vel { 0.141,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.6 GL   21.4 GFR   11.8 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 37) cost   -6.000, vel { 0.140,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.3 GL   21.0 GFR   11.8 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.0 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 38) cost   -6.000, vel { 0.138,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.0 GL   20.6 GFR   12.0 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.3 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 39) cost   -6.000, vel { 0.137,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.3 GL   20.1 GFR   12.2 ALGN    0.0 BWD  0.2 TTC  0.0 HSM  5.6 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 40) cost   -6.000, vel { 0.135,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.3 GL   20.1 GFR   12.4 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  5.9 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 41) cost   -6.000, vel { 0.144,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   21.9 GFR   11.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.6 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 42) cost   -6.000, vel { 0.144,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   21.9 GFR   11.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.6 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 43) cost   -6.000, vel { 0.144,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   21.9 GFR   11.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.6 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 44) cost   -6.000, vel { 0.144,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   21.9 GFR   11.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.6 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 45) cost   -6.000, vel { 0.144,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   21.9 GFR   11.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.6 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 46) cost   -6.000, vel { 0.144,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   21.9 GFR   11.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.6 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 47) cost   -6.000, vel { 0.144,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   21.9 GFR   11.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.6 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 48) cost   -6.000, vel { 0.144,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   21.9 GFR   11.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.6 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 49) cost   -6.000, vel { 0.144,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   21.9 GFR   11.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.6 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 50) cost   -6.000, vel { 0.144,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   21.9 GFR   11.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.6 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 51) cost   -6.000, vel { 0.144,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   21.9 GFR   11.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.6 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 52) cost   -6.000, vel { 0.144,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   21.9 GFR   11.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.6 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 53) cost   -6.000, vel { 0.144,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   21.9 GFR   11.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.6 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 54) cost   -6.000, vel { 0.144,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   21.9 GFR   11.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.6 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 55) cost   -6.000, vel { 0.144,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   21.9 GFR   11.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.6 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 56) cost   -6.000, vel { 0.145,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   21.9 GFR   11.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.6 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 57) cost   -6.000, vel { 0.146,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   21.9 GFR   11.4 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  4.6 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 58) cost   -6.000, vel { 0.147,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.1 GL   22.3 GFR   11.4 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  4.6 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 59) cost   -6.000, vel { 0.149,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.4 GL   22.8 GFR   11.2 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  4.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 60) cost   -6.000, vel { 0.150,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.7 GL   22.3 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  4.9 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 61) cost   -6.000, vel { 0.152,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    2.2 GL   23.2 GFR   11.0 ALGN    0.3 BWD  0.2 TTC  0.0 HSM  4.9 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 62) cost   -6.000, vel { 0.154,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    2.2 GL   24.1 GFR   11.4 ALGN    0.4 BWD  0.2 TTC  0.0 HSM  4.9 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 63) cost   -6.000, vel { 0.151,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   22.8 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 64) cost   -6.000, vel { 0.151,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   22.8 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 65) cost   -6.000, vel { 0.150,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   22.8 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 66) cost   -6.000, vel { 0.150,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.7 GL   22.3 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.9 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 67) cost   -6.000, vel { 0.149,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.7 GL   22.3 GFR   11.2 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.9 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 68) cost   -6.000, vel { 0.148,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.7 GL   22.3 GFR   11.2 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.9 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 69) cost   -6.000, vel { 0.147,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.4 GL   21.9 GFR   11.4 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  4.0 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 70) cost   -6.000, vel { 0.146,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.1 GL   21.4 GFR   11.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.1 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 71) cost   -6.000, vel { 0.144,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    0.8 GL   21.9 GFR   11.8 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  4.2 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 72) cost   -6.000, vel { 0.151,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   22.8 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 73) cost   -6.000, vel { 0.151,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   22.8 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 74) cost   -6.000, vel { 0.151,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   22.8 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 75) cost   -6.000, vel { 0.151,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   22.8 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 76) cost   -6.000, vel { 0.151,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   22.8 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 77) cost   -6.000, vel { 0.151,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   22.8 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 78) cost   -6.000, vel { 0.151,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   22.8 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 79) cost   -6.000, vel { 0.151,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   22.8 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 80) cost   -6.000, vel { 0.151,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   22.8 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 81) cost   -6.000, vel { 0.151,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   22.8 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 82) cost   -6.000, vel { 0.151,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   22.8 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 83) cost   -6.000, vel { 0.151,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   22.8 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 84) cost   -6.000, vel { 0.151,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   22.8 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 85) cost   -6.000, vel { 0.151,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   22.8 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 86) cost   -6.000, vel { 0.151,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   22.8 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 87) cost   -6.000, vel { 0.151,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   22.8 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 88) cost   -6.000, vel { 0.152,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   22.8 GFR   11.0 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 89) cost   -6.000, vel { 0.153,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    1.9 GL   22.8 GFR   10.8 ALGN    0.3 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 90) cost   -6.000, vel { 0.154,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    2.2 GL   23.2 GFR   11.0 ALGN    0.3 BWD  0.2 TTC  0.0 HSM  3.8 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 91) cost   -6.000, vel { 0.155,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    2.5 GL   23.6 GFR   11.0 ALGN    0.3 BWD  0.2 TTC  0.0 HSM  3.7 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 92) cost   -6.000, vel { 0.157,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    2.2 GL   24.1 GFR   11.2 ALGN    0.3 BWD  0.2 TTC  0.0 HSM  3.7 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 93) cost   -6.000, vel { 0.158,  0.000,  0.209} OBS -0.1 OSC  0.0 PTH    2.5 GL   24.5 GFR   11.6 ALGN    0.4 BWD  0.2 TTC  0.0 HSM  3.7 SMTH  0.5 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 94) cost   -6.000, vel { 0.100,  0.000, -0.262} OBS -0.1 OSC  0.0 PTH    0.3 GL   40.7 GFR    3.0 ALGN    0.6 BWD  0.2 TTC  0.0 HSM  0.2 SMTH  0.1 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 95) cost   -6.000, vel { 0.100,  0.000, -0.204} OBS -0.1 OSC  0.0 PTH    0.0 GL   40.2 GFR    2.6 ALGN    0.5 BWD  0.2 TTC  0.0 HSM  0.1 SMTH  0.1 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 96) cost   -6.000, vel { 0.100,  0.000, -0.145} OBS -0.1 OSC  0.0 PTH    0.0 GL   40.2 GFR    2.6 ALGN    0.4 BWD  0.1 TTC  0.0 HSM  0.1 SMTH  0.1 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 97) cost   -6.000, vel { 0.100,  0.000, -0.087} OBS -0.1 OSC  0.0 PTH    0.6 GL   40.2 GFR    3.2 ALGN    0.3 BWD  0.1 TTC  0.0 HSM  0.1 SMTH  0.1 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 98) cost   -6.000, vel { 0.100,  0.000, -0.029} OBS -0.1 OSC  0.0 PTH    0.8 GL   39.8 GFR    3.4 ALGN    0.2 BWD  0.0 TTC  0.0 HSM  0.0 SMTH  0.1 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
 99) cost   -6.000, vel { 0.100,  0.000,  0.000} OBS -0.1 OSC  0.0 PTH    0.8 GL   39.8 GFR    3.6 ALGN    0.2 BWD  0.0 TTC  0.0 HSM  0.0 SMTH  0.1 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
100) cost   -6.000, vel { 0.100,  0.000,  0.029} OBS -0.1 OSC  0.0 PTH    1.1 GL   39.8 GFR    3.6 ALGN    0.2 BWD  0.0 TTC  0.0 HSM  0.0 SMTH  0.1 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
101) cost   -6.000, vel { 0.100,  0.000,  0.087} OBS -0.1 OSC  0.0 PTH    1.1 GL   39.8 GFR    3.8 ALGN    0.1 BWD  0.1 TTC  0.0 HSM  0.1 SMTH  0.1 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
102) cost   -6.000, vel { 0.100,  0.000,  0.145} OBS -0.1 OSC  0.0 PTH 7040.0 GL 11200.0 GFR    3.8 ALGN    0.0 BWD  0.1 TTC  0.0 HSM  0.1 SMTH  0.1 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
103) cost   -6.000, vel { 0.100,  0.000,  0.204} OBS -0.1 OSC  0.0 PTH 7040.3 GL 11200.4 GFR    3.4 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  0.1 SMTH  0.1 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
104) cost   -6.000, vel { 0.100,  0.000,  0.262} OBS -0.1 OSC  0.0 PTH 7040.3 GL 11200.4 GFR    3.8 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  0.2 SMTH  0.1 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
105) cost   -6.000, vel { 0.175,  0.000, -0.262} OBS -0.1 OSC  0.0 PTH    1.7 GL   42.4 GFR    4.4 ALGN    0.8 BWD  0.2 TTC  0.0 HSM  0.2 SMTH  0.2 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
106) cost   -6.000, vel { 0.175,  0.000, -0.204} OBS -0.1 OSC  0.0 PTH    1.7 GL   42.0 GFR    4.4 ALGN    0.7 BWD  0.2 TTC  0.0 HSM  0.1 SMTH  0.2 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
107) cost   -6.000, vel { 0.175,  0.000, -0.145} OBS -0.1 OSC  0.0 PTH    1.1 GL   41.1 GFR    4.8 ALGN    0.6 BWD  0.1 TTC  0.0 HSM  0.1 SMTH  0.2 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
108) cost   -6.000, vel { 0.175,  0.000, -0.087} OBS -0.1 OSC  0.0 PTH    0.8 GL   40.2 GFR    5.4 ALGN    0.5 BWD  0.1 TTC  0.0 HSM  0.1 SMTH  0.2 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
109) cost   -6.000, vel { 0.175,  0.000, -0.029} OBS -0.1 OSC  0.0 PTH    0.6 GL   39.4 GFR    5.8 ALGN    0.5 BWD  0.0 TTC  0.0 HSM  0.0 SMTH  0.2 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
110) cost   -6.000, vel { 0.175,  0.000,  0.000} OBS -0.1 OSC  0.0 PTH    0.6 GL   38.9 GFR    6.0 ALGN    0.5 BWD  0.0 TTC  0.0 HSM  0.0 SMTH  0.2 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
111) cost   -6.000, vel { 0.175,  0.000,  0.029} OBS -0.1 OSC  0.0 PTH    0.3 GL   38.5 GFR    6.0 ALGN    0.4 BWD  0.0 TTC  0.0 HSM  0.0 SMTH  0.2 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
112) cost   -6.000, vel { 0.175,  0.000,  0.087} OBS -0.1 OSC  0.0 PTH    0.0 GL   37.6 GFR    6.2 ALGN    0.4 BWD  0.1 TTC  0.0 HSM  0.1 SMTH  0.2 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
113) cost   -6.000, vel { 0.175,  0.000,  0.145} OBS -0.1 OSC  0.0 PTH    0.6 GL   36.3 GFR    6.0 ALGN    0.3 BWD  0.1 TTC  0.0 HSM  0.1 SMTH  0.2 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
114) cost   -6.000, vel { 0.175,  0.000,  0.204} OBS -0.1 OSC  0.0 PTH    0.8 GL   35.4 GFR    5.8 ALGN    0.2 BWD  0.2 TTC  0.0 HSM  0.1 SMTH  0.2 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
115) cost   -6.000, vel { 0.175,  0.000,  0.262} OBS -0.1 OSC  0.0 PTH    1.1 GL   35.0 GFR    5.6 ALGN    0.1 BWD  0.2 TTC  0.0 HSM  0.2 SMTH  0.2 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
116) cost   -6.000, vel { 0.250,  0.000, -0.262} OBS -0.1 OSC  0.0 PTH    3.9 GL   45.5 GFR    5.6 ALGN    1.1 BWD  0.2 TTC  0.0 HSM  0.2 SMTH  0.3 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
117) cost   -6.000, vel { 0.250,  0.000, -0.204} OBS -0.1 OSC  0.0 PTH    3.3 GL   44.6 GFR    6.4 ALGN    0.9 BWD  0.2 TTC  0.0 HSM  0.1 SMTH  0.3 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
118) cost   -6.000, vel { 0.250,  0.000, -0.145} OBS -0.1 OSC  0.0 PTH    3.0 GL   43.3 GFR    7.0 ALGN    0.9 BWD  0.1 TTC  0.0 HSM  0.1 SMTH  0.3 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
119) cost   -6.000, vel { 0.250,  0.000, -0.087} OBS -0.1 OSC  0.0 PTH    2.5 GL   42.0 GFR    7.6 ALGN    0.8 BWD  0.1 TTC  0.0 HSM  0.1 SMTH  0.3 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
120) cost   -6.000, vel { 0.250,  0.000, -0.029} OBS -0.1 OSC  0.0 PTH    2.5 GL   41.1 GFR    8.0 ALGN    0.8 BWD  0.0 TTC  0.0 HSM  0.0 SMTH  0.3 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
121) cost   -6.000, vel { 0.250,  0.000,  0.000} OBS -0.1 OSC  0.0 PTH    2.2 GL   40.2 GFR    8.2 ALGN    0.8 BWD  0.0 TTC  0.0 HSM  0.0 SMTH  0.3 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
122) cost   -6.000, vel { 0.250,  0.000,  0.029} OBS -0.1 OSC  0.0 PTH    1.9 GL   39.4 GFR    8.4 ALGN    0.7 BWD  0.0 TTC  0.0 HSM  0.0 SMTH  0.3 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
123) cost   -6.000, vel { 0.250,  0.000,  0.087} OBS -0.1 OSC  0.0 PTH    1.7 GL   38.1 GFR    8.4 ALGN    0.6 BWD  0.1 TTC  0.0 HSM  0.1 SMTH  0.3 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
124) cost   -6.000, vel { 0.250,  0.000,  0.145} OBS -0.1 OSC  0.0 PTH    1.4 GL   37.2 GFR    8.4 ALGN    0.6 BWD  0.1 TTC  0.0 HSM  0.1 SMTH  0.3 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
125) cost   -6.000, vel { 0.250,  0.000,  0.204} OBS -0.1 OSC  0.0 PTH    1.1 GL   35.4 GFR    8.2 ALGN    0.5 BWD  0.2 TTC  0.0 HSM  0.1 SMTH  0.3 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
126) cost   -6.000, vel { 0.250,  0.000,  0.262} OBS -0.1 OSC  0.0 PTH    0.6 GL   34.1 GFR    7.8 ALGN    0.3 BWD  0.2 TTC  0.0 HSM  0.2 SMTH  0.3 HDIR  0.0 PSI  0.0 FSI  0.0 PSPD  0.0
[ERROR] [1694037768.395796261, 239.547000000]: Could not find a valid trajectory (cost -7.00), applying zero velocity to base
[ INFO] [1694037768.396752317, 239.547000000]: Full control cycle time: real {0.385683754}, sim {0.056000000}
rayvburn commented 1 year ago

The problem seems not to be avoidable. Higher refresh rates of the costmap or a smaller granularity of the forward simulation may help. Currently, the rectangular kernel and smaller separation distance were chosen.