void ompl::tools::LightningDB::addPathHelper(ompl::geometric::PathGeometric &solutionPath)
{
// Create a new planner data instance
ompl::base::PlannerDataPtr plannerData(new ompl::base::PlannerData(si_));
// Add the states to one nodes files
for (std::size_t i = 0; i < solutionPath.getStates().size(); ++i)
{
ompl::base::PlannerDataVertex vert( solutionPath.getStates()[i] ); // TODO tag?
plannerData->addVertex(vert);
}
// Deep copy the states in the vertices so that when the planner goes out of scope, all data remains intact
plannerData->decoupleFromPlanner();
// Add to nearest neighbor tree
nn_->add(plannerData);
numUnsavedPaths_++;
}
EReturn OMPLsolver::getSimplifiedPath(ompl::geometric::PathGeometric &pg,
Eigen::MatrixXd & traj, ob::PlannerTerminationCondition &ptc)
{
if (smooth_->data)
{
HIGHLIGHT("Simplifying solution");
int original_cnt = pg.getStateCount();
ros::Time start = ros::Time::now();
//ompl_simple_setup_->simplifySolution(d);
// Lets do our own simplifier ~:)
if (original_cnt >= 3)
{
og::PathSimplifierPtr psf_ = ompl_simple_setup_->getPathSimplifier();
const ob::SpaceInformationPtr &si =
ompl_simple_setup_->getSpaceInformation();
bool tryMore = false;
if (ptc == false) tryMore = psf_->reduceVertices(pg);
if (ptc == false) psf_->collapseCloseVertices(pg);
int times = 0;
while (tryMore && ptc == false)
{
tryMore = psf_->reduceVertices(pg);
times++;
}
if (si->getStateSpace()->isMetricSpace())
{
if (ptc == false)
tryMore = psf_->shortcutPath(pg);
else
tryMore = false;
times = 0;
while (tryMore && ptc == false)
{
tryMore = psf_->shortcutPath(pg);
times++;
}
}
std::vector<ob::State*> &states = pg.getStates();
// Calculate number of states required
unsigned int length = 0;
const int n1 = states.size() - 1;
for (int i = 0; i < n1; ++i)
length += si->getStateSpace()->validSegmentCount(states[i],
states[i + 1]);
// // Forward reducing
// HIGHLIGHT("States before forward reducing: "<<pg.getStateCount());
// pg.interpolate(length);
//
// bool need_backward = true;
// for (int i = states.size() - 1; i > 0; i--)
// {
// if (si->checkMotion(states[0], states[i]))
// {
// ob::State *start = si->cloneState(states[0]);
// pg.keepAfter(states[i]);
// pg.prepend(start);
// need_backward = (i == states.size() - 1) ? false : true;
// break;
// }
// }
//
// // Backward reducing
// ob::State *mid;
// if (need_backward)
// {
// HIGHLIGHT("States before backward reducing: "<<pg.getStateCount());
// pg.interpolate(length);
// for (int i = 1; i < states.size(); i++)
// {
// if (si->checkMotion(states[states.size() - 1], states[i]))
// {
// ob::State *goal = si->cloneState(states[states.size() - 1]);
// pg.keepBefore(states[i]);
// pg.append(goal);
// mid = si->cloneState(states[i]);
// break;
// }
// }
// }
pg.interpolate(length);
}
// if (ompl_simple_setup_->haveSolutionPath())
// {
// pg.interpolate();
// }
HIGHLIGHT_NAMED("OMPLSolver",
"Simplification took "<<ros::Duration(ros::Time::now()-start).toSec()<<"sec. States: "<<original_cnt<<"->"<<pg.getStateCount());
}
convertPath(pg, traj);
return SUCCESS;
}
bool ompl::geometric::LightningRetrieveRepair::repairPath(const base::PlannerTerminationCondition &ptc,
ompl::geometric::PathGeometric &primaryPath)
{
// \todo: we should reuse our collision checking from the previous step to make this faster
OMPL_INFORM("LightningRetrieveRepair: Repairing path");
// Error check
if (primaryPath.getStateCount() < 2)
{
OMPL_ERROR("LightningRetrieveRepair: Cannot repair a path with less than 2 states");
return false;
}
// Loop through every pair of states and make sure path is valid.
// If not, replan between those states
for (std::size_t toID = 1; toID < primaryPath.getStateCount(); ++toID)
{
std::size_t fromID = toID - 1; // this is our last known valid state
ompl::base::State *fromState = primaryPath.getState(fromID);
ompl::base::State *toState = primaryPath.getState(toID);
// Check if our planner is out of time
if (ptc == true)
{
OMPL_DEBUG("LightningRetrieveRepair: Repair path function interrupted because termination condition is true.");
return false;
}
// Check path between states
if (!si_->checkMotion(fromState, toState))
{
// Path between (from, to) states not valid, but perhaps to STATE is
// Search until next valid STATE is found in existing path
std::size_t subsearchID = toID;
ompl::base::State *new_to;
OMPL_DEBUG("LightningRetrieveRepair: Searching for next valid state, because state %d to %d was not valid out %d total states",
fromID,toID,primaryPath.getStateCount());
while (subsearchID < primaryPath.getStateCount())
{
new_to = primaryPath.getState(subsearchID);
if (si_->isValid(new_to))
{
OMPL_DEBUG("LightningRetrieveRepair: State %d was found to valid, we can now repair between states", subsearchID);
// This future state is valid, we can stop searching
toID = subsearchID;
toState = new_to;
break;
}
++subsearchID; // keep searching for a new state to plan to
}
// Check if we ever found a next state that is valid
if (subsearchID >= primaryPath.getStateCount())
{
// We never found a valid state to plan to, instead we reached the goal state and it too wasn't valid. This is bad.
// I think this is a bug.
OMPL_ERROR("LightningRetrieveRepair: No state was found valid in the remainder of the path. Invalid goal state. This should not happen.");
return false;
}
// Plan between our two valid states
PathGeometric newPathSegment(si_);
// Not valid motion, replan
OMPL_DEBUG("LightningRetrieveRepair: Planning from %d to %d", fromID, toID);
if (!replan(fromState, toState, newPathSegment, ptc))
{
OMPL_INFORM("LightningRetrieveRepair: Unable to repair path between state %d and %d", fromID, toID);
return false;
}
// TODO make sure not approximate solution
// Reference to the path
std::vector<base::State*> &primaryPathStates = primaryPath.getStates();
// Remove all invalid states between (fromID, toID) - not including those states themselves
while (fromID != toID - 1)
{
OMPL_INFORM("LightningRetrieveRepair: Deleting state %d", fromID + 1);
primaryPathStates.erase(primaryPathStates.begin() + fromID + 1);
--toID; // because vector has shrunk
OMPL_INFORM("LightningRetrieveRepair: toID is now %d", toID);
}
// Insert new path segment into current path
OMPL_DEBUG("LightningRetrieveRepair: Inserting new %d states into old path. Previous length: %d",
newPathSegment.getStateCount()-2, primaryPathStates.size());
// Note: skip first and last states because they should be same as our start and goal state, same as `fromID` and `toID`
for (std::size_t i = 1; i < newPathSegment.getStateCount() - 1; ++i)
{
std::size_t insertLocation = toID + i - 1;
OMPL_DEBUG("LightningRetrieveRepair: Inserting newPathSegment state %d into old path at position %d",
i, insertLocation);
primaryPathStates.insert(primaryPathStates.begin() + insertLocation,
si_->cloneState(newPathSegment.getStates()[i]) );
}
OMPL_DEBUG("LightningRetrieveRepair: Inserted new states into old path. New length: %d", primaryPathStates.size());
// Set the toID to jump over the newly inserted states to the next unchecked state. Subtract 2 because we ignore start and goal
toID = toID + newPathSegment.getStateCount() - 2;
OMPL_DEBUG("LightningRetrieveRepair: Continuing searching at state %d", toID);
}
}
OMPL_INFORM("LightningRetrieveRepair: Done repairing");
return true;
}