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:
74  PathHybridization(base::SpaceInformationPtr si);
75 
78  PathHybridization(base::SpaceInformationPtr si, base::OptimizationObjectivePtr obj);
79 
81 
83  const geometric::PathGeometricPtr &getHybridPath() const;
84 
86  void computeHybridPath();
87 
91  unsigned int recordPath(const geometric::PathGeometricPtr &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  using kind = boost::vertex_property_tag;
118  };
119 
120  using HGraph = 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 
127  using Vertex = boost::graph_traits<HGraph>::vertex_descriptor;
128  using Edge = boost::graph_traits<HGraph>::edge_descriptor;
129 
130  struct PathInfo
131  {
132  PathInfo(const geometric::PathGeometricPtr &path)
133  : path_(path), states_(path->getStates()), cost_(base::Cost())
134  {
135  vertices_.reserve(states_.size());
136  }
137 
138  bool operator==(const PathInfo &other) const
139  {
140  return path_ == other.path_;
141  }
142 
143  bool operator<(const PathInfo &other) const
144  {
145  return path_ < other.path_;
146  }
147 
148  geometric::PathGeometricPtr path_;
149  const std::vector<base::State *> &states_;
150  base::Cost cost_;
151  std::vector<Vertex> vertices_;
152  };
154 
155  void attemptNewEdge(const PathInfo &p, const PathInfo &q, int indexP, int indexQ);
156 
157  base::SpaceInformationPtr si_;
158  base::OptimizationObjectivePtr obj_;
159  HGraph g_;
160  boost::property_map<HGraph, vertex_state_t>::type stateProperty_;
161  Vertex root_;
162  Vertex goal_;
163  std::set<PathInfo> paths_;
164  geometric::PathGeometricPtr hpath_;
165 
167  std::string name_;
168  };
169  }
170 }
171 
172 #endif
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
Definition of an abstract state.
Definition: State.h:50
Definition of a geometric path.
Definition: PathGeometric.h:66
Given multiple geometric paths, attempt to combine them in order to obtain a shorter solution.
const std::string & getName() const
Get the name of the algorithm.
const geometric::PathGeometricPtr & getHybridPath() const
Get the currently computed hybrid path. computeHybridPath() needs to have been called before.
void clear()
Clear all the stored paths.
void computeHybridPath()
Run Dijkstra's algorithm to find out the lowest-cost path among the mixed ones.
void print(std::ostream &out=std::cout) const
Print information about the computed path.
PathHybridization(base::SpaceInformationPtr si)
The constructor needs to know about the space information of the paths it will operate on.
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...
unsigned int recordPath(const geometric::PathGeometricPtr &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.
Main namespace. Contains everything in this library.
Definition: AppBase.h:22