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 {
65 OMPL_CLASS_FORWARD(LightningDB);
67
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
A shared pointer wrapper for ompl::base::PlannerData.
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.
Main namespace. Contains everything in this library.