Drake
connection.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <cmath>
4 #include <memory>
5 #include <ostream>
6 #include <string>
7 #include <unordered_set>
8 #include <vector>
9 
13 
14 namespace drake {
15 namespace maliput {
16 namespace multilane {
17 
18 /// XY-plane-only parameters for an endpoint of a connection, specified in
19 /// the world frame.
20 ///
21 /// The three components are:
22 /// - x: x position
23 /// - y: y position
24 /// - heading: counter-clockwise around z=cross(x,y). It is the heading of
25 /// reference path (radians, zero == x-direction).
26 class EndpointXy {
27  public:
29 
30  // Constructs an EndpointXy with all zero parameters.
31  EndpointXy() = default;
32 
33  EndpointXy(double x, double y, double heading)
34  : x_(x), y_(y), heading_(heading) {}
35 
36  /// Returns an EndpointXy with reversed direction.
37  EndpointXy reverse() const {
38  return EndpointXy(x_, y_,
39  std::atan2(-std::sin(heading_), -std::cos(heading_)));
40  }
41 
42  double x() const { return x_; }
43 
44  double y() const { return y_; }
45 
46  double heading() const { return heading_; }
47 
48  private:
49  double x_{};
50  double y_{};
51  double heading_{};
52 };
53 
54 /// Streams a string representation of `endpoint_xy` into `out`. Returns
55 /// `out`. This method is provided for the purposes of debugging or
56 /// text-logging. It is not intended for serialization.
57 std::ostream& operator<<(std::ostream& out, const EndpointXy& endpoint_xy);
58 
59 /// Out-of-plane parameters for an endpoint of a connection, specified in
60 /// the world frame.
61 ///
62 /// The four components are:
63 /// - z: elevation
64 /// - z_dot: grade (rate of change of elevation with respect to
65 /// arc length of the reference path)
66 /// - theta: superelevation (rotation of road surface around r = 0 centerline;
67 /// when theta > 0, elevation at r > 0 is above elevation at r < 0)
68 /// - theta_dot: rate of change of superelevation with respect to arc length
69 /// of the reference path
70 class EndpointZ {
71  public:
73  // Constructs an EndpointZ with all zero parameters.
74  EndpointZ() = default;
75 
76  EndpointZ(double z, double z_dot, double theta, double theta_dot)
77  : z_(z), z_dot_(z_dot), theta_(theta), theta_dot_(theta_dot) {}
78 
79  /// Returns an EndpointZ with reversed direction.
80  ///
81  /// Reversing direction is equivalent to rotating s (and along with it, r)
82  /// around the h-axis by 180 degrees, thus flipping the signs of z_dot
83  /// and theta.
84  EndpointZ reverse() const {
85  return EndpointZ(z_, -z_dot_, -theta_, theta_dot_);
86  }
87 
88  double z() const { return z_; }
89 
90  double z_dot() const { return z_dot_; }
91 
92  double theta() const { return theta_; }
93 
94  double theta_dot() const { return theta_dot_; }
95 
96  private:
97  double z_{};
98  double z_dot_{};
99 
100  double theta_{};
101  double theta_dot_{};
102 };
103 
104 /// Streams a string representation of `endpoint_z` into `out`. Returns
105 /// `out`. This method is provided for the purposes of debugging or
106 /// text-logging. It is not intended for serialization.
107 std::ostream& operator<<(std::ostream& out, const EndpointZ& endpoint_z);
108 
109 /// Complete set of parameters for an endpoint of a connection,
110 /// specified in the world frame. It comprises two subsets of parameters:
111 /// those pertaining only to the xy ground-plane, and those pertaining to
112 /// out-of-plane aspects of an endpoint.
113 class Endpoint {
114  public:
116 
117  // Constructs an Endpoint with all zero parameters.
118  Endpoint() = default;
119 
120  Endpoint(const EndpointXy& xy, const EndpointZ& z) : xy_(xy), z_(z) {}
121 
122  /// Returns an Endpoint with reversed direction.
123  Endpoint reverse() const { return Endpoint(xy_.reverse(), z_.reverse()); }
124 
125  /// Returns the subset of parameters pertaining to the xy ground-plane.
126  const EndpointXy& xy() const { return xy_; }
127 
128  /// Returns the subset of parameters pertaining to out-of-ground-plane
129  /// aspects.
130  const EndpointZ& z() const { return z_; }
131 
132  private:
133  EndpointXy xy_;
134  EndpointZ z_;
135 };
136 
137 /// Streams a string representation of `endpoint` into `out`. Returns
138 /// `out`. This method is provided for the purposes of debugging or
139 /// text-logging. It is not intended for serialization.
140 std::ostream& operator<<(std::ostream& out, const Endpoint& endpoint);
141 
142 /// Specification for path offset along a circular arc.
143 /// * radius: radius of the arc, which must be positive
144 /// * d_theta: angle of arc segment (Δθ)
145 /// * d_theta > 0 is counterclockwise ('veer to left')
146 /// * d_theta < 0 is clockwise ('veer to right')
147 class ArcOffset {
148  public:
150 
151  /// Constructs an ArcOffset with all zero parameters.
152  ArcOffset() = default;
153 
154  ArcOffset(double radius, double d_theta)
155  : radius_(radius), d_theta_(d_theta) {
156  DRAKE_DEMAND(radius_ > 0.);
157  }
158 
159  double radius() const { return radius_; }
160 
161  double d_theta() const { return d_theta_; }
162 
163  private:
164  double radius_{};
165  double d_theta_{};
166 };
167 
168 /// Representation of a reference path connecting two endpoints.
169 ///
170 /// Upon building the RoadGeometry, a Connection yields a Segment
171 /// bearing multiple Lanes with offsets from the reference path. The
172 /// Segment will belong to its own Junction, unless the Connection was
173 /// grouped with other Connections into a Group.
174 ///
175 /// Two connection geometries are supported: line and arc. These
176 /// primitives determine the projection of the reference path onto the
177 /// (locally-flat) plane of the earth. The out-of-plane shape of
178 /// the path will be determined by the EndpointZ (elevation) parameters
179 /// of the endpoints.
180 class Connection {
181  public:
183 
184  /// Possible connection geometries: line- or arc-segment.
185  enum Type { kLine, kArc };
186 
187  /// Constructs a line-segment connection.
188  ///
189  /// Segment's reference curve begins at `start` and extends on the plane
190  /// z=0 `line_length` distance with `start.xy().heading()` heading angle.
191  /// `end_z` will be used to build elevation and superelevation information of
192  /// the road.
193  ///
194  /// `line_length` must be non negative.
195  ///
196  /// Segments will contain `num_lanes` lanes, which must be greater than zero.
197  /// First Lane centerline will be placed at `r0` distance from the reference
198  /// curve. Each lane's width will be `lane_width`, which should be greater or
199  /// equal to zero.
200  ///
201  /// `left_shoulder` and `right_shoulder` are extra spaces added to the right
202  /// and left side of the first and last lanes of the Segment. They will be
203  /// added to Segment's bounds and must be greater or equal to zero.
204  Connection(const std::string& id, const Endpoint& start,
205  const EndpointZ& end_z, int num_lanes, double r0,
206  double lane_width, double left_shoulder, double right_shoulder,
207  double line_length);
208 
209  /// Constructs an arc-segment connection.
210  ///
211  /// Segment's reference curve begins at `start` and extends on the plane z=0
212  /// with `arc_offset.radius()` and angle span of `arc_offset.d_theta()`.
213  /// `end_z` will be used to build elevation and superelevation information of
214  /// the road.
215  ///
216  /// `arc_offset.radius()` must be positive. `arc_offset.d_theta()` > 0
217  /// indicates a counterclockwise arc from `start` with initial heading angle
218  /// `start.heading()`.
219  ///
220  /// Segments will contain `num_lanes` lanes, which must be greater than zero.
221  /// First Lane centerline will be placed at `r0` distance from the reference
222  /// curve. Each lane's width will be `lane_width`, which should be greater or
223  /// equal to zero.
224  ///
225  /// `left_shoulder` and `right_shoulder` are extra spaces added to the right
226  /// and left side of the first and last lanes of the Segment. They will be
227  /// added to Segment's bounds and must be greater or equal to zero.
228  Connection(const std::string& id, const Endpoint& start,
229  const EndpointZ& end_z, int num_lanes, double r0,
230  double lane_width, double left_shoulder, double right_shoulder,
231  const ArcOffset& arc_offset);
232 
233  /// Returns the geometric type of the path.
234  Type type() const { return type_; }
235 
236  /// Returns the ID string.
237  const std::string& id() const { return id_; }
238 
239  /// Returns the parameters of the start point.
240  const Endpoint& start() const { return start_; }
241 
242  /// Returns the parameters of the endpoint.
243  const Endpoint& end() const { return end_; }
244 
245  /// Returns the length of the line (for line connections only).
246  double line_length() const {
247  DRAKE_DEMAND(type_ == kLine);
248  return line_length_;
249  }
250 
251  /// Returns the radius of the arc (for arc connections only).
252  double radius() const {
253  DRAKE_DEMAND(type_ == kArc);
254  return radius_;
255  }
256 
257  /// Returns the angle of the arc (for arc connections only).
258  double d_theta() const {
259  DRAKE_DEMAND(type_ == kArc);
260  return d_theta_;
261  }
262 
263  /// Returns the number of lanes the Segment will contain.
264  int num_lanes() const { return num_lanes_; }
265 
266  /// Returns the lateral offset from the reference curve to the first Lane
267  /// centerline.
268  double r0() const { return r0_; }
269 
270  /// Returns lanes' width.
271  double lane_width() const { return lane_width_; }
272 
273  /// Returns the left shoulder distance of the segment.
274  double left_shoulder() const { return left_shoulder_; }
275 
276  /// Returns the right shoulder distance of the segment.
277  double right_shoulder() const { return right_shoulder_; }
278 
279  /// Returns `lane_index` lane lateral distance to the reference curve.
280  ///
281  /// `lane_index` must be non-negative and smaller than the number of lanes of
282  /// this connection.
283  double lane_offset(int lane_index) const {
284  DRAKE_DEMAND(lane_index >= 0 && lane_index < num_lanes_);
285  return r0_ + lane_width_ * static_cast<double>(lane_index);
286  }
287 
288  /// Returns the distance from the reference curve to the right extent of the
289  /// Segment.
290  double r_min() const { return r_min_; }
291 
292  /// Returns the distance from the reference curve to the left extent of the
293  /// Segment.
294  double r_max() const { return r_max_; }
295 
296  /// Returns an Endpoint describing the start of the `lane_index` lane.
297  Endpoint LaneStart(int lane_index) const;
298 
299  /// Returns an Endpoint describing the end of the `lane_index` lane.
300  Endpoint LaneEnd(int lane_index) const;
301 
302  /// Creates a RoadCurve that describes the reference curve of this Connection.
303  std::unique_ptr<RoadCurve> CreateRoadCurve() const;
304 
305  private:
306  const Type type_{};
307  const std::string id_;
308  const Endpoint start_{};
309  Endpoint end_{};
310  const int num_lanes_{};
311  const double r0_{};
312  const double lane_width_{};
313  const double left_shoulder_{};
314  const double right_shoulder_{};
315  const double r_min_{};
316  const double r_max_{};
317  std::unique_ptr<RoadCurve> road_curve_;
318  // Bits specific to type_ == kLine:
319  double line_length_{};
320  // Bits specific to type_ == kArc:
321  double radius_{};
322  double d_theta_{};
323  double theta0_{};
324  double cx_{};
325  double cy_{};
326 };
327 
328 /// A group of Connections.
329 ///
330 /// Upon building the RoadGeometry, a Group yields a Junction containing the
331 /// corresponding Segments specified by all the Connections in the Group.
332 class Group {
333  public:
335 
336  /// Constructs an empty Group with the specified `id`.
337  explicit Group(const std::string& id) : id_(id) {}
338 
339  /// Constructs a Group with `id`, populated by `connections`.
340  ///
341  /// `connections` must not contain duplicates.
342  Group(const std::string& id,
343  const std::vector<const Connection*>& connections)
344  : id_(id) {
345  for (const Connection* connection : connections) {
346  Add(connection);
347  }
348  }
349 
350  /// Adds a Connection.
351  ///
352  /// `connection` must not already be added.
353  void Add(const Connection* connection) {
354  auto result = connection_set_.insert(connection);
355  DRAKE_DEMAND(result.second);
356  connection_vector_.push_back(connection);
357  }
358 
359  /// Returns the ID string.
360  const std::string& id() const { return id_; }
361 
362  /// Returns the grouped Connections.
363  const std::vector<const Connection*>& connections() const {
364  return connection_vector_;
365  }
366 
367  private:
368  std::string id_;
369  std::unordered_set<const Connection*> connection_set_;
370  std::vector<const Connection*> connection_vector_;
371 };
372 
373 } // namespace multilane
374 } // namespace maliput
375 } // namespace drake
const Expression xy_
Definition: symbolic_polynomial_test.cc:47
XY-plane-only parameters for an endpoint of a connection, specified in the world frame.
Definition: connection.h:26
const std::vector< const Connection * > & connections() const
Returns the grouped Connections.
Definition: connection.h:363
Type type() const
Returns the geometric type of the path.
Definition: connection.h:234
EndpointXy(double x, double y, double heading)
Definition: connection.h:33
EndpointZ reverse() const
Returns an EndpointZ with reversed direction.
Definition: connection.h:84
Definition: automotive_demo.cc:88
Complete set of parameters for an endpoint of a connection, specified in the world frame...
Definition: connection.h:113
Expression sin(const Expression &e)
Definition: symbolic_expression.cc:599
A group of Connections.
Definition: connection.h:332
double lane_width() const
Returns lanes&#39; width.
Definition: connection.h:271
ArcOffset(double radius, double d_theta)
Definition: connection.h:154
Expression atan2(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:652
STL namespace.
double lane_offset(int lane_index) const
Returns lane_index lane lateral distance to the reference curve.
Definition: connection.h:283
EndpointXy reverse() const
Returns an EndpointXy with reversed direction.
Definition: connection.h:37
double line_length() const
Returns the length of the line (for line connections only).
Definition: connection.h:246
double r_min() const
Returns the distance from the reference curve to the right extent of the Segment. ...
Definition: connection.h:290
double y() const
Definition: connection.h:44
double theta() const
Definition: connection.h:92
double d_theta() const
Returns the angle of the arc (for arc connections only).
Definition: connection.h:258
double z() const
Definition: connection.h:88
double theta_dot() const
Definition: connection.h:94
std::vector< Number > result
Definition: ipopt_solver.cc:151
Definition: arc_road_curve.cc:11
void Add(const Connection *connection)
Adds a Connection.
Definition: connection.h:353
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
Out-of-plane parameters for an endpoint of a connection, specified in the world frame.
Definition: connection.h:70
Group(const std::string &id, const std::vector< const Connection * > &connections)
Constructs a Group with id, populated by connections.
Definition: connection.h:342
double heading() const
Definition: connection.h:46
Provides Drake&#39;s assertion implementation.
double right_shoulder() const
Returns the right shoulder distance of the segment.
Definition: connection.h:277
#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
Type
Possible connection geometries: line- or arc-segment.
Definition: connection.h:185
const std::string & id() const
Returns the ID string.
Definition: connection.h:360
double left_shoulder() const
Returns the left shoulder distance of the segment.
Definition: connection.h:274
const Endpoint & end() const
Returns the parameters of the endpoint.
Definition: connection.h:243
double radius() const
Returns the radius of the arc (for arc connections only).
Definition: connection.h:252
double r_max() const
Returns the distance from the reference curve to the left extent of the Segment.
Definition: connection.h:294
double z_dot() const
Definition: connection.h:90
const Expression z_
Definition: symbolic_expansion_test.cc:31
const EndpointZ & z() const
Returns the subset of parameters pertaining to out-of-ground-plane aspects.
Definition: connection.h:130
double z
Definition: vtk_util_test.cc:27
double radius() const
Definition: connection.h:159
EndpointZ(double z, double z_dot, double theta, double theta_dot)
Definition: connection.h:76
const double lane_width_
Definition: dragway_test.cc:341
Representation of a reference path connecting two endpoints.
Definition: connection.h:180
Expression cos(const Expression &e)
Definition: symbolic_expression.cc:607
Specification for path offset along a circular arc.
Definition: connection.h:147
Endpoint reverse() const
Returns an Endpoint with reversed direction.
Definition: connection.h:123
double d_theta() const
Definition: connection.h:161
std::ostream & operator<<(std::ostream &out, const EndpointXy &endpoint_xy)
Streams a string representation of endpoint_xy into out.
Definition: connection.cc:11
const EndpointXy & xy() const
Returns the subset of parameters pertaining to the xy ground-plane.
Definition: connection.h:126
Endpoint(const EndpointXy &xy, const EndpointZ &z)
Definition: connection.h:120
double r0() const
Returns the lateral offset from the reference curve to the first Lane centerline. ...
Definition: connection.h:268
const Endpoint & start() const
Returns the parameters of the start point.
Definition: connection.h:240
#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...
int num_lanes() const
Returns the number of lanes the Segment will contain.
Definition: connection.h:264
const std::string & id() const
Returns the ID string.
Definition: connection.h:237