Loading...
Searching...
No Matches
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
50namespace ompl
51{
52 namespace tools
53 {
61 OMPL_CLASS_FORWARD(ThunderDB);
63
64 using SPARSdbPtr = std::shared_ptr<ompl::geometric::SPARSdb>;
65
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() == 0u;
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
Object that handles loading/storing a PlannerData object to/from a binary stream. Serialization of ve...
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:59
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition of an abstract state.
Definition: State.h:50
Definition of a geometric path.
Definition: PathGeometric.h:66
Save and load entire paths from file.
Definition: ThunderDB.h:71
bool saveIfChanged(const std::string &fileName)
Save loaded database to file, except skips saving if no paths have been added.
Definition: ThunderDB.cpp:165
ompl::tools::SPARSdbPtr & getSPARSdb()
Hook for debugging.
Definition: ThunderDB.cpp:258
ompl::base::PlannerDataStorage plannerDataStorage_
Helper class for storing each plannerData instance.
Definition: ThunderDB.h:163
void setSPARSdb(ompl::tools::SPARSdbPtr &prm)
Create the database structure for saving experiences.
Definition: ThunderDB.cpp:250
void setSavingEnabled(bool saving_enabled)
Setter for enabling experience database saving.
Definition: ThunderDB.h:144
bool load(const std::string &fileName)
Load database from file.
Definition: ThunderDB.cpp:60
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
virtual ~ThunderDB()
Deconstructor.
Definition: ThunderDB.cpp:54
void debugVertex(const ompl::base::PlannerDataVertex &vertex)
Print info to screen.
Definition: ThunderDB.cpp:296
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
int getNumPathsInserted() const
Get number of unsaved paths.
Definition: ThunderDB.h:132
void getAllPlannerDatas(std::vector< ompl::base::PlannerDataPtr > &plannerDatas) const
Get a vector of all the planner datas in the database.
Definition: ThunderDB.cpp:263
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
bool isEmpty()
Check if anything has been loaded into DB.
Definition: ThunderDB.h:153
base::SpaceInformationPtr si_
The created space information.
Definition: ThunderDB.h:160
std::shared_ptr< ompl::geometric::SPARSdb > SPARSdbPtr
Definition: ThunderDB.h:64
Main namespace. Contains everything in this library.
Struct for passing around partially solved solutions.
Definition: SPARSdb.h:234