37 #ifndef OMPL_CONTROL_PLANNERS_SYCLOP_SYCLOP_
38 #define OMPL_CONTROL_PLANNERS_SYCLOP_SYCLOP_
40 #include <boost/graph/astar_search.hpp>
41 #include <boost/graph/graph_traits.hpp>
42 #include <boost/graph/adjacency_list.hpp>
43 #include <boost/unordered_map.hpp>
44 #include "ompl/control/planners/PlannerIncludes.h"
45 #include "ompl/control/planners/syclop/Decomposition.h"
46 #include "ompl/control/planners/syclop/GridDecomposition.h"
47 #include "ompl/datastructures/PDF.h"
91 typedef boost::function<void(int, int, std::vector<int>&)>
LeadComputeFn;
124 virtual void setup(
void);
126 virtual void clear(
void);
232 static const int NUM_FREEVOL_SAMPLES = 100000;
233 static const int COVGRID_LENGTH = 128;
234 static const int NUM_REGION_EXPANSIONS = 100;
235 static const int NUM_TREE_SELECTIONS = 50;
239 static const double PROB_ABANDON_LEAD_EARLY ;
240 static const double PROB_KEEP_ADDING_TO_AVAIL ;
241 static const double PROB_SHORTEST_PATH ;
246 #pragma pack(push, 4) // push default byte alignment to stack and align the following structure to 4 byte boundary
273 #pragma pack (pop) // Restoring default byte alignment
275 #pragma pack(push, 4) // push default byte alignment to stack and align the following structure to 4 byte boundary
312 #pragma pack (pop) // Restoring default byte alignment
314 #pragma pack(push, 4) // push default byte alignment to stack and align the following structure to 4 byte boundary
348 #pragma pack (pop) // Restoring default byte alignment
360 return graph_[boost::vertex(rid,graph_)];
400 virtual ~CoverageGrid()
406 virtual void project(
const base::State* s, std::vector<double>& coord)
const
420 typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, Region, Adjacency> RegionGraph;
421 typedef boost::graph_traits<RegionGraph>::vertex_descriptor Vertex;
422 typedef boost::graph_traits<RegionGraph>::vertex_iterator VertexIter;
423 typedef boost::property_map<RegionGraph, boost::vertex_index_t>::type VertexIndexMap;
424 typedef boost::graph_traits<RegionGraph>::edge_iterator EdgeIter;
427 friend class DecompositionHeuristic;
429 class DecompositionHeuristic :
public boost::astar_heuristic<RegionGraph, double>
432 DecompositionHeuristic(
const Syclop* s,
const Region& goal) : syclop(s), goalRegion(goal)
436 double operator()(Vertex v)
438 const Region& region = syclop->getRegionFromIndex(v);
439 return region.alpha*goalRegion.alpha;
443 const Region& goalRegion;
446 struct found_goal {};
448 class GoalVisitor :
public boost::default_astar_visitor
451 GoalVisitor(
const unsigned int goal) : goalRegion(goal)
454 void examine_vertex(Vertex v,
const RegionGraph& g)
460 const unsigned int goalRegion;
468 int sampleUniform(
void)
472 return regions.sample(rng.uniform01());
474 void insert(
const int r)
476 if (regToElem.count(r) == 0)
477 regToElem[r] = regions.add(r, 1);
480 PDF<int>::Element* elem = regToElem[r];
481 regions.update(elem, regions.getWeight(elem)+1);
489 std::size_t size(
void)
const
491 return regions.size();
495 return regions.empty();
500 boost::unordered_map<const int, PDF<int>::Element*> regToElem;
505 void initRegion(Region& r);
508 void setupRegionEstimates(
void);
511 void updateRegion(Region& r);
514 void initEdge(Adjacency& a,
const Region* source,
const Region* target);
517 void setupEdgeEstimates(
void);
520 void updateEdge(Adjacency& a);
524 bool updateCoverageEstimate(Region& r,
const base::State* s);
528 bool updateConnectionEstimate(
const Region& c,
const Region& d,
const base::State* s);
532 void buildGraph(
void);
535 void clearGraphDetails(
void);
538 int selectRegion(
void);
541 void computeAvailableRegions(
void);
544 void defaultComputeLead(
int startRegion,
int goalRegion, std::vector<int>& lead);
547 double defaultEdgeCost(
int r,
int s);
552 std::vector<int> lead_;
556 std::vector<EdgeCostFactorFn> edgeCostFactors_;
558 CoverageGrid covGrid_;
564 boost::unordered_map<std::pair<int,int>, Adjacency*> regionsToEdge_;
566 unsigned int numMotions_;
568 RegionSet startRegions_;
570 RegionSet goalRegions_;