00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef OMPL_GEOMETRIC_PATH_HYBRIDIZATION_
00038 #define OMPL_GEOMETRIC_PATH_HYBRIDIZATION_
00039
00040 #include "ompl/base/SpaceInformation.h"
00041 #include "ompl/geometric/PathGeometric.h"
00042 #include <boost/graph/graph_traits.hpp>
00043 #include <boost/graph/adjacency_list.hpp>
00044 #include <iostream>
00045 #include <set>
00046
00047 namespace ompl
00048 {
00049 namespace geometric
00050 {
00051
00053
00054 ClassForward(PathHybridization);
00056
00070 class PathHybridization
00071 {
00072 public:
00073
00075 PathHybridization(const base::SpaceInformationPtr &si);
00076 ~PathHybridization(void);
00077
00079 const base::PathPtr& getHybridPath(void) const;
00080
00082 void computeHybridPath(void);
00083
00086 unsigned int recordPath(const base::PathPtr &pp, bool matchAcrossGaps);
00087
00089 std::size_t pathCount(void) const;
00090
00096 void matchPaths(const geometric::PathGeometric &p, const geometric::PathGeometric &q, double gapCost,
00097 std::vector<int> &indexP, std::vector<int> &indexQ) const;
00098
00100 void clear(void);
00101
00103 void print(std::ostream &out = std::cout) const;
00104
00105 private:
00106
00108 struct vertex_state_t {
00109 typedef boost::vertex_property_tag kind;
00110 };
00111
00112 typedef boost::adjacency_list <
00113 boost::vecS, boost::vecS, boost::undirectedS,
00114 boost::property < vertex_state_t, base::State*,
00115 boost::property < boost::vertex_predecessor_t, unsigned long int,
00116 boost::property < boost::vertex_rank_t, unsigned long int > > >,
00117 boost::property < boost::edge_weight_t, double >
00118 > HGraph;
00119
00120 typedef boost::graph_traits<HGraph>::vertex_descriptor Vertex;
00121 typedef boost::graph_traits<HGraph>::edge_descriptor Edge;
00122
00123 struct PathInfo
00124 {
00125 PathInfo(const base::PathPtr &path) : path_(path), states_(static_cast<PathGeometric*>(path.get())->getStates()), length_(0.0)
00126 {
00127 vertices_.reserve(states_.size());
00128 }
00129
00130 bool operator==(const PathInfo &other) const
00131 {
00132 return path_ == other.path_;
00133 }
00134
00135 bool operator<(const PathInfo &other) const
00136 {
00137 return path_ < other.path_;
00138 }
00139
00140 base::PathPtr path_;
00141 const std::vector<base::State*> &states_;
00142 double length_;
00143 std::vector<Vertex> vertices_;
00144 };
00146
00147 void attemptNewEdge(const PathInfo &p, const PathInfo &q, int indexP, int indexQ);
00148
00149 base::SpaceInformationPtr si_;
00150 HGraph g_;
00151 boost::property_map<HGraph, vertex_state_t>::type stateProperty_;
00152 Vertex root_;
00153 Vertex goal_;
00154 std::set<PathInfo> paths_;
00155 base::PathPtr hpath_;
00156
00157 msg::Interface msg_;
00158 };
00159 }
00160 }
00161
00162 #endif