Loading...
Searching...
No Matches
ThunderDB.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, JSK, The University of Tokyo.
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 JSK, The University of Tokyo 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: Dave Coleman */
36
37// OMPL
38#include <ompl/tools/thunder/ThunderDB.h>
39#include <ompl/base/ScopedState.h>
40#include <ompl/util/Time.h>
41#include <ompl/util/Console.h>
42#include <ompl/tools/config/SelfConfig.h>
43#include <ompl/base/PlannerDataStorage.h>
44
45// Boost
46#include <boost/filesystem.hpp>
47
48ompl::tools::ThunderDB::ThunderDB(const base::StateSpacePtr &space) : numPathsInserted_(0), saving_enabled_(true)
49{
50 // Set space information
51 si_ = std::make_shared<base::SpaceInformation>(space);
52}
53
55{
56 if (numPathsInserted_)
57 OMPL_WARN("The database is being unloaded with unsaved experiences");
58}
59
60bool ompl::tools::ThunderDB::load(const std::string &fileName)
61{
62 // Error checking
63 if (fileName.empty())
64 {
65 OMPL_ERROR("Empty filename passed to save function");
66 return false;
67 }
68 if (!boost::filesystem::exists(fileName))
69 {
70 OMPL_INFORM("Database file does not exist: %s.", fileName.c_str());
71 return false;
72 }
73 if (!spars_)
74 {
75 OMPL_ERROR("SPARSdb planner has not been passed into the ThunderDB yet");
76 return false;
77 }
78
79 // Load database from file, track loading time
80 time::point start = time::now();
81
82 OMPL_INFORM("Loading database from file: %s", fileName.c_str());
83
84 // Open a binary input stream
85 std::ifstream iStream(fileName.c_str(), std::ios::binary);
86
87 // Get the total number of paths saved
88 double numPaths = 0;
89 iStream >> numPaths;
90
91 // Check that the number of paths makes sense
92 if (numPaths < 0 || numPaths > std::numeric_limits<double>::max())
93 {
94 OMPL_WARN("Number of paths to load %d is a bad value", numPaths);
95 return false;
96 }
97
98 if (numPaths > 1)
99 {
100 OMPL_ERROR("Currently more than one planner data is disabled from loading");
101 return false;
102 }
103
104 // Create a new planner data instance
105 auto plannerData(std::make_shared<ompl::base::PlannerData>(si_));
106
107 // Note: the StateStorage class checks if the states match for us
108 plannerDataStorage_.load(iStream, *plannerData.get());
109
110 OMPL_INFORM("ThunderDB: Loaded planner data with \n %d vertices\n %d edges\n %d start states\n %d goal states",
111 plannerData->numVertices(), plannerData->numEdges(), plannerData->numStartVertices(),
112 plannerData->numGoalVertices());
113
114 // Add to SPARSdb
115 OMPL_INFORM("Adding plannerData to SPARSdb:");
116 spars_->setPlannerData(*plannerData);
117
118 // Output the number of connected components
119 OMPL_INFORM(" %d connected components", spars_->getNumConnectedComponents());
120
121 // Close file
122 iStream.close();
123
124 double loadTime = time::seconds(time::now() - start);
125 OMPL_INFORM("Loaded database from file in %f sec ", loadTime);
126 return true;
127}
128
129bool ompl::tools::ThunderDB::addPath(ompl::geometric::PathGeometric &solutionPath, double &insertionTime)
130{
131 // Error check
132 if (!spars_)
133 {
134 OMPL_ERROR("SPARSdb planner has not been passed into the ThunderDB yet");
135 insertionTime = 0;
136 return false;
137 }
138
139 // Prevent inserting into database
140 if (!saving_enabled_)
141 {
142 OMPL_WARN("ThunderDB: Saving is disabled so not adding path");
143 return false;
144 }
145
146 bool result;
147 double seconds = 120; // 10; // a large number, should never need to use this
149
150 // Benchmark runtime
151 time::point startTime = time::now();
152 {
153 result = spars_->addPathToRoadmap(ptc, solutionPath);
154 }
155 insertionTime = time::seconds(time::now() - startTime);
156
157 OMPL_INFORM("SPARSdb now has %d states", spars_->getNumVertices());
158
159 // Record this new addition
160 numPathsInserted_++;
161
162 return result;
163}
164
165bool ompl::tools::ThunderDB::saveIfChanged(const std::string &fileName)
166{
167 if (numPathsInserted_)
168 return save(fileName);
169 else
170 OMPL_INFORM("Not saving because database has not changed");
171 return true;
172}
173
174bool ompl::tools::ThunderDB::save(const std::string &fileName)
175{
176 // Disabled
177 if (!saving_enabled_)
178 {
179 OMPL_WARN("Not saving because option disabled for ExperienceDB");
180 return false;
181 }
182
183 // Error checking
184 if (fileName.empty())
185 {
186 OMPL_ERROR("Empty filename passed to save function");
187 return false;
188 }
189 if (!spars_)
190 {
191 OMPL_ERROR("SPARSdb planner has not been passed into the ThunderDB yet");
192 return false;
193 }
194
195 // Save database from file, track saving time
196 time::point start = time::now();
197
198 OMPL_INFORM("Saving database to file: %s", fileName.c_str());
199
200 // Open a binary output stream
201 std::ofstream outStream(fileName.c_str(), std::ios::binary);
202
203 // Populate multiple planner Datas
204 std::vector<ompl::base::PlannerDataPtr> plannerDatas;
205
206 // TODO: make this more than 1 planner data perhaps
207 auto data(std::make_shared<base::PlannerData>(si_));
208 spars_->getPlannerData(*data);
209 OMPL_INFORM("Get planner data from SPARS2 with \n %d vertices\n %d edges\n %d start states\n %d goal states",
210 data->numVertices(), data->numEdges(), data->numStartVertices(), data->numGoalVertices());
211
212 plannerDatas.push_back(data);
213
214 // Write the number of paths we will be saving
215 double numPaths = plannerDatas.size();
216 outStream << numPaths;
217
218 // Start saving each planner data object
219 for (std::size_t i = 0; i < numPaths; ++i)
220 {
221 ompl::base::PlannerData &pd = *plannerDatas[i].get();
222
223 OMPL_INFORM("Saving experience %d with %d verticies and %d edges", i, pd.numVertices(), pd.numEdges());
224
225 if (false) // debug code
226 {
227 for (std::size_t i = 0; i < pd.numVertices(); ++i)
228 {
229 OMPL_INFORM("Vertex %d:", i);
230 debugVertex(pd.getVertex(i));
231 }
232 }
233
234 // Save a single planner data
235 plannerDataStorage_.store(pd, outStream);
236 }
237
238 // Close file
239 outStream.close();
240
241 // Benchmark
242 double loadTime = time::seconds(time::now() - start);
243 OMPL_INFORM("Saved database to file in %f sec with %d planner datas", loadTime, plannerDatas.size());
244
245 numPathsInserted_ = 0;
246
247 return true;
248}
249
251{
252 // OMPL_INFORM("-------------------------------------------------------");
253 // OMPL_INFORM("setSPARSdb ");
254 // OMPL_INFORM("-------------------------------------------------------");
255 spars_ = prm;
256}
257
262
263void ompl::tools::ThunderDB::getAllPlannerDatas(std::vector<ompl::base::PlannerDataPtr> &plannerDatas) const
264{
265 if (!spars_)
266 {
267 OMPL_ERROR("SPARSdb planner has not been passed into the ThunderDB yet");
268 return;
269 }
270
271 auto data(std::make_shared<base::PlannerData>(si_));
272 spars_->getPlannerData(*data);
273 plannerDatas.push_back(data);
274
275 // OMPL_DEBUG("ThunderDB::getAllPlannerDatas: Number of planner databases found: %d", plannerDatas.size());
276}
277
278bool ompl::tools::ThunderDB::findNearestStartGoal(int nearestK, const base::State *start, const base::State *goal,
281{
282 bool result = spars_->getSimilarPaths(nearestK, start, goal, candidateSolution, ptc);
283
284 if (!result)
285 {
286 OMPL_INFORM("RETRIEVE COULD NOT FIND SOLUTION ");
287 OMPL_INFORM("spars::getSimilarPaths() returned false - retrieve could not find solution");
288 return false;
289 }
290
291 OMPL_INFORM("spars::getSimilarPaths() returned true - found a solution of size %d",
292 candidateSolution.getStateCount());
293 return true;
294}
295
297{
298 debugState(vertex.getState());
299}
300
301void ompl::tools::ThunderDB::debugState(const ompl::base::State *state)
302{
303 si_->printState(state, std::cout);
304}
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition PlannerData.h:59
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,...
unsigned int numEdges() const
Retrieve the number of edges in this structure.
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition of an abstract state.
Definition State.h:50
Definition of a geometric path.
bool saveIfChanged(const std::string &fileName)
Save loaded database to file, except skips saving if no paths have been added.
ompl::tools::SPARSdbPtr & getSPARSdb()
Hook for debugging.
void setSPARSdb(ompl::tools::SPARSdbPtr &prm)
Create the database structure for saving experiences.
ThunderDB(const base::StateSpacePtr &space)
Constructor needs the state space used for planning.
Definition ThunderDB.cpp:48
bool load(const std::string &fileName)
Load database from file.
Definition ThunderDB.cpp:60
bool save(const std::string &fileName)
Save loaded database to file.
virtual ~ThunderDB()
Deconstructor.
Definition ThunderDB.cpp:54
void debugVertex(const ompl::base::PlannerDataVertex &vertex)
Print info to screen.
bool addPath(ompl::geometric::PathGeometric &solutionPath, double &insertionTime)
Add a new solution path to our database. Des not actually save to file so experience will be lost if ...
void getAllPlannerDatas(std::vector< ompl::base::PlannerDataPtr > &plannerDatas) const
Get a vector of all the planner datas in the database.
bool findNearestStartGoal(int nearestK, const base::State *start, const base::State *goal, ompl::geometric::SPARSdb::CandidateSolution &candidateSolution, const base::PlannerTerminationCondition &ptc)
Find the k nearest paths to our queries one.
base::SpaceInformationPtr si_
The created space information.
Definition ThunderDB.h:160
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition Console.h:66
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time)
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::shared_ptr< ompl::geometric::SPARSdb > SPARSdbPtr
Definition ThunderDB.h:64
Struct for passing around partially solved solutions.
Definition SPARSdb.h:234