Drake
lane.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <cmath>
4 #include <memory>
5 
6 #include <Eigen/Dense>
7 
15 #include "drake/common/unused.h"
17 
18 namespace drake {
19 namespace maliput {
20 namespace multilane {
21 
23 
24 typedef Vector2<double> V2;
26 
32 class Rot3 {
33  public:
35 
36  Rot3(double roll, double pitch, double yaw) : rpy_(roll, pitch, yaw) {}
37 
39  V3 apply(const V3& in) const { return math::rpy2rotmat(rpy_) * in; }
40 
41  double yaw() const { return rpy_(2); }
42  double pitch() const { return rpy_(1); }
43  double roll() const { return rpy_(0); }
44 
45  private:
46  Eigen::Matrix<double, 3, 1, Eigen::DontAlign> rpy_;
47 };
48 
49 
51 class Lane : public api::Lane {
52  public:
54 
55 
56  Lane(const api::LaneId& id, const api::Segment* segment,
72  const api::RBounds& lane_bounds,
73  const api::RBounds& driveable_bounds,
74  const api::HBounds& elevation_bounds,
75  const RoadCurve *road_curve)
76  : id_(id), segment_(segment),
77  lane_bounds_(lane_bounds),
78  driveable_bounds_(driveable_bounds),
79  elevation_bounds_(elevation_bounds),
80  road_curve_(road_curve) {
81  DRAKE_DEMAND(lane_bounds_.min() >= driveable_bounds_.min());
82  DRAKE_DEMAND(lane_bounds_.max() <= driveable_bounds_.max());
83  DRAKE_DEMAND(road_curve != nullptr);
84  }
85 
86  // TODO(maddog@tri.global) Allow superelevation to have a center-of-rotation
87  // which is different from r = 0.
88 
89  const CubicPolynomial& elevation() const {
90  return road_curve_->elevation();
91  }
92 
94  return road_curve_->superelevation();
95  }
96 
97  void SetStartBp(BranchPoint* bp) { start_bp_ = bp; }
98  void SetEndBp(BranchPoint* bp) { end_bp_ = bp; }
99 
100  BranchPoint* start_bp() { return start_bp_; }
101 
102  BranchPoint* end_bp() { return end_bp_; }
103 
104  ~Lane() override = default;
105 
106  private:
107  const api::LaneId do_id() const override { return id_; }
108 
109  const api::Segment* do_segment() const override;
110 
111  int do_index() const override { return 0; } // Only one lane per segment!
112 
113  const api::Lane* do_to_left() const override { return nullptr; }
114 
115  const api::Lane* do_to_right() const override { return nullptr; }
116 
117  const api::BranchPoint* DoGetBranchPoint(
118  const api::LaneEnd::Which which_end) const override;
119 
120  const api::LaneEndSet* DoGetConfluentBranches(
121  const api::LaneEnd::Which which_end) const override;
122 
123  const api::LaneEndSet* DoGetOngoingBranches(
124  const api::LaneEnd::Which which_end) const override;
125 
126  std::unique_ptr<api::LaneEnd> DoGetDefaultBranch(
127  const api::LaneEnd::Which which_end) const override;
128 
129  double do_length() const override;
130 
131  api::RBounds do_lane_bounds(double) const override { return lane_bounds_; }
132 
133  api::RBounds do_driveable_bounds(double) const override {
134  return driveable_bounds_;
135  }
136 
137  api::HBounds do_elevation_bounds(double, double) const override {
138  return elevation_bounds_;
139  }
140 
141  api::GeoPosition DoToGeoPosition(
142  const api::LanePosition& lane_pos) const override;
143 
144  api::Rotation DoGetOrientation(
145  const api::LanePosition& lane_pos) const override;
146 
147  api::LanePosition DoEvalMotionDerivatives(
149  const api::IsoLaneVelocity& velocity) const override;
150 
151  api::LanePosition DoToLanePosition(
152  const api::GeoPosition& geo_position,
153  api::GeoPosition* nearest_position,
154  double* distance) const override;
155 
156  // The geometry here revolves around an abstract "world function"
157  //
158  // W: (p,r,h) --> (x,y,z)
159  //
160  // which maps a `Lane`-frame position to its corresponding representation in
161  // world coordinates (with the caveat that instead of the lane's native
162  // longitudinal coordinate 's', the reference curve parameter 'p' is used).
163  //
164  // W is derived from the three functions which define the lane:
165  //
166  // G: p --> (x,y) = the reference curve, a.k.a. xy_of_p()
167  // Z: p --> z / q_max = the elevation function, a.k.a. elevation_
168  // Θ: p --> θ / q_max = the superelevation function, a.k.a. superelevation_
169  //
170  // as:
171  //
172  // (x,y,z) = W(p,r,h) = (G(p), Z(p)) + R_αβγ*(0,r,h)
173  //
174  // where R_αβγ is the roll/pitch/yaw rotation given by angles:
175  //
176  // α = Θ(p)
177  // β = -atan(dZ/dp) at p
178  // γ = atan2(dG_y/dp, dG_x/dp) at p
179  //
180  // (R_αβγ is essentially the orientation of the (s,r,h) `Lane`-frame
181  // at a location (s,0,0) on the reference-line of the lane. However, it
182  // is *not* necessarily the correct orientation at r != 0 or h != 0.)
183  //
184  // The following methods compute various terms derived from the above which
185  // see repeated use.
186  //
187  // Note that xy_of_p(), xy_dot_of_p(), heading_of_p() and heading_dot_of_p(),
188  // as well as the reference to the elevation and superelevation objects live
189  // on the RoadCurve object.
190 
191  // Returns the parametric position p along the reference curve corresponding
192  // to longitudinal position @p s along the lane.
193  double p_from_s(const double s) const;
194 
195  // Returns the rotation R_αβγ, evaluated at @p p along the reference curve.
196  Rot3 Rabg_of_p(const double p) const;
197 
198  // Returns W' = ∂W/∂p, the partial differential of W with respect to p,
199  // evaluated at @p p, @p r, @p h.
200  //
201  // (@p Rabg must be the result of Rabg_of_p(p) --- passed in here to
202  // avoid recomputing it.)
203  V3 W_prime_of_prh(const double p, const double r, const double h,
204  const Rot3& Rabg) const;
205 
206  // Returns the s-axis unit-vector, expressed in the world frame,
207  // of the (s,r,h) `Lane`-frame (with respect to the world frame).
208  //
209  // (@p Rabg must be the result of Rabg_of_p(p) --- passed in here to
210  // avoid recomputing it.)
211  V3 s_hat_of_prh(const double p, const double r, const double h,
212  const Rot3& Rabg) const;
213 
214  // Returns the r-axis unit-vector, expressed in the world frame,
215  // of the (s,r,h) `Lane`-frame (with respect to the world frame).
216  //
217  // (@p Rabg must be the result of Rabg_of_p(p) --- passed in here to
218  // avoid recomputing it.)
219  V3 r_hat_of_Rabg(const Rot3& Rabg) const;
220 
221  const api::LaneId id_;
222  const api::Segment* segment_{};
223  BranchPoint* start_bp_{};
224  BranchPoint* end_bp_{};
225 
226  const api::RBounds lane_bounds_;
227  const api::RBounds driveable_bounds_;
228  const api::HBounds elevation_bounds_;
229  const RoadCurve* road_curve_{};
230 };
231 
232 
233 } // namespace multilane
234 } // namespace maliput
235 } // namespace drake
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
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:128
A BranchPoint is a node in the network of a RoadGeometry at which Lanes connect to one another...
Definition: branch_point.h:66
double position
Definition: system_identification_test.cc:205
Definition: automotive_demo.cc:88
Eigen::Matrix< Scalar, 2, 1 > Vector2
A column vector of size 2, templated on scalar type.
Definition: eigen_types.h:30
void SetEndBp(BranchPoint *bp)
Definition: lane.h:98
BranchPoint * end_bp()
Definition: lane.h:102
Defines an interface for a path in a Segment object surface.
Definition: road_curve.h:24
V3 apply(const V3 &in) const
Applies the rotation to a 3-vector.
Definition: lane.h:39
An implementation of api::BranchPoint.
Definition: branch_point.h:41
Base class for the multilane implementation of api::Lane.
Definition: lane.h:51
A set of LaneEnds.
Definition: branch_point.h:22
A Lane represents a lane of travel in a road network.
Definition: lane.h:32
const CubicPolynomial & superelevation() const
Definition: lane.h:93
Isometric velocity vector in a Lane-frame.
Definition: lane_data.h:242
Definition: arc_road_curve.cc:11
TypeSpecificIdentifier<T> represents an identifier specifically identifying an entity of type T...
Definition: type_specific_identifier.h:38
#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:59
#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:47
An R^3 rotation parameterized by roll, pitch, yaw.
Definition: lane.h:32
Bounds in the lateral dimension (r component) of a Lane-frame, consisting of a pair of minimum and ma...
Definition: lane_data.h:274
A 3-dimensional rotation.
Definition: lane_data.h:53
BranchPoint * start_bp()
Definition: lane.h:100
double yaw() const
Definition: lane.h:41
A cubic polynomial, f(p) = a + b*p + c*p^2 + d*p^3.
Definition: cubic_polynomial.h:13
Bounds in the elevation dimension (h component) of a Lane-frame, consisting of a pair of minimum and ...
Definition: lane_data.h:309
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:25
A 3-dimensional position in a Lane-frame, consisting of three components:
Definition: lane_data.h:187
const CubicPolynomial & elevation() const
Definition: lane.h:89
double pitch() const
Definition: lane.h:42
Vector3< double > V3
Definition: lane.h:25
Matrix3< typename Derived::Scalar > rpy2rotmat(const Eigen::MatrixBase< Derived > &rpy)
We use an extrinsic rotation about Space-fixed x-y-z axes by angles [rpy(0), rpy(1), rpy(2)].
Definition: roll_pitch_yaw_not_using_quaternion.h:67
double roll() const
Definition: lane.h:43
int r
Definition: rgbd_camera.cc:89
void SetStartBp(BranchPoint *bp)
Definition: lane.h:97
#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:35
const maliput::api::Segment * segment_
Definition: mobil_planner_test.cc:134
Vector6< double > velocity
Definition: pose_smoother.cc:29
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
Vector2< double > V2
Definition: lane.h:22