search

sjwo / path_planner

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

Delayed dynamic obstacle info #22

Open sjwo opened 3 years ago

sjwo commented 3 years ago

Only from sim_local.launch.

I.e., see "dynamic_obstacles 0" on last line of ASCII world being passed to BIT*.

sjwo commented 3 years ago
sjwo commented 3 years ago

Ok issue happens even with AFB's map/test file (cannon_wait_1.test) with BIT*, but works fine with RBPC planner, so assuming it's my code not my map/test files.

sjwo commented 3 years ago

Ok, looks like Executive::planLoop makes deep copy before Executive::updateDynamicObstacle is called for the first time:

             path_planner: Initializing planner
             path_planner: Setting running state
             path_planner: 1630427420.412017822: Executive.planLoop() starting 
             path_planner: DEBUG: Executive.planLoop(): m_GaussianDynamicObstaclesManager.size(): 0
             path_planner: DEBUG: Executive.planLoop(): m_GaussianDynamicObstaclesManager.get_deep_copy().size(): 0
             path_planner: 1630427420.412017822: Executive.planLoop() about to call planner.plan()
             path_planner: DEBUG: BitStarPlanner.plan(): dynamic_obstacles_copy.size(): 0
             path_planner: DEBUG: BitStarPlanner constructed world:
             path_planner: 10.000000000
             path_planner: ##___##########
             path_planner: ##___##########
             path_planner: ##___##########
             path_planner: ##___##########
             path_planner: ##___##########
             path_planner: ##___##########
             path_planner: ##_____________
             path_planner: ##_____________
             path_planner: ##_____________
             path_planner: ##_____________
             path_planner: dynamic_obstacles 0
             path_planner: DEBUG: Executive.updateDynamicObstacle() called
             path_planner: DEBUG: Executive.updateDynamicObstacle() has acquired m_GaussianDynamicObstaclesManagerMutex
             path_planner: DEBUG: Executive.updateDynamicObstacle(): PRIOR to update, m_GaussianDynamicObstaclesManager.size(): 0
             path_planner: DEBUG: GaussianDynamicObstaclesManager.update() called with non-ignored mmsi; INITIAL size is 0
             path_planner: DEBUG: GaussianDynamicObstaclesManager.update(): FINAL size is 1
             path_planner: DEBUG: Executive.updateDynamicObstacle(): AFTER update, m_GaussianDynamicObstaclesManager.size(): 1
             path_planner: thread 'main' panicked at 'called `Result::unwrap()` on an `Err` value: "\xB0\xB8%\u{7f}\x8AU"', library/std/src/env
                        ~  .rs:792:51
             path_planner: note: run with `RUST_BACKTRACE=1` environment variable to display a backtrace
             path_planner: DEBUG: Executive.updateDynamicObstacle() called
             path_planner: DEBUG: Executive.updateDynamicObstacle() has acquired m_GaussianDynamicObstaclesManagerMutex
             path_planner: DEBUG: Executive.updateDynamicObstacle(): PRIOR to update, m_GaussianDynamicObstaclesManager.size(): 1
             path_planner: DEBUG: GaussianDynamicObstaclesManager.update() called with non-ignored mmsi; INITIAL size is 1
             path_planner: DEBUG: GaussianDynamicObstaclesManager.update(): FINAL size is 1
             path_planner: DEBUG: Executive.updateDynamicObstacle(): AFTER update, m_GaussianDynamicObstaclesManager.size(): 1
sjwo commented 3 years ago

Looks like dynamic obstacle deep copy in Executive and ASCII world construction BitStarPlanner wrapper worked fine back at f65a1ba2060274598b8d17139de1f0de16b7f4e3. Which means something in one or more of the following three commits introduced the regression:

commit 87b56ac8074ead8e0adbf041603ef4033c113379 (origin/master, master)
Author: Stephen Josef Wissow <sjw1000@wildcats.unh.edu>
Date:   Tue Aug 31 12:51:35 2021 -0400

    Add debug output (#22)

commit 567d94ea347ca99fd85995a993bb2ff2b9f2dedb
Author: Stephen Josef Wissow <sjw1000@wildcats.unh.edu>
Date:   Mon Aug 30 13:21:37 2021 -0400

    Handle BIT*'s Gaussian parameters in Project 11, and parameterize which solution number to use (#21, sjwo/planner#25)

commit 75e0a0fa928eeaffea014e1a756fd089ac054fbb
Author: Stephen Josef Wissow <sjw1000@wildcats.unh.edu>
Date:   Mon Aug 30 13:19:10 2021 -0400

    Ignore results folder

However, planner takes so long to return that no plan is available within cycle.

So, impossible to test within Project 11...