Loading...
Searching...
No Matches
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
54namespace ompl
55{
56 namespace tools
57 {
63
65 OMPL_CLASS_FORWARD(LightningDB);
67
70
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
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
Object that handles loading/storing a PlannerData object to/from a binary stream. Serialization of ve...
Definition of an abstract state.
Definition State.h:50
Definition of a geometric path.
Save and load entire paths from file.
Definition LightningDB.h:73
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.
LightningDB(const base::StateSpacePtr &space)
Constructor needs the state space used for planning.
base::SpaceInformationPtr si_
The created space information.
int getNumUnsavedPaths() const
Get number of unsaved paths.
ompl::base::PlannerDataStorage plannerDataStorage_
Helper class for storing each plannerData instance.
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.
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 isEmpty()
Check if anything has been loaded into DB.
Includes various tools such as self config, benchmarking, etc.
Main namespace. Contains everything in this library.