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  {
52 
53  OMPL_CLASS_FORWARD(PathHybridization);
55 
70  {
71  public:
75 
77  const base::PathPtr &getHybridPath() const;
78 
80  void computeHybridPath();
81 
85  unsigned int recordPath(const base::PathPtr &pp, bool matchAcrossGaps);
86 
88  std::size_t pathCount() const;
89 
95  void matchPaths(const geometric::PathGeometric &p, const geometric::PathGeometric &q, double gapCost,
96  std::vector<int> &indexP, std::vector<int> &indexQ) const;
97 
99  void clear();
100 
102  void print(std::ostream &out = std::cout) const;
103 
105  const std::string &getName() const;
106 
107  private:
109  struct vertex_state_t
110  {
111  typedef boost::vertex_property_tag kind;
112  };
113 
114  typedef boost::adjacency_list<
115  boost::vecS, boost::vecS, boost::undirectedS,
116  boost::property<vertex_state_t, base::State *,
117  boost::property<boost::vertex_predecessor_t, unsigned long int,
118  boost::property<boost::vertex_rank_t, unsigned long int>>>,
119  boost::property<boost::edge_weight_t, double>>
120  HGraph;
121 
122  typedef boost::graph_traits<HGraph>::vertex_descriptor Vertex;
123  typedef boost::graph_traits<HGraph>::edge_descriptor Edge;
124 
125  struct PathInfo
126  {
127  PathInfo(const base::PathPtr &path)
128  : 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
void clear()
Clear all the stored paths.
const base::PathPtr & getHybridPath() const
Get the currently computed hybrid path. computeHybridPath() needs to have been called before...
const std::string & getName() const
Get the name of the algorithm.
void computeHybridPath()
Run Dijkstra&#39;s algorithm to find out the shortest path among the mixed ones.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:49
void print(std::ostream &out=std::cout) const
Print information about the computed path.
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.
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 a geometric path.
Definition: PathGeometric.h:60
PathHybridization(base::SpaceInformationPtr si)
The constructor needs to know about the space information of the paths it will operate on...
Given multiple geometric paths, attempt to combine them in order to obtain a shorter solution...
A shared pointer wrapper for ompl::base::Path.