LightningDB.h
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  Desc: Implementation of the Lightning Framework for experienced-based planning
37 
38  Paper: Berenson, Dmitry, Pieter Abbeel, and Ken Goldberg.
39  "A robot path planning framework that learns from experience."
40  Robotics and Automation (ICRA), 2012 IEEE International Conference on. IEEE, 2012.
41 */
42 
43 #ifndef OMPL_TOOLS_LIGHTNING_LIGHTNINGDB_
44 #define OMPL_TOOLS_LIGHTNING_LIGHTNINGDB_
45 
46 #include "ompl/base/StateSpace.h"
47 #include "ompl/geometric/PathGeometric.h"
48 #include "ompl/base/PlannerData.h"
49 #include "ompl/base/PlannerDataStorage.h"
50 #include "ompl/base/State.h"
51 #include "ompl/base/SpaceInformation.h"
52 #include "ompl/datastructures/NearestNeighbors.h"
53 
54 namespace ompl
55 {
56  namespace tools
57  {
64  OMPL_CLASS_FORWARD(LightningDB);
67 
72  class LightningDB
73  {
74  public:
78  LightningDB(const base::StateSpacePtr &space);
79 
83  virtual ~LightningDB();
84 
90  bool load(const std::string &fileName);
91 
99  void addPath(geometric::PathGeometric &solutionPath, double &insertionTime);
100  void addPathHelper(geometric::PathGeometric &solutionPath);
101 
107  bool saveIfChanged(const std::string &fileName);
108 
114  bool save(const std::string &fileName);
115 
119  void getAllPlannerDatas(std::vector<ompl::base::PlannerDataPtr> &plannerDatas) const;
120 
124  std::vector<ompl::base::PlannerDataPtr> findNearestStartGoal(int nearestK, const base::State *start,
125  const base::State *goal);
126 
128  std::size_t getExperiencesCount() const;
129 
131  std::size_t getStatesCount() const;
132 
134  int getNumUnsavedPaths() const
135  {
136  return numUnsavedPaths_;
137  }
138 
143  bool isEmpty()
144  {
145  return getExperiencesCount() == 0;
146  }
147 
148  private:
152  double distanceFunction(const ompl::base::PlannerDataPtr &a, const ompl::base::PlannerDataPtr &b) const;
153 
154  protected:
156  base::SpaceInformationPtr si_;
157 
160 
161  // A nearest-neighbors datastructure containing the tree of start/goal states combined
162  std::shared_ptr<NearestNeighbors<ompl::base::PlannerDataPtr>> nn_;
163 
164  // Reusable plannerData instance for filling in start and goal and performing searches on the tree
165  ompl::base::PlannerDataPtr nnSearchKey_;
166 
167  // Track unsaved paths to determine if a save is required
168  int numUnsavedPaths_{0};
169 
170  }; // end of class LightningDB
171 
172  } // end of namespace
173 
174 } // end of namespace
175 #endif
std::size_t getExperiencesCount() const
Get the total number of paths stored in the database.
ompl::base::PlannerDataStorage plannerDataStorage_
Helper class for storing each plannerData instance.
Definition: LightningDB.h:223
A shared pointer wrapper for ompl::base::PlannerData.
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.
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
void getAllPlannerDatas(std::vector< ompl::base::PlannerDataPtr > &plannerDatas) const
Get a vector of all the paths in the nearest neighbor tree.
base::SpaceInformationPtr si_
The created space information.
Definition: LightningDB.h:220
bool isEmpty()
Check if anything has been loaded into DB.
Definition: LightningDB.h:207
bool save(const std::string &fileName)
Save loaded database to file.
Object that handles loading/storing a PlannerData object to/from a binary stream. Serialization of ve...
bool saveIfChanged(const std::string &fileName)
Save loaded database to file, except skips saving if no paths have been added.
int getNumUnsavedPaths() const
Get number of unsaved paths.
Definition: LightningDB.h:198
bool load(const std::string &fileName)
Load database from file.
Definition: LightningDB.cpp:70
LightningDB(const base::StateSpacePtr &space)
Constructor needs the state space used for planning.
Definition: LightningDB.cpp:47
Main namespace. Contains everything in this library.
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 ...