37 #ifndef OMPL_BASE_SPACES_REEDS_SHEPP_STATE_SPACE_
38 #define OMPL_BASE_SPACES_REEDS_SHEPP_STATE_SPACE_
40 #include "ompl/base/spaces/SE2StateSpace.h"
41 #include "ompl/base/MotionValidator.h"
42 #include <boost/math/constants/constants.hpp>
77 double t=std::numeric_limits<double>::max(),
double u=0.,
double v=0.,
78 double w=0.,
double x=0.);
99 bool& firstTime, ReedsSheppPath& path,
State *state)
const;
103 double zero = std::numeric_limits<double>::epsilon();
112 virtual void interpolate(
const State *from,
const ReedsSheppPath& path,
const double t,
140 virtual bool checkMotion(
const State *s1,
const State *s2, std::pair<State*, double> &lastValid)
const;
143 void defaultSettings(
void);
double rho_
Turning radius.
const ReedsSheppPathSegmentType * type_
Complete description of a ReedsShepp path.
virtual void sanityChecks(void) const
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
virtual double distance(const State *state1, const State *state2) const
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
static const ReedsSheppPathSegmentType reedsSheppPathType[18][5]
Reeds-Shepp path types.
Abstract definition for a class checking the validity of motions – path segments between states...
ReedsSheppPath reedsShepp(const State *state1, const State *state2) const
Return the shortest Reeds-Shepp path from SE(2) state state1 to SE(2) state state2.
virtual void interpolate(const State *from, const State *to, const double t, State *state) const
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
A state space representing SE(2)
virtual bool checkMotion(const State *s1, const State *s2) const
Check if the path between two states (from s1 to s2) is valid. This function assumes s1 is valid...
ReedsSheppPathSegmentType
The Reeds-Shepp path segment types.
A Reeds-Shepp motion validator that only uses the state validity checker. Motions are checked for val...
Definition of an abstract state.
MotionValidator(SpaceInformation *si)
Constructor.
An SE(2) state space where distance is measured by the length of Reeds-Shepp curves.
virtual void sanityChecks(void) const
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
Check whether calling StateSpace::interpolate() works as expected.