int main(int, char**)
{
// plan for hybrid car in SE(2) with discrete gears
ob::StateSpacePtr SE2(new ob::SE2StateSpace());
ob::StateSpacePtr velocity(new ob::RealVectorStateSpace(1));
// set the range for gears: [-1,3] inclusive
ob::StateSpacePtr gear(new ob::DiscreteStateSpace(-1,3));
ob::StateSpacePtr stateSpace = SE2 + velocity + gear;
// set the bounds for the R^2 part of SE(2)
ob::RealVectorBounds bounds(2);
bounds.setLow(-100);
bounds.setHigh(100);
SE2->as<ob::SE2StateSpace>()->setBounds(bounds);
// set the bounds for the velocity
ob::RealVectorBounds velocityBound(1);
velocityBound.setLow(0);
velocityBound.setHigh(60);
velocity->as<ob::RealVectorStateSpace>()->setBounds(velocityBound);
// create start and goal states
ob::ScopedState<> start(stateSpace);
ob::ScopedState<> goal(stateSpace);
// Both start and goal are states with high velocity with the car in third gear.
// However, to make the turn, the car cannot stay in third gear and will have to
// shift to first gear.
start[0] = start[1] = -90.; // position
start[2] = boost::math::constants::pi<double>()/2; // orientation
start[3] = 40.; // velocity
start->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2)->value = 3; // gear
goal[0] = goal[1] = 90.; // position
goal[2] = 0.; // orientation
goal[3] = 40.; // velocity
goal->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2)->value = 3; // gear
oc::ControlSpacePtr cmanifold(new oc::RealVectorControlSpace(stateSpace, 2));
// set the bounds for the control manifold
ob::RealVectorBounds cbounds(2);
// bounds for steering input
cbounds.setLow(0, -1.);
cbounds.setHigh(0, 1.);
// bounds for brake/gas input
cbounds.setLow(1, -20.);
cbounds.setHigh(1, 20.);
cmanifold->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
oc::SimpleSetup setup(cmanifold);
setup.setStartAndGoalStates(start, goal, 5.);
setup.setStateValidityChecker(boost::bind(
&isStateValid, setup.getSpaceInformation().get(), _1));
setup.setStatePropagator(boost::bind(
&propagate, setup.getSpaceInformation().get(), _1, _2, _3, _4));
setup.getSpaceInformation()->setPropagationStepSize(.1);
setup.getSpaceInformation()->setMinMaxControlDuration(2, 3);
// try to solve the problem
if (setup.solve(30))
{
// print the (approximate) solution path: print states along the path
// and controls required to get from one state to the next
oc::PathControl& path(setup.getSolutionPath());
// print out full state on solution path
// (format: x, y, theta, v, u0, u1, dt)
for(unsigned int i=0; i<path.getStateCount(); ++i)
{
const ob::State* state = path.getState(i);
const ob::SE2StateSpace::StateType *se2 =
state->as<ob::CompoundState>()->as<ob::SE2StateSpace::StateType>(0);
const ob::RealVectorStateSpace::StateType *velocity =
state->as<ob::CompoundState>()->as<ob::RealVectorStateSpace::StateType>(1);
const ob::DiscreteStateSpace::StateType *gear =
state->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2);
std::cout << se2->getX() << ' ' << se2->getY() << ' ' << se2->getYaw()
<< ' ' << velocity->values[0] << ' ' << gear->value << ' ';
if (i==0)
// null controls applied for zero seconds to get to start state
std::cout << "0 0 0";
else
{
// print controls and control duration needed to get from state i-1 to state i
const double* u =
path.getControl(i-1)->as<oc::RealVectorControlSpace::ControlType>()->values;
std::cout << u[0] << ' ' << u[1] << ' ' << path.getControlDuration(i-1);
}
std::cout << std::endl;
}
if (!setup.haveExactSolutionPath())
{
std::cout << "Solution is approximate. Distance to actual goal is " <<
setup.getProblemDefinition()->getSolutionDifference() << std::endl;
}
}
return 0;
}
void plan(void)
{
// construct the state space we are planning in
ob::StateSpacePtr space(new ob::SE2StateSpace());
// set the bounds for the R^2 part of SE(2)
ob::RealVectorBounds bounds(2);
bounds.setLow(0);
bounds.setHigh(2);
space->as<ob::SE2StateSpace>()->setBounds(bounds);
// create triangulation that ignores obstacle and respects propositions
MyDecomposition* ptd = new MyDecomposition(bounds);
// helper method that adds an obstacle, as well as three propositions p0,p1,p2
addObstaclesAndPropositions(ptd);
ptd->setup();
oc::PropositionalDecompositionPtr pd(ptd);
// create a control space
oc::ControlSpacePtr cspace(new oc::RealVectorControlSpace(space, 2));
// set the bounds for the control space
ob::RealVectorBounds cbounds(2);
cbounds.setLow(-.5);
cbounds.setHigh(.5);
cspace->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
oc::SpaceInformationPtr si(new oc::SpaceInformation(space, cspace));
si->setStateValidityChecker(std::bind(&isStateValid, si.get(), ptd, std::placeholders::_1));
si->setStatePropagator(std::bind(&propagate, std::placeholders::_1,
std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
si->setPropagationStepSize(0.025);
//LTL co-safety sequencing formula: visit p2,p0 in that order
std::vector<unsigned int> sequence(2);
sequence[0] = 2;
sequence[1] = 0;
oc::AutomatonPtr cosafety = oc::Automaton::SequenceAutomaton(3, sequence);
//LTL safety avoidance formula: never visit p1
std::vector<unsigned int> toAvoid(1);
toAvoid[0] = 1;
oc::AutomatonPtr safety = oc::Automaton::AvoidanceAutomaton(3, toAvoid);
//construct product graph (propDecomp x A_{cosafety} x A_{safety})
oc::ProductGraphPtr product(new oc::ProductGraph(pd, cosafety, safety));
// LTLSpaceInformation creates a hybrid space of robot state space x product graph.
// It takes the validity checker from SpaceInformation and expands it to one that also
// rejects any hybrid state containing rejecting automaton states.
// It takes the state propagator from SpaceInformation and expands it to one that
// follows continuous propagation with setting the next decomposition region
// and automaton states accordingly.
//
// The robot state space, given by SpaceInformation, is referred to as the "lower space".
oc::LTLSpaceInformationPtr ltlsi(new oc::LTLSpaceInformation(si, product));
// LTLProblemDefinition creates a goal in hybrid space, corresponding to any
// state in which both automata are accepting
oc::LTLProblemDefinitionPtr pdef(new oc::LTLProblemDefinition(ltlsi));
// create a start state
ob::ScopedState<ob::SE2StateSpace> start(space);
start->setX(0.2);
start->setY(0.2);
start->setYaw(0.0);
// addLowerStartState accepts a state in lower space, expands it to its
// corresponding hybrid state (decomposition region containing the state, and
// starting states in both automata), and adds that as an official start state.
pdef->addLowerStartState(start.get());
//LTL planner (input: LTL space information, product automaton)
oc::LTLPlanner* ltlPlanner = new oc::LTLPlanner(ltlsi, product);
ltlPlanner->setProblemDefinition(pdef);
// attempt to solve the problem within thirty seconds of planning time
// considering the above cosafety/safety automata, a solution path is any
// path that visits p2 followed by p0 while avoiding obstacles and avoiding p1.
ob::PlannerStatus solved = ltlPlanner->as<ob::Planner>()->solve(30.0);
if (solved)
{
std::cout << "Found solution:" << std::endl;
// The path returned by LTLProblemDefinition is through hybrid space.
// getLowerSolutionPath() projects it down into the original robot state space
// that we handed to LTLSpaceInformation.
pdef->getLowerSolutionPath()->print(std::cout);
}
else
std::cout << "No solution found" << std::endl;
delete ltlPlanner;
}
void planWithSimpleSetup()
{
// construct the state space we are planning in
auto space(std::make_shared<ob::SE2StateSpace>());
// set the bounds for the R^2 part of SE(2)
ob::RealVectorBounds bounds(2);
bounds.setLow(-1);
bounds.setHigh(1);
space->setBounds(bounds);
// create a control space
auto cspace(std::make_shared<oc::RealVectorControlSpace>(space, 2));
// set the bounds for the control space
ob::RealVectorBounds cbounds(2);
cbounds.setLow(-0.3);
cbounds.setHigh(0.3);
cspace->setBounds(cbounds);
// define a simple setup class
oc::SimpleSetup ss(cspace);
// set the state propagation routine
ss.setStatePropagator(propagate);
// set state validity checking for this space
oc::SpaceInformation *si = ss.getSpaceInformation().get();
ss.setStateValidityChecker(
[si](const ob::State *state) { return isStateValid(si, state); });
// create a start state
ob::ScopedState<ob::SE2StateSpace> start(space);
start->setX(-0.5);
start->setY(0.0);
start->setYaw(0.0);
ob::ScopedState<ob::SE2StateSpace> goal(start);
goal->setX(0.5);
// set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05);
auto td(std::make_shared<MyTriangularDecomposition>(bounds));
// print the triangulation to stdout
td->print(std::cout);
// hand the triangulation to SyclopEST
auto planner(std::make_shared<oc::SyclopEST>(ss.getSpaceInformation(), td));
// hand the SyclopEST planner to SimpleSetup
ss.setPlanner(planner);
// attempt to solve the problem within ten seconds of planning time
ob::PlannerStatus solved = ss.solve(10.0);
if (solved)
{
std::cout << "Found solution:" << std::endl;
// print the path to screen
ss.getSolutionPath().asGeometric().print(std::cout);
}
else
std::cout << "No solution found" << std::endl;
}
KPIECE(const Workspace &workspace, const Agent &agent, const InstanceFileMap &args) :
workspace(workspace), agent(agent), goalEdge(NULL), samplesGenerated(0), edgesAdded(0), edgesRejected(0) {
collisionCheckDT = args.doubleVal("Collision Check Delta t");
dfpair(stdout, "collision check dt", "%g", collisionCheckDT);
const typename Workspace::WorkspaceBounds &agentStateVarRanges = agent.getStateVarRanges(workspace.getBounds());
stateSpaceDim = agentStateVarRanges.size();
StateSpace *baseSpace = new StateSpace(stateSpaceDim);
ompl::base::RealVectorBounds bounds(stateSpaceDim);
for(unsigned int i = 0; i < stateSpaceDim; ++i) {
bounds.setLow(i, agentStateVarRanges[i].first);
bounds.setHigh(i, agentStateVarRanges[i].second);
}
baseSpace->setBounds(bounds);
ompl::base::StateSpacePtr space = ompl::base::StateSpacePtr(baseSpace);
const std::vector< std::pair<double, double> > &controlBounds = agent.getControlBounds();
controlSpaceDim = controlBounds.size();
ompl::control::RealVectorControlSpace *baseCSpace = new ompl::control::RealVectorControlSpace(space, controlSpaceDim);
ompl::base::RealVectorBounds cbounds(controlSpaceDim);
for(unsigned int i = 0; i < controlSpaceDim; ++i) {
cbounds.setLow(i, controlBounds[i].first);
cbounds.setHigh(i, controlBounds[i].second);
}
baseCSpace->setBounds(cbounds);
ompl::control::ControlSpacePtr cspace(baseCSpace);
// construct an instance of space information from this control space
spaceInfoPtr = ompl::control::SpaceInformationPtr(new ompl::control::SpaceInformation(space, cspace));
// set state validity checking for this space
spaceInfoPtr->setStateValidityChecker(boost::bind(&KPIECE::isStateValid, this, spaceInfoPtr.get(), _1));
// set the state propagation routine
spaceInfoPtr->setStatePropagator(boost::bind(&KPIECE::propagate, this, _1, _2, _3, _4));
pdef = ompl::base::ProblemDefinitionPtr(new ompl::base::ProblemDefinition(spaceInfoPtr));
spaceInfoPtr->setPropagationStepSize(args.doubleVal("Steering Delta t"));
dfpair(stdout, "steering dt", "%g", args.doubleVal("Steering Delta t"));
spaceInfoPtr->setMinMaxControlDuration(stol(args.value("KPIECE Min Control Steps")),stol(args.value("KPIECE Max Control Steps")));
dfpair(stdout, "min control duration", "%u", stol(args.value("KPIECE Min Control Steps")));
dfpair(stdout, "max control duration", "%u", stol(args.value("KPIECE Max Control Steps")));
spaceInfoPtr->setup();
kpiece = new ompl::control::KPIECE1(spaceInfoPtr);
kpiece->setGoalBias(args.doubleVal("KPIECE Goal Bias"));
dfpair(stdout, "goal bias", "%g", args.doubleVal("KPIECE Goal Bias"));
kpiece->setBorderFraction(args.doubleVal("KPIECE Border Fraction"));
dfpair(stdout, "border fraction", "%g", args.doubleVal("KPIECE Border Fraction"));
kpiece->setCellScoreFactor(args.doubleVal("KPIECE Cell Score Good"), args.doubleVal("KPIECE Cell Score Bad"));
dfpair(stdout, "cell score good", "%g", args.doubleVal("KPIECE Cell Score Good"));
dfpair(stdout, "cell score bad", "%g", args.doubleVal("KPIECE Cell Score Bad"));
kpiece->setMaxCloseSamplesCount(stol(args.value("KPIECE Max Close Samples")));
dfpair(stdout, "max closed samples", "%u", stol(args.value("KPIECE Max Close Samples")));
ompl::base::RealVectorRandomLinearProjectionEvaluator *projectionEvaluator = new ompl::base::RealVectorRandomLinearProjectionEvaluator(baseSpace, stateSpaceDim);
ompl::base::ProjectionEvaluatorPtr projectionPtr = ompl::base::ProjectionEvaluatorPtr(projectionEvaluator);
kpiece->setProjectionEvaluator(projectionPtr);
}
void lasclip(std::string &outfile, std::string &shapefile,
std::string &layername, std::vector<std::string> &files,
std::set<int> &classes, bool quiet) {
if (outfile.empty())
g_argerr("An output file is required.");
if (shapefile.empty())
g_argerr("A shape file is required.");
if (files.size() == 0)
g_argerr("At least one input file is required.");
if (classes.size() == 0)
g_warn("No classes specified, matching all classes.");
/* Attempt to open and load geometries from the shape file. */
OGRRegisterAll();
OGRLayer *layer;
OGRFeature *feat;
OGRGeometry *og;
OGRwkbGeometryType type;
gg::GeometryCollection *geomColl;
gg::Geometry *geom;
OGRDataSource *ds = OGRSFDriverRegistrar::Open(shapefile.c_str(), FALSE);
if (ds == nullptr)
g_runerr("Couldn't open shapefile.");
if (layername.empty()) {
layer = ds->GetLayer(0);
} else {
layer = ds->GetLayerByName(layername.c_str());
}
if (layer == nullptr)
g_runerr("Couldn't get layer.");
type = layer->GetGeomType();
if (type != wkbPolygon && type != wkbMultiPolygon)
g_runerr("Geometry must be polygon or multipolygon.");
const GEOSContextHandle_t gctx = OGRGeometry::createGEOSContext();
const gg::GeometryFactory *gf = gg::GeometryFactory::getDefaultInstance();
const gg::CoordinateSequenceFactory *cf =
gf->getCoordinateSequenceFactory();
std::vector<gg::Geometry *> geoms;
while ((feat = layer->GetNextFeature()) != NULL) {
og = feat->GetGeometryRef();
geom = (gg::Geometry *) og->exportToGEOS(gctx);
geoms.push_back(geom);
}
GDALClose(ds);
if (geoms.size() == 0)
g_runerr("No geometries were found.");
/* The geometry collection is used for checking whether a las file intersects
the region of interest. */
geomColl = gf->createGeometryCollection(geoms);
const gg::Envelope *env = geomColl->getEnvelopeInternal();
Bounds cbounds(env->getMinX(), env->getMinY(), env->getMaxX(),
env->getMaxY());
/* Loop over files and figure out which ones are relevant. */
liblas::ReaderFactory rf;
liblas::Header *dsth = nullptr;
std::vector<unsigned int> indices;
for (unsigned int i = 0; i < files.size(); ++i) {
std::ifstream in(files[i].c_str(), std::ios::in | std::ios::binary);
liblas::Reader r = rf.CreateWithStream(in);
liblas::Header h = r.GetHeader();
if (i == 0)
dsth = new liblas::Header(h);
std::vector<gg::Coordinate> coords;
coords.push_back(gg::Coordinate(h.GetMinX(), h.GetMinY()));
coords.push_back(gg::Coordinate(h.GetMaxX(), h.GetMinY()));
coords.push_back(gg::Coordinate(h.GetMaxX(), h.GetMaxY()));
coords.push_back(gg::Coordinate(h.GetMinX(), h.GetMaxY()));
coords.push_back(gg::Coordinate(h.GetMinX(), h.GetMinY()));
gg::CoordinateSequence *cs = cf->create(&coords);
gg::LinearRing *lr = gf->createLinearRing(cs);
gg::Polygon *bounds = gf->createPolygon(lr, NULL);
if (bounds->intersects(geomColl))
indices.push_back(i);
in.close();
}
if (indices.size() == 0)
g_runerr("No files matched the given bounds.");
std::ofstream out(outfile, std::ios::out | std::ios::binary);
liblas::WriterFactory wf;
liblas::Writer w(out, *dsth);
liblas::Header::RecordsByReturnArray recs;
int count = 0;
double bounds[] = { G_DBL_MAX_POS, G_DBL_MAX_NEG, G_DBL_MAX_POS,
G_DBL_MAX_NEG, G_DBL_MAX_POS, G_DBL_MAX_NEG };
g_trace("Using points from " << indices.size() << " files.");
for (int i = 0; i < 5; ++i)
recs.push_back(0);
for (unsigned int i = 0; i < indices.size(); ++i) {
std::ifstream in(files[indices[i]].c_str(),
std::ios::in | std::ios::binary);
liblas::Reader r = rf.CreateWithStream(in);
liblas::Header h = r.GetHeader();
g_trace("Processing file " << files[indices[i]]);
while (r.ReadNextPoint()) {
liblas::Point pt = r.GetPoint();
int cls = pt.GetClassification().GetClass();
if (classes.size() > 0 && !Util::inList(classes, cls))
continue;
double x = pt.GetX();
double y = pt.GetY();
const gg::Coordinate c(x, y);
gg::Point *p = gf->createPoint(c);
if (cbounds.contains(x, y) && geomColl->contains(p)) {
++recs[cls];
++count;
w.WritePoint(pt);
if (pt.GetX() < bounds[0])
bounds[0] = pt.GetX();
if (pt.GetX() > bounds[1])
bounds[1] = pt.GetX();
if (pt.GetY() < bounds[2])
bounds[2] = pt.GetY();
if (pt.GetY() > bounds[3])
bounds[3] = pt.GetY();
if (pt.GetZ() < bounds[4])
bounds[4] = pt.GetZ();
if (pt.GetZ() > bounds[5])
bounds[5] = pt.GetZ();
}
}
in.close();
}
// Set the total count and update the point record counts.
dsth->SetMin(bounds[0], bounds[2], bounds[4]);
dsth->SetMax(bounds[1], bounds[3], bounds[5]);
dsth->SetPointRecordsCount(count);
for (unsigned int i = 0; i < recs.size(); ++i)
dsth->SetPointRecordsByReturnCount(i, recs[i]);
w.WriteHeader();
}
bool OmplEnvXYTHETA::initialize(envire::TraversabilityGrid* trav_grid,
boost::shared_ptr<TravData> grid_data) {
// Will define a control problem in SE2 (X, Y, THETA).
LOG_INFO("Create OMPL SE2 environment");
mpStateSpace = ompl::base::StateSpacePtr(new ob::SE2StateSpace());
ob::RealVectorBounds bounds(2);
bounds.setLow (0, 0);
bounds.setHigh(0, trav_grid->getCellSizeX());
bounds.setLow (1, 0);
bounds.setHigh(1, trav_grid->getCellSizeY());
mpStateSpace->as<ob::SE2StateSpace>()->setBounds(bounds);
mpStateSpace->setLongestValidSegmentFraction(1/(double)trav_grid->getCellSizeX());
mpControlSpace = ompl::control::ControlSpacePtr(
new ompl::control::RealVectorControlSpace(mpStateSpace, 2));
ompl::base::RealVectorBounds cbounds(2);
// Requires a control-planner.
double turning_speed = mConfig.mMobility.mTurningSpeed;
if(turning_speed == 0) {
LOG_WARN("Rotational velocity of zero is not allowed, abort");
return false;
}
cbounds.setLow(0, -mConfig.mMobility.mSpeed);
cbounds.setHigh(0, mConfig.mMobility.mSpeed);
cbounds.setLow(1, -turning_speed);
cbounds.setHigh(1, turning_speed);
mpControlSpace->as<ompl::control::RealVectorControlSpace>()->setBounds(cbounds);
// Control space information inherits from base space informartion.
mpControlSpaceInformation = ompl::control::SpaceInformationPtr(
new ompl::control::SpaceInformation(mpStateSpace,mpControlSpace));
mpODESolver = ompl::control::ODESolverPtr(
new ompl::control::ODEAdaptiveSolver<> (mpControlSpaceInformation, &simpleOde));
// setStatePropagator can also be used to define a post-propagator.
mpControlSpaceInformation->setStatePropagator(
ompl::control::ODESolver::getStatePropagator(mpODESolver, &postPropagate));
/// \todo "What does these methods do?"
mpControlSpaceInformation->setPropagationStepSize(4);
mpControlSpaceInformation->setMinMaxControlDuration(1,10);
mpTravMapValidator = ob::StateValidityCheckerPtr(new TravMapValidator(
mpControlSpaceInformation, trav_grid, grid_data, mConfig));
mpControlSpaceInformation->setStateValidityChecker(mpTravMapValidator);
mpControlSpaceInformation->setup();
// Create problem definition.
mpProblemDefinition = ob::ProblemDefinitionPtr(new ob::ProblemDefinition(mpControlSpaceInformation));
// Create optimizations and balance them in getBalancedObjective().
mpPathLengthOptimization = ob::OptimizationObjectivePtr(
new ob::PathLengthOptimizationObjective(mpControlSpaceInformation));
mpTravGridObjective = ob::OptimizationObjectivePtr(new TravGridObjective(mpControlSpaceInformation, false,
trav_grid, grid_data, mConfig));
mpProblemDefinition->setOptimizationObjective(getBalancedObjective(mpControlSpaceInformation));
// Control based planner, optimization is not supported by OMPL.
mpPlanner = ob::PlannerPtr(new ompl::control::RRT(mpControlSpaceInformation));
// Set the problem instance for our planner to solve
mpPlanner->setProblemDefinition(mpProblemDefinition);
mpPlanner->setup(); // Calls mpSpaceInformation->setup() as well.
return true;
}
void planWithSimpleSetup(double start_x, double start_y, double start_theta,
double goal_x, double goal_y, double goal_theta)
{
/// construct the state space we are planning in
ob::StateSpacePtr space(new ob::SE2StateSpace());
/// set the bounds for the R^2 part of SE(2)
ob::RealVectorBounds bounds(2);
bounds.setLow(-1);
bounds.setHigh(1);
space->as<ob::SE2StateSpace>()->setBounds(bounds);
// create a control space
oc::ControlSpacePtr cspace(new DemoControlSpace(space));
// set the bounds for the control space
ob::RealVectorBounds cbounds(2);
cbounds.setLow(-0.3);
cbounds.setHigh(0.3);
cspace->as<DemoControlSpace>()->setBounds(cbounds);
// define a simple setup class
oc::SimpleSetup ss(cspace);
// set state validity checking for this space
ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
// Setting the propagation routine for this space:
// KinematicCarModel does NOT use ODESolver
//ss.setStatePropagator(oc::StatePropagatorPtr(new KinematicCarModel(ss.getSpaceInformation())));
// Use the ODESolver to propagate the system. Call KinematicCarPostIntegration
// when integration has finished to normalize the orientation values.
// oc::ODEBasicSolver<> odeSolver(ss.getSpaceInformation(), &KinematicCarODE);
// ss.setStatePropagator(odeSolver.getStatePropagator(&KinematicCarPostIntegration));
oc::ODESolverPtr odeSolver(new oc::ODEBasicSolver<> (ss.getSpaceInformation(), &KinematicCarODE));
ss.setStatePropagator(oc::ODESolver::getStatePropagator(odeSolver/*, &KinematicCarPostIntegration*/));
/// create a start state
ob::ScopedState<ob::SE2StateSpace> start(space);
start->setX(start_x);
start->setY(start_y);
start->setYaw(start_theta);
/// create a goal state; use the hard way to set the elements
ob::ScopedState<ob::SE2StateSpace> goal(space);
goal->setX(goal_x);
goal->setY(goal_y);
goal->setYaw(goal_theta);
/// set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05);
/// we want to have a reasonable value for the propagation step size
ss.setup();
/// attempt to solve the problem within one second of planning time
ob::PlannerStatus solved = ss.solve(10.0);
if (solved)
{
std::cout << "Found solution:" << std::endl;
/// print the path to screen
ss.getSolutionPath().asGeometric().print(std::cout);
}
else
std::cout << "No solution found" << std::endl;
}
