All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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/base/goals/GoalSampleableRegion.h"
39 #include "ompl/geometric/planners/rrt/RRTConnect.h"
40 #include "ompl/geometric/planners/rrt/RRT.h"
41 #include "ompl/geometric/planners/kpiece/LBKPIECE1.h"
42 #include "ompl/geometric/planners/kpiece/KPIECE1.h"
43 
45 {
46  base::PlannerPtr planner;
47  if (!goal)
48  throw Exception("Unable to allocate default planner for unspecified goal definition");
49 
50  // if we can sample the goal region, use a bi-directional planner
51  if (goal->hasType(base::GOAL_SAMPLEABLE_REGION))
52  {
53  // if we have a default projection
54  if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
55  planner = base::PlannerPtr(new LBKPIECE1(goal->getSpaceInformation()));
56  else
57  planner = base::PlannerPtr(new RRTConnect(goal->getSpaceInformation()));
58  }
59  // other use a single-tree planner
60  else
61  {
62  // if we have a default projection
63  if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
64  planner = base::PlannerPtr(new KPIECE1(goal->getSpaceInformation()));
65  else
66  planner = base::PlannerPtr(new RRT(goal->getSpaceInformation()));
67  }
68 
69  if (!planner)
70  throw Exception("Unable to allocate default planner");
71 
72  return planner;
73 }
74 
76  configured_(false), planTime_(0.0), simplifyTime_(0.0), last_status_(base::PlannerStatus::UNKNOWN)
77 {
78  si_.reset(new base::SpaceInformation(space));
79  pdef_.reset(new base::ProblemDefinition(si_));
80  psk_.reset(new PathSimplifier(si_));
81  params_.include(si_->params());
82 }
83 
85 {
86  if (!configured_ || !si_->isSetup() || !planner_->isSetup())
87  {
88  if (!si_->isSetup())
89  si_->setup();
90  if (!planner_)
91  {
92  if (pa_)
93  planner_ = pa_(si_);
94  if (!planner_)
95  {
96  OMPL_INFORM("No planner specified. Using default.");
97  planner_ = getDefaultPlanner(getGoal());
98  }
99  }
100  planner_->setProblemDefinition(pdef_);
101  if (!planner_->isSetup())
102  planner_->setup();
103 
104  params_.clear();
105  params_.include(si_->params());
106  params_.include(planner_->params());
107  configured_ = true;
108  }
109 }
110 
112 {
113  if (planner_)
114  planner_->clear();
115  if (pdef_)
116  pdef_->clearSolutionPaths();
117 }
118 
119 // we provide a duplicate implementation here to allow the planner to choose how the time is turned into a planner termination condition
121 {
122  setup();
123  last_status_ = base::PlannerStatus::UNKNOWN;
124  time::point start = time::now();
125  last_status_ = planner_->solve(time);
126  planTime_ = time::seconds(time::now() - start);
127  if (last_status_)
128  OMPL_INFORM("Solution found in %f seconds", planTime_);
129  else
130  OMPL_INFORM("No solution found after %f seconds", planTime_);
131  return last_status_;
132 }
133 
135 {
136  setup();
137  last_status_ = base::PlannerStatus::UNKNOWN;
138  time::point start = time::now();
139  last_status_ = planner_->solve(ptc);
140  planTime_ = time::seconds(time::now() - start);
141  if (last_status_)
142  OMPL_INFORM("Solution found in %f seconds", planTime_);
143  else
144  OMPL_INFORM("No solution found after %f seconds", planTime_);
145  return last_status_;
146 }
147 
149 {
150  if (pdef_)
151  {
152  const base::PathPtr &p = pdef_->getSolutionPath();
153  if (p)
154  {
155  time::point start = time::now();
156  psk_->simplify(static_cast<PathGeometric&>(*p), ptc);
157  simplifyTime_ = time::seconds(time::now() - start);
158  OMPL_INFORM("Path simplification took %f seconds", simplifyTime_);
159  return;
160  }
161  }
162  OMPL_WARN("No solution to simplify");
163 }
164 
166 {
167  if (pdef_)
168  {
169  const base::PathPtr &p = pdef_->getSolutionPath();
170  if (p)
171  {
172  time::point start = time::now();
173  if (duration < std::numeric_limits<double>::epsilon())
174  psk_->simplifyMax(static_cast<PathGeometric&>(*p));
175  else
176  psk_->simplify(static_cast<PathGeometric&>(*p), duration);
177  simplifyTime_ = time::seconds(time::now() - start);
178  OMPL_INFORM("Path simplification took %f seconds", simplifyTime_);
179  return;
180  }
181  }
182  OMPL_WARN("No solution to simplify");
183 }
184 
186 {
187  if (pdef_)
188  {
189  const base::PathPtr &p = pdef_->getSolutionPath();
190  if (p)
191  return static_cast<PathGeometric&>(*p);
192  }
193  throw Exception("No solution path");
194 }
195 
197 {
198  return haveSolutionPath() && (!pdef_->hasApproximateSolution() || pdef_->getSolutionDifference() < std::numeric_limits<double>::epsilon());
199 }
200 
202 {
203  pd.clear();
204  if (planner_)
205  planner_->getPlannerData(pd);
206 }
207 
208 void ompl::geometric::SimpleSetup::print(std::ostream &out) const
209 {
210  if (si_)
211  {
212  si_->printProperties(out);
213  si_->printSettings(out);
214  }
215  if (planner_)
216  {
217  planner_->printProperties(out);
218  planner_->printSettings(out);
219  }
220  if (pdef_)
221  pdef_->print(out);
222 }