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));
}
}
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());
}
}
