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 
10 #include "drake/automotive/maliput/multilane/road_curve.h"
11 #include "drake/common/drake_assert.h"
12 #include "drake/common/drake_copyable.h"
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 line.
143 /// * length: length of the line, which must be nonnegative.
144 class LineOffset {
145  public:
147 
148  LineOffset() = default;
149 
150  explicit LineOffset(double length) : length_(length) {
151  DRAKE_DEMAND(length_ >= 0.);
152  }
153 
154  double length() const { return length_; }
155 
156  private:
157  double length_{};
158 };
159 
160 /// Streams a string representation of `line_offset` into `out`. Returns `out`.
161 /// This method is provided for the purposes of debugging or text-logging.
162 /// It is not intended for serialization.
163 std::ostream& operator<<(std::ostream& out, const LineOffset& line_offset);
164 
165 /// Specification for path offset along a circular arc.
166 /// * radius: radius of the arc, which must be positive
167 /// * d_theta: angle of arc segment (Δθ)
168 /// * d_theta > 0 is counterclockwise ('veer to left')
169 /// * d_theta < 0 is clockwise ('veer to right')
170 class ArcOffset {
171  public:
173 
174  /// Constructs an ArcOffset with all zero parameters.
175  ArcOffset() = default;
176 
177  ArcOffset(double radius, double d_theta)
178  : radius_(radius), d_theta_(d_theta) {
179  DRAKE_DEMAND(radius_ > 0.);
180  }
181 
182  double radius() const { return radius_; }
183 
184  double d_theta() const { return d_theta_; }
185 
186  private:
187  double radius_{};
188  double d_theta_{};
189 };
190 
191 /// Streams a string representation of `arc_offset` into `out`. Returns `out`.
192 /// This method is provided for the purposes of debugging or text-logging.
193 /// It is not intended for serialization.
194 std::ostream& operator<<(std::ostream& out, const ArcOffset& arc_offset);
195 
196 /// Representation of a reference path connecting two endpoints.
197 ///
198 /// Upon building the RoadGeometry, a Connection yields a Segment
199 /// bearing multiple Lanes with offsets from the reference path. The
200 /// Segment will belong to its own Junction, unless the Connection was
201 /// grouped with other Connections into a Group.
202 ///
203 /// Two connection geometries are supported: line and arc. These
204 /// primitives determine the projection of the reference path onto the
205 /// (locally-flat) plane of the earth. The out-of-plane shape of
206 /// the path will be determined by the EndpointZ (elevation) parameters
207 /// of the endpoints.
208 class Connection {
209  public:
211 
212  /// Possible connection geometries: line- or arc-segment.
213  enum Type { kLine, kArc };
214 
215  /// Constructs a line-segment connection.
216  ///
217  /// Segment's reference curve begins at `start` and extends on the plane
218  /// z=0 `line_length` distance with `start.xy().heading()` heading angle.
219  /// `end_z` will be used to build elevation and superelevation information of
220  /// the road.
221  ///
222  /// `line_length` must be non negative.
223  ///
224  /// Segments will contain `num_lanes` lanes, which must be greater than zero.
225  /// First Lane centerline will be placed at `r0` distance from the reference
226  /// curve. Each lane's width will be `lane_width`, which should be greater or
227  /// equal to zero.
228  ///
229  /// `left_shoulder` and `right_shoulder` are extra spaces added to the right
230  /// and left side of the first and last lanes of the Segment. They will be
231  /// added to Segment's bounds and must be greater or equal to zero.
232  Connection(const std::string& id, const Endpoint& start,
233  const EndpointZ& end_z, int num_lanes, double r0,
234  double lane_width, double left_shoulder, double right_shoulder,
235  double line_length);
236 
237  /// Constructs an arc-segment connection.
238  ///
239  /// Segment's reference curve begins at `start` and extends on the plane z=0
240  /// with `arc_offset.radius()` and angle span of `arc_offset.d_theta()`.
241  /// `end_z` will be used to build elevation and superelevation information of
242  /// the road.
243  ///
244  /// `arc_offset.radius()` must be positive. `arc_offset.d_theta()` > 0
245  /// indicates a counterclockwise arc from `start` with initial heading angle
246  /// `start.heading()`.
247  ///
248  /// Segments will contain `num_lanes` lanes, which must be greater than zero.
249  /// First Lane centerline will be placed at `r0` distance from the reference
250  /// curve. Each lane's width will be `lane_width`, which should be greater or
251  /// equal to zero.
252  ///
253  /// `left_shoulder` and `right_shoulder` are extra spaces added to the right
254  /// and left side of the first and last lanes of the Segment. They will be
255  /// added to Segment's bounds and must be greater or equal to zero.
256  Connection(const std::string& id, const Endpoint& start,
257  const EndpointZ& end_z, int num_lanes, double r0,
258  double lane_width, double left_shoulder, double right_shoulder,
259  const ArcOffset& arc_offset);
260 
261  /// Returns the geometric type of the path.
262  Type type() const { return type_; }
263 
264  /// Returns the ID string.
265  const std::string& id() const { return id_; }
266 
267  /// Returns the parameters of the start point.
268  const Endpoint& start() const { return start_; }
269 
270  /// Returns the parameters of the endpoint.
271  const Endpoint& end() const { return end_; }
272 
273  /// Returns the length of the line (for line connections only).
274  double line_length() const {
275  DRAKE_DEMAND(type_ == kLine);
276  return line_length_;
277  }
278 
279  /// Returns the radius of the arc (for arc connections only).
280  double radius() const {
281  DRAKE_DEMAND(type_ == kArc);
282  return radius_;
283  }
284 
285  /// Returns the angle of the arc (for arc connections only).
286  double d_theta() const {
287  DRAKE_DEMAND(type_ == kArc);
288  return d_theta_;
289  }
290 
291  /// Returns the number of lanes the Segment will contain.
292  int num_lanes() const { return num_lanes_; }
293 
294  /// Returns the lateral offset from the reference curve to the first Lane
295  /// centerline.
296  double r0() const { return r0_; }
297 
298  /// Returns lanes' width.
299  double lane_width() const { return lane_width_; }
300 
301  /// Returns the left shoulder distance of the segment.
302  double left_shoulder() const { return left_shoulder_; }
303 
304  /// Returns the right shoulder distance of the segment.
305  double right_shoulder() const { return right_shoulder_; }
306 
307  /// Returns `lane_index` lane lateral distance to the reference curve.
308  ///
309  /// `lane_index` must be non-negative and smaller than the number of lanes of
310  /// this connection.
311  double lane_offset(int lane_index) const {
312  DRAKE_DEMAND(lane_index >= 0 && lane_index < num_lanes_);
313  return r0_ + lane_width_ * static_cast<double>(lane_index);
314  }
315 
316  /// Returns the distance from the reference curve to the right extent of the
317  /// Segment.
318  double r_min() const { return r_min_; }
319 
320  /// Returns the distance from the reference curve to the left extent of the
321  /// Segment.
322  double r_max() const { return r_max_; }
323 
324  /// Returns an Endpoint describing the start of the `lane_index` lane.
325  Endpoint LaneStart(int lane_index) const;
326 
327  /// Returns an Endpoint describing the end of the `lane_index` lane.
328  Endpoint LaneEnd(int lane_index) const;
329 
330  /// Creates a RoadCurve that describes the reference curve of this Connection.
331  std::unique_ptr<RoadCurve> CreateRoadCurve() const;
332 
333  private:
334  const Type type_{};
335  const std::string id_;
336  const Endpoint start_{};
337  Endpoint end_{};
338  const int num_lanes_{};
339  const double r0_{};
340  const double lane_width_{};
341  const double left_shoulder_{};
342  const double right_shoulder_{};
343  const double r_min_{};
344  const double r_max_{};
345  std::unique_ptr<RoadCurve> road_curve_;
346  // Bits specific to type_ == kLine:
347  double line_length_{};
348  // Bits specific to type_ == kArc:
349  double radius_{};
350  double d_theta_{};
351  double theta0_{};
352  double cx_{};
353  double cy_{};
354 };
355 
356 /// A group of Connections.
357 ///
358 /// Upon building the RoadGeometry, a Group yields a Junction containing the
359 /// corresponding Segments specified by all the Connections in the Group.
360 class Group {
361  public:
363 
364  /// Constructs an empty Group with the specified `id`.
365  explicit Group(const std::string& id) : id_(id) {}
366 
367  /// Constructs a Group with `id`, populated by `connections`.
368  ///
369  /// `connections` must not contain duplicates.
370  Group(const std::string& id,
371  const std::vector<const Connection*>& connections)
372  : id_(id) {
373  for (const Connection* connection : connections) {
374  Add(connection);
375  }
376  }
377 
378  /// Adds a Connection.
379  ///
380  /// `connection` must not already be added.
381  void Add(const Connection* connection) {
382  auto result = connection_set_.insert(connection);
383  DRAKE_DEMAND(result.second);
384  connection_vector_.push_back(connection);
385  }
386 
387  /// Returns the ID string.
388  const std::string& id() const { return id_; }
389 
390  /// Returns the grouped Connections.
391  const std::vector<const Connection*>& connections() const {
392  return connection_vector_;
393  }
394 
395  private:
396  std::string id_;
397  std::unordered_set<const Connection*> connection_set_;
398  std::vector<const Connection*> connection_vector_;
399 };
400 
401 } // namespace multilane
402 } // namespace maliput
403 } // namespace drake
const Expression xy_
Definition: symbolic_polynomial_test.cc:47
LineOffset(double length)
Definition: connection.h:150
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:391
Type type() const
Returns the geometric type of the path.
Definition: connection.h:262
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: agent_trajectory.cc:5
Complete set of parameters for an endpoint of a connection, specified in the world frame...
Definition: connection.h:113
double r0
Definition: multilane_connection_test.cc:364
Expression sin(const Expression &e)
Definition: symbolic_expression.cc:613
A group of Connections.
Definition: connection.h:360
double lane_width() const
Returns lanes&#39; width.
Definition: connection.h:299
ArcOffset(double radius, double d_theta)
Definition: connection.h:177
Expression atan2(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:666
const Endpoint start
Definition: multilane_builder_test.cc:399
STL namespace.
double lane_offset(int lane_index) const
Returns lane_index lane lateral distance to the reference curve.
Definition: connection.h:311
std::ostream & operator<<(std::ostream &out, const LaneLayout &lane_layout)
Streams a string representation of lane_layout into out.
Definition: builder.cc:19
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:274
int num_lanes
Definition: multilane_connection_test.cc:365
double length() const
Definition: connection.h:154
double r_min() const
Returns the distance from the reference curve to the right extent of the Segment. ...
Definition: connection.h:318
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:286
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:381
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
EndpointZ end_z
Definition: multilane_connection_test.cc:363
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:370
double heading() const
Definition: connection.h:46
Specification for path offset along a line.
Definition: connection.h:144
double right_shoulder() const
Returns the right shoulder distance of the segment.
Definition: connection.h:305
#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:213
const double radius_
Definition: proximity_engine_test.cc:290
const std::string & id() const
Returns the ID string.
Definition: connection.h:388
double left_shoulder() const
Returns the left shoulder distance of the segment.
Definition: connection.h:302
const Endpoint & end() const
Returns the parameters of the endpoint.
Definition: connection.h:271
double radius() const
Returns the radius of the arc (for arc connections only).
Definition: connection.h:280
double r_max() const
Returns the distance from the reference curve to the left extent of the Segment.
Definition: connection.h:322
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
const double length_
Definition: dragway_test.cc:340
double z
Definition: vtk_util_test.cc:27
double radius() const
Definition: connection.h:182
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:208
Expression cos(const Expression &e)
Definition: symbolic_expression.cc:621
Specification for path offset along a circular arc.
Definition: connection.h:170
Endpoint reverse() const
Returns an Endpoint with reversed direction.
Definition: connection.h:123
double d_theta() const
Definition: connection.h:184
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:296
const Endpoint & start() const
Returns the parameters of the start point.
Definition: connection.h:268
#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
int num_lanes() const
Returns the number of lanes the Segment will contain.
Definition: connection.h:292
const std::string & id() const
Returns the ID string.
Definition: connection.h:265