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
47ompl::tools::LightningDB::LightningDB(const base::StateSpacePtr &space)
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
70bool 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
125{
126 // Benchmark runtime
127 time::point startTime = time::now();
128 {
129 addPathHelper(solutionPath);
130 }
131 insertionTime = time::seconds(time::now() - startTime);
132}
133
134void 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
156bool 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
165bool 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
211void 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
221std::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
244double 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}
A shared pointer wrapper for ompl::base::PlannerData.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
Definition of an abstract state.
Definition: State.h:50
Definition of a geometric path.
Definition: PathGeometric.h:66
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...
std::size_t getStatesCount() const
Get the total number of states stored in the database, across all paths.
void getAllPlannerDatas(std::vector< ompl::base::PlannerDataPtr > &plannerDatas) const
Get a vector of all the paths in the nearest neighbor tree.
virtual ~LightningDB()
Deconstructor.
Definition: LightningDB.cpp:64
LightningDB(const base::StateSpacePtr &space)
Constructor needs the state space used for planning.
Definition: LightningDB.cpp:47
base::SpaceInformationPtr si_
The created space information.
Definition: LightningDB.h:156
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 ...
bool save(const std::string &fileName)
Save loaded database to file.
bool saveIfChanged(const std::string &fileName)
Save loaded database to file, except skips saving if no paths have been added.
std::size_t getExperiencesCount() const
Get the total number of paths stored in the database.
bool load(const std::string &fileName)
Load database from file.
Definition: LightningDB.cpp:70
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.
#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_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::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