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/base/OptimizationObjective.h"
42 #include "ompl/geometric/PathGeometric.h"
43 #include <boost/graph/graph_traits.hpp>
44 #include <boost/graph/adjacency_list.hpp>
45 #include <iostream>
46 #include <set>
47 
48 namespace ompl
49 {
50  namespace geometric
51  {
53 
54  OMPL_CLASS_FORWARD(PathHybridization);
56 
71  {
72  public:
75 
79 
81 
83  const base::PathPtr &getHybridPath() const;
84 
86  void computeHybridPath();
87 
91  unsigned int recordPath(const base::PathPtr &pp, bool matchAcrossGaps);
92 
94  std::size_t pathCount() const;
95 
101  void matchPaths(const geometric::PathGeometric &p, const geometric::PathGeometric &q, double gapValue,
102  std::vector<int> &indexP, std::vector<int> &indexQ) const;
103 
105  void clear();
106 
108  void print(std::ostream &out = std::cout) const;
109 
111  const std::string &getName() const;
112 
113  private:
115  struct vertex_state_t
116  {
117  typedef boost::vertex_property_tag kind;
118  };
119 
120  typedef boost::adjacency_list<
121  boost::vecS, boost::vecS, boost::undirectedS,
122  boost::property<vertex_state_t, base::State *,
123  boost::property<boost::vertex_predecessor_t, unsigned long int,
124  boost::property<boost::vertex_rank_t, base::Cost>>>,
125  boost::property<boost::edge_weight_t, base::Cost>>
126  HGraph;
127 
128  typedef boost::graph_traits<HGraph>::vertex_descriptor Vertex;
129  typedef boost::graph_traits<HGraph>::edge_descriptor Edge;
130 
131  struct PathInfo
132  {
133  PathInfo(const base::PathPtr &path)
134  : path_(path), states_(static_cast<PathGeometric *>(path.get())->getStates()), cost_(base::Cost())
135  {
136  vertices_.reserve(states_.size());
137  }
138 
139  bool operator==(const PathInfo &other) const
140  {
141  return path_ == other.path_;
142  }
143 
144  bool operator<(const PathInfo &other) const
145  {
146  return path_ < other.path_;
147  }
148 
149  base::PathPtr path_;
150  const std::vector<base::State *> &states_;
151  base::Cost cost_;
152  std::vector<Vertex> vertices_;
153  };
155 
156  void attemptNewEdge(const PathInfo &p, const PathInfo &q, int indexP, int indexQ);
157 
160  HGraph g_;
161  boost::property_map<HGraph, vertex_state_t>::type stateProperty_;
162  Vertex root_;
163  Vertex goal_;
164  std::set<PathInfo> paths_;
165  base::PathPtr hpath_;
166 
168  std::string name_;
169  };
170  }
171 }
172 
173 #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 lowest-cost 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.
A shared pointer wrapper for ompl::base::OptimizationObjective.
void matchPaths(const geometric::PathGeometric &p, const geometric::PathGeometric &q, double gapValue, 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...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
A shared pointer wrapper for ompl::base::Path.