Thunder.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_THUNDER_
39 #define OMPL_TOOLS_THUNDER_THUNDER_
40 
41 #include <ompl/tools/experience/ExperienceSetup.h> // the parent class
42 
43 #include <ompl/tools/thunder/ThunderDB.h>
44 #include <ompl/geometric/planners/experience/ThunderRetrieveRepair.h>
45 
46 #include <ompl/base/Planner.h>
47 #include <ompl/base/PlannerData.h>
48 #include <ompl/base/ProblemDefinition.h>
49 #include <ompl/base/SpaceInformation.h>
50 #include <ompl/base/ProblemDefinition.h>
51 #include <ompl/base/StateSpace.h> // for storing to file
52 
53 #include <ompl/geometric/PathGeometric.h>
54 #include <ompl/geometric/PathSimplifier.h>
55 
56 #include <ompl/tools/multiplan/ParallelPlan.h>
57 
58 #include <ompl/util/Console.h>
59 #include <ompl/util/Exception.h>
60 
61 namespace ompl
62 {
63  namespace tools
64  {
78  OMPL_CLASS_FORWARD(Thunder);
81 
86  class Thunder : public ompl::tools::ExperienceSetup
87  {
88  public:
90  explicit Thunder(const base::SpaceInformationPtr &si);
91 
95  explicit Thunder(const base::StateSpacePtr &space);
96 
97  private:
99  void initialize();
100 
101  public:
103  void printResultsInfo(std::ostream &out = std::cout) const override;
104 
106  void printLogs(std::ostream &out = std::cout) const override;
107 
110  {
111  return planner_;
112  }
113 
116  {
117  return static_cast<ompl::geometric::ThunderRetrieveRepair &>(*rrPlanner_);
118  }
119 
123  void setRepairPlanner(const base::PlannerPtr &planner) override
124  {
125  static_cast<ompl::geometric::ThunderRetrieveRepair &>(*rrPlanner_).setRepairPlanner(planner);
126  }
127 
132 
134  base::PlannerStatus solve(double time = 1.0) override;
135 
137  base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override;
138 
140  bool save() override;
141 
143  bool saveIfChanged() override;
144 
148  void clear() override;
149 
151  void print(std::ostream &out = std::cout) const override;
152 
155  void setup() override;
156 
158  void getAllPlannerDatas(std::vector<ompl::base::PlannerDataPtr> &plannerDatas) const override;
159 
161  std::size_t getExperiencesCount() const override;
162 
164  void convertPlannerData(const ompl::base::PlannerDataPtr &plannerData,
166 
174 
176  ompl::tools::ThunderDBPtr getExperienceDB();
177 
179  bool doPostProcessing() override;
180 
181  protected:
183  base::PlannerPtr rrPlanner_;
184 
186  base::PlannerPtr planner2_;
187 
189  bool dualThreadScratchEnabled_{true};
190 
192  ompl::tools::ParallelPlanPtr pp_;
193 
195  ompl::tools::ThunderDBPtr experienceDB_;
196 
198  std::vector<ompl::geometric::PathGeometric> queuedSolutionPaths_;
199 
200  }; // end of class Thunder
201 
202  } // end of namespace
203 
204 } // end of namespace
205 #endif
void setRepairPlanner(const base::PlannerPtr &planner) override
Set the planner to use for repairing experience paths inside the ThunderRetrieveRepair planner....
Definition: Thunder.h:187
The Thunder Framework's Retrieve-Repair component.
A shared pointer wrapper for ompl::base::PlannerData.
void print(std::ostream &out=std::cout) const override
Print information about the current setup.
Definition: Thunder.cpp:407
bool reversePathIfNecessary(ompl::geometric::PathGeometric &path1, ompl::geometric::PathGeometric &path2)
If path1 and path2 have a better start/goal match when reverse, then reverse path2.
Definition: Thunder.cpp:480
ompl::tools::ParallelPlanPtr pp_
Instance of parallel planning to use for computing solutions in parallel.
Definition: Thunder.h:256
base::PlannerPtr planner2_
Definition: Thunder.h:250
bool doPostProcessing() override
Allow accumlated experiences to be processed.
Definition: Thunder.cpp:507
void setRepairPlanner(const base::PlannerPtr &planner)
Set the planner that will be used for repairing invalid paths recalled from experience.
ompl::tools::ThunderDBPtr experienceDB_
A shared object between all the planners for saving and loading previous experience.
Definition: Thunder.h:259
std::vector< ompl::geometric::PathGeometric > queuedSolutionPaths_
Accumulated experiences to be later added to experience database.
Definition: Thunder.h:262
Definition of a geometric path.
Definition: PathGeometric.h:97
void convertPlannerData(const ompl::base::PlannerDataPtr &plannerData, ompl::geometric::PathGeometric &path)
Convert PlannerData to PathGeometric. Assume ordering of vertices is order of path.
Definition: Thunder.cpp:473
A shared pointer wrapper for ompl::base::Planner.
bool saveIfChanged() override
Save the experience database to file if there has been a change.
Definition: Thunder.cpp:391
base::PlannerPtr planner_
The maintained planner instance.
Definition: SimpleSetup.h:352
void clear() override
Clear all planning data. This only includes data generated by motion plan computation....
Definition: Thunder.cpp:151
Thunder(const base::SpaceInformationPtr &si)
Constructor needs the state space used for planning.
Definition: Thunder.cpp:46
std::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition: Planner.h:501
base::PlannerStatus solve(double time=1.0) override
Run the planner for up to a specified amount of time (default is 1 second)
Definition: Thunder.cpp:379
void setPlannerAllocator(const base::PlannerAllocator &pa)
Set the planner allocator to use. This is only used if no planner has been set. This is optional – a ...
Definition: Thunder.cpp:167
void setup() override
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: Thunder.cpp:71
void printResultsInfo(std::ostream &out=std::cout) const override
Display debug data about potential available solutions.
Definition: Thunder.cpp:397
ompl::base::PlannerPtr & getPlanner()
Get the current planner.
Definition: Thunder.h:173
Create the set of classes typically needed to solve a geometric problem.
void getAllPlannerDatas(std::vector< ompl::base::PlannerDataPtr > &plannerDatas) const override
Get a vector of all the planning data in the database.
Definition: Thunder.cpp:468
ompl::geometric::ThunderRetrieveRepair & getRetrieveRepairPlanner() const
Get a pointer to the retrieve repair planner.
Definition: Thunder.h:179
bool save() override
Save the experience database to file.
Definition: Thunder.cpp:385
base::PlannerPtr rrPlanner_
Definition: Thunder.h:247
ompl::tools::ThunderDBPtr getExperienceDB()
Hook for getting access to debug data.
Definition: Thunder.cpp:502
void printLogs(std::ostream &out=std::cout) const override
Display debug data about overall results from Thunder since being loaded.
Definition: Thunder.cpp:433
std::size_t getExperiencesCount() const override
Get the total number of paths stored in the database.
Definition: Thunder.cpp:463
bool dualThreadScratchEnabled_
Definition: Thunder.h:253
Main namespace. Contains everything in this library.
Definition: AppBase.h:21