roadmap.hh 11.5 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#ifndef HPP_CORE_ROADMAP_HH
# define HPP_CORE_ROADMAP_HH

22
# include <iostream>
23

24
25
26
# include <hpp/core/fwd.hh>
# include <hpp/core/config.hh>

27
# include <hpp/core/connected-component.hh>
28
# include <hpp/util/serialization-fwd.hh>
29

30
31
namespace hpp {
  namespace core {
Florent Lamiraux's avatar
Florent Lamiraux committed
32
33
34
    /// \addtogroup roadmap
    /// \{

35
36
37
38
39
    /// Roadmap built by random path planning methods
    /// Nodes are configurations, paths are collision-free paths.
    class HPP_CORE_DLLAPI Roadmap {
    public:
      /// Return shared pointer to new instance.
40
      static RoadmapPtr_t create (const DistancePtr_t& distance, const DevicePtr_t& robot);
Joseph Mirabel's avatar
Joseph Mirabel committed
41
42
43
44

      /// Clear the roadmap by deleting nodes and edges.
      virtual void clear ();

45
46
47
      /// Add a node with given configuration
      /// \param config configuration
      ///
48
49
50
51
52
      /// If configuration is alread in the roadmap, return the node
      /// containing the configuration. Otherwise, create a new node and a new
      /// connected component with this node.
      NodePtr_t addNode (const ConfigurationPtr_t& config);

53
54
55
56
57
      NodePtr_t addNode (const Configuration_t& config)
      {
        return addNode (ConfigurationPtr_t (new Configuration_t(config)));
      }

58
59
      /// Get nearest node to a configuration in the roadmap.
      /// \param configuration configuration
60
61
      /// \param reverse if true, compute distance from given configuration to nodes in roadmap,
      /// if false from nodes in roadmap to given configuration
62
      /// \retval distance to the nearest node.
63
      NodePtr_t nearestNode (const Configuration_t& configuration,
64
           value_type& minDistance, bool reverse = false);
65

66
67
68
69
70
71
      NodePtr_t nearestNode (const ConfigurationPtr_t& configuration,
           value_type& minDistance, bool reverse = false)
      {
        return nearestNode (*configuration, minDistance, reverse);
      }

72
73
74
      /// Get nearest node to a configuration in a connected component.
      /// \param configuration configuration
      /// \param connectedComponent the connected component
75
76
      /// \param reverse if true, compute distance from given configuration to nodes in roadmap,
      /// if false from nodes in roadmap to given configuration
77
      /// \retval distance to the nearest node.
78
      NodePtr_t nearestNode (const Configuration_t& configuration,
79
			     const ConnectedComponentPtr_t& connectedComponent,
80
           value_type& minDistance, bool reverse = false);
81

82
83
84
85
86
87
88
      NodePtr_t nearestNode (const ConfigurationPtr_t& configuration,
			     const ConnectedComponentPtr_t& connectedComponent,
           value_type& minDistance, bool reverse = false)
      {
        return nearestNode (*configuration, connectedComponent, minDistance, reverse);
      }

89
90
91
92
      /// Get nearest node to a configuration in the roadmap.
      /// \param configuration configuration
      /// \param k number of nearest nodes to return
      /// if false from nodes in roadmap to given configuration
93
      /// \return k nearest nodes
94
      Nodes_t nearestNodes (const Configuration_t& configuration,
95
96
                            size_type k);

97
98
99
100
101
102
      Nodes_t nearestNodes (const ConfigurationPtr_t& configuration,
                            size_type k)
      {
        return nearestNodes (*configuration, k);
      }

103
104
105
      /// Get nearest node to a configuration in a connected component.
      /// \param configuration configuration
      /// \param connectedComponent the connected component
106
      /// \param k number of nearest nodes to return
107
      /// if false from nodes in roadmap to given configuration
108
      /// \return k nearest nodes in the connected component
109
      Nodes_t nearestNodes (const Configuration_t& configuration,
110
111
112
113
                            const ConnectedComponentPtr_t&
                            connectedComponent,
                            size_type k);

114
115
116
117
118
119
120
121
      Nodes_t nearestNodes (const ConfigurationPtr_t& configuration,
                            const ConnectedComponentPtr_t&
                            connectedComponent,
                            size_type k)
      {
        return nearestNodes (*configuration, connectedComponent, k);
      }

Joseph Mirabel's avatar
Joseph Mirabel committed
122
123
124
125
126
127
      /// Neighbor search.
      /// \copydoc NearestNeighbor::withinBall
      NodeVector_t nodesWithinBall (const Configuration_t& configuration,
                                    const ConnectedComponentPtr_t& connectedComponent,
                                    value_type maxDistance);

128
      /// Add a node and two edges
129
130
131
132
133
      /// \param from node from which the edge starts,
      /// \param to configuration to which the edge stops
      /// \param path path between both configurations
      /// \return node containing configuration <c>to</c>.
      /// Add the symmetric edge with reverse path.
134
135
136
137
138
139
      /// \note this function simplifies the management of connected components
      ///       since it adds the new node in the connected component of
      ///       <c>from</c>.
      NodePtr_t addNodeAndEdges (const NodePtr_t from,
				 const ConfigurationPtr_t& to,
				 const PathPtr_t path);
140

141
      /// Add a node and one edge
142
143
144
145
146
147
148
149
150
151
152
153
      /// \param from node from which the edge starts,
      /// \param to configuration to which the edge stops
      /// \param path path between both configurations
      /// \return node containing configuration <c>to</c>.
      /// Add the oriented edge (from -> to)
      /// \note this function simplifies the management of connected components
      ///       since it adds the new node in the connected component of
      ///       <c>from</c>.
      NodePtr_t addNodeAndEdge (const NodePtr_t from,
                 const ConfigurationPtr_t& to,
                 const PathPtr_t path);

154
155
156
157
158
159
160
161
162
163
164
165
166
      /// Add a node and one edge
      /// \param from configuration from which the edge starts,
      /// \param to node to which the edge stops
      /// \param path path between both configurations
      /// \return node containing configuration <c>from</c>.
      /// Add the oriented edge (from -> to)
      /// \note this function simplifies the management of connected components
      ///       since it adds the new node in the connected component of
      ///       <c>to</c>.
      NodePtr_t addNodeAndEdge (const ConfigurationPtr_t& from,
                      const NodePtr_t to,
                      const PathPtr_t path);

167

168
169
170
171
172
173
174
175
176
177
178
179
      /// Add an edge between two nodes.
      EdgePtr_t addEdge (const NodePtr_t& n1, const NodePtr_t& n2,
			 const PathPtr_t& path);

      /// Add two edges between two nodes
      /// \param from first node
      /// \param to second node
      /// \param path path going from <c>from</c> to <c>to</c>.
      /// the reverse edge is added with the reverse path.
      void addEdges (const NodePtr_t from, const NodePtr_t& to,
		     const PathPtr_t& path);

180
181
182
183
      /// Add a goal configuration
      /// \param config configuration
      /// If configuration is already in the roadmap, tag corresponding node
      /// as goal node. Otherwise create a new node.
Joseph Mirabel's avatar
Joseph Mirabel committed
184
      NodePtr_t addGoalNode (const ConfigurationPtr_t& config);
185
186
187
188
189
190
191
192
193
194
195
196

      void resetGoalNodes ()
      {
	goalNodes_.clear ();
      }

      void initNode (const ConfigurationPtr_t& config)
      {
	initNode_ = addNode (config);
      }

      virtual ~Roadmap ();
197
198
      /// Check that a path exists between the initial node and one goal node.
      bool pathExists () const;
199
200
201
202
203
204
205
206
207
208
209
210
      const Nodes_t& nodes () const
      {
	return nodes_;
      }
      const Edges_t& edges () const
      {
	return edges_;
      }
      NodePtr_t initNode () const
      {
	return initNode_;
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
211
      const NodeVector_t& goalNodes () const
212
213
214
      {
	return goalNodes_;
      }
215
      /// Get list of connected component of the roadmap
216
217
      const ConnectedComponents_t& connectedComponents () const;

218
219
220
221
222
223
      /// Get nearestNeighbor object
      NearestNeighborPtr_t nearestNeighbor();

      /// Set new NearestNeighbor (roadmap must be empty)
      void nearestNeighbor(NearestNeighborPtr_t nearestNeighbor);

224
225
226
227
228
      /// \name Distance used for nearest neighbor search
      /// \{
      /// Get distance function
      const DistancePtr_t& distance () const;
      /// \}
229
230
231
      /// Print roadmap in a stream
      std::ostream& print (std::ostream& os) const;

232
233
234
    protected:
      /// Constructor
      /// \param distance distance function for nearest neighbor computations
235
      Roadmap (const DistancePtr_t& distance, const DevicePtr_t& robot);
236

237
238
      Roadmap () {};

239
240
241
242
243
      /// Add a new connected component in the roadmap.
      /// \param node node pointing to the connected component.
      /// \note The node is added in the connected component.
      void addConnectedComponent (const NodePtr_t& node);

Joseph Mirabel's avatar
Joseph Mirabel committed
244
245
246
247
248
249
250
      /// Give child class the opportunity to get the event
      /// "A node has been added to the roadmap"
      virtual void push_node (const NodePtr_t& n)
      {
        nodes_.push_back (n);
      }

Joseph Mirabel's avatar
Joseph Mirabel committed
251
252
253
254
255
256
257
258
259
      /// Give child class the opportunity to get the event
      /// "An edge has been added to the roadmap"
      /// \note you must always call the parent implementation first
      /// \code
      /// void YourRoadmap::push_edge(const EdgePtr_t e) {
      ///   Roadmap::push_edge(e);
      ///   // Your code here.
      /// }
      /// \endcode
260
      virtual void impl_addEdge (const EdgePtr_t& e);
Joseph Mirabel's avatar
Joseph Mirabel committed
261

Joseph Mirabel's avatar
Joseph Mirabel committed
262
263
264
265
266
      /// Node factory
      /// Reimplement the function if you want to create an instance of a
      /// child class of Node
      virtual NodePtr_t createNode (const ConfigurationPtr_t& configuration) const;

267
268
269
      /// Store weak pointer to itself
      void init (RoadmapWkPtr_t weak);

270
    private:
271
272
273
274
275
276
277
278
279
280
281
      /// Add a node with given configuration
      /// \param config configuration
      /// \param connectedComponent Connected component the node will belong
      ///        to.
      ///
      /// If configuration is alread in the connected component, return the node
      /// containing the configuration. Otherwise, create a new node with given
      /// connected component.
      NodePtr_t addNode (const ConfigurationPtr_t& config,
			 ConnectedComponentPtr_t connectedComponent);

282
283
284
285
286
287
288
289
290
291
      /// Update the graph of connected components after new connection
      /// \param cc1, cc2 the two connected components that have just been
      /// connected.
      void connect (const ConnectedComponentPtr_t& cc1,
		    const ConnectedComponentPtr_t& cc2);

      /// Merge two connected components
      /// \param cc1 the connected component to merge into
      /// \param the connected components to merge into cc1.
      void merge (const ConnectedComponentPtr_t& cc1,
292
		  ConnectedComponent::RawPtrs_t& ccs);
293

294
      const DistancePtr_t distance_;
295
      ConnectedComponents_t connectedComponents_;
296
297
298
      Nodes_t nodes_;
      Edges_t edges_;
      NodePtr_t initNode_;
Joseph Mirabel's avatar
Joseph Mirabel committed
299
      NodeVector_t goalNodes_;
300
      NearestNeighborPtr_t nearestNeighbor_;
301
      RoadmapWkPtr_t weak_;
302
303

      HPP_SERIALIZABLE();
304
    }; // class Roadmap
305
    std::ostream& operator<< (std::ostream& os, const Roadmap& r);
Florent Lamiraux's avatar
Florent Lamiraux committed
306
    /// \}
307
308
309
  } //   namespace core
} // namespace hpp
#endif // HPP_CORE_ROADMAP_HH