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