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), invalid_request_(false)
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  logInform("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 
120 {
121  setup();
122  invalid_request_ = false;
123  time::point start = time::now();
124  base::PlannerStatus result = planner_->solve(time);
125  planTime_ = time::seconds(time::now() - start);
126  if (result)
127  logInform("Solution found in %f seconds", planTime_);
128  else
129  {
130  if (planTime_ < time)
131  invalid_request_ = true;
132  logInform("No solution found after %f seconds", planTime_);
133  }
134  return result;
135 }
136 
138 {
139  setup();
140  invalid_request_ = false;
141  time::point start = time::now();
142  base::PlannerStatus result = planner_->solve(ptc);
143  planTime_ = time::seconds(time::now() - start);
144  if (result)
145  logInform("Solution found in %f seconds", planTime_);
146  else
147  {
148  if (!ptc())
149  invalid_request_ = true;
150  logInform("No solution found after %f seconds", planTime_);
151  }
152  return result;
153 }
154 
156 {
157  if (pdef_)
158  {
159  const base::PathPtr &p = pdef_->getSolutionPath();
160  if (p)
161  {
162  time::point start = time::now();
163  psk_->simplify(static_cast<PathGeometric&>(*p), ptc);
164  simplifyTime_ = time::seconds(time::now() - start);
165  logInform("Path simplification took %f seconds", simplifyTime_);
166  return;
167  }
168  }
169  logWarn("No solution to simplify");
170 }
171 
173 {
174  if (pdef_)
175  {
176  const base::PathPtr &p = pdef_->getSolutionPath();
177  if (p)
178  {
179  time::point start = time::now();
180  if (duration < std::numeric_limits<double>::epsilon())
181  psk_->simplifyMax(static_cast<PathGeometric&>(*p));
182  else
183  psk_->simplify(static_cast<PathGeometric&>(*p), duration);
184  simplifyTime_ = time::seconds(time::now() - start);
185  logInform("Path simplification took %f seconds", simplifyTime_);
186  return;
187  }
188  }
189  logWarn("No solution to simplify");
190 }
191 
193 {
194  if (pdef_)
195  {
196  const base::PathPtr &p = pdef_->getSolutionPath();
197  if (p)
198  return static_cast<PathGeometric&>(*p);
199  }
200  throw Exception("No solution path");
201 }
202 
204 {
205  return haveSolutionPath() && (!pdef_->hasApproximateSolution() || pdef_->getSolutionDifference() < std::numeric_limits<double>::epsilon());
206 }
207 
209 {
210  pd.clear();
211  if (planner_)
212  planner_->getPlannerData(pd);
213 }
214 
215 void ompl::geometric::SimpleSetup::print(std::ostream &out) const
216 {
217  if (si_)
218  {
219  si_->printProperties(out);
220  si_->printSettings(out);
221  }
222  if (planner_)
223  {
224  planner_->printProperties(out);
225  planner_->printSettings(out);
226  }
227  if (pdef_)
228  pdef_->print(out);
229 }