00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef OMPL_BASE_SPACES_REEDS_SHEPP_STATE_SPACE_
00038 #define OMPL_BASE_SPACES_REEDS_SHEPP_STATE_SPACE_
00039
00040 #include "ompl/base/spaces/SE2StateSpace.h"
00041 #include "ompl/base/MotionValidator.h"
00042 #include <boost/math/constants/constants.hpp>
00043
00044 namespace ompl
00045 {
00046 namespace base
00047 {
00048
00064 class ReedsSheppStateSpace : public SE2StateSpace
00065 {
00066 public:
00067
00069 enum ReedsSheppPathSegmentType { RS_NOP=0, RS_LEFT=1, RS_STRAIGHT=2, RS_RIGHT=3 };
00071 static const ReedsSheppPathSegmentType reedsSheppPathType[18][5];
00073 class ReedsSheppPath
00074 {
00075 public:
00076 ReedsSheppPath(const ReedsSheppPathSegmentType* type=reedsSheppPathType[0],
00077 double t=std::numeric_limits<double>::max(), double u=0., double v=0.,
00078 double w=0., double x=0.);
00079 double length() const { return totalLength_; }
00080
00082 ReedsSheppPathSegmentType type_[5];
00084 double length_[5];
00086 double totalLength_;
00087 };
00088
00089 ReedsSheppStateSpace(double turningRadius = 1.0)
00090 : SE2StateSpace(), rho_(turningRadius)
00091 {
00092 }
00093
00094 virtual double distance(const State *state1, const State *state2) const;
00095
00096 virtual void interpolate(const State *from, const State *to, const double t,
00097 State *state) const;
00098 virtual void interpolate(const State *from, const State *to, const double t,
00099 bool& firstTime, ReedsSheppPath& path, State *state) const;
00100
00101 virtual void sanityChecks(void) const
00102 {
00103 double zero = std::numeric_limits<double>::epsilon();
00104 double eps = .1;
00105 StateSpace::sanityChecks(zero, eps, ~STATESPACE_INTERPOLATION);
00106 }
00107
00109 ReedsSheppPath reedsShepp(const State *state1, const State *state2) const;
00110
00111 protected:
00112 virtual void interpolate(const State *from, const ReedsSheppPath& path, const double t,
00113 State *state) const;
00114
00116 double rho_;
00117 };
00118
00125 class ReedsSheppMotionValidator : public MotionValidator
00126 {
00127 public:
00128 ReedsSheppMotionValidator(SpaceInformation* si) : MotionValidator(si)
00129 {
00130 defaultSettings();
00131 }
00132 ReedsSheppMotionValidator(const SpaceInformationPtr &si) : MotionValidator(si)
00133 {
00134 defaultSettings();
00135 }
00136 virtual ~ReedsSheppMotionValidator(void)
00137 {
00138 }
00139 virtual bool checkMotion(const State *s1, const State *s2) const;
00140 virtual bool checkMotion(const State *s1, const State *s2, std::pair<State*, double> &lastValid) const;
00141 private:
00142 ReedsSheppStateSpace *stateSpace_;
00143 void defaultSettings(void);
00144 };
00145
00146 }
00147 }
00148
00149 #endif