Drake
lane.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <string>
5 
6 #include "drake/automotive/maliput/api/lane_data.h"
7 #include "drake/automotive/maliput/api/type_specific_identifier.h"
8 #include "drake/common/autodiff.h"
9 #include "drake/common/drake_copyable.h"
10 #include "drake/common/drake_optional.h"
11 #include "drake/common/symbolic.h"
12 
13 namespace drake {
14 namespace maliput {
15 namespace api {
16 
17 class BranchPoint;
18 class Segment;
19 class LaneEndSet;
20 
21 
22 /// Persistent identifier for a Lane element.
24 
25 
26 /// A Lane represents a lane of travel in a road network. A Lane defines
27 /// a curvilinear coordinate system covering the road surface, with a
28 /// longitudinal 's' coordinate that expresses the arc-length along a
29 /// central reference curve. The reference curve nominally represents
30 /// an ideal travel trajectory along the Lane.
31 ///
32 /// Lanes are grouped by Segment. All Lanes belonging to a Segment
33 /// represent the same road surface, but with different coordinate
34 /// parameterizations (e.g., each Lane has its own reference curve).
35 class Lane {
36  public:
38 
39  virtual ~Lane() = default;
40 
41  /// Returns the persistent identifier.
42  const LaneId id() const { return do_id(); }
43 
44  /// Returns the Segment to which this Lane belongs.
45  const Segment* segment() const { return do_segment(); }
46 
47  /// Returns the index of this Lane within the Segment which owns it.
48  int index() const { return do_index(); }
49 
50  /// Returns a pointer to the adjacent Lane to the left of this Lane.
51  ///
52  /// Left is considered the +r direction with regards to the (s,r,h) frame,
53  /// e.g., "to the left along the +s direction".
54  ///
55  /// @returns nullptr iff parent Segment has no Lane to the left.
56  const Lane* to_left() const { return do_to_left(); }
57 
58  /// Returns a pointer to the adjacent Lane to the right of this Lane.
59  ///
60  /// Right is considered the -r direction with regards to the (s,r,h) frame,
61  /// e.g., "to the right along the +s direction".
62  ///
63  /// @returns nullptr iff parent Segment has no Lane to the right.
64  const Lane* to_right() const { return do_to_right(); }
65 
66  /// Returns the arc-length of the Lane along its reference curve.
67  ///
68  /// The value of length() is also the maximum s-coordinate for this Lane;
69  /// i.e., the domain of s is [0, length()].
70  double length() const { return do_length(); }
71 
72  /// Returns the nominal lateral (r) bounds for the lane as a function of s.
73  ///
74  /// These are the lateral bounds for a position that is considered to be
75  /// "staying in the lane".
76  RBounds lane_bounds(double s) const { return do_lane_bounds(s); }
77 
78 
79  /// Returns the driveable lateral (r) bounds of the lane as a function of s.
80  ///
81  /// These are the lateral bounds for a position that is considered to be
82  /// "on pavement", reflecting the physical extent of the paved surface of
83  /// the lane's segment.
84  RBounds driveable_bounds(double s) const { return do_driveable_bounds(s); }
85 
86  /// Returns the elevation (`h`) bounds of the lane as a function of `(s, r)`.
87  ///
88  /// These are the elevation bounds for a position that is considered to be
89  /// within the volume modelled by the RoadGeometry.
90  HBounds elevation_bounds(double s, double r) const {
91  return do_elevation_bounds(s, r); }
92 
93  /// Returns the GeoPosition corresponding to the given LanePosition.
94  ///
95  /// @pre The s component of @p lane_pos must be in domain [0, Lane::length()].
96  /// @pre The r component of @p lane_pos must be in domain [Rmin, Rmax]
97  /// derived from Lane::driveable_bounds().
98  //
99  // TODO(jadecastro): Generalize `Lane::ToGeoPosition` (and possibly others)
100  // with another member function `Lane::ToGeoPositionT<T>()`.
101  GeoPosition ToGeoPosition(const LanePosition& lane_pos) const {
102  return DoToGeoPosition(lane_pos);
103  }
104 
105  /// Generalization of ToGeoPosition to arbitrary scalar types, where the
106  /// structures `LanePositionT<T>` and `GeoPositionT<T>` are used in place of
107  /// `LanePosition` and `GeoPosition`, respectively.
108  ///
109  /// When the arguments are of type AutoDiffXd, the return value is a
110  /// GeoPositionT<AutoDiffXd> containing the same partial derivatives as those
111  /// appearing in lane_pos. The provided lane_pos must be internally
112  /// consistent; the s, r, and h variables must have derivatives of equal size,
113  /// and where the i-th derivative of one variable is taken with respect to the
114  /// same quantity as the i-th derviative of another variable.
115  ///
116  /// Instantiated templates for the following kinds of T's are provided:
117  /// - double
118  /// - drake::AutoDiffXd
119  /// - drake::symbolic::Expression
120  ///
121  /// They are already available to link against in the containing library.
122  ///
123  /// Note: This is an experimental API that is not necessarily implemented in
124  /// all back-end implementations.
125 
126  // TODO(jadecastro): Apply this implementation in all the subclasses of
127  // `api::Lane`.
128  template <typename T>
129  GeoPositionT<T> ToGeoPositionT(const LanePositionT<T>& lane_pos) const;
130 
131  /// Determines the LanePosition corresponding to GeoPosition @p geo_pos.
132  ///
133  /// The return value is the LanePosition of the point within the Lane's
134  /// driveable-bounds which is closest to @p geo_pos (as measured by the
135  /// Cartesian metric in the world frame). If @p nearest_point is non-null,
136  /// then it will be populated with the GeoPosition of that nearest point. If
137  /// @p distance is non-null, then it will be populated with the Cartesian
138  /// distance from @p geo_pos to that nearest point.
139  ///
140  /// This method guarantees that its result satisfies the condition that
141  /// `ToGeoPosition(result)` is within `linear_tolerance()` of
142  /// `*nearest_position`.
144  GeoPosition* nearest_point,
145  double* distance) const {
146  return DoToLanePosition(geo_pos, nearest_point, distance);
147  }
148 
149  /// Generalization of ToLanePosition to arbitrary scalar types, where the
150  /// structures `LanePositionT<T>` and `GeoPositionT<T>` are used in place of
151  /// `LanePosition` and `GeoPosition`, respectively.
152  ///
153  /// When the arguments are of type AutoDiffXd, the return value is a
154  /// LanePositionT<AutoDiffXd> containing the same partial derivatives as those
155  /// appearing in geo_pos. The provided geo_pos must be internally consistent;
156  /// the x, y, and z variables must have derivatives of equal size, and where
157  /// the i-th derivative of one variable is taken with respect to the same
158  /// quantity as the i-th derviative of another variable. If either @p
159  /// nearest_point or @p distance are non-null, they will also contain @p
160  /// geo_pos's partial derivatives.
161  ///
162  /// Instantiated templates for the following kinds of T's are provided:
163  /// - double
164  /// - drake::AutoDiffXd
165  /// - drake::symbolic::Expression
166  ///
167  /// They are already available to link against in the containing library.
168  ///
169  /// Note: This is an experimental API that is not necessarily implemented in
170  /// all back-end implementations.
171  //
172  // TODO(jadecastro): Consider having the client enforce the geo_pos AutoDiffXd
173  // coherency contract rather than api::Lane. Reevaluate this once an AutoDiff
174  // consumer for api::ToLanePositionT() exists.
175 
176  // TODO(jadecastro): Apply this implementation in all the subclasses of
177  // `api::Lane`.
178  template <typename T>
180  GeoPositionT<T>* nearest_point,
181  T* distance) const;
182 
183  // TODO(maddog@tri.global) Method to convert LanePosition to that of
184  // another Lane. (Should assert that both
185  // lanes belong to same Segment.) It should look
186  // something like this:
187  // LanePosition ToOtherLane(const LanePosition& in_this_lane,
188  // const Lane* other_lane) const;
189 
190  /// Returns the rotation which expresses the orientation of the
191  /// `Lane`-frame basis at @p lane_pos with respect to the
192  /// world frame basis.
193  Rotation GetOrientation(const LanePosition& lane_pos) const {
194  return DoGetOrientation(lane_pos);
195  }
196 
197  /// Computes derivatives of LanePosition given a velocity vector @p velocity.
198  /// @p velocity is a isometric velocity vector oriented in the `Lane`-frame
199  /// at @p position.
200  ///
201  /// @returns `Lane`-frame derivatives packed into a LanePosition struct.
203  const IsoLaneVelocity& velocity) const {
204  return DoEvalMotionDerivatives(position, velocity);
205  }
206 
207  // TODO(maddog@tri.global) Design/implement this.
208  // void EvalSurfaceDerivatives(...) const { return do_(); }
209 
210 
211  /// Returns the lane's BranchPoint for the end specified by @p which_end.
212  const BranchPoint* GetBranchPoint(const LaneEnd::Which which_end) const {
213  return DoGetBranchPoint(which_end);
214  }
215 
216  /// Returns the set of LaneEnd's which connect with this lane on the
217  /// same side of the BranchPoint at @p which_end. At a minimum,
218  /// this set will include this Lane.
220  const LaneEnd::Which which_end) const {
221  return DoGetConfluentBranches(which_end);
222  }
223 
224  /// Returns the set of LaneEnd's which continue onward from this lane at the
225  /// BranchPoint at @p which_end.
227  const LaneEnd::Which which_end) const {
228  return DoGetOngoingBranches(which_end);
229  }
230 
231  /// Returns the default ongoing LaneEnd connected at @p which_end,
232  /// or nullopt if no default branch has been established at @p which_end.
234  const LaneEnd::Which which_end) const {
235  return DoGetDefaultBranch(which_end);
236  }
237 
238  protected:
239  Lane() = default;
240 
241  private:
242  /// @name NVI implementations of the public methods.
243  /// These must satisfy the constraints/invariants of the
244  /// corresponding public methods.
245  ///@{
246  virtual const LaneId do_id() const = 0;
247 
248  virtual const Segment* do_segment() const = 0;
249 
250  virtual int do_index() const = 0;
251 
252  virtual const Lane* do_to_left() const = 0;
253 
254  virtual const Lane* do_to_right() const = 0;
255 
256  virtual double do_length() const = 0;
257 
258  virtual RBounds do_lane_bounds(double s) const = 0;
259 
260  virtual RBounds do_driveable_bounds(double s) const = 0;
261 
262  virtual HBounds do_elevation_bounds(double s, double r) const = 0;
263 
264  virtual GeoPosition DoToGeoPosition(const LanePosition& lane_pos) const = 0;
265 
266  virtual LanePosition DoToLanePosition(
267  const GeoPosition& geo_pos,
268  GeoPosition* nearest_point,
269  double* distance) const = 0;
270 
271  virtual Rotation DoGetOrientation(const LanePosition& lane_pos) const = 0;
272 
273  virtual LanePosition DoEvalMotionDerivatives(
274  const LanePosition& position, const IsoLaneVelocity& velocity) const = 0;
275 
276  virtual const BranchPoint* DoGetBranchPoint(
277  const LaneEnd::Which which_end) const = 0;
278 
279  virtual const LaneEndSet* DoGetConfluentBranches(
280  const LaneEnd::Which which_end) const = 0;
281 
282  virtual const LaneEndSet* DoGetOngoingBranches(
283  const LaneEnd::Which which_end) const = 0;
284 
285  virtual optional<LaneEnd> DoGetDefaultBranch(
286  const LaneEnd::Which which_end) const = 0;
287 
288  // AutoDiffXd overload of DoToGeoPosition().
289  virtual GeoPositionT<AutoDiffXd> DoToGeoPositionAutoDiff(
290  const LanePositionT<AutoDiffXd>&) const {
291  throw std::runtime_error(
292  "DoToGeoPosition has been instantiated with AutoDiffXd arguments, "
293  "but a Lane backend has not overridden its AutoDiffXd specialization.");
294  }
295 
296  // AutoDiffXd overload of DoToLanePosition().
297  virtual LanePositionT<AutoDiffXd> DoToLanePositionAutoDiff(
300  AutoDiffXd*) const {
301  throw std::runtime_error(
302  "DoToLanePosition has been instantiated with AutoDiffXd arguments, "
303  "but a Lane backend has not overridden its AutoDiffXd specialization.");
304  }
305 
306  // symbolic::Expression overload of DoToGeoPosition().
307  virtual GeoPositionT<symbolic::Expression> DoToGeoPositionSymbolic(
309  throw std::runtime_error(
310  "DoToGeoPosition has been instantiated with symbolic::Expression "
311  "arguments, but a Lane backend has not overridden its "
312  "symbolic::Expression specialization.");
313  }
314 
315  // symbolic::Expression overload of DoToLanePosition().
316  virtual LanePositionT<symbolic::Expression> DoToLanePositionSymbolic(
319  throw std::runtime_error(
320  "DoToLanePosition has been instantiated with symbolic::Expression "
321  "arguments, but a Lane backend has not overridden its "
322  "symbolic::Expression specialization.");
323  }
324 
325  // TODO(jadecastro): Template the entire `api::Lane` class to prevent explicit
326  // virtual functions for each member function.
327  ///@}
328 };
329 
330 } // namespace api
331 } // namespace maliput
332 } // namespace drake
Rotation GetOrientation(const LanePosition &lane_pos) const
Returns the rotation which expresses the orientation of the Lane-frame basis at lane_pos with respect...
Definition: lane.h:193
Eigen::AutoDiffScalar< Eigen::VectorXd > AutoDiffXd
An autodiff variable with a dynamic number of partials.
Definition: eigen_autodiff_types.h:25
const BranchPoint * GetBranchPoint(const LaneEnd::Which which_end) const
Returns the lane&#39;s BranchPoint for the end specified by which_end.
Definition: lane.h:212
A BranchPoint is a node in the network of a RoadGeometry at which Lanes connect to one another...
Definition: branch_point.h:67
LanePosition EvalMotionDerivatives(const LanePosition &position, const IsoLaneVelocity &velocity) const
Computes derivatives of LanePosition given a velocity vector velocity.
Definition: lane.h:202
Definition: automotive_demo.cc:89
GeoPosition ToGeoPosition(const LanePosition &lane_pos) const
Returns the GeoPosition corresponding to the given LanePosition.
Definition: lane.h:101
GeoPositionT< T > ToGeoPositionT(const LanePositionT< T > &lane_pos) const
Generalization of ToGeoPosition to arbitrary scalar types, where the structures LanePositionT<T> and ...
LanePositionT< T > ToLanePositionT(const GeoPositionT< T > &geo_pos, GeoPositionT< T > *nearest_point, T *distance) const
Generalization of ToLanePosition to arbitrary scalar types, where the structures LanePositionT<T> and...
const double position
Definition: robot_plan_interpolator_test.cc:65
const Lane * to_right() const
Returns a pointer to the adjacent Lane to the right of this Lane.
Definition: lane.h:64
const LaneId id() const
Returns the persistent identifier.
Definition: lane.h:42
A position in 3-dimensional geographical Cartesian space, i.e., in the world frame, consisting of three components x, y, and z.
Definition: lane_data.h:138
const LaneEndSet * GetOngoingBranches(const LaneEnd::Which which_end) const
Returns the set of LaneEnd&#39;s which continue onward from this lane at the BranchPoint at which_end...
Definition: lane.h:226
const LaneEndSet * GetConfluentBranches(const LaneEnd::Which which_end) const
Returns the set of LaneEnd&#39;s which connect with this lane on the same side of the BranchPoint at whic...
Definition: lane.h:219
A set of LaneEnds.
Definition: branch_point.h:23
A Lane represents a lane of travel in a road network.
Definition: lane.h:35
stx::optional< T > optional
Definition: drake_optional.h:14
Isometric velocity vector in a Lane-frame.
Definition: lane_data.h:286
int index() const
Returns the index of this Lane within the Segment which owns it.
Definition: lane.h:48
TypeSpecificIdentifier<T> represents an identifier specifically identifying an entity of type T...
Definition: type_specific_identifier.h:39
HBounds elevation_bounds(double s, double r) const
Returns the elevation (h) bounds of the lane as a function of (s, r).
Definition: lane.h:90
const Segment * segment() const
Returns the Segment to which this Lane belongs.
Definition: lane.h:45
GeoPositionT< double > GeoPosition
Definition: lane_data.h:189
Bounds in the lateral dimension (r component) of a Lane-frame, consisting of a pair of minimum and ma...
Definition: lane_data.h:318
A 3-dimensional rotation.
Definition: lane_data.h:54
Represents a symbolic form of an expression.
Definition: symbolic_expression.h:172
LanePosition ToLanePosition(const GeoPosition &geo_pos, GeoPosition *nearest_point, double *distance) const
Determines the LanePosition corresponding to GeoPosition geo_pos.
Definition: lane.h:143
Bounds in the elevation dimension (h component) of a Lane-frame, consisting of a pair of minimum and ...
Definition: lane_data.h:363
A Segment represents a bundle of adjacent Lanes which share a continuously traversable road surface...
Definition: segment.h:27
Which
Labels for the endpoints of a Lane.
Definition: lane_data.h:26
const Lane * to_left() const
Returns a pointer to the adjacent Lane to the left of this Lane.
Definition: lane.h:56
double length() const
Returns the arc-length of the Lane along its reference curve.
Definition: lane.h:70
virtual ~Lane()=default
optional< LaneEnd > GetDefaultBranch(const LaneEnd::Which which_end) const
Returns the default ongoing LaneEnd connected at which_end, or nullopt if no default branch has been ...
Definition: lane.h:233
RBounds driveable_bounds(double s) const
Returns the driveable lateral (r) bounds of the lane as a function of s.
Definition: lane.h:84
RBounds lane_bounds(double s) const
Returns the nominal lateral (r) bounds for the lane as a function of s.
Definition: lane.h:76
#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
Vector6< double > velocity
Definition: pose_smoother.cc:29