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 at(const Connection& connection, api::LaneEnd::Which end,
64  Direction direction) const {
65  const Endpoint endpoint = end == api::LaneEnd::Which::kStart
66  ? connection.start()
67  : connection.end();
68  return direction == Direction::kForward ? Spec(endpoint)
69  : Spec(endpoint.reverse());
70  }
71 };
72 
73 /// Provides methods to build an EndReference::Spec.
74 class EndReference {
75  public:
76  /// Defines how a Connection's reference curve ends.
77  ///
78  /// Objects of this class should be created using EndReference::z_at()
79  /// methods.
80  class Spec {
81  public:
83 
84  const EndpointZ& endpoint_z() const { return endpoint_z_; }
85 
86  private:
87  // Allows EndReference factory to build objects of this class.
88  friend class EndReference;
89 
90  /// Constructs a Spec at that specifies with `endpoint_z` how a
91  /// Connection's reference curve ends.
92  explicit Spec(const EndpointZ& endpoint_z) : endpoint_z_(endpoint_z) {}
93 
94  // Describes the connection's reference curve end-point.
95  EndpointZ endpoint_z_;
96  };
97 
99 
100  EndReference() = default;
101 
102  /// Builds a Spec at `connection`'s `end` side with `direction` direction.
103  /// When `direction` == `Direction::kReverse`, `end`-side endpoint's
104  /// EndpointZ is reversed.
105  Spec z_at(const Connection& connection, api::LaneEnd::Which end,
106  Direction direction) const {
107  const EndpointZ endpoint_z = end == api::LaneEnd::Which::kStart
108  ? connection.start().z()
109  : connection.end().z();
110  return direction == Direction::kForward ? Spec(endpoint_z)
111  : Spec(endpoint_z.reverse());
112  }
113 
114  /// Builds an Spec at `endpoint_z` with `direction` direction.
115  /// When `direction` == `Direction::kReverse`, `endpoint_z` is reversed.
116  Spec z_at(const EndpointZ& endpoint_z, Direction direction) const {
117  return direction == Direction::kForward ? Spec(endpoint_z)
118  : Spec(endpoint_z.reverse());
119  }
120 };
121 
122 /// Wraps all the lane-related specifications in a Connection.
123 class LaneLayout {
124  public:
126 
127  /// Constructs a the lane layout of a connection.
128  ///
129  /// Lane reference paths (which are offsets of parent Segment reference curve)
130  /// are centered within the Lane. Lane spacing will be road geometry fixed
131  /// lane's width. Segment extents will be derived from the composition of
132  /// `left_shoulder` and `right_shoulder` shoulders, number of lanes and lane
133  /// spacing. `ref_lane` lane's centerline will be placed at `ref_r0` distance
134  /// from connection's reference curve.
135  ///
136  /// `left_shoulder` and `right_shoulder` must be nonnegative.
137  /// `num_lanes` must be positive and `ref_lane` must be nonnegative and
138  /// smaller than `num_lanes`.
139  LaneLayout(double left_shoulder, double right_shoulder, int num_lanes,
140  int ref_lane, double ref_r0)
141  : left_shoulder_(left_shoulder),
142  right_shoulder_(right_shoulder),
143  num_lanes_(num_lanes),
144  ref_lane_(ref_lane),
145  ref_r0_(ref_r0) {
146  DRAKE_DEMAND(left_shoulder_ >= 0.);
147  DRAKE_DEMAND(right_shoulder_ >= 0.);
148  DRAKE_DEMAND(num_lanes_ > 0);
149  DRAKE_DEMAND(ref_lane_ >= 0 && ref_lane_ < num_lanes_);
150  }
151 
152  double left_shoulder() const { return left_shoulder_; }
153 
154  double right_shoulder() const { return right_shoulder_; }
155 
156  int num_lanes() const { return num_lanes_; }
157 
158  int ref_lane() const { return ref_lane_; }
159 
160  double ref_r0() const { return ref_r0_; }
161 
162  private:
163  // Extra space added to the right of the first lane.
164  double left_shoulder_{};
165  // Extra space added to the left of the last lane.
166  double right_shoulder_{};
167  // Number of lanes.
168  int num_lanes_{};
169  // Index of the lane from which `ref_r0_` is defined.
170  int ref_lane_{};
171  // Distance from `ref_lane_` lane's centerline to reference curve.
172  double ref_r0_{};
173 };
174 
175 /// Streams a string representation of `lane_layout` into `out`. Returns `out`.
176 /// This method is provided for the purposes of debugging or text-logging.
177 /// It is not intended for serialization.
178 std::ostream& operator<<(std::ostream& out, const LaneLayout& lane_layout);
179 
180 /// Defines a builder interface for multilane. It is used for testing purposes
181 /// only, and derived code should instantiate Builder objects.
182 class BuilderBase {
183  public:
185 
186  BuilderBase() = default;
187 
188  virtual ~BuilderBase() = default;
189 
190  /// Gets `lane_width` value.
191  virtual double get_lane_width() const = 0;
192 
193  /// Gets `elevation_bounds` value.
194  virtual const api::HBounds& get_elevation_bounds() const = 0;
195 
196  /// Gets `linear_tolerance` value.
197  virtual double get_linear_tolerance() const = 0;
198 
199  /// Gets `angular_tolerance` value.
200  virtual double get_angular_tolerance() const = 0;
201 
202  /// Connects `start_spec`'s Endpoint to an end-point linearly displaced from
203  /// `start_spec`'s Endpoint.
204  ///
205  /// `line_offset` specifies the length of displacement (in the direction of
206  /// the heading of `start_spec`'s Endpoint). `end_spec` specifies the
207  /// elevation characteristics at the end-point.
208  /// `lane_layout` defines the number of lanes, their width, extra shoulder
209  /// asphalt extensions and placing with respect to connection's reference
210  /// curve.
211  virtual const Connection* Connect(const std::string& id,
212  const LaneLayout& lane_layout,
213  const StartReference::Spec& start_spec,
214  const LineOffset& line_offset,
215  const EndReference::Spec& end_spec) = 0;
216 
217  /// Connects `start_spec`'s Endpoint to an end-point displaced from
218  /// `start_spec`'s Endpoint via an arc.
219  ///
220  /// `arc_offset` specifies the shape of the arc. `end_spec` specifies the
221  /// elevation characteristics at the end-point.
222  /// `r0` is the distance from the reference curve to the first Lane
223  /// centerline. `left_shoulder` and `right_shoulder` are extra lateral
224  /// distances added to the extents of the Segment after the first and last
225  /// Lanes positions are determined.
226  /// `lane_layout` defines the number of lanes, their width, extra shoulder
227  /// asphalt extensions and placing with respect to connection's reference
228  /// curve.
229  virtual const Connection* Connect(const std::string& id,
230  const LaneLayout& lane_layout,
231  const StartReference::Spec& start_spec,
232  const ArcOffset& arc_offset,
233  const EndReference::Spec& end_spec) = 0;
234 
235  /// Sets the default branch for one end of a connection.
236  ///
237  /// The default branch for the `in_end` of connection `in` at Lane
238  /// `in_lane_index`will set to be `out_end` of connection `out` at Lane
239  /// `out_lane_index`. The specified connections must actually be joined at the
240  /// specified ends (i.e., the Endpoint's for those ends must be coincident and
241  /// (anti)parallel within the tolerances for the Builder).
242  virtual void SetDefaultBranch(const Connection* in, int in_lane_index,
243  api::LaneEnd::Which in_end,
244  const Connection* out, int out_lane_index,
245  api::LaneEnd::Which out_end) = 0;
246 
247  /// Creates a new empty connection group with ID string `id`.
248  virtual Group* MakeGroup(const std::string& id) = 0;
249 
250  /// Creates a new connection group with ID `id`, populated with the
251  /// given `connections`.
252  virtual Group* MakeGroup(
253  const std::string& id,
254  const std::vector<const Connection*>& connections) = 0;
255 
256  /// Produces a RoadGeometry, with the ID `id`.
257  virtual std::unique_ptr<const api::RoadGeometry> Build(
258  const api::RoadGeometryId& id) const = 0;
259 };
260 
261 /// Factory interface to construct BuilderBase instances.
262 ///
263 /// Defined for testing purposes, and production code must use BuilderFactory
264 /// objects.
266  public:
268 
269  BuilderFactoryBase() = default;
270 
271  virtual ~BuilderFactoryBase() = default;
272 
273  /// Creates a BuilderBase instance.
274  ///
275  /// `lane_width`, `elevation_bounds`, `linear_tolerance` and
276  /// `angular_tolerance` are BuilderBase properties.
277  virtual std::unique_ptr<BuilderBase> Make(
278  double lane_width, const api::HBounds& elevation_bounds,
279  double linear_tolerance, double angular_tolerance) const = 0;
280 };
281 
282 /// Convenient builder class which makes it easy to construct a multilane road
283 /// network.
284 ///
285 /// multilane is a simple road-network implementation:
286 /// - multiple lanes per segment;
287 /// - constant lane width, lane_bounds, and elevation_bounds, same for all
288 /// lanes;
289 /// - only linear and constant-curvature-arc primitives in XY-plane;
290 /// - cubic polynomials (parameterized on XY-arc-length) for elevation
291 /// and superelevation;
292 /// - superelevation (bank of road) rotates around the reference line (r = 0)
293 /// of the path.
294 ///
295 /// The Builder class simplifies the assembly of multilane road network
296 /// components into a valid RoadGeometry. In the Builder model, an Endpoint
297 /// specifies a point in world coordinates (along with a direction, slope,
298 /// and superelevation parameters). A Connection is a path from an explicit
299 /// start Endpoint to an end Endpoint calculated via a linear (LineOffset) or
300 /// arc displacement (ArcOffset). A Group is a collection of Connections.
301 ///
302 /// Builder::Build() constructs a RoadGeometry. Each Connection yields a
303 /// Segment bearing multiple Lanes. Each Group yields a Junction containing
304 /// the Segments associated with the grouped Connections; ungrouped
305 /// Connections each receive their own Junction.
306 ///
307 /// Specific suffixes are used to name Maliput entities. The following list
308 /// explains the naming convention:
309 ///
310 /// - Junctions: "j:" + Group::id(), or "j" + Connection::id() for an
311 /// ungrouped Connection.
312 /// - Segments: "s:" + Connection::id()
313 /// - Lanes: "l:" + Connection::id() + "_" + lane_index
314 /// - BranchPoints: "bp:" + branch_point_index
315 ///
316 /// Note: 'lane_index' is the index in the Segment, and 'branch_point_index' is
317 /// is the index in the RoadGeometry.
318 class Builder : public BuilderBase {
319  public:
321 
322  /// Constructs a Builder which can be used to specify and assemble a
323  /// multilane implementation of an api::RoadGeometry.
324  ///
325  /// `lane_width` is the width assigned to all Lanes. It must be greater or
326  /// equal to zero. Lane reference path (which are offsets of parent Segment
327  /// reference curve) are centered within the Lane. Lane spacing will be
328  /// `lane_width` too. Segment extents will be derived from the composition of
329  /// left and right shoulders, number of lanes and lane spacing. The
330  /// `elevation_bounds` is applied uniformly to all lanes of every segment.
331  /// `linear_tolerance` and `angular_tolerance` specify the respective
332  /// tolerances for the resulting RoadGeometry.
333  Builder(double lane_width, const api::HBounds& elevation_bounds,
334  double linear_tolerance, double angular_tolerance);
335 
336  /// Gets `lane_width` value.
337  double get_lane_width() const override { return lane_width_; }
338 
339  /// Gets `elevation_bounds` value.
340  const api::HBounds& get_elevation_bounds() const override {
341  return elevation_bounds_;
342  }
343 
344  /// Gets `linear_tolerance` value.
345  double get_linear_tolerance() const override { return linear_tolerance_; }
346 
347  /// Gets `angular_tolerance` value.
348  double get_angular_tolerance() const override { return angular_tolerance_; }
349 
350  const Connection* Connect(const std::string& id,
351  const LaneLayout& lane_layout,
352  const StartReference::Spec& start_spec,
353  const LineOffset& line_offset,
354  const EndReference::Spec& end_spec) override;
355 
356  const Connection* Connect(const std::string& id,
357  const LaneLayout& lane_layout,
358  const StartReference::Spec& start_spec,
359  const ArcOffset& arc_offset,
360  const EndReference::Spec& end_spec) override;
361 
362  void SetDefaultBranch(const Connection* in, int in_lane_index,
363  const api::LaneEnd::Which in_end, const Connection* out,
364  int out_lane_index,
365  const api::LaneEnd::Which out_end) override;
366 
367  Group* MakeGroup(const std::string& id) override;
368 
369  Group* MakeGroup(const std::string& id,
370  const std::vector<const Connection*>& connections) override;
371 
372  std::unique_ptr<const api::RoadGeometry> Build(
373  const api::RoadGeometryId& id) const override;
374 
375  private:
376  // EndpointFuzzyOrder is an arbitrary strict complete ordering of Endpoints
377  // useful for, e.g., std::map. It provides a comparison operation that
378  // treats two Endpoints within `linear_tolerance` of one another as
379  // equivalent.
380  //
381  // This is used to match up the endpoints of Connections, to determine
382  // how Connections are linked to one another. Exact numeric equality
383  // would not be robust given the use of floating-point values in Endpoints.
384  class EndpointFuzzyOrder {
385  public:
386  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(EndpointFuzzyOrder)
387 
388  explicit EndpointFuzzyOrder(const double linear_tolerance)
389  : lin_tol_(linear_tolerance) {}
390 
391  bool operator()(const Endpoint& lhs, const Endpoint& rhs) const {
392  switch (fuzzy_compare(rhs.xy().x(), lhs.xy().x())) {
393  case -1: { return true; }
394  case 1: { return false; }
395  case 0: {
396  switch (fuzzy_compare(rhs.xy().y(), lhs.xy().y())) {
397  case -1: { return true; }
398  case 1: { return false; }
399  case 0: {
400  switch (fuzzy_compare(rhs.z().z(), lhs.z().z())) {
401  case -1: { return true; }
402  case 1: { return false; }
403  case 0: { return false; }
404  default: { DRAKE_ABORT(); }
405  }
406  }
407  default: { DRAKE_ABORT(); }
408  }
409  }
410  default: { DRAKE_ABORT(); }
411  }
412  }
413 
414  private:
415  int fuzzy_compare(const double a, const double b) const {
416  if (a < (b - lin_tol_)) {
417  return -1;
418  } else if (a > (b + lin_tol_)) {
419  return 1;
420  } else {
421  return 0;
422  }
423  }
424 
425  double lin_tol_{};
426  };
427 
428  struct DefaultBranch {
429  DefaultBranch() = default;
430 
431  DefaultBranch(const Connection* ain, int ain_lane_index,
432  const api::LaneEnd::Which ain_end, const Connection* aout,
433  int aout_lane_index, const api::LaneEnd::Which aout_end)
434  : in(ain),
435  in_lane_index(ain_lane_index),
436  in_end(ain_end),
437  out(aout),
438  out_lane_index(aout_lane_index),
439  out_end(aout_end) {}
440 
441  const Connection* in{};
442  const int in_lane_index{};
443  api::LaneEnd::Which in_end{};
444  const Connection* out{};
445  const int out_lane_index{};
446  api::LaneEnd::Which out_end{};
447  };
448 
449  std::vector<Lane*> BuildConnection(
450  const Connection* const cnx, Junction* const junction,
451  RoadGeometry* const rg,
452  std::map<Endpoint, BranchPoint*, EndpointFuzzyOrder>* const bp_map) const;
453 
454  BranchPoint* FindOrCreateBranchPoint(
455  const Endpoint& point,
456  RoadGeometry* rg,
457  std::map<Endpoint, BranchPoint*, EndpointFuzzyOrder>* const bp_map) const;
458 
459  void AttachBranchPoint(
460  const Endpoint& point, Lane* const lane, const api::LaneEnd::Which end,
461  RoadGeometry* rg,
462  std::map<Endpoint, BranchPoint*, EndpointFuzzyOrder>* bp_map) const;
463 
464  double lane_width_{};
465  api::HBounds elevation_bounds_;
466  double linear_tolerance_{};
467  double angular_tolerance_{};
468  std::vector<std::unique_ptr<Connection>> connections_;
469  std::vector<DefaultBranch> default_branches_;
470  std::vector<std::unique_ptr<Group>> groups_;
471 };
472 
473 /// Implements a BuilderFactoryBase to construct Builder objects.
475  public:
477 
478  BuilderFactory() = default;
479 
480  std::unique_ptr<BuilderBase> Make(double lane_width,
482  double linear_tolerance,
483  double angular_tolerance) const override {
484  return std::make_unique<Builder>(lane_width, elevation_bounds,
485  linear_tolerance, angular_tolerance);
486  }
487 };
488 
489 } // namespace multilane
490 } // namespace maliput
491 } // namespace drake
double ref_r0() const
Definition: builder.h:160
double right_shoulder() const
Definition: builder.h:154
EndpointZ reverse() const
Returns an EndpointZ with reversed direction.
Definition: connection.h:84
Definition: automotive_demo.cc:90
Complete set of parameters for an endpoint of a connection, specified in the world frame...
Definition: connection.h:113
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:318
A group of Connections.
Definition: connection.h:360
std::ostream & operator<<(std::ostream &out, const LaneLayout &lane_layout)
Streams a string representation of lane_layout into out.
Definition: builder.cc:19
int num_lanes
Definition: multilane_connection_test.cc:365
An implementation of api::BranchPoint.
Definition: branch_point.h:42
double y() const
Definition: connection.h:44
Wraps all the lane-related specifications in a Connection.
Definition: builder.h:123
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:152
double z() const
Definition: connection.h:88
Definition: arc_road_curve.cc:11
double get_lane_width() const override
Gets lane_width value.
Definition: builder.h:337
Factory interface to construct BuilderBase instances.
Definition: builder.h:265
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:63
double x() const
Definition: connection.h:42
#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:156
double get_linear_tolerance() const override
Gets linear_tolerance value.
Definition: builder.h:345
Out-of-plane parameters for an endpoint of a connection, specified in the world frame.
Definition: connection.h:70
Specification for path offset along a line.
Definition: connection.h:144
double get_angular_tolerance() const override
Gets angular_tolerance value.
Definition: builder.h:348
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:28
const Endpoint & end() const
Returns the parameters of the endpoint.
Definition: connection.h:271
TypeSpecificIdentifier< class RoadGeometry > RoadGeometryId
Persistent identifier for a RoadGeometry element.
Definition: road_geometry.h:19
std::unique_ptr< BuilderBase > Make(double lane_width, const api::HBounds &elevation_bounds, double linear_tolerance, double angular_tolerance) const override
Creates a BuilderBase instance.
Definition: builder.h:480
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:130
Which
Labels for the endpoints of a Lane.
Definition: lane_data.h:27
const api::HBounds & get_elevation_bounds() const override
Gets elevation_bounds value.
Definition: builder.h:340
int ref_lane() const
Definition: builder.h:158
const double lane_width_
Definition: dragway_test.cc:341
Defines how a Connection&#39;s reference curve ends.
Definition: builder.h:80
Representation of a reference path connecting two endpoints.
Definition: connection.h:208
Specification for path offset along a circular arc.
Definition: connection.h:170
Implements a BuilderFactoryBase to construct Builder objects.
Definition: builder.h:474
Endpoint reverse() const
Returns an Endpoint with reversed direction.
Definition: connection.h:123
#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
Spec z_at(const EndpointZ &endpoint_z, Direction direction) const
Builds an Spec at endpoint_z with direction direction.
Definition: builder.h:116
const EndpointXy & xy() const
Returns the subset of parameters pertaining to the xy ground-plane.
Definition: connection.h:126
A simple api::RoadGeometry implementation that only supports a single lane per segment.
Definition: road_geometry.h:20
An api::Junction implementation.
Definition: junction.h:18
const Endpoint & start() const
Returns the parameters of the start point.
Definition: connection.h:268
Provides methods to build an EndReference::Spec.
Definition: builder.h:74
#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:182
Provides careful macros to selectively enable or disable the special member functions for copy-constr...