AnytimePathShorteningDemo.h
1 /*********************************************************************
2 * Rice University Software Distribution License
3 *
4 * Copyright (c) 2010, Rice University
5 * All Rights Reserved.
6 *
7 * For a full description see the file named LICENSE.
8 *
9 *********************************************************************/
10 
11 /* Author: Ryan Luna */
12 
13 #include <ompl/geometric/planners/rrt/RRT.h>
14 #include <ompl/geometric/planners/rrt/RRTConnect.h>
15 #include <ompl/geometric/planners/prm/PRM.h>
16 #include <ompl/geometric/planners/est/EST.h>
17 #include <ompl/geometric/planners/sbl/SBL.h>
18 #include <ompl/geometric/planners/kpiece/KPIECE1.h>
19 #include <ompl/geometric/planners/kpiece/BKPIECE1.h>
20 #include <ompl/geometric/planners/kpiece/LBKPIECE1.h>
21 #include <ompl/geometric/planners/rrt/RRTstar.h>
22 
23 #include <iostream>
24 #include <fstream>
25 
26 using namespace ompl;
27 
28 enum ProblemType
29 {
30  BARRIERS = 0,
31  CUBICLES,
32  EASY,
33  INVALID_PROBLEM
34 };
35 
36 static const char * ProblemTypeStrings[3] = {"Barriers", "Cubicles", "Easy"};
37 
38 enum OptimizationType
39 {
40  SHORTCUT = 0,
41  HYBRIDIZE,
42  ALTERNATE,
43  NONE,
44  INVALID_OPTIMIZATION
45 };
46 
47 static const char * OptTypeStrings[4] = {"Shortcut", "Hybridize", "Alternating", "None"};
48 
49 enum PlannerType
50 {
51  RRT = 0,
52  RRTSTAR,
53  EST,
54  SBL,
55  RRTCONNECT,
56  PRM,
57  PRMSTAR,
58  KPIECE,
59  BKPIECE,
60  LBKPIECE,
61  INVALID_PLANNER
62 };
63 
64 static const char* PlannerTypeStrings[10] = {"RRT", "RRT*", "EST", "SBL", "RRTConnect", "PRM", "PRM*", "KPIECE", "BKPIECE", "LBKPIECE"};
65 
66 void readArgsFromFile(const char* filename, ProblemType &probType, OptimizationType &optType, double &runtime, std::vector<PlannerType>& planners)
67 {
68  std::cout << __FUNCTION__ << " " << filename << std::endl;
69 
70  std::ifstream fin(filename);
71  if (!fin)
72  {
73  OMPL_ERROR("FATAL: Failed to open %s", filename);
74  return;
75  }
76 
77  std::string problem, optimization;
78  fin >> problem;
79  fin >> optimization;
80 
81  if (problem == "barriers")
82  probType = BARRIERS;
83  else if (problem == "cubicles")
84  probType = CUBICLES;
85  else if (problem == "easy")
86  probType = EASY;
87 
88  if (optimization == "shortcut")
89  optType = SHORTCUT;
90  else if (optimization == "hybridize")
91  optType = HYBRIDIZE;
92  else if (optimization == "alternate")
93  optType = ALTERNATE;
94 
95  fin >> runtime;
96 
97  std::string plannerArg;
98  while (!fin.eof() && fin.good())
99  {
100  plannerArg = "";
101  fin >> plannerArg;
102  if (plannerArg == "")
103  continue;
104 
105  if (plannerArg == "rrt")
106  planners.push_back(RRT);
107  else if (plannerArg == "rrtstar")
108  planners.push_back(RRTSTAR);
109  else if (plannerArg == "est")
110  planners.push_back(EST);
111  else if (plannerArg == "sbl")
112  planners.push_back(SBL);
113  else if (plannerArg == "rrtconnect")
114  planners.push_back(RRTCONNECT);
115  else if (plannerArg == "prm")
116  planners.push_back(PRM);
117  else if (plannerArg == "prmstar")
118  planners.push_back(PRMSTAR);
119  else if (plannerArg == "kpiece")
120  planners.push_back(KPIECE);
121  else if (plannerArg == "bkpiece")
122  planners.push_back(BKPIECE);
123  else if (plannerArg == "lbkpiece")
124  planners.push_back(LBKPIECE);
125  else
126  std::cout << "Unknown planner argument: " << plannerArg << ". Skipping..." << std::endl;
127  }
128 
129  fin.close();
130 }
131 
132 void parseArguments(int argc, char **argv, ProblemType &probType, OptimizationType &optType, double &runtime, std::vector<PlannerType>& planners)
133 {
134  if (argc == 2)
135  {
136  readArgsFromFile(argv[1], probType, optType, runtime, planners);
137  return;
138  }
139 
140  std::string problem(argv[1]);
141  std::string optimization(argv[2]);
142  runtime = atof(argv[3]);
143 
144  probType = INVALID_PROBLEM;
145  optType = INVALID_OPTIMIZATION;
146 
147  if (problem == "barriers")
148  probType = BARRIERS;
149  else if (problem == "cubicles")
150  probType = CUBICLES;
151  else if (problem == "easy")
152  probType = EASY;
153 
154  if (optimization == "shortcut")
155  optType = SHORTCUT;
156  else if (optimization == "hybridize")
157  optType = HYBRIDIZE;
158  else if (optimization == "alternate")
159  optType = ALTERNATE;
160  else if (optimization == "none")
161  optType = NONE;
162 
163  for (int i = 4; i < argc; ++i)
164  {
165  std::string plannerArg (argv[i]);
166  if (plannerArg == "rrt")
167  planners.push_back(RRT);
168  else if (plannerArg == "rrtstar")
169  planners.push_back(RRTSTAR);
170  else if (plannerArg == "est")
171  planners.push_back(EST);
172  else if (plannerArg == "sbl")
173  planners.push_back(SBL);
174  else if (plannerArg == "rrtconnect")
175  planners.push_back(RRTCONNECT);
176  else if (plannerArg == "prm")
177  planners.push_back(PRM);
178  else if (plannerArg == "prmstar")
179  planners.push_back(PRMSTAR);
180  else if (plannerArg == "kpiece")
181  planners.push_back(KPIECE);
182  else if (plannerArg == "bkpiece")
183  planners.push_back(BKPIECE);
184  else if (plannerArg == "lbkpiece")
185  planners.push_back(LBKPIECE);
186  else
187  std::cout << "Unknown planner argument: " << plannerArg << ". Skipping..." << std::endl;
188  }
189 }
190 
191 bool validArguments(const ProblemType& probType, const OptimizationType &optType, const double& runtime, const std::vector<PlannerType>& planners)
192 {
193  bool valid = true;
194  if (probType == INVALID_PROBLEM)
195  {
196  std::cout << "FATAL: Problem type is invalid!" << std::endl;
197  valid = false;
198  }
199 
200  if (optType == INVALID_OPTIMIZATION)
201  {
202  std::cout << "FATAL: Optimization type is invalid!" << std::endl;
203  valid = false;
204  }
205 
206  if (runtime <= 0.0)
207  {
208  std::cout << "FATAL: Runtime must be strictly positive!" << std::endl;
209  valid = false;
210  }
211 
212  if (planners.size() == 0)
213  {
214  std::cout << "FATAL: At least one planner must be declared!" << std::endl;
215  valid = false;
216  }
217 
218  if (valid)
219  {
220  std::cout << "Solving the " << ProblemTypeStrings[probType] << " problem using " << OptTypeStrings[optType] << " optimization for " << runtime << " seconds with " << std::endl;
221  for (const auto & planner : planners)
222  std::cout << PlannerTypeStrings[planner] << " ";
223  std::cout << std::endl;
224  }
225 
226  return valid;
227 }
228 
229 base::PlannerPtr allocPlanner(PlannerType type, const base::SpaceInformationPtr& si)
230 {
231  assert (type != INVALID_PLANNER);
232 
233  base::PlannerPtr planner;
234  planner.reset();
235 
236  switch (type)
237  {
238  case RRT:
239  planner = std::make_shared<geometric::RRT>(si);
240  break;
241 
242  case RRTSTAR:
243  planner = std::make_shared<geometric::RRTstar>(si);
244  break;
245 
246  case EST:
247  planner = std::make_shared<geometric::EST>(si);
248  break;
249 
250  case SBL:
251  planner = std::make_shared<geometric::SBL>(si);
252  break;
253 
254  case RRTCONNECT:
255  planner = std::make_shared<geometric::RRTConnect>(si);
256  break;
257 
258  case PRM:
259  planner = std::make_shared<geometric::PRM>(si);
260  break;
261 
262  case PRMSTAR:
263  planner = std::make_shared<geometric::PRM>(si, true);
264  break;
265 
266  case KPIECE:
267  planner = std::make_shared<geometric::KPIECE1>(si);
268  break;
269 
270  case BKPIECE:
271  planner = std::make_shared<geometric::BKPIECE1>(si);
272  break;
273 
274  case LBKPIECE:
275  planner = std::make_shared<geometric::LBKPIECE1>(si);
276  break;
277 
278  default:
279  OMPL_ERROR("UNKNOWN PLANNER TYPE");
280  }
281 
282  return planner;
283 }
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
A shared pointer wrapper for ompl::base::Planner.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
A shared pointer wrapper for ompl::base::SpaceInformation.