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