PathGeometric.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
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 Willow Garage 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: Ioan Sucan */
36 
37 #ifndef OMPL_GEOMETRIC_PATH_GEOMETRIC_
38 #define OMPL_GEOMETRIC_PATH_GEOMETRIC_
39 
40 #include "ompl/base/SpaceInformation.h"
41 #include "ompl/base/Path.h"
42 #include <vector>
43 #include <utility>
44 
45 namespace ompl
46 {
47  namespace base
48  {
50  OMPL_CLASS_FORWARD(OptimizationObjective);
52  }
53 
55  namespace geometric
56  {
60  class PathGeometric : public base::Path
61  {
62  public:
64  PathGeometric(const base::SpaceInformationPtr &si) : base::Path(si)
65  {
66  }
67 
69  PathGeometric(const PathGeometric &path);
70 
72  PathGeometric(const base::SpaceInformationPtr &si, const base::State *state);
73 
75  PathGeometric(const base::SpaceInformationPtr &si, const base::State *state1, const base::State *state2);
76 
77  ~PathGeometric() override
78  {
79  freeMemory();
80  }
81 
83  PathGeometric &operator=(const PathGeometric &other);
84 
90  base::Cost cost(const base::OptimizationObjectivePtr &obj) const override;
91 
93  double length() const override;
94 
96  bool check() const override;
97 
116  double smoothness() const;
117 
130  double clearance() const;
131 
133  void print(std::ostream &out) const override;
138  virtual void printAsMatrix(std::ostream &out) const;
139 
148  void interpolate(unsigned int count);
149 
154  void interpolate();
155 
157  void subdivide();
158 
160  void reverse();
161 
173  std::pair<bool, bool> checkAndRepair(unsigned int attempts);
174 
185  void overlay(const PathGeometric &over, unsigned int startIndex = 0);
186 
188  void append(const base::State *state);
189 
201  void append(const PathGeometric &path);
202 
204  void prepend(const base::State *state);
205 
208  void keepAfter(const base::State *state);
209 
212  void keepBefore(const base::State *state);
213 
215  void random();
216 
219  bool randomValid(unsigned int attempts);
227  int getClosestIndex(const base::State *state) const;
228 
231  std::vector<base::State *> &getStates()
232  {
233  return states_;
234  }
235 
237  base::State *getState(unsigned int index)
238  {
239  return states_[index];
240  }
241 
243  const base::State *getState(unsigned int index) const
244  {
245  return states_[index];
246  }
247 
249  std::size_t getStateCount() const
250  {
251  return states_.size();
252  }
253 
255  void clear();
256 
259  protected:
261  void freeMemory();
262 
264  void copyFrom(const PathGeometric &other);
265 
267  std::vector<base::State *> states_;
268  };
269  }
270 }
271 
272 #endif
base::Cost cost(const base::OptimizationObjectivePtr &obj) const override
The sum of the costs for the sequence of segments that make up the path, computed using OptimizationO...
void keepAfter(const base::State *state)
Keep the part of the path that is after state (getClosestIndex() is used to find out which way-point ...
virtual void printAsMatrix(std::ostream &out) const
Print the path as a real-valued matrix where the i-th row represents the i-th state along the path...
void freeMemory()
Free the memory corresponding to the states on this path.
void interpolate()
Insert a number of states in a path so that the path is made up of (approximately) the states checked...
double length() const override
Compute the length of a geometric path (sum of lengths of segments that make up the path) ...
void print(std::ostream &out) const override
Print the path to a stream.
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
void copyFrom(const PathGeometric &other)
Copy data to this path from another path instance.
void overlay(const PathGeometric &over, unsigned int startIndex=0)
Overlay the path over on top of the current path. States are added to the current path if needed (by ...
void random()
Set this path to a random segment.
const base::State * getState(unsigned int index) const
Get the state located at index along the path.
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
PathGeometric(const base::SpaceInformationPtr &si)
Construct a path instance for a given space information.
Definition: PathGeometric.h:64
base::State * getState(unsigned int index)
Get the state located at index along the path.
std::pair< bool, bool > checkAndRepair(unsigned int attempts)
Check if the path is valid. If it is not, attempts are made to fix the path by sampling around invali...
bool randomValid(unsigned int attempts)
Set this path to a random valid segment. Sample attempts times for valid segments. Returns true on success.
Main namespace. Contains everything in this library.
Definition: Cost.h:42
Abstract definition of a path.
Definition: Path.h:67
void reverse()
Reverse the path.
bool check() const override
Check if the path is valid.
double clearance() const
Compute the clearance of the way-points along the path (no interpolation is performed). Detailed formula follows.
A shared pointer wrapper for ompl::base::SpaceInformation.
void prepend(const base::State *state)
Prepend state to the start of this path. The memory for state is copied.
Definition of an abstract state.
Definition: State.h:49
PathGeometric & operator=(const PathGeometric &other)
Assignment operator.
A shared pointer wrapper for ompl::base::OptimizationObjective.
void clear()
Remove all states and clear memory.
void keepBefore(const base::State *state)
Keep the part of the path that is before state (getClosestIndex() is used to find out which way-point...
void subdivide()
Add a state at the middle of each segment.
Definition of a geometric path.
Definition: PathGeometric.h:60
double smoothness() const
Compute a notion of smoothness for this path. The closer the value is to 0, the smoother the path...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
std::vector< base::State * > states_
The list of states that make up the path.
int getClosestIndex(const base::State *state) const
Get the index of the way-point along the path that is closest to state. Returns -1 for an empty path...
std::vector< base::State * > & getStates()
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...