ThunderDB.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 */
37 
38 #ifndef OMPL_TOOLS_THUNDER_THUNDERDB_
39 #define OMPL_TOOLS_THUNDER_THUNDERDB_
40 
41 #include <ompl/base/StateSpace.h>
42 #include <ompl/geometric/PathGeometric.h>
43 #include <ompl/base/PlannerData.h>
44 #include <ompl/base/PlannerDataStorage.h>
45 #include <ompl/base/State.h>
46 #include <ompl/base/SpaceInformation.h>
47 #include <ompl/datastructures/NearestNeighbors.h>
48 #include <ompl/tools/thunder/SPARSdb.h>
49 
50 namespace ompl
51 {
52  namespace tools
53  {
60  OMPL_CLASS_FORWARD(ThunderDB);
63 
64  using SPARSdbPtr = std::shared_ptr<ompl::geometric::SPARSdb>;
65 
70  class ThunderDB
71  {
72  public:
76  ThunderDB(const base::StateSpacePtr &space);
77 
79  virtual ~ThunderDB();
80 
86  bool load(const std::string &fileName);
87 
95  bool addPath(ompl::geometric::PathGeometric &solutionPath, double &insertionTime);
96 
102  bool saveIfChanged(const std::string &fileName);
103 
109  bool save(const std::string &fileName);
110 
114  void getAllPlannerDatas(std::vector<ompl::base::PlannerDataPtr> &plannerDatas) const;
115 
118 
121 
123  bool findNearestStartGoal(int nearestK, const base::State *start, const base::State *goal,
126 
128  void debugVertex(const ompl::base::PlannerDataVertex &vertex);
129  void debugState(const ompl::base::State *state);
130 
133  {
134  return numPathsInserted_;
135  }
136 
139  {
140  return saving_enabled_;
141  }
142 
144  void setSavingEnabled(bool saving_enabled)
145  {
146  saving_enabled_ = saving_enabled;
147  }
148 
153  bool isEmpty()
154  {
155  return !spars_->getNumVertices();
156  }
157 
158  protected:
160  base::SpaceInformationPtr si_; // TODO: is this even necessary?
161 
164 
165  // Track unsaved paths to determine if a save is required
166  int numPathsInserted_;
167 
168  // Use SPARSdb's graph datastructure to store experience
170 
171  // Allow the database to save to file (new experiences)
172  bool saving_enabled_;
173 
174  }; // end of class ThunderDB
175 
176  } // end of namespace
177 
178 } // end of namespace
179 #endif
ompl::tools::SPARSdbPtr & getSPARSdb()
Hook for debugging.
Definition: ThunderDB.cpp:258
base::SpaceInformationPtr si_
The created space information.
Definition: ThunderDB.h:160
Object that handles loading/storing a PlannerData object to/from a binary stream. Serialization of ve...
bool getSavingEnabled()
Getter for enabling experience database saving.
Definition: ThunderDB.h:138
bool save(const std::string &fileName)
Save loaded database to file.
Definition: ThunderDB.cpp:174
int getNumPathsInserted() const
Get number of unsaved paths.
Definition: ThunderDB.h:132
A shared pointer wrapper for ompl::base::StateSpace.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Struct for passing around partially solved solutions.
Definition: SPARSdb.h:240
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 class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
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:64
ompl::base::PlannerDataStorage plannerDataStorage_
Helper class for storing each plannerData instance.
Definition: ThunderDB.h:163
A shared pointer wrapper for ompl::base::SpaceInformation.
bool load(const std::string &fileName)
Load database from file.
Definition: ThunderDB.cpp:60
Definition of an abstract state.
Definition: State.h:49
Save and load entire paths from file.
Definition: ThunderDB.h:70
bool isEmpty()
Check if anything has been loaded into DB.
Definition: ThunderDB.h:153
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 ~ThunderDB()
Deconstructor.
Definition: ThunderDB.cpp:54
ThunderDB(const base::StateSpacePtr &space)
Constructor needs the state space used for planning.
Definition: ThunderDB.cpp:48
void setSPARSdb(ompl::tools::SPARSdbPtr &prm)
Create the database structure for saving experiences.
Definition: ThunderDB.cpp:250
void debugVertex(const ompl::base::PlannerDataVertex &vertex)
Print info to screen.
Definition: ThunderDB.cpp:296
Definition of a geometric path.
Definition: PathGeometric.h:60
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 setSavingEnabled(bool saving_enabled)
Setter for enabling experience database saving.
Definition: ThunderDB.h:144