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 
16 
17 namespace drake {
18 namespace maliput {
19 namespace multilane {
20 class RoadGeometry;
21 
22 /// Convenient builder class which makes it easy to construct a multilane road
23 /// network.
24 ///
25 /// multilane is a simple road-network implementation:
26 /// - multiple lanes per segment;
27 /// - constant lane width, lane_bounds, and elevation_bounds, same for all
28 /// lanes;
29 /// - only linear and constant-curvature-arc primitives in XY-plane;
30 /// - cubic polynomials (parameterized on XY-arc-length) for elevation
31 /// and superelevation;
32 /// - superelevation (bank of road) rotates around the reference line (r = 0)
33 /// of the path.
34 ///
35 /// The Builder class simplifies the assembly of multilane road network
36 /// components into a valid RoadGeometry. In the Builder model, an Endpoint
37 /// specifies a point in world coordinates (along with a direction, slope,
38 /// and superelevation parameters). A Connection is a path from an explicit
39 /// start Endpoint to an end Endpoint calculated via a linear or arc
40 /// displacement (ArcOffset). A Group is a collection of Connections.
41 ///
42 /// Builder::Build() constructs a RoadGeometry. Each Connection yields a
43 /// Segment bearing multiple Lanes. Each Group yields a Junction containing
44 /// the Segments associated with the grouped Connections; ungrouped
45 /// Connections each receive their own Junction.
46 ///
47 /// Specific suffixes are used to name Maliput entities. The following list
48 /// explains the naming convention:
49 ///
50 /// - Junctions: "j:" + Group::id(), or "j" + Connection::id() for an
51 /// ungrouped Connection.
52 /// - Segments: "s:" + Connection::id()
53 /// - Lanes: "l:" + Connection::id() + "_" + lane_index
54 /// - BranchPoints: "bp:" + branch_point_index
55 ///
56 /// Note: 'lane_index' is the index in the Segment, and 'branch_point_index' is
57 /// is the index in the RoadGeometry.
58 class Builder {
59  public:
61 
62  /// Constructs a Builder which can be used to specify and assemble a
63  /// multilane implementation of an api::RoadGeometry.
64  ///
65  /// `lane_width` is the width assigned to all Lanes. It must be greater or
66  /// equal to zero. Lane reference path (which are offsets of parent Segment
67  /// reference curve) are centered within the Lane. Lane spacing will be
68  /// `lane_width` too. Segment extents will be derived from the composition of
69  /// left and right shoulders, number of lanes and lane spacing. The
70  /// `elevation_bounds` is applied uniformly to all lanes of every segment.
71  /// `linear_tolerance` and `angular_tolerance` specify the respective
72  /// tolerances for the resulting RoadGeometry.
73  Builder(double lane_width, const api::HBounds& elevation_bounds,
74  double linear_tolerance, double angular_tolerance);
75 
76  /// Connects `start` to an end-point linearly displaced from `start`.
77  /// `length` specifies the length of displacement (in the direction of the
78  /// heading of `start`). `z_end` specifies the elevation characteristics at
79  /// the end-point.
80  /// `r0` is the distance from the reference curve to the first Lane
81  /// centerline. `left_shoulder` and `right_shoulder` are extra lateral
82  /// distances added to the extents of the Segment after the first and last
83  /// Lanes positions are determined.
84  const Connection* Connect(const std::string& id, int num_lanes, double r0,
85  double left_shoulder, double right_shoulder,
86  const Endpoint& start, double length,
87  const EndpointZ& z_end);
88 
89  /// Connects `start` to an end-point displaced from `start` via an arc.
90  /// `arc` specifies the shape of the arc. `z_end` specifies the elevation
91  /// characteristics at the end-point.
92  /// `r0` is the distance from the reference curve to the first Lane
93  /// centerline. `left_shoulder` and `right_shoulder` are extra lateral
94  /// distances added to the extents of the Segment after the first and last
95  /// Lanes positions are determined.
96  const Connection* Connect(const std::string& id, int num_lanes, double r0,
97  double left_shoulder, double right_shoulder,
98  const Endpoint& start, const ArcOffset& arc,
99  const EndpointZ& z_end);
100 
101  /// Sets the default branch for one end of a connection.
102  ///
103  /// The default branch for the `in_end` of connection `in` at Lane
104  /// `in_lane_index`will set to be `out_end` of connection `out` at Lane
105  /// `out_lane_index`. The specified connections must actually be joined at the
106  /// specified ends (i.e., the Endpoint's for those ends must be coincident and
107  /// (anti)parallel within the tolerances for the Builder).
108  void SetDefaultBranch(const Connection* in, int in_lane_index,
109  const api::LaneEnd::Which in_end, const Connection* out,
110  int out_lane_index, const api::LaneEnd::Which out_end);
111 
112  /// Creates a new empty connection group with ID string `id`.
113  Group* MakeGroup(const std::string& id);
114 
115  /// Creates a new connection group with ID `id`, populated with the
116  /// given `connections`.
117  Group* MakeGroup(const std::string& id,
118  const std::vector<const Connection*>& connections);
119 
120  /// Produces a RoadGeometry, with the ID `id`.
121  std::unique_ptr<const api::RoadGeometry> Build(
122  const api::RoadGeometryId& id) const;
123 
124  private:
125  // EndpointFuzzyOrder is an arbitrary strict complete ordering of Endpoints
126  // useful for, e.g., std::map. It provides a comparison operation that
127  // treats two Endpoints within `linear_tolerance` of one another as
128  // equivalent.
129  //
130  // This is used to match up the endpoints of Connections, to determine
131  // how Connections are linked to one another. Exact numeric equality
132  // would not be robust given the use of floating-point values in Endpoints.
133  class EndpointFuzzyOrder {
134  public:
135  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(EndpointFuzzyOrder)
136 
137  explicit EndpointFuzzyOrder(const double linear_tolerance)
138  : lin_tol_(linear_tolerance) {}
139 
140  bool operator()(const Endpoint& lhs, const Endpoint& rhs) const {
141  switch (fuzzy_compare(rhs.xy().x(), lhs.xy().x())) {
142  case -1: { return true; }
143  case 1: { return false; }
144  case 0: {
145  switch (fuzzy_compare(rhs.xy().y(), lhs.xy().y())) {
146  case -1: { return true; }
147  case 1: { return false; }
148  case 0: {
149  switch (fuzzy_compare(rhs.z().z(), lhs.z().z())) {
150  case -1: { return true; }
151  case 1: { return false; }
152  case 0: { return false; }
153  default: { DRAKE_ABORT(); }
154  }
155  }
156  default: { DRAKE_ABORT(); }
157  }
158  }
159  default: { DRAKE_ABORT(); }
160  }
161  }
162 
163  private:
164  int fuzzy_compare(const double a, const double b) const {
165  if (a < (b - lin_tol_)) {
166  return -1;
167  } else if (a > (b + lin_tol_)) {
168  return 1;
169  } else {
170  return 0;
171  }
172  }
173 
174  double lin_tol_{};
175  };
176 
177  struct DefaultBranch {
178  DefaultBranch() = default;
179 
180  DefaultBranch(const Connection* ain, int ain_lane_index,
181  const api::LaneEnd::Which ain_end, const Connection* aout,
182  int aout_lane_index, const api::LaneEnd::Which aout_end)
183  : in(ain),
184  in_lane_index(ain_lane_index),
185  in_end(ain_end),
186  out(aout),
187  out_lane_index(aout_lane_index),
188  out_end(aout_end) {}
189 
190  const Connection* in{};
191  const int in_lane_index{};
192  api::LaneEnd::Which in_end{};
193  const Connection* out{};
194  const int out_lane_index{};
195  api::LaneEnd::Which out_end{};
196  };
197 
198  std::vector<Lane*> BuildConnection(
199  const Connection* const cnx, Junction* const junction,
200  RoadGeometry* const rg,
201  std::map<Endpoint, BranchPoint*, EndpointFuzzyOrder>* const bp_map) const;
202 
203  BranchPoint* FindOrCreateBranchPoint(
204  const Endpoint& point,
205  RoadGeometry* rg,
206  std::map<Endpoint, BranchPoint*, EndpointFuzzyOrder>* const bp_map) const;
207 
208  void AttachBranchPoint(
209  const Endpoint& point, Lane* const lane, const api::LaneEnd::Which end,
210  RoadGeometry* rg,
211  std::map<Endpoint, BranchPoint*, EndpointFuzzyOrder>* bp_map) const;
212 
213  const double lane_width_{};
214  const api::HBounds elevation_bounds_;
215  const double linear_tolerance_{};
216  const double angular_tolerance_{};
217  std::vector<std::unique_ptr<Connection>> connections_;
218  std::vector<DefaultBranch> default_branches_;
219  std::vector<std::unique_ptr<Group>> groups_;
220 };
221 
222 } // namespace multilane
223 } // namespace maliput
224 } // namespace drake
Definition: automotive_demo.cc:88
Complete set of parameters for an endpoint of a connection, specified in the world frame...
Definition: connection.h:113
Convenient builder class which makes it easy to construct a multilane road network.
Definition: builder.h:58
A group of Connections.
Definition: connection.h:332
An implementation of api::BranchPoint.
Definition: branch_point.h:42
double y() const
Definition: connection.h:44
Base class for the multilane implementation of api::Lane.
Definition: lane.h:25
std::unique_ptr< const api::RoadGeometry > Build(const api::RoadGeometryId &id) const
Produces a RoadGeometry, with the ID id.
Definition: builder.cc:174
Group * MakeGroup(const std::string &id)
Creates a new empty connection group with ID string id.
Definition: builder.cc:60
double z() const
Definition: connection.h:88
Definition: arc_road_curve.cc:11
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
void SetDefaultBranch(const Connection *in, int in_lane_index, const api::LaneEnd::Which in_end, const Connection *out, int out_lane_index, const api::LaneEnd::Which out_end)
Sets the default branch for one end of a connection.
Definition: builder.cc:51
Out-of-plane parameters for an endpoint of a connection, specified in the world frame.
Definition: connection.h:70
Provides Drake&#39;s assertion implementation.
const Connection * Connect(const std::string &id, int num_lanes, double r0, double left_shoulder, double right_shoulder, const Endpoint &start, double length, const EndpointZ &z_end)
Connects start to an end-point linearly displaced from start.
Definition: builder.cc:30
TypeSpecificIdentifier< class RoadGeometry > RoadGeometryId
Persistent identifier for a RoadGeometry element.
Definition: road_geometry.h:19
Bounds in the elevation dimension (h component) of a Lane-frame, consisting of a pair of minimum and ...
Definition: lane_data.h:343
const EndpointZ & z() const
Returns the subset of parameters pertaining to out-of-ground-plane aspects.
Definition: connection.h:130
Builder(const Builder &)=delete
Which
Labels for the endpoints of a Lane.
Definition: lane_data.h:24
const double lane_width_
Definition: dragway_test.cc:341
Representation of a reference path connecting two endpoints.
Definition: connection.h:180
Specification for path offset along a circular arc.
Definition: connection.h:147
#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 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
#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
Provides careful macros to selectively enable or disable the special member functions for copy-constr...