void ompl::control::PDST::getPlannerData(ompl::base::PlannerData &data) const { base::Planner::getPlannerData(data); double dt = siC_->getPropagationStepSize(); base::State *scratch = si_->allocState(); std::vector<Motion *> motions; motions.reserve(priorityQueue_.size()); priorityQueue_.getContent(motions); // Add goal vertex if (lastGoalMotion_ != nullptr) data.addGoalVertex(lastGoalMotion_->endState_); for (auto &motion : motions) if (!motion->isSplit_) { // We only add one edge for each motion that has been split into smaller segments Motion *cur = motion, *ancestor; unsigned int duration = findDurationAndAncestor(cur, cur->endState_, scratch, ancestor); if (cur->parent_ == nullptr) data.addStartVertex(base::PlannerDataVertex(cur->endState_)); else if (data.hasControls()) { data.addEdge(base::PlannerDataVertex(ancestor->startState_), base::PlannerDataVertex(cur->endState_), PlannerDataEdgeControl(cur->control_, duration * dt)); if (ancestor->parent_) { // Include an edge between start state of parent ancestor motion and // the start state of the ancestor motion, which lies somewhere on // the parent ancestor motion. cur = ancestor; duration = findDurationAndAncestor(cur->parent_, cur->startState_, scratch, ancestor); data.addEdge(base::PlannerDataVertex(ancestor->startState_), base::PlannerDataVertex(cur->startState_), PlannerDataEdgeControl(ancestor->control_, duration * dt)); } } else { data.addEdge(base::PlannerDataVertex(ancestor->startState_), base::PlannerDataVertex(cur->endState_)); if (ancestor->parent_) { // Include an edge between start state of parent ancestor motion and // the start state of the ancestor motion, which lies somewhere on // the parent ancestor motion. cur = ancestor; findDurationAndAncestor(cur->parent_, cur->startState_, scratch, ancestor); data.addEdge(base::PlannerDataVertex(ancestor->startState_), base::PlannerDataVertex(cur->startState_)); } } } si_->freeState(scratch); }
void ompl::control::EST::getPlannerData(base::PlannerData &data) const { Planner::getPlannerData(data); std::vector<MotionInfo> motions; tree_.grid.getContent(motions); double stepSize = siC_->getPropagationStepSize(); if (lastGoalMotion_) data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state)); for (unsigned int i = 0 ; i < motions.size() ; ++i) for (unsigned int j = 0 ; j < motions[i].size() ; ++j) { if (motions[i][j]->parent) { if (data.hasControls()) data.addEdge (base::PlannerDataVertex (motions[i][j]->parent->state), base::PlannerDataVertex (motions[i][j]->state), PlannerDataEdgeControl(motions[i][j]->control, motions[i][j]->steps * stepSize)); else data.addEdge (base::PlannerDataVertex (motions[i][j]->parent->state), base::PlannerDataVertex (motions[i][j]->state)); } else data.addStartVertex (base::PlannerDataVertex (motions[i][j]->state)); } }
void ompl::control::EST::getPlannerData(base::PlannerData &data) const { Planner::getPlannerData(data); std::vector<MotionInfo> motionInfo; tree_.grid.getContent(motionInfo); double stepSize = siC_->getPropagationStepSize(); if (lastGoalMotion_) data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state)); for (auto &mi : motionInfo) for (auto &motion : mi.motions_) { if (motion->parent) { if (data.hasControls()) data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state), PlannerDataEdgeControl(motion->control, motion->steps * stepSize)); else data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state)); } else data.addStartVertex(base::PlannerDataVertex(motion->state)); } }