search

sjwo / path_planner

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

figure out nasty loopy thing #9

Closed sjwo closed 3 years ago

sjwo commented 3 years ago

image etc....

sjwo commented 3 years ago

Looks like it's in my C++ wrapper, after I've received plan from BIT* planner, and while I'm trying to parse it/convert it to Alex's stuff.

sjwo commented 3 years ago

Ah, here's the problem! BitStarPlanner is receiving a null raw plan:

image

Why is this the case? Can I confirm Rust is sending it? Is there a pipe issue?

sjwo commented 3 years ago

No luck troubleshooting from within project11 and test_scenario_runner. Going to update my C++ example wrapper and go from there.

sjwo commented 3 years ago

So I think at least part of the issue is that my planner is slow and the timeout I gave it (1.9 s) was too short for it to return anything.

sjwo commented 3 years ago

Got somethin'!

image

We're getting chunks!

             path_planner: DEBUG: BitStarPlanner about to get plan chunks: 
             path_planner: DEBUG: BitStarPlanner CHILD will make this system call: /home/aifs2/sjw/code/project11/catkin_ws/src/path_planner
                        ~  /path_planner/src/planner/bit_star_planner/target/release/app -v dubins -u 1 -t 10.0 --start-x 40.123471 --start-
                        ~  y 70.460669 --start-theta 1.570796 --goal-x 70.000000 --goal-y 117.999990 --goal-theta 4.712389
             path_planner: DEBUG: BitStarPlanner got plan chunk: 1
             path_planner: DEBUG: BitStarPlanner got plan chunk: 58.251373
             path_planner: DEBUG: BitStarPlanner got plan chunk: 1
             path_planner: DEBUG: BitStarPlanner got plan chunk: 40.123471 70.460669 1.570796 40.123471 70.460669 1.570796 0.530351 55.10978
                        ~  1 2.611241 1.000000 RSR
             path_planner: DEBUG: BitStarPlanner got plan chunk: 25
             path_planner: DEBUG: BitStarPlanner got plan chunk: 40.123471 70.460669 1.570796 40.123471 70.460669 1.570796 0.735115 34.02052
                        ~  9 0.482605 1.000000 RSR
             path_planner: DEBUG: BitStarPlanner got plan chunk: 40.123471 70.460669 1.570796 40.123471 70.460669 1.570796 0.530351 55.10978
                        ~  1 2.611241 1.000000 RSR
             path_planner: DEBUG: BitStarPlanner got plan chunk: 40.123471 70.460669 1.570796 40.123471 70.460669 1.570796 0.622226 37.70092
                        ~  9 2.166393 1.000000 RSR
             path_planner: DEBUG: BitStarPlanner got plan chunk: 40.123471 70.460669 1.570796 40.123471 70.460669 1.570796 0.072672 30.24860
                        ~  9 2.766115 1.000000 LSR
             path_planner: DEBUG: BitStarPlanner got plan chunk: 40.123471 70.460669 1.570796 40.123471 70.460669 1.570796 0.199980 29.21895
                        ~  0 2.050297 1.000000 RSR
             path_planner: DEBUG: BitStarPlanner got plan chunk: 40.123471 70.460669 1.570796 40.123471 70.460669 1.570796 1.256102 43.80737
                        ~  7 0.198956 1.000000 RSL
             path_planner: DEBUG: BitStarPlanner got plan chunk: 40.123471 70.460669 1.570796 40.123471 70.460669 1.570796 1.665595 12.68075
                        ~  8 1.223103 1.000000 LSL
             path_planner: DEBUG: BitStarPlanner got plan chunk: 40.123471 70.460669 1.570796 40.123471 70.460669 1.570796 0.470694 60.60194
                        ~  3 0.853151 1.000000 RSL
             path_planner: DEBUG: BitStarPlanner got plan chunk: 40.123471 70.460669 1.570796 40.123471 70.460669 1.570796 0.063250 52.05460
                        ~  8 0.078564 1.000000 RSR
             path_planner: solution with cost 58.251373 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.123471
             path_planner: step 1 created DubinsPath with qi[0] of 40.123471
             path_planner: step 1 created DubinsWrapper with length 58.251373
             path_planner: step 1 updated DubinsPlan, which now has totalTime 58.251373
             path_planner: parent exits
             path_planner: DEBUG: BitStarPlanner got plan chunk: 40.123471 70.460669 1.570796 40.123471 70.460669 1.570796 0.504911 41.55294
                        ~  0 2.381985 1.000000 LSL
sjwo commented 3 years ago

Looks like 04bd822 resolved the nasty loopy thing.