void ompl::control::KPIECE1::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
Grid::CellArray cells;
tree_.grid.getCells(cells);
if (PlannerData *cpd = dynamic_cast<control::PlannerData*>(&data))
{
double delta = siC_->getPropagationStepSize();
for (unsigned int i = 0 ; i < cells.size() ; ++i)
for (unsigned int j = 0 ; j < cells[i]->data->motions.size() ; ++j)
{
const Motion* m = cells[i]->data->motions[j];
if (m->parent)
cpd->recordEdge(m->parent->state, m->state, m->control, m->steps * delta);
else
cpd->recordEdge(NULL, m->state, NULL, 0.);
cpd->tagState(m->state, cells[i]->border ? 2 : 1);
}
}
else
{
for (unsigned int i = 0 ; i < cells.size() ; ++i)
for (unsigned int j = 0 ; j < cells[i]->data->motions.size() ; ++j)
{
const Motion* m = cells[i]->data->motions[j];
data.recordEdge(m->parent ? m->parent->state : NULL, m->state);
data.tagState(m->state, cells[i]->border ? 2 : 1);
}
}
}
void ompl::control::KPIECE1::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
Grid::CellArray cells;
tree_.grid.getCells(cells);
double delta = siC_->getPropagationStepSize();
if (lastGoalMotion_)
data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
for (unsigned int i = 0 ; i < cells.size() ; ++i)
{
for (unsigned int j = 0 ; j < cells[i]->data->motions.size() ; ++j)
{
const Motion* m = cells[i]->data->motions[j];
if (m->parent)
{
if (data.hasControls())
data.addEdge(base::PlannerDataVertex (m->parent->state),
base::PlannerDataVertex (m->state, cells[i]->border ? 2 : 1),
control::PlannerDataEdgeControl (m->control, m->steps * delta));
else
data.addEdge(base::PlannerDataVertex (m->parent->state),
base::PlannerDataVertex (m->state, cells[i]->border ? 2 : 1));
}
else
data.addStartVertex(base::PlannerDataVertex (m->state, cells[i]->border ? 2 : 1));
// A state created as a parent first may have an improper tag variable
data.tagState(m->state, cells[i]->border ? 2 : 1);
}
}
}
void ompl::geometric::LazyPRM::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
// Explicitly add start and goal states. Tag all states known to be valid as 1.
// Unchecked states are tagged as 0.
for (size_t i = 0; i < startM_.size(); ++i)
data.addStartVertex(base::PlannerDataVertex(stateProperty_[startM_[i]], 1));
for (size_t i = 0; i < goalM_.size(); ++i)
data.addGoalVertex(base::PlannerDataVertex(stateProperty_[goalM_[i]], 1));
// Adding edges and all other vertices simultaneously
foreach(const Edge e, boost::edges(g_))
{
const Vertex v1 = boost::source(e, g_);
const Vertex v2 = boost::target(e, g_);
data.addEdge(base::PlannerDataVertex(stateProperty_[v1]),
base::PlannerDataVertex(stateProperty_[v2]));
// Add the reverse edge, since we're constructing an undirected roadmap
data.addEdge(base::PlannerDataVertex(stateProperty_[v2]),
base::PlannerDataVertex(stateProperty_[v1]));
// Add tags for the newly added vertices
data.tagState(stateProperty_[v1], (vertexValidityProperty_[v1] & VALIDITY_TRUE) == 0 ? 0 : 1);
data.tagState(stateProperty_[v2], (vertexValidityProperty_[v2] & VALIDITY_TRUE) == 0 ? 0 : 1);
}
}
void ompl::geometric::LazyRRT::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<Motion*> motions;
if (nn_)
nn_->list(motions);
for (unsigned int i = 0 ; i < motions.size() ; ++i)
{
data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
if (motions[i]->valid)
data.tagState(motions[i]->state, 1);
}
}
void ompl::geometric::LazyRRT::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<Motion*> motions;
if (nn_)
nn_->list(motions);
if (lastGoalMotion_)
data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state, 1));
for (unsigned int i = 0 ; i < motions.size() ; ++i)
{
if (motions[i]->parent == NULL)
data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
else
data.addEdge(base::PlannerDataVertex(motions[i]->parent ? motions[i]->parent->state : NULL),
base::PlannerDataVertex(motions[i]->state));
data.tagState(motions[i]->state, motions[i]->valid ? 1 : 0);
}
}
void ompl::geometric::LazyRRT::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<Motion *> motions;
if (nn_)
nn_->list(motions);
if (lastGoalMotion_)
data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state, 1));
for (auto &motion : motions)
{
if (motion->parent == nullptr)
data.addStartVertex(base::PlannerDataVertex(motion->state));
else
data.addEdge(base::PlannerDataVertex(motion->parent ? motion->parent->state : nullptr),
base::PlannerDataVertex(motion->state));
data.tagState(motion->state, motion->valid ? 1 : 0);
}
}
