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 
14 
15 namespace drake {
16 namespace maliput {
17 namespace multilane {
18 
19 /// XY-plane-only parameters for an endpoint of a connection, specified in
20 /// the world frame.
21 ///
22 /// The three components are:
23 /// - x: x position
24 /// - y: y position
25 /// - heading: counter-clockwise around z=cross(x,y). It is the heading of
26 /// reference path (radians, zero == x-direction).
27 class EndpointXy {
28  public:
30 
31  // Constructs an EndpointXy with all zero parameters.
32  EndpointXy() = default;
33 
34  EndpointXy(double x, double y, double heading)
35  : x_(x), y_(y), heading_(heading) {}
36 
37  /// Returns an EndpointXy with reversed direction.
38  EndpointXy reverse() const {
39  return EndpointXy(x_, y_,
40  std::atan2(-std::sin(heading_), -std::cos(heading_)));
41  }
42 
43  double x() const { return x_; }
44 
45  double y() const { return y_; }
46 
47  double heading() const { return heading_; }
48 
49  private:
50  double x_{};
51  double y_{};
52  double heading_{};
53 };
54 
55 /// Streams a string representation of `endpoint_xy` into `out`. Returns
56 /// `out`. This method is provided for the purposes of debugging or
57 /// text-logging. It is not intended for serialization.
58 std::ostream& operator<<(std::ostream& out, const EndpointXy& endpoint_xy);
59 
60 /// Out-of-plane parameters for an endpoint of a connection, specified in
61 /// the world frame.
62 ///
63 /// The four components are:
64 /// - z: elevation
65 /// - z_dot: grade (rate of change of elevation with respect to
66 /// arc length of the reference path)
67 /// - theta: superelevation (rotation of road surface around r = 0 centerline;
68 /// when theta > 0, elevation at r > 0 is above elevation at r < 0)
69 /// - theta_dot: rate of change of superelevation with respect to arc length
70 /// of the reference path. It is optional because it may be
71 /// unknown when building a RoadGeometry and the Builder may need
72 /// to adjust it to force the same orientation for all r at a
73 /// certain s coordinate of the Segment surface.
74 class EndpointZ {
75  public:
77  // Constructs an EndpointZ with all zero parameters.
78  EndpointZ() = default;
79 
80  EndpointZ(double z, double z_dot, double theta, optional<double> theta_dot)
81  : z_(z), z_dot_(z_dot), theta_(theta), theta_dot_(theta_dot) {}
82 
83  /// Returns an EndpointZ with reversed direction.
84  ///
85  /// Reversing direction is equivalent to rotating s (and along with it, r)
86  /// around the h-axis by 180 degrees, thus flipping the signs of z_dot
87  /// and theta. theta_dot will remain the same.
88  EndpointZ reverse() const {
89  return EndpointZ(z_, -z_dot_, -theta_, theta_dot_);
90  }
91 
92  double z() const { return z_; }
93 
94  double z_dot() const { return z_dot_; }
95 
96  double theta() const { return theta_; }
97 
98  const optional<double>& theta_dot() const { return theta_dot_; }
99 
100  optional<double>& get_mutable_theta_dot() { return theta_dot_; }
101 
102  private:
103  double z_{};
104  double z_dot_{};
105 
106  double theta_{};
107  optional<double> theta_dot_{};
108 };
109 
110 /// Streams a string representation of `endpoint_z` into `out`. Returns
111 /// `out`. This method is provided for the purposes of debugging or
112 /// text-logging. It is not intended for serialization.
113 std::ostream& operator<<(std::ostream& out, const EndpointZ& endpoint_z);
114 
115 /// Complete set of parameters for an endpoint of a connection,
116 /// specified in the world frame. It comprises two subsets of parameters:
117 /// those pertaining only to the xy ground-plane, and those pertaining to
118 /// out-of-plane aspects of an endpoint.
119 class Endpoint {
120  public:
122 
123  // Constructs an Endpoint with all zero parameters.
124  Endpoint() = default;
125 
126  Endpoint(const EndpointXy& xy, const EndpointZ& z) : xy_(xy), z_(z) {}
127 
128  /// Returns an Endpoint with reversed direction.
129  Endpoint reverse() const { return Endpoint(xy_.reverse(), z_.reverse()); }
130 
131  /// Returns the subset of parameters pertaining to the xy ground-plane.
132  const EndpointXy& xy() const { return xy_; }
133 
134  /// Returns the subset of parameters pertaining to out-of-ground-plane
135  /// aspects.
136  const EndpointZ& z() const { return z_; }
137 
138  EndpointZ& get_mutable_z() { return z_; }
139 
140  private:
141  EndpointXy xy_;
142  EndpointZ z_;
143 };
144 
145 /// Streams a string representation of `endpoint` into `out`. Returns
146 /// `out`. This method is provided for the purposes of debugging or
147 /// text-logging. It is not intended for serialization.
148 std::ostream& operator<<(std::ostream& out, const Endpoint& endpoint);
149 
150 /// Specification for path offset along a line.
151 /// * length: length of the line, which must be nonnegative.
152 class LineOffset {
153  public:
155 
156  LineOffset() = default;
157 
158  explicit LineOffset(double length) : length_(length) {
159  DRAKE_DEMAND(length_ >= 0.);
160  }
161 
162  double length() const { return length_; }
163 
164  private:
165  double length_{};
166 };
167 
168 /// Streams a string representation of `line_offset` into `out`. Returns `out`.
169 /// This method is provided for the purposes of debugging or text-logging.
170 /// It is not intended for serialization.
171 std::ostream& operator<<(std::ostream& out, const LineOffset& line_offset);
172 
173 /// Specification for path offset along a circular arc.
174 /// * radius: radius of the arc, which must be positive
175 /// * d_theta: angle of arc segment (Δθ)
176 /// * d_theta > 0 is counterclockwise ('veer to left')
177 /// * d_theta < 0 is clockwise ('veer to right')
178 class ArcOffset {
179  public:
181 
182  /// Constructs an ArcOffset with all zero parameters.
183  ArcOffset() = default;
184 
185  ArcOffset(double radius, double d_theta)
186  : radius_(radius), d_theta_(d_theta) {
187  DRAKE_DEMAND(radius_ > 0.);
188  }
189 
190  double radius() const { return radius_; }
191 
192  double d_theta() const { return d_theta_; }
193 
194  private:
195  double radius_{};
196  double d_theta_{};
197 };
198 
199 /// Streams a string representation of `arc_offset` into `out`. Returns `out`.
200 /// This method is provided for the purposes of debugging or text-logging.
201 /// It is not intended for serialization.
202 std::ostream& operator<<(std::ostream& out, const ArcOffset& arc_offset);
203 
204 /// Representation of a reference path connecting two endpoints.
205 ///
206 /// Upon building the RoadGeometry, a Connection yields a Segment
207 /// bearing multiple Lanes with offsets from the reference path. The
208 /// Segment will belong to its own Junction, unless the Connection was
209 /// grouped with other Connections into a Group.
210 ///
211 /// Two connection geometries are supported: line and arc. These
212 /// primitives determine the projection of the reference path onto the
213 /// (locally-flat) plane of the earth. The out-of-plane shape of
214 /// the path will be determined by the EndpointZ (elevation) parameters
215 /// of the endpoints.
216 class Connection {
217  public:
219 
220  /// Possible connection geometries: line- or arc-segment.
221  enum Type { kLine, kArc };
222 
223  /// Constructs a line-segment connection.
224  ///
225  /// Segment's reference curve begins at `start` and extends on the plane
226  /// z=0 `line_offset.length()` distance with `start.xy().heading()` heading
227  /// angle.
228  /// `end_z` will be used to build elevation and superelevation information of
229  /// the road.
230  ///
231  /// `line_offset` holds the length of the line.
232  ///
233  /// Segments will contain `num_lanes` lanes, which must be greater than zero.
234  /// First Lane centerline will be placed at `r0` distance from the reference
235  /// curve. Each lane's width will be `lane_width`, which should be greater or
236  /// equal to zero.
237  ///
238  /// `left_shoulder` and `right_shoulder` are extra spaces added to the right
239  /// and left side of the first and last lanes of the Segment. They will be
240  /// added to Segment's bounds and must be greater or equal to zero.
241  ///
242  /// `scale_length` constrains the maximum level of detail captured by the
243  /// underlying RoadCurve.
244  ///
245  /// `linear_tolerance` and `computation_policy` set the efficiency vs. speed
246  /// balance for computations in the underlying RoadCurve.
247  Connection(const std::string& id, const Endpoint& start,
248  const EndpointZ& end_z, int num_lanes, double r0,
249  double lane_width, double left_shoulder, double right_shoulder,
250  const LineOffset& line_offset, double linear_tolerance,
251  double scale_length, ComputationPolicy computation_policy);
252 
253  /// Constructs an arc-segment connection.
254  ///
255  /// Segment's reference curve begins at `start` and extends on the plane z=0
256  /// with `arc_offset.radius()` and angle span of `arc_offset.d_theta()`.
257  /// `end_z` will be used to build elevation and superelevation information of
258  /// the road.
259  ///
260  /// `arc_offset.radius()` must be positive. `arc_offset.d_theta()` > 0
261  /// indicates a counterclockwise arc from `start` with initial heading angle
262  /// `start.heading()`.
263  ///
264  /// Segments will contain `num_lanes` lanes, which must be greater than zero.
265  /// First Lane centerline will be placed at `r0` distance from the reference
266  /// curve. Each lane's width will be `lane_width`, which should be greater or
267  /// equal to zero.
268  ///
269  /// `left_shoulder` and `right_shoulder` are extra spaces added to the right
270  /// and left side of the first and last lanes of the Segment. They will be
271  /// added to Segment's bounds and must be greater or equal to zero.
272  ///
273  /// `linear_tolerance` applies to all RoadCurve-level computations. It must be
274  /// positive.
275  ///
276  /// `scale_length` constrains the maximum level of detail captured by the
277  /// underlying RoadCurve. It must be positive.
278  ///
279  /// `computation_policy` sets the speed vs. accuracy for computations in the
280  /// underlying RoadCurve.
281  Connection(const std::string& id, const Endpoint& start,
282  const EndpointZ& end_z, int num_lanes, double r0,
283  double lane_width, double left_shoulder, double right_shoulder,
284  const ArcOffset& arc_offset, double linear_tolerance,
285  double scale_length, ComputationPolicy computation_policy);
286 
287  /// Returns the geometric type of the path.
288  Type type() const { return type_; }
289 
290  /// Returns the ID string.
291  const std::string& id() const { return id_; }
292 
293  /// Returns the parameters of the start point.
294  const Endpoint& start() const { return start_; }
295 
296  /// Returns the parameters of the endpoint.
297  const Endpoint& end() const { return end_; }
298 
299  /// Returns the length of the line (for line connections only).
300  double line_length() const {
301  DRAKE_DEMAND(type_ == kLine);
302  return line_length_;
303  }
304 
305  /// Returns the radius of the arc (for arc connections only).
306  double radius() const {
307  DRAKE_DEMAND(type_ == kArc);
308  return radius_;
309  }
310 
311  /// Returns the angle of the arc (for arc connections only).
312  double d_theta() const {
313  DRAKE_DEMAND(type_ == kArc);
314  return d_theta_;
315  }
316 
317  /// Returns the number of lanes the Segment will contain.
318  int num_lanes() const { return num_lanes_; }
319 
320  /// Returns the lateral offset from the reference curve to the first Lane
321  /// centerline.
322  double r0() const { return r0_; }
323 
324  /// Returns lanes' width.
325  double lane_width() const { return lane_width_; }
326 
327  /// Returns the left shoulder distance of the segment.
328  double left_shoulder() const { return left_shoulder_; }
329 
330  /// Returns the right shoulder distance of the segment.
331  double right_shoulder() const { return right_shoulder_; }
332 
333  /// Returns `lane_index` lane lateral distance to the reference curve.
334  ///
335  /// `lane_index` must be non-negative and smaller than the number of lanes of
336  /// this connection.
337  double lane_offset(int lane_index) const {
338  DRAKE_DEMAND(lane_index >= 0 && lane_index < num_lanes_);
339  return r0_ + lane_width_ * static_cast<double>(lane_index);
340  }
341 
342  /// Returns the distance from the reference curve to the right extent of the
343  /// Segment.
344  double r_min() const { return r_min_; }
345 
346  /// Returns the distance from the reference curve to the left extent of the
347  /// Segment.
348  double r_max() const { return r_max_; }
349 
350  /// Returns the linear tolerance, in meters, that applies to RoadCurve
351  /// instances as constructed by this Connection. Refer to RoadCurve class
352  /// documentation for further details.
353  double linear_tolerance() const { return linear_tolerance_; }
354 
355  /// Returns the scale length, in meters, that applies to RoadCurve instances
356  /// as constructed by this Connection. Refer to RoadCurve class documentation
357  /// for further details.
358  double scale_length() const { return scale_length_; }
359 
360  /// Returns the computation policy that applies to RoadCurve instances as
361  /// constructed by this Connection. Refer to RoadCurve class documentation
362  /// for further details.
363  ComputationPolicy computation_policy() const { return computation_policy_; }
364 
365  /// Returns an Endpoint describing the start of the `lane_index` lane.
366  Endpoint LaneStart(int lane_index) const;
367 
368  /// Returns an Endpoint describing the end of the `lane_index` lane.
369  Endpoint LaneEnd(int lane_index) const;
370 
371  /// Creates a RoadCurve that describes the reference curve of this Connection.
372  std::unique_ptr<RoadCurve> CreateRoadCurve() const;
373 
374  private:
375  const Type type_{};
376  const std::string id_;
377  const Endpoint start_{};
378  Endpoint end_{};
379  const int num_lanes_{};
380  const double r0_{};
381  const double lane_width_{};
382  const double left_shoulder_{};
383  const double right_shoulder_{};
384  const double r_min_{};
385  const double r_max_{};
386  const double linear_tolerance_{};
387  const double scale_length_{};
388  const ComputationPolicy computation_policy_;
389  std::unique_ptr<RoadCurve> road_curve_;
390  // Bits specific to type_ == kLine:
391  double line_length_{};
392  // Bits specific to type_ == kArc:
393  double radius_{};
394  double d_theta_{};
395  double theta0_{};
396  double cx_{};
397  double cy_{};
398 };
399 
400 /// A group of Connections.
401 ///
402 /// Upon building the RoadGeometry, a Group yields a Junction containing the
403 /// corresponding Segments specified by all the Connections in the Group.
404 class Group {
405  public:
407 
408  /// Constructs an empty Group with the specified `id`.
409  explicit Group(const std::string& id) : id_(id) {}
410 
411  /// Constructs a Group with `id`, populated by `connections`.
412  ///
413  /// `connections` must not contain duplicates.
414  Group(const std::string& id,
415  const std::vector<const Connection*>& connections)
416  : id_(id) {
417  for (const Connection* connection : connections) {
418  Add(connection);
419  }
420  }
421 
422  /// Adds a Connection.
423  ///
424  /// `connection` must not already be added.
425  void Add(const Connection* connection) {
426  auto result = connection_set_.insert(connection);
427  DRAKE_DEMAND(result.second);
428  connection_vector_.push_back(connection);
429  }
430 
431  /// Returns the ID string.
432  const std::string& id() const { return id_; }
433 
434  /// Returns the grouped Connections.
435  const std::vector<const Connection*>& connections() const {
436  return connection_vector_;
437  }
438 
439  private:
440  std::string id_;
441  std::unordered_set<const Connection*> connection_set_;
442  std::vector<const Connection*> connection_vector_;
443 };
444 
445 } // namespace multilane
446 } // namespace maliput
447 } // namespace drake
optional< double > & get_mutable_theta_dot()
Definition: connection.h:100
const Expression xy_
Definition: symbolic_polynomial_test.cc:47
LineOffset(double length)
Definition: connection.h:158
XY-plane-only parameters for an endpoint of a connection, specified in the world frame.
Definition: connection.h:27
const std::vector< const Connection * > & connections() const
Returns the grouped Connections.
Definition: connection.h:435
Type type() const
Returns the geometric type of the path.
Definition: connection.h:288
EndpointXy(double x, double y, double heading)
Definition: connection.h:34
EndpointZ reverse() const
Returns an EndpointZ with reversed direction.
Definition: connection.h:88
Definition: automotive_demo.cc:90
Complete set of parameters for an endpoint of a connection, specified in the world frame...
Definition: connection.h:119
double r0
Definition: multilane_connection_test.cc:361
Expression sin(const Expression &e)
Definition: symbolic_expression.cc:613
A group of Connections.
Definition: connection.h:404
double lane_width() const
Returns lanes&#39; width.
Definition: connection.h:325
ArcOffset(double radius, double d_theta)
Definition: connection.h:185
Expression atan2(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:666
STL namespace.
double lane_offset(int lane_index) const
Returns lane_index lane lateral distance to the reference curve.
Definition: connection.h:337
EndpointXy reverse() const
Returns an EndpointXy with reversed direction.
Definition: connection.h:38
double line_length() const
Returns the length of the line (for line connections only).
Definition: connection.h:300
int num_lanes
Definition: multilane_connection_test.cc:362
EndpointZ(double z, double z_dot, double theta, optional< double > theta_dot)
Definition: connection.h:80
double length() const
Definition: connection.h:162
double r_min() const
Returns the distance from the reference curve to the right extent of the Segment. ...
Definition: connection.h:344
double y() const
Definition: connection.h:45
double theta() const
Definition: connection.h:96
EndpointZ & get_mutable_z()
Definition: connection.h:138
stx::optional< T > optional
Definition: drake_optional.h:22
double d_theta() const
Returns the angle of the arc (for arc connections only).
Definition: connection.h:312
double z() const
Definition: connection.h:92
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:425
double x() const
Definition: connection.h:43
#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:360
Out-of-plane parameters for an endpoint of a connection, specified in the world frame.
Definition: connection.h:74
Group(const std::string &id, const std::vector< const Connection * > &connections)
Constructs a Group with id, populated by connections.
Definition: connection.h:414
double heading() const
Definition: connection.h:47
Specification for path offset along a line.
Definition: connection.h:152
Provides Drake&#39;s assertion implementation.
double right_shoulder() const
Returns the right shoulder distance of the segment.
Definition: connection.h:331
#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:221
const double radius_
Definition: proximity_engine_test.cc:288
double scale_length() const
Returns the scale length, in meters, that applies to RoadCurve instances as constructed by this Conne...
Definition: connection.h:358
const std::string & id() const
Returns the ID string.
Definition: connection.h:432
Provides drake::optional as an alias for the appropriate implementation of std::optional or std::expe...
double left_shoulder() const
Returns the left shoulder distance of the segment.
Definition: connection.h:328
const Endpoint & end() const
Returns the parameters of the endpoint.
Definition: connection.h:297
double radius() const
Returns the radius of the arc (for arc connections only).
Definition: connection.h:306
ComputationPolicy computation_policy() const
Returns the computation policy that applies to RoadCurve instances as constructed by this Connection...
Definition: connection.h:363
double r_max() const
Returns the distance from the reference curve to the left extent of the Segment.
Definition: connection.h:348
double z_dot() const
Definition: connection.h:94
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:136
const double length_
Definition: dragway_test.cc:341
double z
Definition: vtk_util_test.cc:27
std::ostream & operator<<(std::ostream &out, const StartReference::Spec &start_spec)
Streams a string representation of start_spec into out.
Definition: builder.cc:19
double radius() const
Definition: connection.h:190
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:49
Representation of a reference path connecting two endpoints.
Definition: connection.h:216
Expression cos(const Expression &e)
Definition: symbolic_expression.cc:621
Specification for path offset along a circular arc.
Definition: connection.h:178
Endpoint reverse() const
Returns an Endpoint with reversed direction.
Definition: connection.h:129
double d_theta() const
Definition: connection.h:192
const optional< double > & theta_dot() const
Definition: connection.h:98
const EndpointXy & xy() const
Returns the subset of parameters pertaining to the xy ground-plane.
Definition: connection.h:132
Endpoint(const EndpointXy &xy, const EndpointZ &z)
Definition: connection.h:126
double r0() const
Returns the lateral offset from the reference curve to the first Lane centerline. ...
Definition: connection.h:322
const Endpoint & start() const
Returns the parameters of the start point.
Definition: connection.h:294
#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
double linear_tolerance() const
Returns the linear tolerance, in meters, that applies to RoadCurve instances as constructed by this C...
Definition: connection.h:353
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
const std::string & id() const
Returns the ID string.
Definition: connection.h:291