LightningDB.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/lightning/LightningDB.h"
39 #include "ompl/base/ScopedState.h"
40 #include "ompl/util/Time.h"
41 #include "ompl/tools/config/SelfConfig.h"
42 #include "ompl/base/PlannerDataStorage.h"
43 
44 // Boost
45 #include <boost/filesystem.hpp>
46 
48 {
49  si_ = std::make_shared<base::SpaceInformation>(space);
50 
51  // Set nearest neighbor type
52  nn_ = std::make_shared<ompl::NearestNeighborsSqrtApprox<ompl::base::PlannerDataPtr>>();
53 
54  // Use our custom distance function for nearest neighbor tree
55  nn_->setDistanceFunction([this](const ompl::base::PlannerDataPtr &a, const ompl::base::PlannerDataPtr &b)
56  {
57  return distanceFunction(a, b);
58  });
59 
60  // Load the PlannerData instance to be used for searching
61  nnSearchKey_ = std::make_shared<ompl::base::PlannerData>(si_);
62 }
63 
65 {
66  if (numUnsavedPaths_)
67  OMPL_WARN("The database is being unloaded with unsaved experiences");
68 }
69 
70 bool ompl::tools::LightningDB::load(const std::string &fileName)
71 {
72  // Error checking
73  if (fileName.empty())
74  {
75  OMPL_ERROR("Empty filename passed to save function");
76  return false;
77  }
78  if (!boost::filesystem::exists(fileName))
79  {
80  OMPL_WARN("Database file does not exist: %s", fileName.c_str());
81  return false;
82  }
83 
84  // Load database from file, track loading time
85  time::point start = time::now();
86 
87  OMPL_INFORM("Loading database from file: %s", fileName.c_str());
88 
89  // Open a binary input stream
90  std::ifstream iStream(fileName.c_str(), std::ios::binary);
91 
92  // Get the total number of paths saved
93  double numPaths = 0;
94  iStream >> numPaths;
95 
96  // Check that the number of paths makes sense
97  if (numPaths < 0 || numPaths > std::numeric_limits<double>::max())
98  {
99  OMPL_WARN("Number of paths to load %d is a bad value", numPaths);
100  return false;
101  }
102 
103  // Start loading all the PlannerDatas
104  for (std::size_t i = 0; i < numPaths; ++i)
105  {
106  // Create a new planner data instance
107  auto plannerData(std::make_shared<ompl::base::PlannerData>(si_));
108 
109  // Note: the StateStorage class checks if the states match for us
110  plannerDataStorage_.load(iStream, *plannerData.get());
111 
112  // Add to nearest neighbor tree
113  nn_->add(plannerData);
114  }
115 
116  // Close file
117  iStream.close();
118 
119  double loadTime = time::seconds(time::now() - start);
120  OMPL_INFORM("Loaded database from file in %f sec with %d paths", loadTime, nn_->size());
121  return true;
122 }
123 
124 void ompl::tools::LightningDB::addPath(ompl::geometric::PathGeometric &solutionPath, double &insertionTime)
125 {
126  // Benchmark runtime
127  time::point startTime = time::now();
128  {
129  addPathHelper(solutionPath);
130  }
131  insertionTime = time::seconds(time::now() - startTime);
132 }
133 
134 void ompl::tools::LightningDB::addPathHelper(ompl::geometric::PathGeometric &solutionPath)
135 {
136  // Create a new planner data instance
137  auto plannerData(std::make_shared<ompl::base::PlannerData>(si_));
138 
139  // Add the states to one nodes files
140  for (auto &i : solutionPath.getStates())
141  {
142  ompl::base::PlannerDataVertex vert(i); // TODO tag?
143 
144  plannerData->addVertex(vert);
145  }
146 
147  // Deep copy the states in the vertices so that when the planner goes out of scope, all data remains intact
148  plannerData->decoupleFromPlanner();
149 
150  // Add to nearest neighbor tree
151  nn_->add(plannerData);
152 
153  numUnsavedPaths_++;
154 }
155 
156 bool ompl::tools::LightningDB::saveIfChanged(const std::string &fileName)
157 {
158  if (numUnsavedPaths_)
159  return save(fileName);
160  else
161  OMPL_INFORM("Not saving because database has not changed");
162  return true;
163 }
164 
165 bool ompl::tools::LightningDB::save(const std::string &fileName)
166 {
167  // Error checking
168  if (fileName.empty())
169  {
170  OMPL_ERROR("Empty filename passed to save function");
171  return false;
172  }
173 
174  // Save database from file, track saving time
175  time::point start = time::now();
176 
177  OMPL_INFORM("Saving database to file: %s", fileName.c_str());
178 
179  // Open a binary output stream
180  std::ofstream outStream(fileName.c_str(), std::ios::binary);
181 
182  // Convert the NN tree to a vector
183  std::vector<ompl::base::PlannerDataPtr> plannerDatas;
184  nn_->list(plannerDatas);
185 
186  // Write the number of paths we will be saving
187  double numPaths = plannerDatas.size();
188  outStream << numPaths;
189 
190  // Start saving each planner data object
191  for (std::size_t i = 0; i < numPaths; ++i)
192  {
193  ompl::base::PlannerData &pd = *plannerDatas[i].get();
194 
195  // Save a single planner data
196  plannerDataStorage_.store(pd, outStream);
197  }
198 
199  // Close file
200  outStream.close();
201 
202  // Benchmark
203  double loadTime = time::seconds(time::now() - start);
204  OMPL_INFORM("Saved database to file in %f sec with %d paths", loadTime, plannerDatas.size());
205 
206  numUnsavedPaths_ = 0;
207 
208  return true;
209 }
210 
211 void ompl::tools::LightningDB::getAllPlannerDatas(std::vector<ompl::base::PlannerDataPtr> &plannerDatas) const
212 {
213  OMPL_DEBUG("LightningDB: getAllPlannerDatas");
214 
215  // Convert the NN tree to a vector
216  nn_->list(plannerDatas);
217 
218  OMPL_DEBUG("Number of paths found: %d", plannerDatas.size());
219 }
220 
221 std::vector<ompl::base::PlannerDataPtr> ompl::tools::LightningDB::findNearestStartGoal(int nearestK,
222  const base::State *start,
223  const base::State *goal)
224 {
225  // Fill in our pre-made PlannerData instance with the new start and goal states to be searched for
226  if (nnSearchKey_->numVertices() == 2)
227  {
228  nnSearchKey_->getVertex(0) = ompl::base::PlannerDataVertex(start);
229  nnSearchKey_->getVertex(1) = ompl::base::PlannerDataVertex(goal);
230  }
231  else
232  {
233  nnSearchKey_->addVertex(ompl::base::PlannerDataVertex(start));
234  nnSearchKey_->addVertex(ompl::base::PlannerDataVertex(goal));
235  }
236  assert(nnSearchKey_->numVertices() == 2);
237 
238  std::vector<ompl::base::PlannerDataPtr> nearest;
239  nn_->nearestK(nnSearchKey_, nearestK, nearest);
240 
241  return nearest;
242 }
243 
244 double ompl::tools::LightningDB::distanceFunction(const ompl::base::PlannerDataPtr &a,
245  const ompl::base::PlannerDataPtr &b) const
246 {
247  // Bi-directional implementation - check path b from [start, goal] and [goal, start]
248  return std::min(
249  // [ a.start, b.start] + [a.goal + b.goal]
250  si_->distance(a->getVertex(0).getState(), b->getVertex(0).getState()) +
251  si_->distance(a->getVertex(a->numVertices() - 1).getState(), b->getVertex(b->numVertices() - 1).getState()),
252  // [ a.start, b.goal] + [a.goal + b.start]
253  si_->distance(a->getVertex(0).getState(), b->getVertex(b->numVertices() - 1).getState()) +
254  si_->distance(a->getVertex(a->numVertices() - 1).getState(), b->getVertex(0).getState()));
255 }
256 
258 {
259  return nn_->size();
260 }
261 
263 {
264  // Loop through every PlannerData and sum the number of states
265  std::size_t statesCount = 0;
266 
267  // Convert the NN tree to a vector
268  std::vector<ompl::base::PlannerDataPtr> plannerDatas;
269  nn_->list(plannerDatas);
270 
271  // Start saving each planner data object
272  for (auto &plannerData : plannerDatas)
273  {
274  statesCount += plannerData->numVertices();
275  }
276 
277  return statesCount;
278 }
void getAllPlannerDatas(std::vector< ompl::base::PlannerDataPtr > &plannerDatas) const
Get a vector of all the paths in the nearest neighbor tree.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
A shared pointer wrapper for ompl::base::StateSpace.
virtual void load(const char *filename, PlannerData &pd)
Load the PlannerData structure from the given stream. The StateSpace that was used to store the data ...
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:76
void addPath(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 ...
LightningDB(const base::StateSpacePtr &space)
Constructor needs the state space used for planning.
Definition: LightningDB.cpp:47
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
std::vector< ompl::base::PlannerDataPtr > findNearestStartGoal(int nearestK, const base::State *start, const base::State *goal)
Find the k nearest paths to our queries one.
bool load(const std::string &fileName)
Load database from file.
Definition: LightningDB.cpp:70
Definition of an abstract state.
Definition: State.h:49
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
ompl::base::PlannerDataStorage plannerDataStorage_
Helper class for storing each plannerData instance.
Definition: LightningDB.h:159
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
point now()
Get the current time point.
Definition: Time.h:70
std::size_t getStatesCount() const
Get the total number of states stored in the database, across all paths.
virtual ~LightningDB()
Deconstructor.
Definition: LightningDB.cpp:64
std::size_t getExperiencesCount() const
Get the total number of paths stored in the database.
Definition of a geometric path.
Definition: PathGeometric.h:60
base::SpaceInformationPtr si_
The created space information.
Definition: LightningDB.h:156
bool saveIfChanged(const std::string &fileName)
Save loaded database to file, except skips saving if no paths have been added.
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:64
A shared pointer wrapper for ompl::base::PlannerData.
virtual void store(const PlannerData &pd, const char *filename)
Store (serialize) the PlannerData structure to the given filename.
std::vector< base::State * > & getStates()
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
bool save(const std::string &fileName)
Save loaded database to file.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68