Re: [ompl-users] RRT* - replanning motion

[Mark M. <mm...@ri...>]

Hi Luca,

The bookkeeping for re-checking all states generated during a previous planning cycle can be quite expensive. It might be better to preserve just the remaining states of the path that the robot has been executing. The states on this path can be stored inside a new a StateSampler-derived class. This new class will return these states the first number of calls to sample(). Subsequent calls to sample() will return uniform samples as usual. You no need to set a SamplerAllocator <http://ompl.kavrakilab.org/classompl_1_1base_1_1StateSpace.html#aa88e8a85586b93866ef3b2a6fb31c114> to specify a function that will return an instance of this sampler.

Mark



> On Dec 15, 2017, at 5:22 AM, bartolomei-luca--- via ompl-users <omp...@li...> wrote:
> 
> Hello everyone!
> 
> I am trying to implement a path planning algorithm for a robot that navigates in an unknown environment. The concept is very simple: plan, move and if an obstacle is detected, then re-plan. Everything is implemented in ROS.
> 
> In order to do so, I am trying to modify the RRT* algorithm. Basically, I want to re-use the existing tree and modify it according to the new incoming information. This issue has already been discussed in the OMPL mailing list, but I would like to explain to you guys my approach. It is different from all the others I have seen.
> 
> I want to create a "storage" for the current tree. Every time the planner is called:
> 
> 1) Read storage variable
> 
> 2) Upload storage in the nn_ variable of the planner
> 
> 3) Plan
> 
> 4) Save the new tree in the storage variable
> 
> Unfortunately it is not working properly. Now I will get into more detail (code-wise) . I have added some marks where the problems happen. They are explained at the bottom of the email.
> 
> 
> 
> At the moment, I am saving the tree in a vector --> std::vector<Motion*> tree_storage
> 
> I have added a temporary storage in the RRT* planner --> std::vector<Motion*>* temp_tree_storage (NOTE: a pointer)
> 
> temp_tree_storage is needed in order to save the information and "upload" them in the variable nn_  of the planner. I have added the following method in order to save/update the tree:
> 
> void ompl::geometric::RRTstarModified::setTree(std::vector<Motion*>* currentTree)  // NOTE: get a pointer as input
>  {
> 
>      temp_tree_storage = currentTree;             // set the address where to save the new tree --> problem:(1)
> 
>      for(int i=0; i<currentTree->size(); ++i) {
> 
>                nn_->add((*currentTree)[i]);             // upload the new tree in the nn_ variable -----> problem:(2)
> 
>      }
> 
> }
> 
> The "solve" method has also been modified in order to save the new tree:
> 
> ompl::base::PlannerStatus ompl::geometric::RRTstarModified::solve(const base::PlannerTerminationCondition &ptc)
> {
> 
>         // Solver stuff ....
> 
>          .......
> 
>        /// Before the "return"
> 
>       temp_tree_storage ->clear();
>        std::vector<Motion*> list;
>        nn_->list(list);                                  // get the new tree
>        for(int i=0; i<list.size(); ++i)
>                    temp_tree_storage->push_back(list[i]);
> 
> }
> 
> Since the information on the space are changing,  I am creating a new planner at every "iteration" (i.e. every time a replanning is needed). This happens in the main class:
> 
> 
> 
> ompl::base::PlannerPtr MyPlannerAlgorithm::allocateGlobalPlanner(const ompl::base::SpaceInformationPtr &si)
> {
>     ompl::geometric::RRTstarModified *rrt_star_planner {new ompl::geometric::RRTstarModified(si)};
>     rrt_star_planner->setTree(&currentTree);  // The method described above
>     ompl::base::PlannerPtr planner(rrt_star_planner);
> 
>     return planner;
> }
> 
> There are 2 main problems:
> 
> (1) The children of some of the motions go crazy -> the information is not saved properly and basically I am reading/saving trash that will make my code crash.
> 
> (2) When I use: nn_->add((*currentTree)[i]);    the code crashes. I have made sure that nn_.reset and nn_->setDistanceFunction before the nn_->add method is called.
> 
> 
> 
> I hope I have explained what's going on clearly. Do you have any suggestion on how to solve the problem? Or do you have any better strategy?
> 
> 
> 
> Thanks a lot for the help!
> 
> Have a nice day,
> Luca
> 
> 
> 
> 
> 
> ------------------------------------------------------------------------------
> Check out the vibrant tech community on one of the world's most
> engaging tech sites, Slashdot.org! http://sdm.link/slashdot_______________________________________________
> ompl-users mailing list
> omp...@li...
> https://lists.sourceforge.net/lists/listinfo/ompl-users