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
259{
260 return spars_;
261}
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,...
Definition: PlannerData.h:175
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.
Definition: PathGeometric.h:66
bool saveIfChanged(const std::string &fileName)
Save loaded database to file, except skips saving if no paths have been added.
Definition: ThunderDB.cpp:165
ompl::tools::SPARSdbPtr & getSPARSdb()
Hook for debugging.
Definition: ThunderDB.cpp:258
void setSPARSdb(ompl::tools::SPARSdbPtr &prm)
Create the database structure for saving experiences.
Definition: ThunderDB.cpp:250
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.
Definition: ThunderDB.cpp:174
virtual ~ThunderDB()
Deconstructor.
Definition: ThunderDB.cpp:54
void debugVertex(const ompl::base::PlannerDataVertex &vertex)
Print info to screen.
Definition: ThunderDB.cpp:296
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 ...
Definition: ThunderDB.cpp:129
void getAllPlannerDatas(std::vector< ompl::base::PlannerDataPtr > &plannerDatas) const
Get a vector of all the planner datas in the database.
Definition: ThunderDB.cpp:263
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.
Definition: ThunderDB.cpp:278
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