trajectory::InterpolatedPtr planOMPL(
const statespace::StateSpace::State* _start,
constraint::TestablePtr _goalTestable,
constraint::SampleablePtr _goalSampler,
statespace::ConstStateSpacePtr _stateSpace,
statespace::InterpolatorPtr _interpolator,
distance::DistanceMetricPtr _dmetric,
constraint::SampleablePtr _sampler,
constraint::TestablePtr _validityConstraint,
constraint::TestablePtr _boundsConstraint,
constraint::ProjectablePtr _boundsProjector,
double _maxPlanTime,
double _maxDistanceBtwValidityChecks)
{
if (_goalTestable == nullptr)
{
throw std::invalid_argument("Testable goal is nullptr.");
}
if (_goalSampler == nullptr)
{
throw std::invalid_argument("Sampleable goal is nullptr.");
}
if (_goalTestable->getStateSpace() != _stateSpace)
{
throw std::invalid_argument("Testable goal does not match StateSpace");
}
if (_goalSampler->getStateSpace() != _stateSpace)
{
throw std::invalid_argument("Sampleable goal does not match StateSpace");
}
auto si = getSpaceInformation(
_stateSpace,
_interpolator,
std::move(_dmetric),
std::move(_sampler),
std::move(_validityConstraint),
std::move(_boundsConstraint),
std::move(_boundsProjector),
_maxDistanceBtwValidityChecks);
// Set the start and goal
auto pdef = ompl_make_shared<::ompl::base::ProblemDefinition>(si);
auto sspace
= ompl_static_pointer_cast<GeometricStateSpace>(si->getStateSpace());
auto start = sspace->allocState(_start);
pdef->addStartState(start); // copies
sspace->freeState(start);
auto goalRegion = ompl_make_shared<GoalRegion>(
si, std::move(_goalTestable), _goalSampler->createSampleGenerator());
pdef->setGoal(goalRegion);
auto planner = ompl_make_shared<PlannerType>(si);
return planOMPL(
planner,
pdef,
std::move(_stateSpace),
std::move(_interpolator),
_maxPlanTime);
}
//Returns Start State for the NFA of the input REGEX.
//This builds an individual NFA for the input REGEX and stores the start and final state.
//'$' represents EPSILON transition.
void buildIndividualNFA(char pattern[], char POSTFIX[])
{
int i = 0;
char ch;
Stack starts, ends;
init(&starts);
init(&ends);
while(ch = POSTFIX[i++])
{
if(ch == '|')//Union
{
STATE start1 = pop(&starts), start2 = pop(&starts);
STATE end1 = pop(&ends), end2 = pop(&ends);
STATE newStart = nextAvailableState++, newEnd = nextAvailableState++;
addTransition(newStart, start1, '$');
addTransition(newStart, start2, '$');
addTransition(end1, newEnd, '$');
addTransition(end2, newEnd, '$');
push(&starts, newStart);push(&ends, newEnd);
}
else if(ch == '#')//Kleene Closure
{
STATE start = pop(&starts), end = pop(&ends);
STATE newStart = nextAvailableState++, newEnd = nextAvailableState++;
addTransition(end, start, '$');
addTransition(newStart, newEnd, '$');
addTransition(newStart, start, '$');
addTransition(end, newEnd, '$');
push(&starts, newStart);push(&ends, newEnd);
}
else if(ch == '&')
{
STATE start1 = pop(&starts), start2 = pop(&starts);
STATE end1 = pop(&ends), end2 = pop(&ends);
addTransition(end2, start1, '$');
push(&starts, start2);push(&ends, end1);
}
else if(ch == '@')
{
char a = POSTFIX[i++], b = POSTFIX[i++];
int j;
STATE start = nextAvailableState++, end = nextAvailableState++;
for(j=a;j<=b;++j)
addTransition(start, end, j);
push(&starts, start);push(&ends, end);
}
else
{
STATE start = nextAvailableState++, end = nextAvailableState++;
addTransition(start, end, ch);
push(&starts, start);push(&ends, end);
}
}
addStartState(pop(&starts));
addFinalState(pop(&ends), pattern);
}
void ompl::base::ProblemDefinition::setStartAndGoalStates(const State *start, const State *goal, const double threshold)
{
clearStartStates();
addStartState(start);
setGoalState(goal, threshold);
}
