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/OptimizationObjective.h"
44 #include "ompl/base/goals/GoalSampleableRegion.h"
45 #include "ompl/util/ClassForward.h"
46 #include "ompl/util/RandomNumbers.h"
47 #include "ompl/util/Console.h"
48 #include <limits>
49 
50 namespace ompl
51 {
52  namespace geometric
53  {
55 
56  OMPL_CLASS_FORWARD(PathSimplifier);
58 
65  class PathSimplifier
66  {
67  public:
70  PathSimplifier(base::SpaceInformationPtr si, const base::GoalPtr &goal = ompl::base::GoalPtr(), const base::OptimizationObjectivePtr& obj=nullptr);
71 
72  virtual ~PathSimplifier() = default;
73 
93  bool reduceVertices(PathGeometric &path, unsigned int maxSteps = 0, unsigned int maxEmptySteps = 0,
94  double rangeRatio = 0.33);
95 
125  bool shortcutPath(PathGeometric &path, unsigned int maxSteps = 0, unsigned int maxEmptySteps = 0,
126  double rangeRatio = 0.33, double snapToVertex = 0.005);
127 
160  bool perturbPath(PathGeometric &path, double stepSize, unsigned int maxSteps = 0, unsigned int maxEmptySteps = 0, double snapToVertex = 0.005);
161 
177  bool collapseCloseVertices(PathGeometric &path, unsigned int maxSteps = 0, unsigned int maxEmptySteps = 0);
178 
188  void smoothBSpline(PathGeometric &path, unsigned int maxSteps = 5,
189  double minChange = std::numeric_limits<double>::epsilon());
190 
196  bool simplifyMax(PathGeometric &path);
197 
200  bool simplify(PathGeometric &path, double maxTime, bool atLeastOnce = true);
201 
204  bool simplify(PathGeometric &path, const base::PlannerTerminationCondition &ptc, bool atLeastOnce = true);
205 
219  bool findBetterGoal(PathGeometric &path, double maxTime, unsigned int samplingAttempts = 10,
220  double rangeRatio = 0.33, double snapToVertex = 0.005);
221 
235  bool findBetterGoal(PathGeometric &path, const base::PlannerTerminationCondition &ptc,
236  unsigned int samplingAttempts = 10, double rangeRatio = 0.33,
237  double snapToVertex = 0.005);
238 
242  void freeStates(bool flag);
243 
246  bool freeStates() const;
247 
248  protected:
249 
250  int selectAlongPath(std::vector<double> dists, std::vector<base::State *> states,
251  double distTo, double threshold, base::State *select_state, int &pos);
252 
254  base::SpaceInformationPtr si_;
255 
257  std::shared_ptr<base::GoalSampleableRegion> gsr_;
258 
261  base::OptimizationObjectivePtr obj_;
262 
264  bool freeStates_;
265 
267  RNG rng_;
268  };
269  }
270 }
271 
272 #endif
base::OptimizationObjectivePtr obj_
The optimization objective to use when making improvements. Will be used on all methods except reduce...
std::shared_ptr< base::GoalSampleableRegion > gsr_
The goal object for the path simplifier. Used for end-of-path improvements.
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...
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 perturbPath(PathGeometric &path, double stepSize, unsigned int maxSteps=0, unsigned int maxEmptySteps=0, double snapToVertex=0.005)
Given a path, attempt to improve the cost by randomly perturbing a randomly selected point on the pat...
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).
bool simplify(PathGeometric &path, double maxTime, bool atLeastOnce=true)
Run simplification algorithms on the path for at most maxTime seconds, and at least once if atLeastOn...
RNG rng_
Instance of random number generator.
PathSimplifier(base::SpaceInformationPtr si, const base::GoalPtr &goal=ompl::base::GoalPtr(), const base::OptimizationObjectivePtr &obj=nullptr)
Create an instance for a specified space information. Optionally, a GoalSampleableRegion may be passe...
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 freeStates_
Flag indicating whether the states removed from a motion should be freed.
bool simplifyMax(PathGeometric &path)
Given a path, attempt to remove vertices from it while keeping the path valid. Then,...
base::SpaceInformationPtr si_
The space information this path simplifier uses.
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...
bool freeStates() const
Return true if the memory of states is freed when they are removed from a path during simplification.
A shared pointer wrapper for ompl::base::Goal.
Main namespace. Contains everything in this library.