GCC Code Coverage Report

Directory: ./
File: include/hpp/core/roadmap.hh
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 8 8 100.0%
Branches: 1 2 50.0%
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 9 void resetGoalNodes() { goalNodes_.clear(); }
169
170
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 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 91 NodePtr_t initNode() const { return initNode_; }
178 363 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