search

sjwo / path_planner

ROS node to interface with path planner
0 stars 0 forks source link

display plan in CAMP #2

Closed sjwo closed 3 years ago

sjwo commented 3 years ago
sjwo commented 3 years ago

So we have some plan displayage in the CAMPage!

https://github.com/sjwo/path_planner/issues/9#issuecomment-875667006

sjwo commented 3 years ago

But it points in the wrong direction, and the controller dies.

So: going to turn off some now unnecessary debug output, and start to focus on what's going on.

sjwo commented 3 years ago

One thing I'm noticing: it looks like the BIT* planner was probably given lat/lon to work in, rather than workspace meters:

             path_planner: DEBUG: BitStarPlanner received following raw plan:
             path_planner: 1
             path_planner: 58.273073
             path_planner: 1
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 0.531170 55.131480 2.610423 1.000000 RSR
             path_planner: 25
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 0.736283 34.050957 0.481437 1.000000 RSR
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 0.531170 55.131480 2.610423 1.000000 RSR
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 0.623365 37.726684 2.165254 1.000000 RSR
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 0.071036 30.244182 2.764479 1.000000 LSR
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 0.201681 29.224860 2.048596 1.000000 RSR
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 1.256543 43.852670 0.199398 1.000000 RSL
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 1.666294 12.631476 1.222404 1.000000 LSL
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 0.471459 60.619383 0.853916 1.000000 RSL
             path_planner: solution with cost 58.273071 has 1 steps found in batch 1
             path_planner: step 1 initialized qi[0] to 0.000000
             path_planner: step 1 updated qi[0] to 40.073571
             path_planner: step 1 created DubinsPath with qi[0] of 40.073571
             path_planner: step 1 created DubinsWrapper with length 58.273073
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 0.064212 52.053667 0.077603 1.000000 RSR
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 0.503907 41.525211 2.382988 1.000000 LSL
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 0.703527 56.046909 1.465766 1.000000 RSL
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 0.627613 56.685655 1.982871 1.000000 RSL
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 1.693579 24.556133 1.733203 1.000000 LSL
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 2.159019 21.607170 2.287111 1.000000 RSR
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 1.272142 64.238859 2.531293 1.000000 RSR
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 0.047889 69.519163 0.905654 1.000000 LSL
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 0.592783 71.265757 3.106540 1.000000 RSR
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 0.270000 52.087775 2.344383 1.000000 RSL
             path_planner: step 1 updated DubinsPlan, which now has totalTime 58.273073
             path_planner: parent exits
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 2.295697 25.333607 1.715964 1.000000 RSL
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 1.027317 52.910077 0.861815 1.000000 RSR
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 2.829900 22.998643 1.081461 1.000000 LSR
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 0.739741 82.670669 1.214600 1.000000 RSR
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 1.253223 37.982397 1.856074 1.000000 LSL
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 1.663570 56.812763 0.095732 1.000000 RSR
             path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 2.011241 34.445312 2.005883 1.000000 RSL
             path_planner: ------------
sjwo commented 3 years ago

So the next thing to do, then, is work out how (and double check whether) AFB handled the lat/lon to meters conversion...

sjwo commented 3 years ago

Todo: look through AFB's

...all the while comparing to my BitStarPlanner.cpp

sjwo commented 3 years ago

Actually, 40.073571 70.464796 is in Isfara, Tajikistan. Negating the longitude at least puts us in the Atlantic, about 100 miles off the coast of Rhode Island. But maybe these numbers are a coincidence? Maybe they're not actually in lat/lon?

sjwo commented 3 years ago

This:

             path_planner: DEBUG: Executive::planLoop() about to call stats.Plan.sample() on expectedStartState with time -1

I think this means that the expectedStartState is still default, like that got messed up somehow. I think the -1 start time indicates that.

sjwo commented 3 years ago

Interesting note: looks like the one-step plan

path_planner: 40.073571 70.464796 1.570796 40.073571 70.464796 1.570796 0.531170 55.131480 2.610423 1.000000 RSR

is reflected, and/or the turns are going in the wrong direction:

image

This suggests the issue is with my conversion either

...and/or:

sjwo commented 3 years ago

It appears the path type (i.e., which dubins word) may not be getting set correctly in BitStarPlanner:

DubinsPath dubins_path = {
            {qi[0], qi[1], qi[2]},
            {param[0], param[1], param[2]},
            rho,
            dubins_path_type(dubins_word_str),
          };
          printf("step %d created DubinsPath with qi[0] of %f (%s: %s)\n", i, dubins_path.qi[0], dubins_word_str.c_str(), dubins_path.type);

because the project11 output shows the following:

             path_planner: solution with cost 58.330795 has 1 steps found in batch 1
             path_planner: step 1 initialized qi[0] to 0.000000
             path_planner: step 1 updated qi[0] to 40.093717
             path_planner: step 1 created DubinsPath with qi[0] of 40.093717 (RSR: (null))
             path_planner: step 1 created DubinsWrapper with length 58.330795
             path_planner: step 1 updated DubinsPlan (with a (null) DubinsWrapper). DubinsPlan now has totalTime 58.330795
             path_planner: parent exits

So, even though we get RSR from the raw plan from BIT* via the pipe, we are not correctly constructing the initial DubinsPath (dubins.h).

sjwo commented 3 years ago

Ah, that's better! image