37 #ifndef OMPL_GEOMETRIC_PATH_HYBRIDIZATION_
38 #define OMPL_GEOMETRIC_PATH_HYBRIDIZATION_
40 #include "ompl/base/SpaceInformation.h"
41 #include "ompl/geometric/PathGeometric.h"
42 #include <boost/graph/graph_traits.hpp>
43 #include <boost/graph/adjacency_list.hpp>
54 OMPL_CLASS_FORWARD(PathHybridization);
97 std::vector<int> &indexP, std::vector<int> &indexQ)
const;
103 void print(std::ostream &out = std::cout)
const;
108 struct vertex_state_t {
109 typedef boost::vertex_property_tag kind;
112 typedef boost::adjacency_list <
113 boost::vecS, boost::vecS, boost::undirectedS,
115 boost::property < boost::vertex_predecessor_t,
unsigned long int,
116 boost::property < boost::vertex_rank_t, unsigned long int > > >,
117 boost::property < boost::edge_weight_t, double >
120 typedef boost::graph_traits<HGraph>::vertex_descriptor Vertex;
121 typedef boost::graph_traits<HGraph>::edge_descriptor Edge;
125 PathInfo(
const base::PathPtr &path) : path_(path), states_(static_cast<PathGeometric*>(path.get())->getStates()), length_(0.0)
127 vertices_.reserve(states_.size());
130 bool operator==(
const PathInfo &other)
const
132 return path_ == other.path_;
135 bool operator<(
const PathInfo &other)
const
137 return path_ < other.path_;
141 const std::vector<base::State*> &states_;
143 std::vector<Vertex> vertices_;
147 void attemptNewEdge(
const PathInfo &p,
const PathInfo &q,
int indexP,
int indexQ);
151 boost::property_map<HGraph, vertex_state_t>::type stateProperty_;
154 std::set<PathInfo> paths_;
void clear(void)
Clear all the stored paths.
void print(std::ostream &out=std::cout) const
Print information about the computed path.
PathHybridization(const base::SpaceInformationPtr &si)
The constructor needs to know about the space information of the paths it will operate on...
const base::PathPtr & getHybridPath(void) const
Get the currently computed hybrid path. computeHybridPath() needs to have been called before...
void computeHybridPath(void)
Run Dijkstra's algorithm to find out the shortest path among the mixed ones.
void matchPaths(const geometric::PathGeometric &p, const geometric::PathGeometric &q, double gapCost, std::vector< int > &indexP, std::vector< int > &indexQ) const
Given two geometric paths p and q, compute the alignment of the paths using dynamic programming in an...
Definition of an abstract state.
unsigned int recordPath(const base::PathPtr &pp, bool matchAcrossGaps)
Add a path to the hybridization. If matchAcrossGaps is true, more possible edge connections are evalu...
std::size_t pathCount(void) const
Get the number of paths that are currently considered as part of the hybridization.
Definition of a geometric path.
Given multiple geometric paths, attempt to combine them in order to obtain a shorter solution...
A boost shared pointer wrapper for ompl::base::Path.