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  OMPL_CLASS_FORWARD(PathHybridization);
56 
71  {
72  public:
73 
77 
79  const base::PathPtr& getHybridPath() const;
80 
82  void computeHybridPath();
83 
86  unsigned int recordPath(const base::PathPtr &pp, bool matchAcrossGaps);
87 
89  std::size_t pathCount() 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();
101 
103  void print(std::ostream &out = std::cout) const;
104 
106  const std::string& getName() const;
107 
108  private:
109 
111  struct vertex_state_t {
112  typedef boost::vertex_property_tag kind;
113  };
114 
115  typedef boost::adjacency_list <
116  boost::vecS, boost::vecS, boost::undirectedS,
117  boost::property < vertex_state_t, base::State*,
118  boost::property < boost::vertex_predecessor_t, unsigned long int,
119  boost::property < boost::vertex_rank_t, unsigned long int > > >,
120  boost::property < boost::edge_weight_t, double >
121  > HGraph;
122 
123  typedef boost::graph_traits<HGraph>::vertex_descriptor Vertex;
124  typedef boost::graph_traits<HGraph>::edge_descriptor Edge;
125 
126  struct PathInfo
127  {
128  PathInfo(const base::PathPtr &path) : path_(path), states_(static_cast<PathGeometric*>(path.get())->getStates()), length_(0.0)
129  {
130  vertices_.reserve(states_.size());
131  }
132 
133  bool operator==(const PathInfo &other) const
134  {
135  return path_ == other.path_;
136  }
137 
138  bool operator<(const PathInfo &other) const
139  {
140  return path_ < other.path_;
141  }
142 
143  base::PathPtr path_;
144  const std::vector<base::State*> &states_;
145  double length_;
146  std::vector<Vertex> vertices_;
147  };
149 
150  void attemptNewEdge(const PathInfo &p, const PathInfo &q, int indexP, int indexQ);
151 
153  HGraph g_;
154  boost::property_map<HGraph, vertex_state_t>::type stateProperty_;
155  Vertex root_;
156  Vertex goal_;
157  std::set<PathInfo> paths_;
158  base::PathPtr hpath_;
159 
161  std::string name_;
162  };
163  }
164 }
165 
166 #endif
const std::string & getName() const
Get the name of the algorithm.
void clear()
Clear all the stored paths.
void computeHybridPath()
Run Dijkstra's algorithm to find out the shortest path among the mixed ones.
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...
Main namespace. Contains everything in this library.
Definition: Cost.h:42
const base::PathPtr & getHybridPath() const
Get the currently computed hybrid path. computeHybridPath() needs to have been called before...
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...
A boost shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:50
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() const
Get the number of paths that are currently considered as part of the hybridization.
Definition of a geometric path.
Definition: PathGeometric.h:60
Given multiple geometric paths, attempt to combine them in order to obtain a shorter solution...
A boost shared pointer wrapper for ompl::base::Path.