PathSimplifier.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, Ryan Luna */
36 
37 #ifndef OMPL_GEOMETRIC_PATH_SIMPLIFIER_
38 #define OMPL_GEOMETRIC_PATH_SIMPLIFIER_
39 
40 #include "ompl/base/SpaceInformation.h"
41 #include "ompl/geometric/PathGeometric.h"
42 #include "ompl/base/PlannerTerminationCondition.h"
43 #include "ompl/base/goals/GoalSampleableRegion.h"
44 #include "ompl/util/ClassForward.h"
45 #include "ompl/util/RandomNumbers.h"
46 #include "ompl/util/Console.h"
47 #include <limits>
48 
49 namespace ompl
50 {
51  namespace geometric
52  {
54 
55  OMPL_CLASS_FORWARD(PathSimplifier);
57 
66  {
67  public:
72 
73  virtual ~PathSimplifier() = default;
74 
105  bool reduceVertices(PathGeometric &path, unsigned int maxSteps = 0, unsigned int maxEmptySteps = 0,
106  double rangeRatio = 0.33);
107 
151  bool shortcutPath(PathGeometric &path, unsigned int maxSteps = 0, unsigned int maxEmptySteps = 0,
152  double rangeRatio = 0.33, double snapToVertex = 0.005);
153 
179  bool collapseCloseVertices(PathGeometric &path, unsigned int maxSteps = 0, unsigned int maxEmptySteps = 0);
180 
194  void smoothBSpline(PathGeometric &path, unsigned int maxSteps = 5,
195  double minChange = std::numeric_limits<double>::epsilon());
196 
203  void simplifyMax(PathGeometric &path);
204 
206  void simplify(PathGeometric &path, double maxTime);
207 
211 
228  bool findBetterGoal(PathGeometric &path, double maxTime, unsigned int samplingAttempts = 10,
229  double rangeRatio = 0.33, double snapToVertex = 0.005);
230 
248  unsigned int samplingAttempts = 10, double rangeRatio = 0.33,
249  double snapToVertex = 0.005);
250 
254  void freeStates(bool flag);
255 
258  bool freeStates() const;
259 
260  protected:
263 
265  std::shared_ptr<base::GoalSampleableRegion> gsr_;
266 
269 
272  };
273  }
274 }
275 
276 #endif
bool freeStates() const
Return true if the memory of states is freed when they are removed from a path during simplification...
bool findBetterGoal(PathGeometric &path, double maxTime, unsigned int samplingAttempts=10, double rangeRatio=0.33, double snapToVertex=0.005)
Attempt to improve the solution path by sampling a new goal state and connecting this state to the so...
bool shortcutPath(PathGeometric &path, unsigned int maxSteps=0, unsigned int maxEmptySteps=0, double rangeRatio=0.33, double snapToVertex=0.005)
Given a path, attempt to shorten it while maintaining its validity. This is an iterative process that...
void simplify(PathGeometric &path, double maxTime)
Run simplification algorithms on the path for at most maxTime seconds.
void simplifyMax(PathGeometric &path)
Given a path, attempt to remove vertices from it while keeping the path valid. Then, try to smooth the path. This function applies the same set of default operations to the path, except in non-metric spaces, with the intention of simplifying it. In non-metric spaces, some operations are skipped because they do not work correctly when the triangle inequality may not hold.
PathSimplifier(base::SpaceInformationPtr si, const base::GoalPtr &goal=ompl::base::GoalPtr())
Create an instance for a specified space information. Optionally, a GoalSampleableRegion may be passe...
RNG rng_
Instance of random number generator.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
std::shared_ptr< base::GoalSampleableRegion > gsr_
The goal object for the path simplifier. Used for end-of-path improvements.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
bool reduceVertices(PathGeometric &path, unsigned int maxSteps=0, unsigned int maxEmptySteps=0, double rangeRatio=0.33)
Given a path, attempt to remove vertices from it while keeping the path valid. This is an iterative p...
base::SpaceInformationPtr si_
The space information this path simplifier uses.
This class contains routines that attempt to simplify geometric paths.
A shared pointer wrapper for ompl::base::SpaceInformation.
A shared pointer wrapper for ompl::base::Goal.
Definition of a geometric path.
Definition: PathGeometric.h:60
bool collapseCloseVertices(PathGeometric &path, unsigned int maxSteps=0, unsigned int maxEmptySteps=0)
Given a path, attempt to remove vertices from it while keeping the path valid. This is an iterative p...
bool freeStates_
Flag indicating whether the states removed from a motion should be freed.
void smoothBSpline(PathGeometric &path, unsigned int maxSteps=5, double minChange=std::numeric_limits< double >::epsilon())
Given a path, attempt to smooth it (the validity of the path is maintained).