Program Listing for File XXL.h
↰ Return to documentation for file (src/ompl/geometric/planners/xxl/XXL.h)
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2015, Rice University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Rice University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Ryan Luna */
#ifndef OMPL_GEOMETRIC_PLANNERS_XXL_XXL_
#define OMPL_GEOMETRIC_PLANNERS_XXL_XXL_
#include <thread>
#include <unordered_map>
#include "ompl/util/Hash.h"
#include "ompl/datastructures/AdjacencyList.h"
#include "ompl/geometric/planners/PlannerIncludes.h"
#include "ompl/geometric/planners/xxl/XXLDecomposition.h"
namespace ompl
{
namespace geometric
{
class XXL : public base::Planner
{
public:
// Standard planner constructor. Must call setDecomposition before calling solve()
XXL(const base::SpaceInformationPtr &si);
// Initialize HiLo with the given decomposition
XXL(const base::SpaceInformationPtr &si, const XXLDecompositionPtr &decomp);
virtual ~XXL();
virtual void getPlannerData(base::PlannerData &data) const;
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc);
virtual void clear();
virtual void setup();
void setDecomposition(const XXLDecompositionPtr &decomp);
double getRandWalkRate() const
{
return rand_walk_rate_;
}
void setRandWalkRate(double rate)
{
if (rate < 0.0 || rate > 1.0)
throw;
rand_walk_rate_ = rate;
}
protected:
// Quickly insert, check membership, and grab a unique integer from a range [0, max)
class PerfectSet
{
public:
PerfectSet(std::size_t max)
{
exists.assign(max, false);
elements.reserve(max / 10); // reserve, but do not "allocate" the space
}
std::size_t numElements() const
{
return elements.size();
}
bool isElement(unsigned int val) const
{
if (val >= (unsigned int)exists.size())
return false;
return exists[val];
}
bool addElement(unsigned int val)
{
if (val >= (unsigned int)exists.size() || exists[val])
return false;
elements.push_back(val);
exists[val] = true;
return true;
}
int getElement(std::size_t idx) const
{
return elements[idx];
}
protected:
std::vector<bool> exists;
std::vector<unsigned int> elements;
};
struct Motion
{
base::State *state;
std::vector<int> levels;
int index;
};
struct Region
{
std::vector<int> allMotions;
std::vector<int> motionsInTree; // subset of allMotions that are connected to the tree
};
class Layer
{
public:
Layer(int _id, int numRegions, int lvl, Layer *_parent)
: regions(numRegions)
, weights(numRegions, 0.5)
, exterior(numRegions, true)
, connections(numRegions, 0)
, selections(numRegions, 0)
, leads(numRegions, 0)
, goalStates(numRegions, std::vector<int>())
, connectionPoints(numRegions)
, regionGraph(new AdjacencyList(numRegions))
, level(lvl)
, id(_id)
, parent(_parent)
{
}
~Layer()
{
delete regionGraph;
for (auto &sublayer : sublayers)
delete sublayer;
}
size_t numRegions() const
{
return regions.size();
}
int getLevel() const
{
return level;
}
Layer *getParent() const
{
return parent;
}
int getID() const
{
return id;
}
Region &getRegion(int r)
{
if (r < 0 || r >= (int)regions.size())
{
OMPL_ERROR("Requested region %d, but there are only %lu regions", r, regions.size());
throw ompl::Exception("Region out of bounds");
}
return regions[r];
}
const Region &getRegion(int r) const
{
if (r < 0 || r >= (int)regions.size())
{
OMPL_ERROR("Requested region %d, but there are only %lu regions", r, regions.size());
throw ompl::Exception("Region out of bounds");
}
return regions[r];
}
const std::vector<double> &getWeights() const
{
return weights;
}
std::vector<double> &getWeights()
{
return weights;
}
const std::vector<bool> &getExterior() const
{
return exterior;
}
std::vector<bool> &getExterior()
{
return exterior;
}
const std::vector<int> &getConnections() const
{
return connections;
}
const std::vector<int> &getSelections() const
{
return selections;
}
const std::vector<std::vector<int>> &getGoalStates() const
{
return goalStates;
}
const std::vector<int> &getGoalStates(int reg) const
{
return goalStates[reg];
}
size_t numGoalStates() const
{
return totalGoalStates;
}
size_t numGoalStates(int reg) const
{
return goalStates[reg].size();
}
void addGoalState(int reg, int id)
{
goalStates[reg].push_back(id);
totalGoalStates++;
}
AdjacencyList &getRegionGraph()
{
return *regionGraph;
}
const AdjacencyList &getRegionGraph() const
{
return *regionGraph;
}
Layer *getSublayer(int l)
{
return sublayers[l];
}
const Layer *getSublayer(int l) const
{
return sublayers[l];
}
void allocateSublayers()
{
if (sublayers.size())
throw;
for (size_t i = 0; i < regions.size(); ++i)
sublayers.push_back(new Layer(i, regions.size(), level + 1, this));
}
bool hasSublayers()
{
return !sublayers.empty();
}
void selectRegion(int r, int count = 1)
{
// numSelections++;
// selections[r]++;
numSelections += count;
selections[r] += count;
}
void connectRegion(int r)
{
numConnections++;
connections[r]++;
connectionPoints.addElement(r);
}
int totalSelections() const
{
return numSelections;
}
int totalConnections() const
{
return numConnections;
}
int connectibleRegions() const
{
return connectionPoints.numElements();
}
int connectibleRegion(int idx) const
{
return connectionPoints.getElement(idx);
}
int leadAppearances(int idx) const
{
return leads[idx];
}
int numLeads() const
{
return numTotalLeads;
}
void markLead(const std::vector<int> &lead)
{
numTotalLeads++;
for (size_t i = 0; i < lead.size(); ++i)
leads[lead[i]]++;
}
protected:
std::vector<Region> regions; // The list of regions in this layer
std::vector<double> weights; // Weight for each region
std::vector<bool> exterior; // Exterior status for the regions in this layer
std::vector<int> connections; // Number of times the search has tried internal connections in this
// region
std::vector<int> selections; // Number of times the search has selected this region for expansion
std::vector<int> leads; // Number of times each region has appeared in a lead
std::vector<std::vector<int>> goalStates; // A list of goal states in each region
PerfectSet connectionPoints; // The set of regions we have tried to do internal connections on
AdjacencyList *regionGraph; // The connectivity of regions in this layer
std::vector<Layer *> sublayers; // The layers descending from this layer
int level; // The level of this layer in the hierarchy (0 is top)
int numSelections{0}; // The total number of selections in this layer
int numConnections{0}; // The total number of internal connection attempts in this layer
int id;
int totalGoalStates{0}; // The total number of goal states in this layer
int numTotalLeads{0};
Layer *parent;
};
void freeMemory();
void allocateLayers(Layer *layer);
void updateRegionConnectivity(const Motion *m1, const Motion *m2, int layer);
Layer *getLayer(const std::vector<int> ®ions, int layer);
int addState(const base::State *state);
int addThisState(base::State *state); // add state using this state memory, no copy
int addGoalState(const base::State *state);
int addStartState(const base::State *state);
// Update the various statistics for the regions and their subregions in the vector
void updateRegionProperties(const std::vector<int> ®ions);
// Update the various statistics for the region specified
void updateRegionProperties(Layer *layer, int region);
// Sample states uniformly at random in the given layer until ptc is triggered
void sampleStates(Layer *layer, const ompl::base::PlannerTerminationCondition &ptc);
bool sampleAlongLead(Layer *layer, const std::vector<int> &lead,
const ompl::base::PlannerTerminationCondition &ptc);
int steerToRegion(Layer *layer, int from, int to);
int expandToRegion(Layer *layer, int from, int to, bool useExisting = false);
bool feasibleLead(Layer *layer, const std::vector<int> &lead,
const ompl::base::PlannerTerminationCondition &ptc);
bool connectLead(Layer *layer, const std::vector<int> &lead, std::vector<int> &candidateRegions,
const ompl::base::PlannerTerminationCondition &ptc);
void connectRegion(Layer *layer, int region, const base::PlannerTerminationCondition &ptc);
void connectRegions(Layer *layer, int r1, int r2, const base::PlannerTerminationCondition &ptc,
bool all = false);
// Compute a new lead in the given decomposition layer from start to goal
void computeLead(Layer *layer, std::vector<int> &lead);
// Search for a solution path in the given layer
bool searchForPath(Layer *layer, const ompl::base::PlannerTerminationCondition &ptc);
// Return a list of neighbors and the edge weights from rid
void getNeighbors(int rid, const std::vector<double> &weights,
std::vector<std::pair<int, double>> &neighbors) const;
// Shortest (weight) path from r1 to r2
bool shortestPath(int r1, int r2, std::vector<int> &path, const std::vector<double> &weights);
// Compute a path from r1 to r2 via a random walk
bool randomWalk(int r1, int r2, std::vector<int> &path);
void getGoalStates();
// Thread that gets us goal states
void getGoalStates(const base::PlannerTerminationCondition &ptc);
bool constructSolutionPath();
bool isStartState(int idx) const;
bool isGoalState(int idx) const;
void writeDebugOutput() const;
// Root layer of the decomposition data
Layer *topLayer_{nullptr};
// Raw storage for all motions explored during search
std::vector<Motion *> motions_;
// Indexes into motions_ for start states
std::vector<int> startMotions_;
// Index into motions_ for goal states
std::vector<int> goalMotions_;
// The number of goal states in each decomposition cell
std::unordered_map<std::vector<int>, int> goalCount_;
base::State *xstate_;
// The number of states in realGraph that have verified edges in the graph
unsigned int statesConnectedInRealGraph_;
unsigned int maxGoalStatesPerRegion_;
unsigned int maxGoalStates_;
// Random number generator
RNG rng_;
base::StateSamplerPtr sampler_;
// A decomposition of the search space
XXLDecompositionPtr decomposition_;
// A lazily constructed graph where edges between states have not been collision checked
AdjacencyList lazyGraph_;
// The verified graph where all edges have been collision checked. An edge in this graph
// should not exist in lazyGraph
AdjacencyList realGraph_;
// Variable for the goal state sampling thread
bool kill_{false};
// Scratch space for shortest path computation
std::vector<int> predecessors_;
std::vector<bool> closedList_;
double rand_walk_rate_{-1.0};
};
} // namespace geometric
} // namespace ompl
#endif