37 #ifndef OMPL_GEOMETRIC_PLANNERS_XXL_XXL_
38 #define OMPL_GEOMETRIC_PLANNERS_XXL_XXL_
41 #include <unordered_map>
42 #include "ompl/util/Hash.h"
43 #include "ompl/datastructures/AdjacencyList.h"
44 #include "ompl/geometric/planners/PlannerIncludes.h"
45 #include "ompl/geometric/planners/xxl/XXLDecomposition.h"
68 XXL(
const base::SpaceInformationPtr &si);
85 double getRandWalkRate()
const
87 return rand_walk_rate_;
89 void setRandWalkRate(
double rate)
91 if (rate < 0.0 || rate > 1.0)
93 rand_walk_rate_ = rate;
103 exists.assign(max,
false);
104 elements.reserve(max / 10);
107 std::size_t numElements()
const
109 return elements.size();
112 bool isElement(
unsigned int val)
const
114 if (val >= (
unsigned int)exists.size())
119 bool addElement(
unsigned int val)
121 if (val >= (
unsigned int)exists.size() || exists[val])
124 elements.push_back(val);
129 int getElement(std::size_t idx)
const
131 return elements[idx];
135 std::vector<bool> exists;
136 std::vector<unsigned int> elements;
142 std::vector<int> levels;
148 std::vector<int> allMotions;
149 std::vector<int> motionsInTree;
155 Layer(
int _id,
int numRegions,
int lvl,
Layer *_parent)
156 : regions(numRegions)
157 , weights(numRegions, 0.5)
158 , exterior(numRegions,
true)
159 , connections(numRegions, 0)
160 , selections(numRegions, 0)
161 , leads(numRegions, 0)
162 , goalStates(numRegions, std::vector<int>())
163 , connectionPoints(numRegions)
174 for (
auto &sublayer : sublayers)
178 size_t numRegions()
const
180 return regions.size();
188 Layer *getParent()
const
200 if (r < 0 || r >= (
int)regions.size())
202 OMPL_ERROR(
"Requested region %d, but there are only %lu regions", r, regions.size());
209 const Region &getRegion(
int r)
const
211 if (r < 0 || r >= (
int)regions.size())
213 OMPL_ERROR(
"Requested region %d, but there are only %lu regions", r, regions.size());
220 const std::vector<double> &getWeights()
const
225 std::vector<double> &getWeights()
230 const std::vector<bool> &getExterior()
const
235 std::vector<bool> &getExterior()
240 const std::vector<int> &getConnections()
const
245 const std::vector<int> &getSelections()
const
250 const std::vector<std::vector<int>> &getGoalStates()
const
255 const std::vector<int> &getGoalStates(
int reg)
const
257 return goalStates[reg];
260 size_t numGoalStates()
const
262 return totalGoalStates;
265 size_t numGoalStates(
int reg)
const
267 return goalStates[reg].size();
270 void addGoalState(
int reg,
int id)
272 goalStates[reg].push_back(
id);
286 Layer *getSublayer(
int l)
291 const Layer *getSublayer(
int l)
const
296 void allocateSublayers()
298 if (sublayers.size())
301 for (
size_t i = 0; i < regions.size(); ++i)
302 sublayers.push_back(
new Layer(i, regions.size(), level + 1,
this));
307 return !sublayers.empty();
310 void selectRegion(
int r,
int count = 1)
314 numSelections += count;
315 selections[r] += count;
318 void connectRegion(
int r)
322 connectionPoints.addElement(r);
325 int totalSelections()
const
327 return numSelections;
330 int totalConnections()
const
332 return numConnections;
335 int connectibleRegions()
const
337 return connectionPoints.numElements();
340 int connectibleRegion(
int idx)
const
342 return connectionPoints.getElement(idx);
345 int leadAppearances(
int idx)
const
352 return numTotalLeads;
355 void markLead(
const std::vector<int> &lead)
358 for (
size_t i = 0; i < lead.size(); ++i)
363 std::vector<Region> regions;
364 std::vector<double> weights;
365 std::vector<bool> exterior;
366 std::vector<int> connections;
368 std::vector<int> selections;
369 std::vector<int> leads;
370 std::vector<std::vector<int>> goalStates;
374 std::vector<Layer *> sublayers;
376 int numSelections{0};
377 int numConnections{0};
379 int totalGoalStates{0};
380 int numTotalLeads{0};
385 void allocateLayers(
Layer *layer);
387 void updateRegionConnectivity(
const Motion *m1,
const Motion *m2,
int layer);
388 Layer *getLayer(
const std::vector<int> ®ions,
int layer);
396 void updateRegionProperties(
const std::vector<int> ®ions);
398 void updateRegionProperties(
Layer *layer,
int region);
402 bool sampleAlongLead(
Layer *layer,
const std::vector<int> &lead,
405 int steerToRegion(
Layer *layer,
int from,
int to);
406 int expandToRegion(
Layer *layer,
int from,
int to,
bool useExisting =
false);
408 bool feasibleLead(
Layer *layer,
const std::vector<int> &lead,
410 bool connectLead(
Layer *layer,
const std::vector<int> &lead, std::vector<int> &candidateRegions,
417 void computeLead(
Layer *layer, std::vector<int> &lead);
423 void getNeighbors(
int rid,
const std::vector<double> &weights,
424 std::vector<std::pair<int, double>> &neighbors)
const;
427 bool shortestPath(
int r1,
int r2, std::vector<int> &path,
const std::vector<double> &weights);
430 bool randomWalk(
int r1,
int r2, std::vector<int> &path);
432 void getGoalStates();
436 bool constructSolutionPath();
438 bool isStartState(
int idx)
const;
439 bool isGoalState(
int idx)
const;
441 void writeDebugOutput()
const;
444 Layer *topLayer_{
nullptr};
447 std::vector<Motion *> motions_;
449 std::vector<int> startMotions_;
451 std::vector<int> goalMotions_;
453 std::unordered_map<std::vector<int>,
int> goalCount_;
458 unsigned int statesConnectedInRealGraph_;
460 unsigned int maxGoalStatesPerRegion_;
461 unsigned int maxGoalStates_;
466 base::StateSamplerPtr sampler_;
481 std::vector<int> predecessors_;
482 std::vector<bool> closedList_;
484 double rand_walk_rate_{-1.0};
The exception type for ompl.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition of an abstract state.
A shared pointer wrapper for ompl::geometric::XXLDecomposition.
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
bool searchForPath(Layer *layer, const ompl::base::PlannerTerminationCondition &ptc)
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()