Loading...
Searching...
No Matches
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
48namespace 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.
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.