All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
PathHybridization.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #ifndef OMPL_GEOMETRIC_PATH_HYBRIDIZATION_
38 #define OMPL_GEOMETRIC_PATH_HYBRIDIZATION_
39 
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>
44 #include <iostream>
45 #include <set>
46 
47 namespace ompl
48 {
49  namespace geometric
50  {
51 
53 
54  ClassForward(PathHybridization);
56 
71  {
72  public:
73 
76  ~PathHybridization(void);
77 
79  const base::PathPtr& getHybridPath(void) const;
80 
82  void computeHybridPath(void);
83 
86  unsigned int recordPath(const base::PathPtr &pp, bool matchAcrossGaps);
87 
89  std::size_t pathCount(void) const;
90 
96  void matchPaths(const geometric::PathGeometric &p, const geometric::PathGeometric &q, double gapCost,
97  std::vector<int> &indexP, std::vector<int> &indexQ) const;
98 
100  void clear(void);
101 
103  void print(std::ostream &out = std::cout) const;
104 
105  private:
106 
108  struct vertex_state_t {
109  typedef boost::vertex_property_tag kind;
110  };
111 
112  typedef boost::adjacency_list <
113  boost::vecS, boost::vecS, boost::undirectedS,
114  boost::property < vertex_state_t, base::State*,
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 >
118  > HGraph;
119 
120  typedef boost::graph_traits<HGraph>::vertex_descriptor Vertex;
121  typedef boost::graph_traits<HGraph>::edge_descriptor Edge;
122 
123  struct PathInfo
124  {
125  PathInfo(const base::PathPtr &path) : path_(path), states_(static_cast<PathGeometric*>(path.get())->getStates()), length_(0.0)
126  {
127  vertices_.reserve(states_.size());
128  }
129 
130  bool operator==(const PathInfo &other) const
131  {
132  return path_ == other.path_;
133  }
134 
135  bool operator<(const PathInfo &other) const
136  {
137  return path_ < other.path_;
138  }
139 
140  base::PathPtr path_;
141  const std::vector<base::State*> &states_;
142  double length_;
143  std::vector<Vertex> vertices_;
144  };
146 
147  void attemptNewEdge(const PathInfo &p, const PathInfo &q, int indexP, int indexQ);
148 
150  HGraph g_;
151  boost::property_map<HGraph, vertex_state_t>::type stateProperty_;
152  Vertex root_;
153  Vertex goal_;
154  std::set<PathInfo> paths_;
155  base::PathPtr hpath_;
156  };
157  }
158 }
159 
160 #endif