SimpleSetup.cpp
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: Ioan Sucan */
36 
37 #include "ompl/geometric/SimpleSetup.h"
38 #include "ompl/tools/config/SelfConfig.h"
39 
41 {
42  return tools::SelfConfig::getDefaultPlanner(goal);
43 }
44 
46  configured_(false), planTime_(0.0), simplifyTime_(0.0), lastStatus_(base::PlannerStatus::UNKNOWN)
47 {
48  si_ = si;
49  pdef_.reset(new base::ProblemDefinition(si_));
50  psk_.reset(new PathSimplifier(si_));
51 }
52 
54  configured_(false), planTime_(0.0), simplifyTime_(0.0), lastStatus_(base::PlannerStatus::UNKNOWN)
55 {
56  si_.reset(new base::SpaceInformation(space));
57  pdef_.reset(new base::ProblemDefinition(si_));
58  psk_.reset(new PathSimplifier(si_));
59 }
60 
62 {
63  if (!configured_ || !si_->isSetup() || !planner_->isSetup())
64  {
65  if (!si_->isSetup())
66  si_->setup();
67  if (!planner_)
68  {
69  if (pa_)
70  planner_ = pa_(si_);
71  if (!planner_)
72  {
73  OMPL_INFORM("No planner specified. Using default.");
75  }
76  }
77  planner_->setProblemDefinition(pdef_);
78  if (!planner_->isSetup())
79  planner_->setup();
80  configured_ = true;
81  }
82 }
83 
85 {
86  if (planner_)
87  planner_->clear();
88  if (pdef_)
89  pdef_->clearSolutionPaths();
90 }
91 
92 // we provide a duplicate implementation here to allow the planner to choose how the time is turned into a planner termination condition
94 {
95  setup();
97  time::point start = time::now();
98  lastStatus_ = planner_->solve(time);
99  planTime_ = time::seconds(time::now() - start);
100  if (lastStatus_)
101  OMPL_INFORM("Solution found in %f seconds", planTime_);
102  else
103  OMPL_INFORM("No solution found after %f seconds", planTime_);
104  return lastStatus_;
105 }
106 
108 {
109  setup();
111  time::point start = time::now();
112  lastStatus_ = planner_->solve(ptc);
113  planTime_ = time::seconds(time::now() - start);
114  if (lastStatus_)
115  OMPL_INFORM("Solution found in %f seconds", planTime_);
116  else
117  OMPL_INFORM("No solution found after %f seconds", planTime_);
118  return lastStatus_;
119 }
120 
122 {
123  if (pdef_)
124  {
125  const base::PathPtr &p = pdef_->getSolutionPath();
126  if (p)
127  {
128  time::point start = time::now();
129  PathGeometric &path = static_cast<PathGeometric&>(*p);
130  std::size_t numStates = path.getStateCount();
131  psk_->simplify(path, ptc);
133  OMPL_INFORM("SimpleSetup: Path simplification took %f seconds and changed from %d to %d states",
134  simplifyTime_, numStates, path.getStateCount());
135  return;
136  }
137  }
138  OMPL_WARN("No solution to simplify");
139 }
140 
142 {
143  if (pdef_)
144  {
145  const base::PathPtr &p = pdef_->getSolutionPath();
146  if (p)
147  {
148  time::point start = time::now();
149  PathGeometric &path = static_cast<PathGeometric&>(*p);
150  std::size_t numStates = path.getStateCount();
151  if (duration < std::numeric_limits<double>::epsilon())
152  psk_->simplifyMax(static_cast<PathGeometric&>(*p));
153  else
154  psk_->simplify(static_cast<PathGeometric&>(*p), duration);
156  OMPL_INFORM("SimpleSetup: Path simplification took %f seconds and changed from %d to %d states",
157  simplifyTime_, numStates, path.getStateCount());
158  return;
159  }
160  }
161  OMPL_WARN("No solution to simplify");
162 }
163 
165 {
166  if (pdef_)
167  {
168  const ompl::base::PathPtr path; // convert to a generic path ptr
169  ompl::base::PlannerSolution solution(path); // a dummy solution
170 
171  // Get our desired solution
172  pdef_->getSolution(solution);
173  return solution.plannerName_;
174  }
175  throw Exception("No problem definition found");
176 }
177 
179 {
180  if (pdef_)
181  {
182  const base::PathPtr &p = pdef_->getSolutionPath();
183  if (p)
184  return static_cast<PathGeometric&>(*p);
185  }
186  throw Exception("No solution path");
187 }
188 
190 {
191  return haveSolutionPath() && (!pdef_->hasApproximateSolution() || pdef_->getSolutionDifference() < std::numeric_limits<double>::epsilon());
192 }
193 
195 {
196  pd.clear();
197  if (planner_)
198  planner_->getPlannerData(pd);
199 }
200 
201 void ompl::geometric::SimpleSetup::print(std::ostream &out) const
202 {
203  if (si_)
204  {
205  si_->printProperties(out);
206  si_->printSettings(out);
207  }
208  if (planner_)
209  {
210  planner_->printProperties(out);
211  planner_->printSettings(out);
212  }
213  if (pdef_)
214  pdef_->print(out);
215 }
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
const std::string getSolutionPlannerName(void) const
Get the best solution&#39;s planer name. Throw an exception if no solution is available.
double simplifyTime_
The amount of time the last path simplification step took.
Definition: SimpleSetup.h:310
PathGeometric & getSolutionPath() const
Get the solution path. Throw an exception if no solution is available.
void getPlannerData(base::PlannerData &pd) const
Get information about the exploration data structure the motion planner used.
base::PlannerAllocator pa_
The optional planner allocator.
Definition: SimpleSetup.h:298
Representation of a solution to a planning problem.
A boost shared pointer wrapper for ompl::base::StateSpace.
bool haveSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful). The solution may be approximate.
Definition: SimpleSetup.h:145
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
base::SpaceInformationPtr si_
The created space information.
Definition: SimpleSetup.h:289
PathSimplifierPtr psk_
The instance of the path simplifier.
Definition: SimpleSetup.h:301
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:62
A boost shared pointer wrapper for ompl::base::Planner.
const base::GoalPtr & getGoal() const
Get the current goal definition.
Definition: SimpleSetup.h:106
SimpleSetup(const base::SpaceInformationPtr &si)
Constructor needs the state space used for planning.
Definition: SimpleSetup.cpp:45
base::PlannerPtr planner_
The maintained planner instance.
Definition: SimpleSetup.h:295
boost::posix_time::ptime point
Representation of a point in time.
Definition: Time.h:50
base::ProblemDefinitionPtr pdef_
The created problem definition.
Definition: SimpleSetup.h:292
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
This class contains routines that attempt to simplify geometric paths.
A boost shared pointer wrapper for ompl::base::SpaceInformation.
The base class for space information. This contains all the information about the space planning is d...
virtual void clear()
Clears the entire data structure.
Definition: PlannerData.cpp:78
bool haveExactSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful) and the solutio...
virtual void setup()
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: SimpleSetup.cpp:61
static base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal)
Given a goal specification, decide on a planner for that goal.
Definition: SelfConfig.cpp:250
The exception type for ompl.
Definition: Exception.h:47
base::PlannerStatus lastStatus_
The status of the last planning request.
Definition: SimpleSetup.h:313
Definition of a problem to be solved. This includes the start state(s) for the system and a goal spec...
virtual base::PlannerStatus solve(double time=1.0)
Run the planner for up to a specified amount of time (default is 1 second)
Definition: SimpleSetup.cpp:93
point now()
Get the current time point.
Definition: Time.h:56
std::string plannerName_
Name of planner type that generated this solution, as received from Planner::getName() ...
bool configured_
Flag indicating whether the classes needed for planning are set up.
Definition: SimpleSetup.h:304
double planTime_
The amount of time the last planning step took.
Definition: SimpleSetup.h:307
void simplifySolution(double duration=0.0)
Attempt to simplify the current solution path. Spent at most duration seconds in the simplification p...
A boost shared pointer wrapper for ompl::base::Goal.
Definition of a geometric path.
Definition: PathGeometric.h:60
virtual void print(std::ostream &out=std::cout) const
Print information about the current setup.
OMPL_DEPRECATED base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal)
Given a goal specification, decide on a planner for that goal.
Definition: SimpleSetup.cpp:40
virtual void clear()
Clear all planning data. This only includes data generated by motion plan computation. Planner settings, start & goal states are not affected.
Definition: SimpleSetup.cpp:84
A boost shared pointer wrapper for ompl::base::Path.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68