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 <vector>
9 
15 
16 namespace drake {
17 namespace maliput {
18 namespace multilane {
19 
20 class RoadGeometry;
21 
22 /// Defines the direction of an Endpoint or EndpointZ.
23 enum class Direction { kForward, kReverse };
24 
25 /// Provides methods to build an StartReference::Spec.
27  public:
28  /// Defines how a Connection's reference curve starts.
29  ///
30  /// Objects of this class should be created using StartReference::at()
31  /// methods.
32  class Spec {
33  public:
35 
36  const Endpoint& endpoint() const { return endpoint_; }
37 
38  private:
39  // Allows StartReference factory to build objects of this class.
40  friend class StartReference;
41 
42  // Constructs a Spec that specifies with `endpoint` how a Connection's
43  // reference curve starts.
44  explicit Spec(const Endpoint& endpoint) : endpoint_(endpoint) {}
45 
46  // Describes the connection's reference curve start-point.
47  Endpoint endpoint_{};
48  };
49 
51 
52  StartReference() = default;
53 
54  /// Builds a Spec at `endpoint` with `direction` direction. When
55  /// `direction` == `Direction::kReverse`, `endpoint` is reversed.
56  Spec at(const Endpoint& endpoint, Direction direction) const {
57  return direction == Direction::kForward ? Spec(endpoint)
58  : Spec(endpoint.reverse());
59  }
60 
61  /// Builds a Spec at `connection`'s `end` side with `direction` direction.
62  /// When `direction` == `Direction::kReverse`, `endpoint` is reversed.
63  /// Spec's theta_dot will be cleared so the Builder adjusts it to match
64  /// continuity constraints.
66  Direction direction) const {
67  Endpoint endpoint = end == api::LaneEnd::Which::kStart ? connection.start()
68  : connection.end();
69  endpoint.get_mutable_z().get_mutable_theta_dot() = {};
70  return direction == Direction::kForward ? Spec(endpoint)
71  : Spec(endpoint.reverse());
72  }
73 };
74 
75 /// Streams a string representation of `start_spec` into `out`. Returns `out`.
76 /// This method is provided for the purposes of debugging or text-logging.
77 /// It is not intended for serialization.
78 std::ostream& operator<<(std::ostream& out,
79  const StartReference::Spec& start_spec);
80 
81 /// Provides methods to build an StartLane::Spec.
82 class StartLane {
83  public:
84  /// Defines how a Connection's lane curve starts.
85  ///
86  /// Objects of this class should be created using StartLane::at() methods.
87  class Spec {
88  public:
90 
91  const Endpoint& endpoint() const { return endpoint_; }
92 
93  int lane_id() const { return lane_id_; }
94 
95  private:
96  // Allows StartLane factory to build objects of this class.
97  friend class StartLane;
98 
99  // Constructs a Spec that specifies with `endpoint` how a Connection's
100  // `lane_id` lane-curve starts.
101  Spec(const Endpoint& endpoint, int lane_id)
102  : endpoint_(endpoint), lane_id_(lane_id) {}
103 
104  // Describes the connection's `lane_id_` lane-curve start-point.
105  Endpoint endpoint_{};
106  // Connection's lane-curve index.
107  int lane_id_{};
108  };
109 
111 
112  StartLane() = delete;
113 
114  /// Constructs a StartLane::Spec factory to hold `start_lane` lane-curve start
115  /// point.
116  ///
117  /// `start_lane` must be non-negative.
118  explicit StartLane(int start_lane) : start_lane_(start_lane) {
119  DRAKE_DEMAND(start_lane_ >= 0);
120  }
121 
122  /// Builds a Spec at `endpoint` with `direction` direction. When
123  /// `direction` == `Direction::kReverse`, `endpoint` is reversed.
124  /// `endpoint`'s theta_dot must be nullopt.
125  Spec at(const Endpoint& endpoint, Direction direction) const {
126  DRAKE_DEMAND(!endpoint.z().theta_dot().has_value());
127  return direction == Direction::kForward
128  ? Spec(endpoint, start_lane_)
129  : Spec(endpoint.reverse(), start_lane_);
130  }
131 
132  /// Builds a Spec at `connection`'s `lane_id` lane at `end` side with
133  /// `direction` direction. When `direction` == `Direction::kReverse`,
134  /// `endpoint` is reversed.
135  ///
136  /// `lane_id` must be non-negative and smaller than `connection`'s number of
137  /// lanes.
138  /// Spec's theta_dot will be cleared so the Builder adjusts it to match
139  /// continuity constraints.
140  Spec at(const Connection& connection, int lane_id, api::LaneEnd::Which end,
141  Direction direction) const {
142  DRAKE_DEMAND(lane_id >= 0 && lane_id < connection.num_lanes());
143  Endpoint endpoint = end == api::LaneEnd::Which::kStart
144  ? connection.LaneStart(lane_id)
145  : connection.LaneEnd(lane_id);
146  endpoint.get_mutable_z().get_mutable_theta_dot() = {};
147  return direction == Direction::kForward
148  ? Spec(endpoint, start_lane_)
149  : Spec(endpoint.reverse(), start_lane_);
150  }
151 
152  private:
153  // Connection's lane-curve index at which Spec will be specified.
154  const int start_lane_{};
155 };
156 
157 /// Streams a string representation of `start_spec` into `out`. Returns `out`.
158 /// This method is provided for the purposes of debugging or text-logging.
159 /// It is not intended for serialization.
160 std::ostream& operator<<(std::ostream& out, const StartLane::Spec& start_spec);
161 
162 /// Provides methods to build an EndReference::Spec.
164  public:
165  /// Defines how a Connection's reference curve ends.
166  ///
167  /// Objects of this class should be created using EndReference::z_at()
168  /// methods.
169  class Spec {
170  public:
172 
173  const EndpointZ& endpoint_z() const { return endpoint_z_; }
174 
175  private:
176  // Allows EndReference factory to build objects of this class.
177  friend class EndReference;
178 
179  /// Constructs a Spec at that specifies with `endpoint_z` how a
180  /// Connection's reference curve ends.
181  explicit Spec(const EndpointZ& endpoint_z) : endpoint_z_(endpoint_z) {}
182 
183  // Describes the connection's reference curve end-point.
184  EndpointZ endpoint_z_;
185  };
186 
188 
189  EndReference() = default;
190 
191  /// Builds a Spec at `connection`'s `end` side with `direction` direction.
192  /// When `direction` == `Direction::kReverse`, `end`-side endpoint's
193  /// EndpointZ is reversed.
194  /// Spec's theta_dot will be cleared so the Builder adjusts it to match
195  /// continuity constraints.
196  Spec z_at(const Connection& connection, api::LaneEnd::Which end,
197  Direction direction) const {
198  EndpointZ endpoint_z = end == api::LaneEnd::Which::kStart
199  ? connection.start().z()
200  : connection.end().z();
201  endpoint_z.get_mutable_theta_dot() = {};
202  return direction == Direction::kForward ? Spec(endpoint_z)
203  : Spec(endpoint_z.reverse());
204  }
205 
206  /// Builds an Spec at `endpoint_z` with `direction` direction.
207  /// When `direction` == `Direction::kReverse`, `endpoint_z` is reversed.
208  Spec z_at(const EndpointZ& endpoint_z, Direction direction) const {
209  return direction == Direction::kForward ? Spec(endpoint_z)
210  : Spec(endpoint_z.reverse());
211  }
212 };
213 
214 /// Streams a string representation of `end_spec` into `out`. Returns `out`.
215 /// This method is provided for the purposes of debugging or text-logging.
216 /// It is not intended for serialization.
217 std::ostream& operator<<(std::ostream& out, const EndReference::Spec& end_spec);
218 
219 /// Provides methods to build an EndLane::Spec.
220 class EndLane {
221  public:
222  /// Defines how a Connection's lane curve ends.
223  ///
224  /// Objects of this class should be created using EndLane::z_at() methods.
225  class Spec {
226  public:
228 
229  const EndpointZ& endpoint_z() const { return endpoint_z_; }
230 
231  int lane_id() const { return lane_id_; }
232 
233  private:
234  // Allows EndLane factory to build objects of this class.
235  friend class EndLane;
236 
237  // Constructs a Spec that specifies with `endpoint_z` how a Connection's
238  // `lane_id` lane-curve ends.
239  explicit Spec(const EndpointZ& endpoint_z, int lane_id)
240  : endpoint_z_(endpoint_z), lane_id_(lane_id) {}
241 
242  // Describes the connection's `lane_id_` lane-curve end-point.
243  EndpointZ endpoint_z_{};
244  // Connection's lane-curve index.
245  int lane_id_{};
246  };
247 
249 
250  EndLane() = delete;
251 
252  /// Constructs a EndLane::Spec factory to hold `end_lane` lane-curve end
253  /// point.
254  ///
255  /// `end_lane` must be non-negative.
256  explicit EndLane(int end_lane) : end_lane_(end_lane) {
257  DRAKE_DEMAND(end_lane_ >= 0);
258  }
259 
260  /// Builds a Spec at `connection`'s `end` side with `direction` direction.
261  /// When `direction` == `Direction::kReverse`, `end`-side endpoint's
262  /// EndpointZ is reversed.
263  ///
264  /// `lane_id` must be non-negative and smaller than `connection`'s number of
265  /// lanes.
266  /// Spec's theta_dot will be cleared so the Builder adjusts it to match
267  /// continuity constraints.
269  Direction direction) const {
270  DRAKE_DEMAND(lane_id >= 0 && lane_id < connection.num_lanes());
271  EndpointZ endpoint_z = end == api::LaneEnd::Which::kStart
272  ? connection.LaneStart(lane_id).z()
273  : connection.LaneEnd(lane_id).z();
274  endpoint_z.get_mutable_theta_dot() = {};
275  return direction == Direction::kForward
276  ? Spec(endpoint_z, end_lane_)
277  : Spec(endpoint_z.reverse(), end_lane_);
278  }
279 
280  /// Builds an Spec at `endpoint_z` with `direction` direction.
281  /// When `direction` == `Direction::kReverse`, `endpoint_z` is reversed.
282  /// `endpoint_z`'s theta_dot must be nullopt.
283  Spec z_at(const EndpointZ& endpoint_z, Direction direction) const {
284  DRAKE_DEMAND(!endpoint_z.theta_dot().has_value());
285  return direction == Direction::kForward
286  ? Spec(endpoint_z, end_lane_)
287  : Spec(endpoint_z.reverse(), end_lane_);
288  }
289 
290  private:
291  // Connection's lane-curve index at which Spec will be specified.
292  const int end_lane_{};
293 };
294 
295 /// Streams a string representation of `end_spec` into `out`. Returns `out`.
296 /// This method is provided for the purposes of debugging or text-logging.
297 /// It is not intended for serialization.
298 std::ostream& operator<<(std::ostream& out, const EndLane::Spec& end_spec);
299 
300 /// Wraps all the lane-related specifications in a Connection.
301 class LaneLayout {
302  public:
304 
305  /// Constructs a the lane layout of a connection.
306  ///
307  /// Lane reference paths (which are offsets of parent Segment reference curve)
308  /// are centered within the Lane. Lane spacing will be road geometry fixed
309  /// lane's width. Segment extents will be derived from the composition of
310  /// `left_shoulder` and `right_shoulder` shoulders, number of lanes and lane
311  /// spacing. `ref_lane` lane's centerline will be placed at `ref_r0` distance
312  /// from connection's reference curve.
313  ///
314  /// `left_shoulder` and `right_shoulder` must be nonnegative.
315  /// `num_lanes` must be positive and `ref_lane` must be nonnegative and
316  /// smaller than `num_lanes`.
317  LaneLayout(double left_shoulder, double right_shoulder, int num_lanes,
318  int ref_lane, double ref_r0)
319  : left_shoulder_(left_shoulder),
320  right_shoulder_(right_shoulder),
321  num_lanes_(num_lanes),
322  ref_lane_(ref_lane),
323  ref_r0_(ref_r0) {
324  DRAKE_DEMAND(left_shoulder_ >= 0.);
325  DRAKE_DEMAND(right_shoulder_ >= 0.);
326  DRAKE_DEMAND(num_lanes_ > 0);
327  DRAKE_DEMAND(ref_lane_ >= 0 && ref_lane_ < num_lanes_);
328  }
329 
330  double left_shoulder() const { return left_shoulder_; }
331 
332  double right_shoulder() const { return right_shoulder_; }
333 
334  int num_lanes() const { return num_lanes_; }
335 
336  int ref_lane() const { return ref_lane_; }
337 
338  double ref_r0() const { return ref_r0_; }
339 
340  private:
341  // Extra space added to the right of the first lane.
342  double left_shoulder_{};
343  // Extra space added to the left of the last lane.
344  double right_shoulder_{};
345  // Number of lanes.
346  int num_lanes_{};
347  // Index of the lane from which `ref_r0_` is defined.
348  int ref_lane_{};
349  // Distance from `ref_lane_` lane's centerline to reference curve.
350  double ref_r0_{};
351 };
352 
353 /// Streams a string representation of `lane_layout` into `out`. Returns `out`.
354 /// This method is provided for the purposes of debugging or text-logging.
355 /// It is not intended for serialization.
356 std::ostream& operator<<(std::ostream& out, const LaneLayout& lane_layout);
357 
358 /// Defines a builder interface for multilane. It is used for testing purposes
359 /// only, and derived code should instantiate Builder objects.
360 class BuilderBase {
361  public:
363 
364  BuilderBase() = default;
365 
366  virtual ~BuilderBase() = default;
367 
368  /// Gets `lane_width` value.
369  virtual double get_lane_width() const = 0;
370 
371  /// Gets `elevation_bounds` value.
372  virtual const api::HBounds& get_elevation_bounds() const = 0;
373 
374  /// Gets `linear_tolerance` value.
375  virtual double get_linear_tolerance() const = 0;
376 
377  /// Gets `angular_tolerance` value.
378  virtual double get_angular_tolerance() const = 0;
379 
380  /// Gets `scale_length` value.
381  virtual double get_scale_length() const = 0;
382 
383  /// Gets `computation_policy` value.
384  virtual ComputationPolicy get_computation_policy() const = 0;
385 
386  /// Connects `start_spec`'s Endpoint to an end-point linearly displaced from
387  /// `start_spec`'s Endpoint.
388  ///
389  /// `line_offset` specifies the length of displacement (in the direction of
390  /// the heading of `start_spec`'s Endpoint). `end_spec` specifies the
391  /// elevation characteristics at the end-point.
392  ///
393  /// `lane_layout` defines the number of lanes, their width, extra shoulder
394  /// asphalt extensions and placing with respect to connection's reference
395  /// curve.
396  virtual const Connection* Connect(const std::string& id,
397  const LaneLayout& lane_layout,
398  const StartReference::Spec& start_spec,
399  const LineOffset& line_offset,
400  const EndReference::Spec& end_spec) = 0;
401 
402  /// Connects `start_spec`'s Endpoint to an end-point displaced from
403  /// `start_spec`'s Endpoint via an arc.
404  ///
405  /// `arc_offset` specifies the shape of the arc. `end_spec` specifies the
406  /// elevation characteristics at the end-point.
407  ///
408  /// `lane_layout` defines the number of lanes, their width, extra shoulder
409  /// asphalt extensions and placing with respect to connection's reference
410  /// curve.
411  virtual const Connection* Connect(const std::string& id,
412  const LaneLayout& lane_layout,
413  const StartReference::Spec& start_spec,
414  const ArcOffset& arc_offset,
415  const EndReference::Spec& end_spec) = 0;
416 
417  /// Creates a Connection whose planar reference curve is a line.
418  ///
419  /// `start_spec.lane_id()` lane starts at `start_spec.endpoint()` and
420  /// `end_spec.lane_id()` lane ends with `end_spec.endpoint_z()` which
421  /// specifies the elevation characteristics.
422  ///
423  /// `line_offset` specifies the length of displacement (in the direction of
424  /// the heading of `start_spec`'s Endpoint).
425  ///
426  /// `lane_layout` defines the number of lanes, their width, extra shoulder
427  /// asphalt extensions and placing with respect to connection's reference
428  /// curve.
429  virtual const Connection* Connect(const std::string& id,
430  const LaneLayout& lane_layout,
431  const StartLane::Spec& start_spec,
432  const LineOffset& line_offset,
433  const EndLane::Spec& end_spec) = 0;
434 
435  /// Creates a Connection whose planar reference curve is an arc.
436  ///
437  /// `start_spec.lane_id()` lane starts at `start_spec.endpoint()` and
438  /// `end_spec.lane_id()` lane ends with `end_spec.endpoint_z()` which
439  /// specifies the elevation characteristics.
440  ///
441  /// `arc_offset` specifies the shape of the arc.
442  ///
443  /// `lane_layout` defines the number of lanes, their width, extra shoulder
444  /// asphalt extensions and placing with respect to connection's reference
445  /// curve.
446  virtual const Connection* Connect(const std::string& id,
447  const LaneLayout& lane_layout,
448  const StartLane::Spec& start_spec,
449  const ArcOffset& arc_offset,
450  const EndLane::Spec& end_spec) = 0;
451 
452  /// Sets the default branch for one end of a connection.
453  ///
454  /// The default branch for the `in_end` of connection `in` at Lane
455  /// `in_lane_index`will set to be `out_end` of connection `out` at Lane
456  /// `out_lane_index`. The specified connections must actually be joined at the
457  /// specified ends (i.e., the Endpoint's for those ends must be coincident and
458  /// (anti)parallel within the tolerances for the Builder).
459  virtual void SetDefaultBranch(const Connection* in, int in_lane_index,
460  api::LaneEnd::Which in_end,
461  const Connection* out, int out_lane_index,
462  api::LaneEnd::Which out_end) = 0;
463 
464  /// Creates a new empty connection group with ID string `id`.
465  virtual Group* MakeGroup(const std::string& id) = 0;
466 
467  /// Creates a new connection group with ID `id`, populated with the
468  /// given `connections`.
469  virtual Group* MakeGroup(
470  const std::string& id,
471  const std::vector<const Connection*>& connections) = 0;
472 
473  /// Produces a RoadGeometry, with the ID `id`.
474  virtual std::unique_ptr<const api::RoadGeometry> Build(
475  const api::RoadGeometryId& id) const = 0;
476 };
477 
478 /// Factory interface to construct BuilderBase instances.
479 ///
480 /// Defined for testing purposes, and production code must use BuilderFactory
481 /// objects.
483  public:
485 
486  BuilderFactoryBase() = default;
487 
488  virtual ~BuilderFactoryBase() = default;
489 
490  /// Creates a BuilderBase instance.
491  ///
492  /// `lane_width`, `elevation_bounds`, `linear_tolerance`,
493  /// `angular_tolerance`, `scale_length` and `computation_policy` are
494  /// BuilderBase properties.
495  virtual std::unique_ptr<BuilderBase> Make(
496  double lane_width, const api::HBounds& elevation_bounds,
497  double linear_tolerance, double angular_tolerance,
498  double scale_length, ComputationPolicy computation_policy) const = 0;
499 };
500 
501 /// Convenient builder class which makes it easy to construct a multilane road
502 /// network.
503 ///
504 /// multilane is a simple road-network implementation:
505 /// - multiple lanes per segment;
506 /// - constant lane width, lane_bounds, and elevation_bounds, same for all
507 /// lanes;
508 /// - only linear and constant-curvature-arc primitives in XY-plane;
509 /// - cubic polynomials (parameterized on XY-arc-length) for elevation
510 /// and superelevation;
511 /// - superelevation (bank of road) rotates around the reference line (r = 0)
512 /// of the path.
513 ///
514 /// The Builder class simplifies the assembly of multilane road network
515 /// components into a valid RoadGeometry. In the Builder model, an Endpoint
516 /// specifies a point in world coordinates (along with a direction, slope,
517 /// and superelevation parameters). A Connection is a path from an explicit
518 /// start Endpoint to an end Endpoint calculated via a linear (LineOffset) or
519 /// arc displacement (ArcOffset). A Group is a collection of Connections.
520 ///
521 /// Builder::Build() constructs a RoadGeometry. Each Connection yields a
522 /// Segment bearing multiple Lanes. Each Group yields a Junction containing
523 /// the Segments associated with the grouped Connections; ungrouped
524 /// Connections each receive their own Junction.
525 ///
526 /// Specific suffixes are used to name Maliput entities. The following list
527 /// explains the naming convention:
528 ///
529 /// - Junctions: "j:" + Group::id(), or "j" + Connection::id() for an
530 /// ungrouped Connection.
531 /// - Segments: "s:" + Connection::id()
532 /// - Lanes: "l:" + Connection::id() + "_" + lane_index
533 /// - BranchPoints: "bp:" + branch_point_index
534 ///
535 /// Note: 'lane_index' is the index in the Segment, and 'branch_point_index' is
536 /// is the index in the RoadGeometry.
537 class Builder : public BuilderBase {
538  public:
540 
541  /// Constructs a Builder which can be used to specify and assemble a
542  /// multilane implementation of an api::RoadGeometry.
543  ///
544  /// `lane_width` is the width assigned to all Lanes. It must be greater or
545  /// equal to zero. Lane reference path (which are offsets of parent Segment
546  /// reference curve) are centered within the Lane. Lane spacing will be
547  /// `lane_width` too. Segment extents will be derived from the composition of
548  /// left and right shoulders, number of lanes and lane spacing. The
549  /// `elevation_bounds` is applied uniformly to all lanes of every segment.
550  /// `linear_tolerance` and `angular_tolerance` specify the respective
551  /// tolerances for the resulting RoadGeometry. `scale_length` constrains
552  /// the maximum level of detail captured by the resulting RoadGeometry.
553  /// `computation_policy` sets the speed vs. accuracy balance for computations.
554  /// `group_factory` allows to create groups.
555  Builder(double lane_width, const api::HBounds& elevation_bounds,
556  double linear_tolerance, double angular_tolerance,
557  double scale_length, ComputationPolicy computation_policy,
558  std::unique_ptr<GroupFactoryBase> group_factory);
559 
560  /// Gets `lane_width` value.
561  double get_lane_width() const override { return lane_width_; }
562 
563  /// Gets `elevation_bounds` value.
564  const api::HBounds& get_elevation_bounds() const override {
565  return elevation_bounds_;
566  }
567 
568  /// Gets `linear_tolerance` value.
569  double get_linear_tolerance() const override { return linear_tolerance_; }
570 
571  /// Gets `angular_tolerance` value.
572  double get_angular_tolerance() const override { return angular_tolerance_; }
573 
574  double get_scale_length() const override { return scale_length_; }
575 
577  return computation_policy_;
578  }
579 
580  const Connection* Connect(const std::string& id,
581  const LaneLayout& lane_layout,
582  const StartReference::Spec& start_spec,
583  const LineOffset& line_offset,
584  const EndReference::Spec& end_spec) override;
585 
586  const Connection* Connect(const std::string& id,
587  const LaneLayout& lane_layout,
588  const StartReference::Spec& start_spec,
589  const ArcOffset& arc_offset,
590  const EndReference::Spec& end_spec) override;
591 
592  const Connection* Connect(const std::string& id,
593  const LaneLayout& lane_layout,
594  const StartLane::Spec& start_spec,
595  const LineOffset& line_offset,
596  const EndLane::Spec& end_spec) override;
597 
598  const Connection* Connect(const std::string& id,
599  const LaneLayout& lane_layout,
600  const StartLane::Spec& start_spec,
601  const ArcOffset& arc_offset,
602  const EndLane::Spec& end_spec) override;
603 
604  void SetDefaultBranch(const Connection* in, int in_lane_index,
605  const api::LaneEnd::Which in_end, const Connection* out,
606  int out_lane_index,
607  const api::LaneEnd::Which out_end) override;
608 
609  Group* MakeGroup(const std::string& id) override;
610 
611  Group* MakeGroup(
612  const std::string& id,
613  const std::vector<const Connection*>& connections) override;
614 
615  std::unique_ptr<const api::RoadGeometry> Build(
616  const api::RoadGeometryId& id) const override;
617 
618  private:
619  // EndpointFuzzyOrder is an arbitrary strict complete ordering of Endpoints
620  // useful for, e.g., std::map. It provides a comparison operation that
621  // treats two Endpoints within `linear_tolerance` of one another as
622  // equivalent.
623  //
624  // This is used to match up the endpoints of Connections, to determine
625  // how Connections are linked to one another. Exact numeric equality
626  // would not be robust given the use of floating-point values in Endpoints.
627  class EndpointFuzzyOrder {
628  public:
629  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(EndpointFuzzyOrder)
630 
631  explicit EndpointFuzzyOrder(const double linear_tolerance)
632  : lin_tol_(linear_tolerance) {}
633 
634  bool operator()(const Endpoint& lhs, const Endpoint& rhs) const {
635  switch (fuzzy_compare(rhs.xy().x(), lhs.xy().x())) {
636  case -1: { return true; }
637  case 1: { return false; }
638  case 0: {
639  switch (fuzzy_compare(rhs.xy().y(), lhs.xy().y())) {
640  case -1: { return true; }
641  case 1: { return false; }
642  case 0: {
643  switch (fuzzy_compare(rhs.z().z(), lhs.z().z())) {
644  case -1: { return true; }
645  case 1: { return false; }
646  case 0: { return false; }
647  default: { DRAKE_ABORT(); }
648  }
649  }
650  default: { DRAKE_ABORT(); }
651  }
652  }
653  default: { DRAKE_ABORT(); }
654  }
655  }
656 
657  private:
658  int fuzzy_compare(const double a, const double b) const {
659  if (a < (b - lin_tol_)) {
660  return -1;
661  } else if (a > (b + lin_tol_)) {
662  return 1;
663  } else {
664  return 0;
665  }
666  }
667 
668  double lin_tol_{};
669  };
670 
671  struct DefaultBranch {
672  DefaultBranch() = default;
673 
674  DefaultBranch(const Connection* ain, int ain_lane_index,
675  const api::LaneEnd::Which ain_end, const Connection* aout,
676  int aout_lane_index, const api::LaneEnd::Which aout_end)
677  : in(ain),
678  in_lane_index(ain_lane_index),
679  in_end(ain_end),
680  out(aout),
681  out_lane_index(aout_lane_index),
682  out_end(aout_end) {}
683 
684  const Connection* in{};
685  const int in_lane_index{};
686  api::LaneEnd::Which in_end{};
687  const Connection* out{};
688  const int out_lane_index{};
689  api::LaneEnd::Which out_end{};
690  };
691 
692  std::vector<Lane*> BuildConnection(
693  const Connection* const cnx, Junction* const junction,
694  RoadGeometry* const rg,
695  std::map<Endpoint, BranchPoint*, EndpointFuzzyOrder>* const bp_map) const;
696 
697  BranchPoint* FindOrCreateBranchPoint(
698  const Endpoint& point,
699  RoadGeometry* rg,
700  std::map<Endpoint, BranchPoint*, EndpointFuzzyOrder>* const bp_map) const;
701 
702  void AttachBranchPoint(
703  const Endpoint& point, Lane* const lane, const api::LaneEnd::Which end,
704  RoadGeometry* rg,
705  std::map<Endpoint, BranchPoint*, EndpointFuzzyOrder>* bp_map) const;
706 
707  double lane_width_{};
708  api::HBounds elevation_bounds_;
709  double linear_tolerance_{};
710  double angular_tolerance_{};
711  double scale_length_{};
712  ComputationPolicy computation_policy_{};
713  std::unique_ptr<GroupFactoryBase> group_factory_;
714  std::vector<std::unique_ptr<Connection>> connections_;
715  std::vector<DefaultBranch> default_branches_;
716  std::vector<std::unique_ptr<Group>> groups_;
717 };
718 
719 /// Implements a BuilderFactoryBase to construct Builder objects.
721  public:
723 
724  BuilderFactory() = default;
725 
726  std::unique_ptr<BuilderBase> Make(
727  double lane_width, const api::HBounds& elevation_bounds,
728  double linear_tolerance, double angular_tolerance, double scale_length,
729  ComputationPolicy computation_policy) const override {
730  return std::make_unique<Builder>(lane_width, elevation_bounds,
731  linear_tolerance, angular_tolerance,
732  scale_length, computation_policy,
733  std::make_unique<GroupFactory>());
734  }
735 };
736 
737 } // namespace multilane
738 } // namespace maliput
739 } // namespace drake
optional< double > & get_mutable_theta_dot()
Definition: connection.h:100
const Endpoint kStart
Definition: multilane_builder_test.cc:560
double ref_r0() const
Definition: builder.h:338
double right_shoulder() const
Definition: builder.h:332
EndpointZ reverse() const
Returns an EndpointZ with reversed direction.
Definition: connection.h:88
double get_scale_length() const override
Gets scale_length value.
Definition: builder.h:574
Definition: bullet_model.cc:22
Complete set of parameters for an endpoint of a connection, specified in the world frame...
Definition: connection.h:119
Spec at(const Endpoint &endpoint, Direction direction) const
Builds a Spec at endpoint with direction direction.
Definition: builder.h:125
Defines how a Connection&#39;s reference curve starts.
Definition: builder.h:32
Convenient builder class which makes it easy to construct a multilane road network.
Definition: builder.h:537
A group of Connections.
Definition: connection.h:407
Provides methods to build an StartLane::Spec.
Definition: builder.h:82
int num_lanes
Definition: multilane_connection_test.cc:369
Direction direction
Definition: loader.cc:39
An implementation of api::BranchPoint.
Definition: branch_point.h:42
double y() const
Definition: connection.h:45
int lane_id() const
Definition: builder.h:231
Wraps all the lane-related specifications in a Connection.
Definition: builder.h:301
Base class for the multilane implementation of api::Lane.
Definition: lane.h:25
Provides methods to build an StartReference::Spec.
Definition: builder.h:26
double left_shoulder() const
Definition: builder.h:330
EndpointZ & get_mutable_z()
Definition: connection.h:138
double z() const
Definition: connection.h:92
Defines how a Connection&#39;s lane curve ends.
Definition: builder.h:225
Definition: arc_road_curve.cc:11
double get_lane_width() const override
Gets lane_width value.
Definition: builder.h:561
Factory interface to construct BuilderBase instances.
Definition: builder.h:482
Spec at(const Connection &connection, api::LaneEnd::Which end, Direction direction) const
Builds a Spec at connection&#39;s end side with direction direction.
Definition: builder.h:65
double x() const
Definition: connection.h:43
Spec at(const Connection &connection, int lane_id, api::LaneEnd::Which end, Direction direction) const
Builds a Spec at connection&#39;s lane_id lane at end side with direction direction.
Definition: builder.h:140
optional< int > lane_id
Definition: loader.cc:45
#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
int num_lanes() const
Definition: builder.h:334
double get_linear_tolerance() const override
Gets linear_tolerance value.
Definition: builder.h:569
Out-of-plane parameters for an endpoint of a connection, specified in the world frame.
Definition: connection.h:74
Specification for path offset along a line.
Definition: connection.h:152
double get_angular_tolerance() const override
Gets angular_tolerance value.
Definition: builder.h:572
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
const api::HBounds elevation_bounds
Definition: multilane_line_road_curve_test.cc:34
ComputationPolicy get_computation_policy() const override
Gets computation_policy value.
Definition: builder.h:576
int lane_id() const
Definition: builder.h:93
Spec z_at(const EndpointZ &endpoint_z, Direction direction) const
Builds an Spec at endpoint_z with direction direction.
Definition: builder.h:283
Endpoint LaneStart(int lane_index) const
Returns an Endpoint describing the start of the lane_index lane.
Definition: connection.cc:131
const Endpoint & end() const
Returns the parameters of the endpoint.
Definition: connection.h:297
TypeSpecificIdentifier< class RoadGeometry > RoadGeometryId
Persistent identifier for a RoadGeometry element.
Definition: road_geometry.h:23
Provides methods to build an EndLane::Spec.
Definition: builder.h:220
Bounds in the elevation dimension (h component) of a Lane-frame, consisting of a pair of minimum and ...
Definition: lane_data.h:360
const EndpointZ & z() const
Returns the subset of parameters pertaining to out-of-ground-plane aspects.
Definition: connection.h:136
Which
Labels for the endpoints of a Lane.
Definition: lane_data.h:27
optional< api::LaneEnd::Which > end
Definition: loader.cc:42
const api::HBounds & get_elevation_bounds() const override
Gets elevation_bounds value.
Definition: builder.h:564
int ref_lane() const
Definition: builder.h:336
std::ostream & operator<<(std::ostream &out, const StartReference::Spec &start_spec)
Streams a string representation of start_spec into out.
Definition: builder.cc:19
const double lane_width_
Definition: dragway_test.cc:342
ComputationPolicy
A policy to guide all computations in a RoadCurve, in terms of speed and accuracy.
Definition: road_curve.h:50
Defines how a Connection&#39;s reference curve ends.
Definition: builder.h:169
Representation of a reference path connecting two endpoints.
Definition: connection.h:216
Specification for path offset along a circular arc.
Definition: connection.h:178
Implements a BuilderFactoryBase to construct Builder objects.
Definition: builder.h:720
Endpoint reverse() const
Returns an Endpoint with reversed direction.
Definition: connection.h:129
#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 optional< double > & theta_dot() const
Definition: connection.h:98
Spec z_at(const EndpointZ &endpoint_z, Direction direction) const
Builds an Spec at endpoint_z with direction direction.
Definition: builder.h:208
Defines how a Connection&#39;s lane curve starts.
Definition: builder.h:87
Spec z_at(const Connection &connection, int lane_id, api::LaneEnd::Which end, Direction direction) const
Builds a Spec at connection&#39;s end side with direction direction.
Definition: builder.h:268
const EndpointXy & xy() const
Returns the subset of parameters pertaining to the xy ground-plane.
Definition: connection.h:132
A simple api::RoadGeometry implementation that only supports a single lane per segment.
Definition: road_geometry.h:21
An api::Junction implementation.
Definition: junction.h:19
Endpoint LaneEnd(int lane_index) const
Returns an Endpoint describing the end of the lane_index lane.
Definition: connection.cc:171
const Endpoint & start() const
Returns the parameters of the start point.
Definition: connection.h:294
std::unique_ptr< BuilderBase > Make(double lane_width, const api::HBounds &elevation_bounds, double linear_tolerance, double angular_tolerance, double scale_length, ComputationPolicy computation_policy) const override
Creates a BuilderBase instance.
Definition: builder.h:726
Provides methods to build an EndReference::Spec.
Definition: builder.h:163
#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
Direction
Defines the direction of an Endpoint or EndpointZ.
Definition: builder.h:23
Defines a builder interface for multilane.
Definition: builder.h:360
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
int num_lanes() const
Returns the number of lanes the Segment will contain.
Definition: connection.h:318