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 
48 ompl::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 
60 bool 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 
129 bool 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 
165 bool 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 
174 bool 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 vertices 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 
263 void 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 
278 bool 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 
301 void ompl::tools::ThunderDB::debugState(const ompl::base::State *state)
302 {
303  si_->printState(state, std::cout);
304 }
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time)
ThunderDB(const base::StateSpacePtr &space)
Constructor needs the state space used for planning.
Definition: ThunderDB.cpp:48
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:116
Definition of an abstract state.
Definition: State.h:113
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:224
point now()
Get the current time point.
Definition: Time.h:122
unsigned int numEdges() const
Retrieve the number of edges in this structure.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
Definition of a geometric path.
Definition: PathGeometric.h:97
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
virtual ~ThunderDB()
Deconstructor.
Definition: ThunderDB.cpp:54
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
void debugVertex(const ompl::base::PlannerDataVertex &vertex)
Print info to screen.
Definition: ThunderDB.cpp:296
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist,...
void getAllPlannerDatas(std::vector< ompl::base::PlannerDataPtr > &plannerDatas) const
Get a vector of all the planner datas in the database.
Definition: ThunderDB.cpp:263
virtual const State * getState() const
Retrieve the state associated with this vertex.
Definition: PlannerData.h:176
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
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
std::shared_ptr< ompl::geometric::SPARSdb > SPARSdbPtr
Definition: ThunderDB.h:128
ompl::tools::SPARSdbPtr & getSPARSdb()
Hook for debugging.
Definition: ThunderDB.cpp:258
bool load(const std::string &fileName)
Load database from file.
Definition: ThunderDB.cpp:60
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
bool save(const std::string &fileName)
Save loaded database to file.
Definition: ThunderDB.cpp:174
bool saveIfChanged(const std::string &fileName)
Save loaded database to file, except skips saving if no paths have been added.
Definition: ThunderDB.cpp:165
void setSPARSdb(ompl::tools::SPARSdbPtr &prm)
Create the database structure for saving experiences.
Definition: ThunderDB.cpp:250
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:128
Struct for passing around partially solved solutions.
Definition: SPARSdb.h:328