Drake
builder.h
Go to the documentation of this file.
1 #pragma once
2
3 #include <map>
4 #include <memory>
5 #include <string>
6 #include <utility>
7 #include <vector>
8
9 #include "ignition/math/Spline.hh"
10 #include "ignition/math/Vector3.hh"
11
12 #include "drake/automotive/maliput/api/lane_data.h"
14 #include "drake/automotive/maliput/rndf/connection.h"
15 #include "drake/automotive/maliput/rndf/directed_waypoint.h"
17 #include "drake/common/drake_copyable.h"
18
19 namespace drake {
20 namespace maliput {
21 namespace rndf {
22
23 /// A class to ease the construction of a RoadGeometry from Connection and
24 /// DirectedWaypoint objects.
25 ///
26 /// RNDF segments and lanes are mapped to Maliput's Segment and Lane entities,
27 /// respectively. This mapping is not straightforward as Maliput is based on
28 /// analytical curve parameterizations while RNDF provides a sampled geometry
29 /// based on waypoints. RNDF waypoints are thus used as control points in a
30 /// cubic spline interpolation that results in SplineLanes. RNDF lanes'
31 /// direction is implied by RNDF waypoint IDs
32 /// (segment_num.lane_num.waypoint_num) which is captured by the
33 /// interpolation and reflected in DirectedWaypoints. These DirectedWaypoints
34 /// are grouped in Connections, which describe not only RNDF lanes but also the
35 /// connections between pairs of exit and entry RNDF waypoints. During the build
36 /// process, interpolated waypoints are added to these Connections so as to keep
37 /// their distribution akin to a grid (and thus enable index-based operations).
38 ///
39 /// Since lanes in an RNDF segment may flow in different directions, segment
40 /// connections are also grouped by their directions relative to the first
41 /// connection found in the collection. An arbitrary point outside the bounding
42 /// box of the road geometry is selected to be a "center of rotation" and the
43 /// sum of all the DirectedWaypoints "momentums" (with normalized tangents) is
44 /// computed. It must be said that the "momentum" equation is used to derive
45 /// connection direction, but it bears no physical meaning. Since all the
46 /// waypoints lay in the @f$z = 0 @f$ plane, the "momentum" vector will only
47 /// contain a non-zero z coordinate value. The sign of the "momentum"'s z
48 /// coordinate will be taken as a reference to define the sense of flow. For
49 /// all other connections, the "momentum" is computed and compared against the
50 /// sign of the reference.
51 ///
52 /// RNDF zones also do not have a direct representation in Maliput either.
53 /// To model zones, fake connections are added between every pair of entry
54 /// and exit waypoints in a zone. The directions of these waypoints are set to
55 /// head towards the centroid of all the zone perimeter waypoints. There's
56 /// currently no support for RNDF zones' parking spots.
57 ///
58 /// The resulting RoadGeometry presents the following naming for its composed
59 /// entities:
60 /// - Lane naming: "l:$1-$2", where
61 /// 1. RNDF exit waypoint ID.
62 /// 2. RNDF entry waypoint ID.
63 /// - Segment naming: "s:$1-$2-$3", where 64 /// 1. RNDF segment ID. 65 /// 2. Direction-based grouping index. 66 /// 3. RNDF segment piece index (in between every 2 waypoints). 67 /// - Junction naming: "j:$1-$2-$3", where
68 /// 1. RNDF segment ID.
69 /// 2. Direction-based grouping index.
70 /// 3. RNDF segment piece index (in between every 2 waypoints).
71 /// - BranchPoint naming: "bp:\$1", where
72 /// 1. Index in RoadGeometry's inner collection.
73 ///
74 /// An example of how this is achieved is depicted in the following example.
75 /// Note that '+' denotes RNDF waypoints, 'x' denotes invalid waypoints
76 /// and 'o' denotes interpolated waypoints. Also '|' denotes Segment boundaries.
77 ///
78 /// <pre>
79 /// 1.1.1 1.1.2
80 /// +-----------------------+ <---- Connection: 1.1.1-1.1.2
81 /// 1.2.1 1.2.2
82 /// +-------------------------------------------+ <---- Connection: 1.2.1-1.2.2
83 /// 1.3.1 1.3.2
84 /// +-----------------------+ <---- Connection: 1.3.1-1.3.2
85 /// </pre>
86 ///
87 /// Mapping the above waypoints to SplineLane and Segment objects will be done
88 /// by the Builder. In the end, we end up with something like:
89 ///
90 /// <pre>
91 /// | |1.1.1 |1.1.2 |
92 /// x--------+-----------------------+----------x <---- Connection: 1.1.1-1.1.2
93 /// |1.2.1 |1.2.3 |1.2.4 |1.2.2
94 /// +--------o-----------------------o----------+ <---- Connection: 1.2.1-1.2.2
95 /// | |1.3.1 |1.3.2 |
96 /// x--------+-----------------------+----------x <---- Connection: 1.3.1-1.3.2
97 /// </pre>
98 ///
99 /// Those new waypoints that appear make possible the concept of Maliput Segment
100 /// as a surface that holds all the trajectories (represented by Maliput Lanes).
101 /// At the building stage, we can now match waypoints across lanes using an
102 /// index-based algorithm, a pair of valid consecutive waypoints in a Connection
103 /// are used to create a SplineLane. At the same time, we should
104 /// create those Lanes inside the same segment, which is not difficult to do
105 /// using the index approach. To sum up, Builder will create the following:
106 ///
107 /// - Segment 1-0-0:
108 /// - Lane 1.2.1-1.2.3
109 /// - Segment 1-0-1:
110 /// - Lane 1.3.1-1.3.2
111 /// - Lane 1.2.3-1.2.4
112 /// - Lane 1.1.1-1.1.2
113 /// - Segment 1-0-2:
114 /// - Lane 1.2.4-1.2.2
115 ///
116 /// General workflow with this class should be:
117 /// -# Create a Builder.
118 /// -# Call SetBoundingBox().
119 /// -# Call CreateSegmentConnections() for each RNDF segment.
120 /// -# Call CreateConnectionsForZones() for each RNDF zone.
121 /// -# Call CreateConnection() for each pair of entry-exit waypoints
122 /// between RNDF lanes and zone perimeters (actually connecting
123 /// their fake inner lanes to the outer real ones).
124 /// -# Call Build() to get the built api::RoadGeometry.
125 class Builder {
126  public:
128
129  /// Constructs a Builder which can be used to specify and assemble
130  /// an RNDF implementation of an api::RoadGeometry.
131  ///
132  /// @param linear_tolerance Linear tolerance for RoadGeometry construction.
133  /// @param angular_tolerance Angular tolerance for RoadGeometry construction.
134  Builder(double linear_tolerance, double angular_tolerance)
135  : linear_tolerance_(linear_tolerance),
136  angular_tolerance_(angular_tolerance) {}
137
138  /// Sets the bounding box of the RNDF map.
139  ///
140  /// @param bounding_box The lower left and upper right corners' position
141  /// pair for the bounding box.
142  /// @remarks Bounding box definition is kept in 3D space for the sake
143  /// of generality, even though there's currently no support for nonplanar
144  /// RNDF geometries and, most of the time, the z-component of the given
145  /// corners will be zero.
146  void SetBoundingBox(const std::pair<ignition::math::Vector3d,
147  ignition::math::Vector3d>& bounding_box) {
148  bounding_box_ = bounding_box;
149  }
150
151  /// Populates the Builder's inner connection map with the given
152  /// @p connections representing an RNDF segment.
153  ///
154  /// To do this, the @p connections' waypoints are first used to derive
155  /// a geometry. Then, these @p connections are grouped based on relative
156  /// direction using the first connection found as a reference. Once grouped,
157  /// extra waypoints are added to each of them on a per group basis as
158  /// necessary to ensure a grid-like distribution of waypoints.
159  /// @param segment_id The RNDF segment ID.
160  /// @param connections A collection of Connections representing each RNDF lane
161  /// in the segment.
162  /// @throw std::runtime_error When @p connections is a nullptr.
163  /// @throw std::runtime_error When @p connections is an empty collection.
164  void CreateSegmentConnections(int segment_id,
165  std::vector<Connection>* connections);
166
167  /// Creates a collection of Connection objects between every pair of entry and
168  /// exit waypoints in @p perimeter_waypoints.
169  ///
170  /// RNDF defines zones as areas where free-path driving is allowed. Since
171  /// Maliput does not cover this concept, and to avoid having dead-end lanes,
172  /// every pair of entry and exit waypoints is connected. These waypoints'
173  /// directions are set to head towards the centroid of all
174  /// @p perimeter_waypoints.
175  /// @param width The width of this zone's inner connections (and thus the
176  /// fake inner lanes' lane bounds).
177  /// @param perimeter_waypoints A collection of DirectedWaypoint objects
178  /// describing the zone's perimeter.
179  /// @throw std::runtime_error When @p perimeter_waypoints is a nullptr.
180  /// @throw std::runtime_error When @p perimeter_waypoints is an empty
181  /// collection.
183  double width, std::vector<DirectedWaypoint>* perimeter_waypoints);
184
185  /// Creates a connection between two RNDF lanes based on a pair of @p exit and
186  /// @p entry ids that map to specific, existing waypoints. This is helpful
187  /// when building intersections.
188  /// @param width The connection's width.
189  /// @param exit_id The start waypoint ID of the connection.
190  /// @param entry_id The end waypoint ID of the connection.
191  /// @throw std::runtime_error When neither @p exit_id nor @p entry_id are
192  /// found.
193  void CreateConnection(double width, const ignition::rndf::UniqueId& exit_id,
194  const ignition::rndf::UniqueId& entry_id);
195
197  ///
198  /// All the groups of connections are traversed. A Junction with a single
199  /// Segment is created per group, and for each connection in that group,
200  /// a Lane is added to the Segment. BranchPoints are updated as needed.
201  /// @param id ID of the api::RoadGeometry to be built.
202  /// @return The built api::RoadGeometry.
203  /// @throw std::runtime_error When the built RoadGeometry does not satisfy
206
207  private:
208  // Inserts a connection of the given @p width, with the given @p key_id, using
209  // the given @p waypoints to the inner connection map.
210  // @pre The given @p waypoints collection size must be at least two (2).
211  // @warning This method will abort if preconditions are not met.
212  void InsertConnection(const std::string& key_id, double width,
213  const std::vector<DirectedWaypoint>& waypoints);
214
215  // The tolerance used by the RoadGeometry to check linear invariants.
216  const double linear_tolerance_{};
217  // The tolerance used by the RoadGeometry to check angular invariants.
218  const double angular_tolerance_{};
219  // A map to hold all the connections while they are created.
220  std::map<std::string, std::vector<std::unique_ptr<Connection>>> connections_;
221  // A map to hold all the DirectedWaypoints that are used as Lane extents.
222  std::map<std::string, DirectedWaypoint> directed_waypoints_;
223  // The coordinates of the bounding box that encloses RNDF's waypoints.
224  std::pair<ignition::math::Vector3d, ignition::math::Vector3d> bounding_box_;
225 };
226
227 } // namespace rndf
228 } // namespace maliput
229 } // namespace drake
void CreateConnectionsForZones(double width, std::vector< DirectedWaypoint > *perimeter_waypoints)
Creates a collection of Connection objects between every pair of entry and exit waypoints in perimete...
Definition: builder.cc:590
void CreateSegmentConnections(int segment_id, std::vector< Connection > *connections)
Populates the Builder&#39;s inner connection map with the given connections representing an RNDF segment...
Definition: builder.cc:623
Definition: automotive_demo.cc:89
Definition: branch_point.cc:7
A class to ease the construction of a RoadGeometry from Connection and DirectedWaypoint objects...
Definition: builder.h:125
void SetBoundingBox(const std::pair< ignition::math::Vector3d, ignition::math::Vector3d > &bounding_box)
Sets the bounding box of the RNDF map.
Definition: builder.h:146
void CreateConnection(double width, const ignition::rndf::UniqueId &exit_id, const ignition::rndf::UniqueId &entry_id)
Creates a connection between two RNDF lanes based on a pair of exit and entry ids that map to specifi...
Definition: builder.cc:576