Loading...
Searching...
No Matches
CForest.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2014, 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/* Authors: Javier V. Gómez, Ioan Sucan, Mark Moll */
36
37#include "ompl/geometric/planners/cforest/CForest.h"
38#include "ompl/geometric/planners/rrt/RRTstar.h"
39#include "ompl/base/objectives/PathLengthOptimizationObjective.h"
40#include "ompl/util/String.h"
41#include <thread>
42
43ompl::geometric::CForest::CForest(const base::SpaceInformationPtr &si) : base::Planner(si, "CForest")
44{
45 specs_.optimizingPaths = true;
46 specs_.multithreaded = true;
47
48 numThreads_ = std::max(std::thread::hardware_concurrency(), 2u);
49 Planner::declareParam<bool>("focus_search", this, &CForest::setFocusSearch, &CForest::getFocusSearch, "0,1");
50 Planner::declareParam<unsigned int>("num_threads", this, &CForest::setNumThreads, &CForest::getNumThreads, "0:64");
51
52 addPlannerProgressProperty("best cost REAL", [this]
53 {
54 return getBestCost();
55 });
56 addPlannerProgressProperty("shared paths INTEGER", [this]
57 {
58 return getNumPathsShared();
59 });
60 addPlannerProgressProperty("shared states INTEGER", [this]
61 {
62 return getNumStatesShared();
63 });
64}
65
66ompl::geometric::CForest::~CForest() = default;
67
68void ompl::geometric::CForest::setNumThreads(unsigned int numThreads)
69{
70 numThreads_ = numThreads != 0u ? numThreads : std::max(std::thread::hardware_concurrency(), 2u);
71}
72
73void ompl::geometric::CForest::addPlannerInstanceInternal(const base::PlannerPtr &planner)
74{
75 if (!planner->getSpecs().canReportIntermediateSolutions)
76 OMPL_WARN("%s cannot report intermediate solutions, not added as CForest planner.", planner->getName().c_str());
77 else
78 {
79 planner->setProblemDefinition(pdef_);
80 if (planner->params().hasParam("focus_search"))
81 planner->params()["focus_search"] = focusSearch_;
82 else
83 OMPL_WARN("%s does not appear to support search focusing.", planner->getName().c_str());
84
85 planners_.push_back(planner);
86 }
87}
88
90{
91 Planner::getPlannerData(data);
92
93 for (std::size_t i = 0; i < planners_.size(); ++i)
94 {
95 base::PlannerData pd(si_);
96 planners_[i]->getPlannerData(pd);
97
98 for (unsigned int j = 0; j < pd.numVertices(); ++j)
99 {
101
102 v.setTag(i);
103 std::vector<unsigned int> edgeList;
104 unsigned int numEdges = pd.getIncomingEdges(j, edgeList);
105 for (unsigned int k = 0; k < numEdges; ++k)
106 {
107 base::Cost edgeWeight;
108 base::PlannerDataVertex &w = pd.getVertex(edgeList[k]);
109
110 w.setTag(i);
111 pd.getEdgeWeight(j, k, &edgeWeight);
112 data.addEdge(v, w, pd.getEdge(j, k), edgeWeight);
113 }
114 }
115
116 for (unsigned int j = 0; j < pd.numGoalVertices(); ++j)
118
119 for (unsigned int j = 0; j < pd.numStartVertices(); ++j)
121 }
122}
123
125{
126 Planner::clear();
127 for (auto &planner : planners_)
128 planner->clear();
129
130 bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
131 numPathsShared_ = 0;
132 numStatesShared_ = 0;
133
134 std::vector<base::StateSamplerPtr> samplers;
135 samplers.reserve(samplers_.size());
136 for (auto &sampler : samplers_)
137 if (sampler.use_count() > 1)
138 samplers.push_back(sampler);
139 samplers_.swap(samplers);
140}
141
143{
144 Planner::setup();
145 if (pdef_->hasOptimizationObjective())
146 opt_ = pdef_->getOptimizationObjective();
147 else
148 {
149 OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed "
150 "planning time.",
151 getName().c_str());
152 opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
153 }
154
155 bestCost_ = opt_->infiniteCost();
156
157 if (planners_.empty())
158 {
159 OMPL_INFORM("%s: Number and type of instances not specified. Defaulting to %d instances of RRTstar.",
160 getName().c_str(), numThreads_);
161 addPlannerInstances<RRTstar>(numThreads_);
162 }
163
164 for (auto &planner : planners_)
165 if (!planner->isSetup())
166 planner->setup();
167
168 // This call is needed to make sure the ParamSet is up to date after changes induced by the planner setup calls
169 // above, via the state space wrappers for CForest.
170 si_->setup();
171}
172
174{
175 checkValidity();
176
177 time::point start = time::now();
178 std::vector<std::thread *> threads(planners_.size());
179 const base::ReportIntermediateSolutionFn prevSolutionCallback =
180 getProblemDefinition()->getIntermediateSolutionCallback();
181
182 if (prevSolutionCallback)
183 OMPL_WARN("Cannot use previously set intermediate solution callback with %s", getName().c_str());
184
185 pdef_->setIntermediateSolutionCallback(
186 [this](const base::Planner *planner, const std::vector<const base::State *> &states, const base::Cost cost) {
187 return newSolutionFound(planner, states, cost);
188 });
189 bestCost_ = opt_->infiniteCost();
190
191 // run each planner in its own thread, with the same ptc.
192 for (std::size_t i = 0; i < threads.size(); ++i)
193 {
194 base::Planner *planner = planners_[i].get();
195 threads[i] = new std::thread([this, planner, &ptc]
196 {
197 return solve(planner, ptc);
198 });
199 }
200
201 for (auto &thread : threads)
202 {
203 thread->join();
204 delete thread;
205 }
206
207 // restore callback
208 getProblemDefinition()->setIntermediateSolutionCallback(prevSolutionCallback);
209 OMPL_INFORM("Solution found in %f seconds", time::seconds(time::now() - start));
210 return {pdef_->hasSolution(), pdef_->hasApproximateSolution()};
211}
212
214{
215 return ompl::toString(bestCost_.value());
216}
217
219{
220 return std::to_string(numPathsShared_);
221}
222
224{
225 return std::to_string(numStatesShared_);
226}
227
228void ompl::geometric::CForest::newSolutionFound(const base::Planner *planner,
229 const std::vector<const base::State *> &states, const base::Cost cost)
230{
231 bool change = false;
232 std::vector<const base::State *> statesToShare;
233 newSolutionFoundMutex_.lock();
234 if (opt_->isCostBetterThan(cost, bestCost_))
235 {
236 ++numPathsShared_;
237 bestCost_ = cost;
238 change = true;
239
240 // Filtering the states to add only those not already added.
241 statesToShare.reserve(states.size());
242 for (auto state : states)
243 {
244 if (statesShared_.find(state) == statesShared_.end())
245 {
246 statesShared_.insert(state);
247 statesToShare.push_back(state);
248 ++numStatesShared_;
249 }
250 }
251 }
252 newSolutionFoundMutex_.unlock();
253
254 if (!change || statesToShare.empty())
255 return;
256
257 for (auto &i : samplers_)
258 {
259 auto *sampler = static_cast<base::CForestStateSampler *>(i.get());
260 const auto *space =
261 static_cast<const base::CForestStateSpaceWrapper *>(sampler->getStateSpace());
262 const base::Planner *cfplanner = space->getPlanner();
263 if (cfplanner != planner)
264 sampler->setStatesToSample(statesToShare);
265 }
266}
267
269{
270 OMPL_DEBUG("Starting %s", planner->getName().c_str());
271 time::point start = time::now();
272 if (planner->solve(ptc))
273 {
274 double duration = time::seconds(time::now() - start);
275 OMPL_DEBUG("Solution found by %s in %lf seconds", planner->getName().c_str(), duration);
276 }
277}
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:59
virtual void setTag(int tag)
Set the integer tag associated with this vertex.
Definition: PlannerData.h:75
virtual const State * getState() const
Retrieve the state associated with this vertex.
Definition: PlannerData.h:80
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
bool getEdgeWeight(unsigned int v1, unsigned int v2, Cost *weight) const
Returns the weight of the edge between the given vertex indices. If there exists an edge between v1 a...
bool markStartState(const State *st)
Mark the given state as a start vertex. If the given state does not exist in a vertex,...
bool markGoalState(const State *st)
Mark the given state as a goal vertex. If the given state does not exist in a vertex,...
unsigned int getIncomingEdges(unsigned int v, std::vector< unsigned int > &edgeList) const
Returns a list of vertices with outgoing edges to the vertex with index v. The number of edges connec...
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
unsigned int numStartVertices() const
Returns the number of start vertices.
const PlannerDataEdge & getEdge(unsigned int v1, unsigned int v2) const
Retrieve a reference to the edge object connecting vertices with indexes v1 and v2....
const PlannerDataVertex & getStartVertex(unsigned int i) const
Retrieve a reference to the ith start vertex object. If i is greater than the number of start vertice...
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist,...
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
unsigned int numGoalVertices() const
Returns the number of goal vertices.
const PlannerDataVertex & getGoalVertex(unsigned int i) const
Retrieve a reference to the ith goal vertex object. If i is greater than the number of goal vertices,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition: Planner.h:223
unsigned int numThreads_
Default number of threads to use when no planner instances are specified by the user.
Definition: CForest.h:208
std::string getBestCost() const
Get best cost among all the planners.
Definition: CForest.cpp:213
void setNumThreads(unsigned int numThreads=0)
Set default number of threads to use when no planner instances are specified by the user.
Definition: CForest.cpp:68
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: CForest.cpp:124
std::string getNumPathsShared() const
Get number of paths shared by the algorithm.
Definition: CForest.cpp:218
std::string getNumStatesShared() const
Get number of states actually shared by the algorithm.
Definition: CForest.cpp:223
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: CForest.cpp:173
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: CForest.cpp:89
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: CForest.cpp:142
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
std::function< void(const Planner *, const std::vector< const base::State * > &, const Cost)> ReportIntermediateSolutionFn
When a planner has an intermediate solution (e.g., optimizing planners), a function with this signatu...
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:52
point now()
Get the current time point.
Definition: Time.h:58
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:64
std::string toString(float val)
convert float to string using classic "C" locale semantics
Definition: String.cpp:82
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49