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/control/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), last_status_(base::PlannerStatus::UNKNOWN)
47 {
48  si_ = si;
49  pdef_.reset(new base::ProblemDefinition(si_));
50 }
51 
53  configured_(false), planTime_(0.0), last_status_(base::PlannerStatus::UNKNOWN)
54 {
55  si_.reset(new SpaceInformation(space->getStateSpace(), space));
56  pdef_.reset(new base::ProblemDefinition(si_));
57 }
58 
60 {
61  if (!configured_ || !si_->isSetup() || !planner_->isSetup())
62  {
63  if (!si_->isSetup())
64  si_->setup();
65  if (!planner_)
66  {
67  if (pa_)
68  planner_ = pa_(si_);
69  if (!planner_)
70  {
71  OMPL_INFORM("No planner specified. Using default.");
72  planner_ = tools::SelfConfig::getDefaultPlanner(getGoal());
73  }
74  }
75  planner_->setProblemDefinition(pdef_);
76  if (!planner_->isSetup())
77  planner_->setup();
78 
79  configured_ = true;
80  }
81 }
82 
84 {
85  if (planner_)
86  planner_->clear();
87  if (pdef_)
88  pdef_->clearSolutionPaths();
89 }
90 
91 // we provide a duplicate implementation here to allow the planner to choose how the time is turned into a planner termination condition
93 {
94  setup();
95  last_status_ = base::PlannerStatus::UNKNOWN;
96  time::point start = time::now();
97  last_status_ = planner_->solve(time);
98  planTime_ = time::seconds(time::now() - start);
99  if (last_status_)
100  OMPL_INFORM("Solution found in %f seconds", planTime_);
101  else
102  OMPL_INFORM("No solution found after %f seconds", planTime_);
103  return last_status_;
104 }
105 
107 {
108  setup();
109  last_status_ = base::PlannerStatus::UNKNOWN;
110  time::point start = time::now();
111  last_status_ = planner_->solve(ptc);
112  planTime_ = time::seconds(time::now() - start);
113  if (last_status_)
114  OMPL_INFORM("Solution found in %f seconds", planTime_);
115  else
116  OMPL_INFORM("No solution found after %f seconds", planTime_);
117  return last_status_;
118 }
119 
121 {
122  if (pdef_)
123  {
124  const base::PathPtr &p = pdef_->getSolutionPath();
125  if (p)
126  return static_cast<PathControl&>(*p);
127  }
128  throw Exception("No solution path");
129 }
130 
132 {
133  return haveSolutionPath() && (!pdef_->hasApproximateSolution() || pdef_->getSolutionDifference() < std::numeric_limits<double>::epsilon());
134 }
135 
137 {
138  pd.clear();
139  if (planner_)
140  planner_->getPlannerData(pd);
141 }
142 
143 void ompl::control::SimpleSetup::print(std::ostream &out) const
144 {
145  if (si_)
146  {
147  si_->printProperties(out);
148  si_->printSettings(out);
149  }
150  if (planner_)
151  {
152  planner_->printProperties(out);
153  planner_->printSettings(out);
154  }
155  if (pdef_)
156  pdef_->print(out);
157 }
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
bool haveExactSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful) and the solutio...
SimpleSetup(const SpaceInformationPtr &si)
Constructor needs the control space used for planning.
Definition: SimpleSetup.cpp:45
base::ProblemDefinitionPtr pdef_
The created problem definition.
Definition: SimpleSetup.h:278
OMPL_DEPRECATED base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal)
Given a goal specification, decide on a planner for that goal.
Definition: SimpleSetup.cpp:40
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition of a control path.
Definition: PathControl.h:60
virtual base::PlannerStatus solve(double time=1.0)
Run the planner for a specified amount of time (default is 1 second)
Definition: SimpleSetup.cpp:92
virtual void setup()
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: SimpleSetup.cpp:59
A boost shared pointer wrapper for ompl::control::ControlSpace.
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.
PathControl & getSolutionPath() const
Get the solution path. Throw an exception if no solution is available.
boost::posix_time::ptime point
Representation of a point in time.
Definition: Time.h:50
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
virtual void print(std::ostream &out=std::cout) const
Print information about the current setup.
virtual void clear()
Clears the entire data structure.
Definition: PlannerData.cpp:78
void getPlannerData(base::PlannerData &pd) const
Get information about the exploration data structure the motion planner used.
A boost shared pointer wrapper for ompl::control::SpaceInformation.
SpaceInformationPtr si_
The created space information.
Definition: SimpleSetup.h:275
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
Definition of a problem to be solved. This includes the start state(s) for the system and a goal spec...
point now()
Get the current time point.
Definition: Time.h:56
A boost shared pointer wrapper for ompl::base::Goal.
Space information containing necessary information for planning with controls. setup() needs to be ca...
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:83
A boost shared pointer wrapper for ompl::base::Path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68