ReedsSheppStateSpace.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Mark Moll */
36 
37 #ifndef OMPL_BASE_SPACES_REEDS_SHEPP_STATE_SPACE_
38 #define OMPL_BASE_SPACES_REEDS_SHEPP_STATE_SPACE_
39 
40 #include "ompl/base/spaces/SE2StateSpace.h"
41 #include "ompl/base/MotionValidator.h"
42 #include <boost/math/constants/constants.hpp>
43 
44 namespace ompl
45 {
46  namespace base
47  {
64  {
65  public:
68  {
69  RS_NOP = 0,
70  RS_LEFT = 1,
71  RS_STRAIGHT = 2,
72  RS_RIGHT = 3
73  };
78  {
79  public:
81  double t = std::numeric_limits<double>::max(), double u = 0., double v = 0.,
82  double w = 0., double x = 0.);
83  double length() const
84  {
85  return totalLength_;
86  }
87 
91  double length_[5];
93  double totalLength_;
94  };
95 
96  ReedsSheppStateSpace(double turningRadius = 1.0) : rho_(turningRadius)
97  {
98  }
99 
100  double distance(const State *state1, const State *state2) const override;
101 
102  void interpolate(const State *from, const State *to, double t, State *state) const override;
103  virtual void interpolate(const State *from, const State *to, double t, bool &firstTime,
104  ReedsSheppPath &path, State *state) const;
105 
106  void sanityChecks() const override
107  {
108  double zero = std::numeric_limits<double>::epsilon();
109  double eps = .1; // rarely such a large error will occur
111  }
112 
114  ReedsSheppPath reedsShepp(const State *state1, const State *state2) const;
115 
116  protected:
117  virtual void interpolate(const State *from, const ReedsSheppPath &path, double t, State *state) const;
118 
120  double rho_;
121  };
122 
130  {
131  public:
133  {
134  defaultSettings();
135  }
137  {
138  defaultSettings();
139  }
140  ~ReedsSheppMotionValidator() override = default;
141  bool checkMotion(const State *s1, const State *s2) const override;
142  bool checkMotion(const State *s1, const State *s2, std::pair<State *, double> &lastValid) const override;
143 
144  private:
145  ReedsSheppStateSpace *stateSpace_;
146  void defaultSettings();
147  };
148  }
149 }
150 
151 #endif
The base class for space information. This contains all the information about the space planning is d...
An SE(2) state space where distance is measured by the length of Reeds-Shepp curves.
A shared pointer wrapper for ompl::base::SpaceInformation.
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.
Definition of an abstract state.
Definition: State.h:50
virtual void sanityChecks() const
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
Definition: StateSpace.cpp:603
bool checkMotion(const State *s1, const State *s2) const override
Check if the path between two states (from s1 to s2) is valid. This function assumes s1 is valid.
A Reeds-Shepp motion validator that only uses the state validity checker. Motions are checked for val...
MotionValidator(SpaceInformation *si)
Constructor.
Abstract definition for a class checking the validity of motions – path segments between states....
static const ReedsSheppPathSegmentType reedsSheppPathType[18][5]
Reeds-Shepp path types.
A state space representing SE(2)
Definition: SE2StateSpace.h:50
ReedsSheppPathSegmentType
The Reeds-Shepp path segment types.
void interpolate(const State *from, const State *to, double t, State *state) const override
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state....
@ STATESPACE_INTERPOLATION
Check whether calling StateSpace::interpolate() works as expected.
Definition: StateSpace.h:145
Complete description of a ReedsShepp path.
double distance(const State *state1, const State *state2) const override
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
void sanityChecks() const override
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
Main namespace. Contains everything in this library.
Definition: AppBase.h:22