Esempio n. 1
0
void CEntropy::getPlannerData(ob::PlannerData& data) const{
  // fill data with the states and edges that were created
  // in the exploration data structure
  // perhaps also fill control::PlannerData
    ob::StateSpacePtr sp = si_->getStateSpace();
      ob::State *state = ( sp->as<ob::RealVectorStateSpace>()->allocState() );
    og::PathGeometric *pathGeometric = new og::PathGeometric(si_);
      for(int j = 0; j < numD; j++){
          (state->as<ob::RealVectorStateSpace::StateType>())->values[j] = start.q[j];
      }
      pathGeometric->append(si_->cloneState(state));
      for(int i = 0; i < solution.states.size(); i++){
          for(int j = 0; j < numD; j++){
              (state->as<ob::RealVectorStateSpace::StateType>())->values[j] = solution.states[i].q[j];
          }     
          pathGeometric->append(si_->cloneState(state));
      }
      for(int j = 0; j < numD; j++){
          (state->as<ob::RealVectorStateSpace::StateType>())->values[j] = goal.q[j];
      }
      pathGeometric->append(si_->cloneState(state));
      si_->freeState(state);
      
    ob::Planner::getPlannerData(data);
    
     for(unsigned int j = 0; j < this->nStatesPath+1; j++){
         data.addEdge(pathGeometric->getState(j),pathGeometric->getState(j+1));
     }
}
Esempio n. 2
0
void KrisLibraryOMPLPlanner::getPlannerData (ob::PlannerData &data) const
{
  if(!planner) return;
  OMPLCSpace* nc_cspace = const_cast<OMPLCSpace*>((const OMPLCSpace*)cspace);
  RoadmapPlanner roadmap(nc_cspace);
  MotionPlannerInterface* iplanner = const_cast<MotionPlannerInterface*>((const MotionPlannerInterface*)planner);
  iplanner->GetRoadmap(roadmap);
  for(size_t i=0;i<roadmap.roadmap.nodes.size();i++) {
    unsigned int id = data.addVertex(ob::PlannerDataVertex(nc_cspace->ToOMPL(roadmap.roadmap.nodes[i]),(int)i));
    Assert(id == i);
  }
  for(size_t i=0;i<roadmap.roadmap.nodes.size();i++) {
    RoadmapPlanner::Roadmap::Iterator e;
    for(roadmap.roadmap.Begin(i,e);!e.end();e++)
      data.addEdge(e.source(),e.target());
  }
}