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  OMPL_CLASS_FORWARD(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
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...
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(void) const
Get the number of paths that are currently considered as part of the hybridization.
Definition of a geometric path.
Definition: PathGeometric.h:55
Given multiple geometric paths, attempt to combine them in order to obtain a shorter solution...
A boost shared pointer wrapper for ompl::base::Path.