Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2014 CNRS |
3 |
|
|
// Authors: Florent Lamiraux |
4 |
|
|
// |
5 |
|
|
|
6 |
|
|
// Redistribution and use in source and binary forms, with or without |
7 |
|
|
// modification, are permitted provided that the following conditions are |
8 |
|
|
// met: |
9 |
|
|
// |
10 |
|
|
// 1. Redistributions of source code must retain the above copyright |
11 |
|
|
// notice, this list of conditions and the following disclaimer. |
12 |
|
|
// |
13 |
|
|
// 2. Redistributions in binary form must reproduce the above copyright |
14 |
|
|
// notice, this list of conditions and the following disclaimer in the |
15 |
|
|
// documentation and/or other materials provided with the distribution. |
16 |
|
|
// |
17 |
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
18 |
|
|
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
19 |
|
|
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR |
20 |
|
|
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT |
21 |
|
|
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, |
22 |
|
|
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT |
23 |
|
|
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, |
24 |
|
|
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY |
25 |
|
|
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
26 |
|
|
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
27 |
|
|
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH |
28 |
|
|
// DAMAGE. |
29 |
|
|
|
30 |
|
|
#ifndef HPP_CORE_ROADMAP_HH |
31 |
|
|
#define HPP_CORE_ROADMAP_HH |
32 |
|
|
|
33 |
|
|
#include <hpp/core/config.hh> |
34 |
|
|
#include <hpp/core/connected-component.hh> |
35 |
|
|
#include <hpp/core/fwd.hh> |
36 |
|
|
#include <hpp/util/serialization-fwd.hh> |
37 |
|
|
#include <iostream> |
38 |
|
|
|
39 |
|
|
namespace hpp { |
40 |
|
|
namespace core { |
41 |
|
|
/// \addtogroup roadmap |
42 |
|
|
/// \{ |
43 |
|
|
|
44 |
|
|
/// Roadmap built by random path planning methods |
45 |
|
|
/// Nodes are configurations, paths are collision-free paths. |
46 |
|
|
class HPP_CORE_DLLAPI Roadmap { |
47 |
|
|
public: |
48 |
|
|
/// Return shared pointer to new instance. |
49 |
|
|
static RoadmapPtr_t create(const DistancePtr_t& distance, |
50 |
|
|
const DevicePtr_t& robot); |
51 |
|
|
|
52 |
|
|
/// Clear the roadmap by deleting nodes and edges. |
53 |
|
|
virtual void clear(); |
54 |
|
|
|
55 |
|
|
/// Add a node with given configuration |
56 |
|
|
/// \param config configuration |
57 |
|
|
/// |
58 |
|
|
/// If configuration is alread in the roadmap, return the node |
59 |
|
|
/// containing the configuration. Otherwise, create a new node and a new |
60 |
|
|
/// connected component with this node. |
61 |
|
|
NodePtr_t addNode(ConfigurationIn_t config); |
62 |
|
|
|
63 |
|
|
/// Get nearest node to a configuration in the roadmap. |
64 |
|
|
/// \param configuration configuration |
65 |
|
|
/// \param reverse if true, compute distance from given configuration to nodes |
66 |
|
|
/// in roadmap, if false from nodes in roadmap to given configuration \retval |
67 |
|
|
/// distance to the nearest node. |
68 |
|
|
NodePtr_t nearestNode(ConfigurationIn_t configuration, |
69 |
|
|
value_type& minDistance, bool reverse = false); |
70 |
|
|
|
71 |
|
|
/// Get nearest node to a configuration in a connected component. |
72 |
|
|
/// \param configuration configuration |
73 |
|
|
/// \param connectedComponent the connected component |
74 |
|
|
/// \param reverse if true, compute distance from given configuration to nodes |
75 |
|
|
/// in roadmap, if false from nodes in roadmap to given configuration \retval |
76 |
|
|
/// distance to the nearest node. |
77 |
|
|
NodePtr_t nearestNode(ConfigurationIn_t configuration, |
78 |
|
|
const ConnectedComponentPtr_t& connectedComponent, |
79 |
|
|
value_type& minDistance, bool reverse = false); |
80 |
|
|
|
81 |
|
|
/// Get nearest node to a configuration in the roadmap. |
82 |
|
|
/// \param configuration configuration |
83 |
|
|
/// \param k number of nearest nodes to return |
84 |
|
|
/// if false from nodes in roadmap to given configuration |
85 |
|
|
/// \return k nearest nodes |
86 |
|
|
Nodes_t nearestNodes(ConfigurationIn_t configuration, size_type k); |
87 |
|
|
|
88 |
|
|
/// Get nearest node to a configuration in a connected component. |
89 |
|
|
/// \param configuration configuration |
90 |
|
|
/// \param connectedComponent the connected component |
91 |
|
|
/// \param k number of nearest nodes to return |
92 |
|
|
/// if false from nodes in roadmap to given configuration |
93 |
|
|
/// \return k nearest nodes in the connected component |
94 |
|
|
Nodes_t nearestNodes(ConfigurationIn_t configuration, |
95 |
|
|
const ConnectedComponentPtr_t& connectedComponent, |
96 |
|
|
size_type k); |
97 |
|
|
|
98 |
|
|
/// Neighbor search. |
99 |
|
|
/// \copydoc NearestNeighbor::withinBall |
100 |
|
|
NodeVector_t nodesWithinBall( |
101 |
|
|
ConfigurationIn_t configuration, |
102 |
|
|
const ConnectedComponentPtr_t& connectedComponent, |
103 |
|
|
value_type maxDistance); |
104 |
|
|
|
105 |
|
|
/// Add a node and two edges |
106 |
|
|
/// \param from node from which the edge starts, |
107 |
|
|
/// \param to configuration to which the edge stops |
108 |
|
|
/// \param path path between both configurations |
109 |
|
|
/// \return node containing configuration <c>to</c>. |
110 |
|
|
/// Add the symmetric edge with reverse path. |
111 |
|
|
/// \note this function simplifies the management of connected components |
112 |
|
|
/// since it adds the new node in the connected component of |
113 |
|
|
/// <c>from</c>. |
114 |
|
|
NodePtr_t addNodeAndEdges(const NodePtr_t from, ConfigurationIn_t to, |
115 |
|
|
const PathPtr_t path); |
116 |
|
|
|
117 |
|
|
/// Add a node and one edge |
118 |
|
|
/// \param from node from which the edge starts, |
119 |
|
|
/// \param to configuration to which the edge stops |
120 |
|
|
/// \param path path between both configurations |
121 |
|
|
/// \return node containing configuration <c>to</c>. |
122 |
|
|
/// Add the oriented edge (from -> to) |
123 |
|
|
/// \note this function simplifies the management of connected components |
124 |
|
|
/// since it adds the new node in the connected component of |
125 |
|
|
/// <c>from</c>. |
126 |
|
|
NodePtr_t addNodeAndEdge(const NodePtr_t from, ConfigurationIn_t to, |
127 |
|
|
const PathPtr_t path); |
128 |
|
|
|
129 |
|
|
/// Add a node and one edge |
130 |
|
|
/// \param from configuration from which the edge starts, |
131 |
|
|
/// \param to node to which the edge stops |
132 |
|
|
/// \param path path between both configurations |
133 |
|
|
/// \return node containing configuration <c>from</c>. |
134 |
|
|
/// Add the oriented edge (from -> to) |
135 |
|
|
/// \note this function simplifies the management of connected components |
136 |
|
|
/// since it adds the new node in the connected component of |
137 |
|
|
/// <c>to</c>. |
138 |
|
|
NodePtr_t addNodeAndEdge(ConfigurationIn_t from, const NodePtr_t to, |
139 |
|
|
const PathPtr_t path); |
140 |
|
|
|
141 |
|
|
/// Add an edge between two nodes. |
142 |
|
|
EdgePtr_t addEdge(const NodePtr_t& n1, const NodePtr_t& n2, |
143 |
|
|
const PathPtr_t& path); |
144 |
|
|
|
145 |
|
|
/// Add two edges between two nodes |
146 |
|
|
/// \param from first node |
147 |
|
|
/// \param to second node |
148 |
|
|
/// \param path path going from <c>from</c> to <c>to</c>. |
149 |
|
|
/// the reverse edge is added with the reverse path. |
150 |
|
|
void addEdges(const NodePtr_t from, const NodePtr_t& to, |
151 |
|
|
const PathPtr_t& path); |
152 |
|
|
|
153 |
|
|
/// Add the nodes and edges of a roadmap into this one. |
154 |
|
|
void merge(const RoadmapPtr_t& other); |
155 |
|
|
|
156 |
|
|
/// Add a PathVector instance in the roadmap |
157 |
|
|
/// Waypoints are inserted as nodes, |
158 |
|
|
/// each elementary path is inserted as an edge |
159 |
|
|
/// \param backAndForth whether to insert the reverse path as well. |
160 |
|
|
void insertPathVector(const PathVectorPtr_t& path, bool backAndForth); |
161 |
|
|
|
162 |
|
|
/// Add a goal configuration |
163 |
|
|
/// \param config configuration |
164 |
|
|
/// If configuration is already in the roadmap, tag corresponding node |
165 |
|
|
/// as goal node. Otherwise create a new node. |
166 |
|
|
NodePtr_t addGoalNode(ConfigurationIn_t config); |
167 |
|
|
|
168 |
|
7 |
void resetGoalNodes() { goalNodes_.clear(); } |
169 |
|
|
|
170 |
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
|
9 |
void initNode(ConfigurationIn_t config) { initNode_ = addNode(config); } |
171 |
|
|
|
172 |
|
|
virtual ~Roadmap(); |
173 |
|
|
/// Check that a path exists between the initial node and one goal node. |
174 |
|
|
bool pathExists() const; |
175 |
|
78 |
const Nodes_t& nodes() const { return nodes_; } |
176 |
|
28 |
const Edges_t& edges() const { return edges_; } |
177 |
|
88 |
NodePtr_t initNode() const { return initNode_; } |
178 |
|
333 |
const NodeVector_t& goalNodes() const { return goalNodes_; } |
179 |
|
|
/// Get list of connected component of the roadmap |
180 |
|
|
const ConnectedComponents_t& connectedComponents() const; |
181 |
|
|
|
182 |
|
|
/// Get nearestNeighbor object |
183 |
|
|
NearestNeighborPtr_t nearestNeighbor(); |
184 |
|
|
|
185 |
|
|
/// Set new NearestNeighbor (roadmap must be empty) |
186 |
|
|
void nearestNeighbor(NearestNeighborPtr_t nearestNeighbor); |
187 |
|
|
|
188 |
|
|
/// \name Distance used for nearest neighbor search |
189 |
|
|
/// \{ |
190 |
|
|
/// Get distance function |
191 |
|
|
const DistancePtr_t& distance() const; |
192 |
|
|
/// \} |
193 |
|
|
/// Print roadmap in a stream |
194 |
|
|
std::ostream& print(std::ostream& os) const; |
195 |
|
|
|
196 |
|
|
protected: |
197 |
|
|
/// Constructor |
198 |
|
|
/// \param distance distance function for nearest neighbor computations |
199 |
|
|
Roadmap(const DistancePtr_t& distance, const DevicePtr_t& robot); |
200 |
|
|
|
201 |
|
3 |
Roadmap() {}; |
202 |
|
|
|
203 |
|
|
/// Add a new connected component in the roadmap. |
204 |
|
|
/// \param node node pointing to the connected component. |
205 |
|
|
/// \note The node is added in the connected component. |
206 |
|
|
void addConnectedComponent(const NodePtr_t& node); |
207 |
|
|
|
208 |
|
|
/// Give child class the opportunity to get the event |
209 |
|
|
/// "A node has been added to the roadmap" |
210 |
|
82 |
virtual void push_node(const NodePtr_t& n) { nodes_.push_back(n); } |
211 |
|
|
|
212 |
|
|
/// Give child class the opportunity to get the event |
213 |
|
|
/// "An edge has been added to the roadmap" |
214 |
|
|
/// \note you must always call the parent implementation first |
215 |
|
|
/// \code |
216 |
|
|
/// void YourRoadmap::push_edge(const EdgePtr_t e) { |
217 |
|
|
/// Roadmap::push_edge(e); |
218 |
|
|
/// // Your code here. |
219 |
|
|
/// } |
220 |
|
|
/// \endcode |
221 |
|
|
virtual void impl_addEdge(const EdgePtr_t& e); |
222 |
|
|
|
223 |
|
|
/// Node factory |
224 |
|
|
/// Reimplement the function if you want to create an instance of a |
225 |
|
|
/// child class of Node |
226 |
|
|
virtual NodePtr_t createNode(ConfigurationIn_t configuration) const; |
227 |
|
|
|
228 |
|
|
/// Store weak pointer to itself |
229 |
|
|
void init(RoadmapWkPtr_t weak); |
230 |
|
|
|
231 |
|
|
private: |
232 |
|
|
/// Add a node with given configuration |
233 |
|
|
/// \param config configuration |
234 |
|
|
/// \param connectedComponent Connected component the node will belong |
235 |
|
|
/// to. |
236 |
|
|
/// |
237 |
|
|
/// If configuration is alread in the connected component, return the node |
238 |
|
|
/// containing the configuration. Otherwise, create a new node with given |
239 |
|
|
/// connected component. |
240 |
|
|
NodePtr_t addNode(ConfigurationIn_t config, |
241 |
|
|
ConnectedComponentPtr_t connectedComponent); |
242 |
|
|
|
243 |
|
|
/// Update the graph of connected components after new connection |
244 |
|
|
/// \param cc1, cc2 the two connected components that have just been |
245 |
|
|
/// connected. |
246 |
|
|
void connect(const ConnectedComponentPtr_t& cc1, |
247 |
|
|
const ConnectedComponentPtr_t& cc2); |
248 |
|
|
|
249 |
|
|
/// Merge two connected components |
250 |
|
|
/// \param cc1 the connected component to merge into |
251 |
|
|
/// \param the connected components to merge into cc1. |
252 |
|
|
void merge(const ConnectedComponentPtr_t& cc1, |
253 |
|
|
ConnectedComponent::RawPtrs_t& ccs); |
254 |
|
|
|
255 |
|
|
const DistancePtr_t distance_; |
256 |
|
|
ConnectedComponents_t connectedComponents_; |
257 |
|
|
Nodes_t nodes_; |
258 |
|
|
Edges_t edges_; |
259 |
|
|
NodePtr_t initNode_; |
260 |
|
|
NodeVector_t goalNodes_; |
261 |
|
|
NearestNeighborPtr_t nearestNeighbor_; |
262 |
|
|
RoadmapWkPtr_t weak_; |
263 |
|
|
|
264 |
|
|
HPP_SERIALIZABLE(); |
265 |
|
|
}; // class Roadmap |
266 |
|
|
std::ostream& operator<<(std::ostream& os, const Roadmap& r); |
267 |
|
|
/// \} |
268 |
|
|
} // namespace core |
269 |
|
|
} // namespace hpp |
270 |
|
|
#endif // HPP_CORE_ROADMAP_HH |
271 |
|
|
|