37#include <ompl/base/SpaceInformation.h>
38#include <ompl/base/spaces/SpaceTimeStateSpace.h>
39#include <ompl/geometric/planners/rrt/RRTConnect.h>
40#include <ompl/geometric/planners/rrt/STRRTstar.h>
41#include <ompl/geometric/SimpleSetup.h>
43#include <ompl/config.h>
56 const auto pos = state->
as<
ob::CompoundState>()->as<ob::RealVectorStateSpace::StateType>(0)->values[0];
59 const auto t = state->
as<
ob::CompoundState>()->as<ob::TimeStateSpace::StateType>(1)->position;
64 return t >= 0 && pos < std::numeric_limits<double>::infinity();
70 explicit SpaceTimeMotionValidator(
const ob::SpaceInformationPtr &si) :
MotionValidator(si),
71 vMax_(
si_->getStateSpace().get()->as<ob::SpaceTimeStateSpace>()->getVMax()),
72 stateSpace_(
si_->getStateSpace().get()) {};
74 bool checkMotion(
const ob::State *s1,
const ob::State *s2)
const override
77 if (!
si_->isValid(s2)) {
83 auto *space = stateSpace_->as<ob::SpaceTimeStateSpace>();
84 auto deltaPos = space->distanceSpace(s1, s2);
85 auto deltaT = s2->
as<ob::CompoundState>()->as<ob::TimeStateSpace::StateType>(1)->position -
86 s1->
as<ob::CompoundState>()->as<ob::TimeStateSpace::StateType>(1)->position;
88 if (!(deltaT > 0 && deltaPos / deltaT <= vMax_)) {
98 bool checkMotion(
const ompl::base::State *,
const ompl::base::State *,
99 std::pair<ob::State *, double> &)
const override
101 throw ompl::Exception(
"SpaceTimeMotionValidator::checkMotion",
"not implemented");
106 ob::StateSpace *stateSpace_;
115 auto vectorSpace(std::make_shared<ob::RealVectorStateSpace>(1));
116 auto space = std::make_shared<ob::SpaceTimeStateSpace>(vectorSpace, vMax);
122 vectorSpace->setBounds(bounds);
125 space->setTimeBounds(0.0, 10.0);
128 ob::SpaceInformationPtr si = std::make_shared<ob::SpaceInformation>(space);
131 si->setStateValidityChecker([](
const ob::State *state) {
return isStateValid(state); });
132 si->setMotionValidator(std::make_shared<SpaceTimeMotionValidator>(si));
146 ss.setStartAndGoalStates(start, goal);
152 strrtStar->setRange(vMax);
155 ss.setPlanner(ob::PlannerPtr(strrtStar));
162 std::cout <<
"Found solution:" << std::endl;
164 ss.getSolutionPath().print(std::cout);
167 std::cout <<
"No solution found" << std::endl;
171int main(
int ,
char ** )
173 std::cout <<
"OMPL version: " << OMPL_VERSION << std::endl;
Definition of a compound state.
Abstract definition for a class checking the validity of motions – path segments between states....
MotionValidator(SpaceInformation *si)
Constructor.
SpaceInformation * si_
The instance of space information this state validity checker operates on.
unsigned int invalid_
Number of invalid segments.
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.
Space-Time RRT* (STRRTstar)
Create the set of classes typically needed to solve a geometric problem.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains code that is specific to planning under geometric constraints.
A class to store the exit status of Planner::solve()