Drake
builder.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <cmath>
4 #include <map>
5 #include <memory>
6 #include <string>
7 #include <tuple>
8 #include <unordered_set>
9 #include <vector>
10 
15 
16 namespace drake {
17 namespace maliput {
18 
19 namespace monolane {
20 class RoadGeometry;
21 
22 /// @class Builder
23 /// Convenient builder class which makes it easy to construct a monolane road
24 /// network.
25 ///
26 /// monolane is a simple road-network implementation:
27 /// - single lane per segment;
28 /// - constant lane_bounds, driveable_bounds, and elevation_bounds,
29 /// same for all lanes;
30 /// - only linear and constant-curvature-arc primitives in XY-plane;
31 /// - cubic polynomials (parameterized on XY-arc-length) for elevation
32 /// and superelevation;
33 /// - superelevation (bank of road) rotates around the reference line (r = 0)
34 /// of the path.
35 ///
36 /// The Builder class simplifies the assembly of monolane road network
37 /// components into a valid RoadGeometry. In the Builder model, an Endpoint
38 /// specifies a point in world coordinates (along with a direction, slope,
39 /// and superelevation parameters). A Connection is a path from an explicit
40 /// start Endpoint to an end Endpoint calculated via a linear or arc
41 /// displacement (ArcOffset). A Group is a collection of Connections.
42 ///
43 /// Builder::Build() constructs a RoadGeometry. Each Connection yields a
44 /// Segment bearing a single Lane. Each Group yields a Junction containing
45 /// the Segments associated with the grouped Connections; ungrouped
46 /// Connections each receive their own Junction.
47 
48 
49 /// XY-plane-only parameters for an endpoint of a connection, specified in
50 /// the world frame.
51 ///
52 /// The three components are:
53 /// - x: x position
54 /// - y: y position
55 /// - heading: heading of reference path (radians, zero == x-direction)
56 class EndpointXy {
57  public:
59 
60  // Constructs an EndpointXy with all zero parameters.
61  EndpointXy() = default;
62 
63  EndpointXy(double x, double y, double heading)
64  : x_(x), y_(y), heading_(heading) {}
65 
66  /// Returns an EndpointXy with reversed direction.
67  EndpointXy reverse() const {
68  return EndpointXy(x_, y_,
69  std::atan2(-std::sin(heading_), -std::cos(heading_)));
70  }
71 
72  double x() const { return x_; }
73 
74  double y() const { return y_; }
75 
76  double heading() const { return heading_; }
77 
78  private:
79  double x_{};
80  double y_{};
81  double heading_{};
82 };
83 
84 
85 /// Out-of-plane parameters for an endpoint of a connection, specified in
86 /// the world frame.
87 ///
88 /// The four components are:
89 /// - z: elevation
90 /// - z_dot: grade (rate of change of elevation with respect to
91 /// arc length of the reference path)
92 /// - theta: superelevation (rotation of road surface around r = 0 centerline;
93 /// when theta > 0, elevation at r > 0 is above elevation at r < 0)
94 /// - theta_dot: rate of change of superelevation with respect to arc length
95 /// of the reference path
96 class EndpointZ {
97  public:
99 
100  // Constructs an EndpointZ with all zero parameters.
101  EndpointZ() = default;
102 
103  EndpointZ(double z, double z_dot, double theta, double theta_dot)
104  : z_(z), z_dot_(z_dot), theta_(theta), theta_dot_(theta_dot) {}
105 
106  /// Returns an EndpointZ with reversed direction.
107  EndpointZ reverse() const {
108  return EndpointZ(z_, -z_dot_, -theta_, theta_dot_);
109  }
110 
111  double z() const { return z_; }
112 
113  double z_dot() const { return z_dot_; }
114 
115  double theta() const { return theta_; }
116 
117  double theta_dot() const { return theta_dot_; }
118 
119  private:
120  double z_{};
121  double z_dot_{};
122 
123  double theta_{};
124  double theta_dot_{};
125 };
126 
127 
128 /// Complete set of parameters for an endpoint of a connection,
129 /// specified in the world frame. It comprises two subsets of parameters:
130 /// those pertaining only to the xy ground-plane, and those pertaining to
131 /// out-of-plane aspects of an endpoint.
132 class Endpoint {
133  public:
135 
136  // Constructs an Endpoint with all zero parameters.
137  Endpoint() = default;
138 
139  Endpoint(const EndpointXy& xy, const EndpointZ& z) : xy_(xy), z_(z) {}
140 
141  /// Returns an Endpoint with reversed direction.
142  Endpoint reverse() const {
143  return Endpoint(xy_.reverse(), z_.reverse());
144  }
145 
146  /// Returns the subset of parameters pertaining to the xy ground-plane.
147  const EndpointXy& xy() const { return xy_; }
148 
149  /// Returns the subset of parameters pertaining to out-of-ground-plane
150  /// aspects.
151  const EndpointZ& z() const { return z_; }
152 
153  private:
154  EndpointXy xy_;
155  EndpointZ z_;
156 };
157 
158 
159 /// Specification for path offset along a circular arc.
160 /// * radius: radius of the arc, which must be non-negative
161 /// * d_theta: angle of arc segment (Δθ)
162 /// * d_theta > 0 is counterclockwise ('veer to left')
163 /// * d_theta < 0 is clockwise ('veer to right')
164 class ArcOffset {
165  public:
167 
168  /// Constructs an ArcOffset with all zero parameters.
169  ArcOffset() = default;
170 
171  ArcOffset(double radius, double d_theta)
172  : radius_(radius), d_theta_(d_theta) {
173  DRAKE_DEMAND(radius_ > 0.);
174  }
175 
176  double radius() const { return radius_; }
177 
178  double d_theta() const { return d_theta_; }
179 
180  private:
181  double radius_{};
182  double d_theta_{};
183 };
184 
185 
186 /// Representation of a reference path connecting two endpoints.
187 ///
188 /// Upon building the RoadGeometry, a Connection yields a Segment
189 /// bearing a single Lane with the specified reference path. The
190 /// Segment will belong to its own Junction, unless the Connection was
191 /// grouped with other Connections into a Group.
192 ///
193 /// Two connection geometries are supported: line and arc. These
194 /// primitives determine the projection of the reference path onto the
195 /// (locally-flat) plane of the earth. The out-of-plane shape of
196 /// the path will be determined by the EndpointZ (elevation) parameters
197 /// of the endpoints.
198 class Connection {
199  public:
201 
202  /// Possible connection geometries: line- or arc-segment.
203  enum Type { kLine, kArc };
204 
205  /// Constructs a line-segment connection joining @p start to @p end.
206  Connection(const std::string& id,
207  const Endpoint& start, const Endpoint& end)
208  : type_(kLine), id_(id), start_(start), end_(end) {}
209 
210  /// Constructs an arc-segment connection joining @p start to @p end.
211  ///
212  /// @p cx, @p cy specify the center of the arc. @p radius is the radius,
213  /// and @p d_theta is the angle of arc.
214  ///
215  /// @p radius must be non-negative. @p d_theta > 0 indicates a
216  /// counterclockwise arc from start to end.
217  Connection(const std::string& id,
218  const Endpoint& start, const Endpoint& end,
219  double cx, double cy, double radius, double d_theta)
220  : type_(kArc), id_(id), start_(start), end_(end),
221  cx_(cx), cy_(cy), radius_(radius), d_theta_(d_theta) {}
222 
223  /// Returns the geometric type of the path.
224  Type type() const { return type_; }
225 
226  /// Returns the ID string.
227  const std::string& id() const { return id_; }
228 
229  /// Returns the parameters of the start point.
230  const Endpoint& start() const { return start_; }
231 
232  /// Returns the parameters of the endpoint.
233  const Endpoint& end() const { return end_; }
234 
235  /// Returns the x-component of the arc center (for arc connections only).
236  double cx() const {
237  DRAKE_DEMAND(type_ == kArc);
238  return cx_;
239  }
240 
241  /// Returns the y-component of the arc center (for arc connections only).
242  double cy() const {
243  DRAKE_DEMAND(type_ == kArc);
244  return cy_;
245  }
246 
247  /// Returns the radius of the arc (for arc connections only).
248  double radius() const {
249  DRAKE_DEMAND(type_ == kArc);
250  return radius_;
251  }
252 
253  /// Returns the angle of the arc (for arc connections only).
254  double d_theta() const {
255  DRAKE_DEMAND(type_ == kArc);
256  return d_theta_;
257  }
258 
259  private:
260  Type type_{};
261  std::string id_;
262  Endpoint start_;
263  Endpoint end_;
264 
265  // Bits specific to type_ == kArc:
266  double cx_{};
267  double cy_{};
268  double radius_{};
269  double d_theta_{};
270 };
271 
272 
273 /// A group of Connections.
274 ///
275 /// Upon building the RoadGeometry, a Group yields a Junction containing the
276 /// corresponding Segments specified by all the Connections in the Group.
277 class Group {
278  public:
280 
281  /// Constructs an empty Group with the specified @p id.
282  explicit Group(const std::string& id) : id_(id) {}
283 
284  /// Constructs a Group with @p id, populated by @p connections.
285  Group(const std::string& id,
286  const std::vector<const Connection*>& connections)
287  : id_(id) {
288  for (const Connection* connection : connections) {
289  Add(connection);
290  }
291  }
292 
293  /// Adds a Connection.
294  void Add(const Connection* connection) {
295  auto result = connection_set_.insert(connection);
296  DRAKE_DEMAND(result.second);
297  connection_vector_.push_back(connection);
298  }
299 
300  /// Returns the ID string.
301  const std::string& id() const { return id_; }
302 
303  /// Returns the grouped Connections.
304  const std::vector<const Connection*>& connections() const {
305  return connection_vector_;
306  }
307 
308  private:
309  std::string id_;
310  std::unordered_set<const Connection*> connection_set_;
311  std::vector<const Connection*> connection_vector_;
312 };
313 
314 
315 // N.B. The Builder class overview documentation lives at the top of this file.
316 class Builder {
317  public:
319 
320  /// Constructs a Builder which can be used to specify and assemble a
321  /// monolane implementation of an api::RoadGeometry.
322  ///
323  /// The bounds @p lane_bounds, @p driveable_bounds, and @p elevation_bounds
324  /// are applied uniformly to the single lanes of every segment;
325  /// @p lane_bounds must be a subset of @p driveable_bounds.
326  /// @p linear_tolerance and @p angular_tolerance specify the respective
327  /// tolerances for the resulting RoadGeometry.
328  Builder(const api::RBounds& lane_bounds,
329  const api::RBounds& driveable_bounds,
330  const api::HBounds& elevation_bounds,
331  const double linear_tolerance,
332  const double angular_tolerance);
333 
334  /// Connects @p start to an end-point linearly displaced from @p start.
335  /// @p length specifies the length of displacement (in the direction of the
336  /// heading of @p start). @p z_end specifies the elevation characteristics
337  /// at the end-point.
338  const Connection* Connect(
339  const std::string& id,
340  const Endpoint& start,
341  const double length,
342  const EndpointZ& z_end);
343 
344  /// Connects @p start to an end-point displaced from @p start via an arc.
345  /// @p arc specifies the shape of the arc. @p z_end specifies the
346  /// elevation characteristics at the end-point.
347  const Connection* Connect(
348  const std::string& id,
349  const Endpoint& start,
350  const ArcOffset& arc,
351  const EndpointZ& z_end);
352 
353  /// Sets the default branch for one end of a connection.
354  ///
355  /// The default branch for the @p in_end of connection @p in will set to be
356  /// @p out_end of connection @p out. The specified connections must
357  /// actually be joined at the specified ends (i.e., the Endpoint's for
358  /// those ends must be coincident and (anti)parallel within the tolerances
359  /// for the Builder).
360  void SetDefaultBranch(
361  const Connection* in, const api::LaneEnd::Which in_end,
362  const Connection* out, const api::LaneEnd::Which out_end);
363 
364  /// Creates a new empty connection group with ID string @p id.
365  Group* MakeGroup(const std::string& id);
366 
367  /// Creates a new connection group with ID @p id, populated with the
368  /// given @p connections.
369  Group* MakeGroup(const std::string& id,
370  const std::vector<const Connection*>& connections);
371 
372  /// Produces a RoadGeometry, with the ID @p id.
373  std::unique_ptr<const api::RoadGeometry> Build(
374  const api::RoadGeometryId& id) const;
375 
376  private:
377  // EndpointFuzzyOrder is an arbitrary strict complete ordering of Endpoints
378  // useful for, e.g., std::map. It provides a comparison operation that
379  // treats two Endpoints within @p linear_tolerance of one another as
380  // equivalent.
381  //
382  // This is used to match up the endpoints of Connections, to determine
383  // how Connections are linked to one another. Exact numeric equality
384  // would not be robust given the use of floating-point values in Endpoints.
385  class EndpointFuzzyOrder {
386  public:
387  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(EndpointFuzzyOrder)
388 
389  explicit EndpointFuzzyOrder(const double linear_tolerance)
390  : lin_tol_(linear_tolerance) {}
391 
392  bool operator()(const Endpoint& lhs, const Endpoint& rhs) const {
393  switch (fuzzy_compare(rhs.xy().x(), lhs.xy().x())) {
394  case -1: { return true; }
395  case 1: { return false; }
396  case 0: {
397  switch (fuzzy_compare(rhs.xy().y(), lhs.xy().y())) {
398  case -1: { return true; }
399  case 1: { return false; }
400  case 0: {
401  switch (fuzzy_compare(rhs.z().z(), lhs.z().z())) {
402  case -1: { return true; }
403  case 1: { return false; }
404  case 0: { return false; }
405  default: { DRAKE_ABORT(); }
406  }
407  }
408  default: { DRAKE_ABORT(); }
409  }
410  }
411  default: { DRAKE_ABORT(); }
412  }
413  }
414 
415  private:
416  int fuzzy_compare(const double a, const double b) const {
417  if (a < (b - lin_tol_)) {
418  return -1;
419  } else if (a > (b + lin_tol_)) {
420  return 1;
421  } else {
422  return 0;
423  }
424  }
425 
426  double lin_tol_{};
427  };
428 
429  struct DefaultBranch {
430  DefaultBranch() = default;
431 
432  DefaultBranch(
433  const Connection* ain, const api::LaneEnd::Which ain_end,
434  const Connection* aout, const api::LaneEnd::Which aout_end)
435  : in(ain), in_end(ain_end), out(aout), out_end(aout_end) {}
436 
437  const Connection* in{};
438  api::LaneEnd::Which in_end{};
439  const Connection* out{};
440  api::LaneEnd::Which out_end{};
441  };
442 
443  Lane* BuildConnection(
444  const Connection* const cnx,
445  Junction* const junction,
446  RoadGeometry* const rg,
447  std::map<Endpoint, BranchPoint*, EndpointFuzzyOrder>* const bp_map) const;
448 
449  BranchPoint* FindOrCreateBranchPoint(
450  const Endpoint& point,
451  RoadGeometry* rg,
452  std::map<Endpoint, BranchPoint*, EndpointFuzzyOrder>* const bp_map) const;
453 
454  void AttachBranchPoint(
455  const Endpoint& point, Lane* const lane, const api::LaneEnd::Which end,
456  RoadGeometry* rg,
457  std::map<Endpoint, BranchPoint*, EndpointFuzzyOrder>* bp_map) const;
458 
459  api::RBounds lane_bounds_;
460  api::RBounds driveable_bounds_;
461  api::HBounds elevation_bounds_;
462  double linear_tolerance_{};
463  double angular_tolerance_{};
464  std::vector<std::unique_ptr<Connection>> connections_;
465  std::vector<DefaultBranch> default_branches_;
466  std::vector<std::unique_ptr<Group>> groups_;
467 };
468 
469 } // namespace monolane
470 } // namespace maliput
471 } // namespace drake
const Expression xy_
Definition: symbolic_polynomial_test.cc:47
double y() const
Definition: builder.h:74
double z_dot() const
Definition: builder.h:113
A simple api::RoadGeometry implementation that only supports a single lane per segment.
Definition: road_geometry.h:20
void Add(const Connection *connection)
Adds a Connection.
Definition: builder.h:294
const Endpoint & start() const
Returns the parameters of the start point.
Definition: builder.h:230
double x() const
Definition: builder.h:72
const Endpoint & end() const
Returns the parameters of the endpoint.
Definition: builder.h:233
Representation of a reference path connecting two endpoints.
Definition: builder.h:198
EndpointZ(double z, double z_dot, double theta, double theta_dot)
Definition: builder.h:103
Definition: automotive_demo.cc:88
A group of Connections.
Definition: builder.h:277
Connection(const std::string &id, const Endpoint &start, const Endpoint &end)
Constructs a line-segment connection joining start to end.
Definition: builder.h:206
double theta_dot() const
Definition: builder.h:117
Expression sin(const Expression &e)
Definition: symbolic_expression.cc:599
Expression atan2(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:652
XY-plane-only parameters for an endpoint of a connection, specified in the world frame.
Definition: builder.h:56
STL namespace.
double theta() const
Definition: builder.h:115
Type
Possible connection geometries: line- or arc-segment.
Definition: builder.h:203
Connection(const std::string &id, const Endpoint &start, const Endpoint &end, double cx, double cy, double radius, double d_theta)
Constructs an arc-segment connection joining start to end.
Definition: builder.h:217
Type type() const
Returns the geometric type of the path.
Definition: builder.h:224
const std::vector< const Connection * > & connections() const
Returns the grouped Connections.
Definition: builder.h:304
double d_theta() const
Definition: builder.h:178
EndpointXy(double x, double y, double heading)
Definition: builder.h:63
ArcOffset(double radius, double d_theta)
Definition: builder.h:171
std::vector< Number > result
Definition: ipopt_solver.cc:151
#define DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Classname)
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN defaults the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:57
const EndpointZ & z() const
Returns the subset of parameters pertaining to out-of-ground-plane aspects.
Definition: builder.h:151
Provides Drake&#39;s assertion implementation.
#define DRAKE_DEMAND(condition)
Evaluates condition and iff the value is false will trigger an assertion failure with a message showi...
Definition: drake_assert.h:45
double cy() const
Returns the y-component of the arc center (for arc connections only).
Definition: builder.h:242
Group(const std::string &id, const std::vector< const Connection * > &connections)
Constructs a Group with id, populated by connections.
Definition: builder.h:285
double z() const
Definition: builder.h:111
Bounds in the lateral dimension (r component) of a Lane-frame, consisting of a pair of minimum and ma...
Definition: lane_data.h:298
const EndpointXy & xy() const
Returns the subset of parameters pertaining to the xy ground-plane.
Definition: builder.h:147
Base class for the monolane implementation of api::Lane.
Definition: lane.h:127
double radius() const
Returns the radius of the arc (for arc connections only).
Definition: builder.h:248
Specification for path offset along a circular arc.
Definition: builder.h:164
TypeSpecificIdentifier< class RoadGeometry > RoadGeometryId
Persistent identifier for a RoadGeometry element.
Definition: road_geometry.h:19
EndpointXy reverse() const
Returns an EndpointXy with reversed direction.
Definition: builder.h:67
const Expression z_
Definition: symbolic_expansion_test.cc:31
Bounds in the elevation dimension (h component) of a Lane-frame, consisting of a pair of minimum and ...
Definition: lane_data.h:343
Which
Labels for the endpoints of a Lane.
Definition: lane_data.h:24
double z
Definition: vtk_util_test.cc:27
double heading() const
Definition: builder.h:76
An implementation of api::BranchPoint.
Definition: branch_point.h:42
Endpoint reverse() const
Returns an Endpoint with reversed direction.
Definition: builder.h:142
Expression cos(const Expression &e)
Definition: symbolic_expression.cc:607
Out-of-plane parameters for an endpoint of a connection, specified in the world frame.
Definition: builder.h:96
double cx() const
Returns the x-component of the arc center (for arc connections only).
Definition: builder.h:236
An api::Junction implementation.
Definition: junction.h:17
#define DRAKE_ABORT()
Aborts the program (via ::abort) with a message showing at least the function name, file, and line.
Definition: drake_assert.h:48
const std::string & id() const
Returns the ID string.
Definition: builder.h:301
EndpointZ reverse() const
Returns an EndpointZ with reversed direction.
Definition: builder.h:107
const std::string & id() const
Returns the ID string.
Definition: builder.h:227
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:33
double d_theta() const
Returns the angle of the arc (for arc connections only).
Definition: builder.h:254
double radius() const
Definition: builder.h:176
Convenient builder class which makes it easy to construct a monolane road network.
Definition: builder.h:316
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
Complete set of parameters for an endpoint of a connection, specified in the world frame...
Definition: builder.h:132
Endpoint(const EndpointXy &xy, const EndpointZ &z)
Definition: builder.h:139