1#include <ompl/control/SpaceInformation.h>
2#include <ompl/base/goals/GoalState.h>
3#include <ompl/base/spaces/SE2StateSpace.h>
4#include <ompl/control/spaces/RealVectorControlSpace.h>
5#include <ompl/control/planners/kpiece/KPIECE1.h>
6#include <ompl/control/planners/rrt/RRT.h>
7#include <ompl/control/planners/est/EST.h>
8#include <ompl/control/planners/syclop/SyclopRRT.h>
9#include <ompl/control/planners/syclop/SyclopEST.h>
10#include <ompl/control/SimpleSetup.h>
11#include <ompl/config.h>
12#include <ompl/extensions/triangle/TriangularDecomposition.h>
27 void project(
const ob::State* s, std::vector<double>& coord)
const override
30 coord[0] = s->as<ob::SE2StateSpace::StateType>()->getX();
31 coord[1] = s->as<ob::SE2StateSpace::StateType>()->getY();
34 void sampleFullState(
const ob::StateSamplerPtr& sampler,
const std::vector<double>& coord,
ob::State* s)
const override
36 sampler->sampleUniform(s);
37 s->as<ob::SE2StateSpace::StateType>()->setXY(coord[0], coord[1]);
40 std::vector<Polygon> createObstacles()
42 std::vector<Polygon> obst;
67bool triContains(
double x,
double y,
double ax,
double ay,
double bx,
double by,
double cx,
double cy)
69 if ((x-ax)*(by-ay) - (bx-ax)*(y-ay) > 0.)
71 if ((x-bx)*(cy-by) - (cx-bx)*(y-by) > 0.)
73 if ((x-cx)*(ay-cy) - (ax-cx)*(y-cy) > 0.)
83 const auto *se2state = state->
as<ob::SE2StateSpace::StateType>();
86 double x = se2state->getX();
87 double y = se2state->getY();
88 return si->satisfiesBounds(state) && !triContains(x,y, -0.5,0.75,-0.75,0.68,-0.5,0.5)
89 && !triContains(x,y, 0,0.5,-0.3,0,0,-0.5)
90 && !triContains(x,y,0,-0.5,0.6,0.6,0,0.5);
95 const auto *se2state = start->
as<ob::SE2StateSpace::StateType>();
96 const auto *pos = se2state->as<ob::RealVectorStateSpace::StateType>(0);
97 const auto *rot = se2state->as<ob::SO2StateSpace::StateType>(1);
98 const auto *rctrl = control->as<oc::RealVectorControlSpace::ControlType>();
100 result->
as<ob::SE2StateSpace::StateType>()->as<ob::RealVectorStateSpace::StateType>(0)->values[0] =
101 (*pos)[0] + (*rctrl)[0] *
duration * cos(rot->value);
103 result->
as<ob::SE2StateSpace::StateType>()->as<ob::RealVectorStateSpace::StateType>(0)->values[1] =
104 (*pos)[1] + (*rctrl)[0] *
duration * sin(rot->value);
106 result->
as<ob::SE2StateSpace::StateType>()->as<ob::SO2StateSpace::StateType>(1)->value =
107 rot->value + (*rctrl)[1];
111void planWithSimpleSetup()
114 auto space(std::make_shared<ob::SE2StateSpace>());
121 space->setBounds(bounds);
124 auto cspace(std::make_shared<oc::RealVectorControlSpace>(space, 2));
128 cbounds.setLow(-0.3);
129 cbounds.setHigh(0.3);
131 cspace->setBounds(cbounds);
137 ss.setStatePropagator(propagate);
141 ss.setStateValidityChecker(
142 [si](
const ob::State *state) {
return isStateValid(si, state); });
154 ss.setStartAndGoalStates(start, goal, 0.05);
156 auto td(std::make_shared<MyTriangularDecomposition>(bounds));
158 td->print(std::cout);
161 auto planner(std::make_shared<oc::SyclopEST>(ss.getSpaceInformation(), td));
163 ss.setPlanner(planner);
170 std::cout <<
"Found solution:" << std::endl;
173 ss.getSolutionPath().asGeometric().print(std::cout);
176 std::cout <<
"No solution found" << std::endl;
179int main(
int ,
char ** )
181 std::cout <<
"OMPL version: " << OMPL_VERSION << std::endl;
182 planWithSimpleSetup();
183 std::cout << std::endl;
The lower and upper bounds for an Rn space.
Definition of a scoped state.
Definition of an abstract state.
const T * as() const
Cast this instance to a desired type.
Definition of an abstract control.
Create the set of classes typically needed to solve a control problem.
A TriangularDecomposition is a triangulation that ignores obstacles.
TriangularDecomposition(const base::RealVectorBounds &bounds, std::vector< Polygon > holes=std::vector< Polygon >(), std::vector< Polygon > intRegs=std::vector< Polygon >())
Creates a TriangularDecomposition over the given bounds, which must be 2-dimensional....
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains sampling based planning routines used by planning under differential constrai...
std::chrono::system_clock::duration duration
Representation of a time duration.
A class to store the exit status of Planner::solve()