Drake
lane_data.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <functional>
4 #include <ostream>
5 #include <string>
6 
7 #include "drake/common/default_scalars.h"
8 #include "drake/common/drake_assert.h"
9 #include "drake/common/drake_copyable.h"
10 #include "drake/common/drake_throw.h"
11 #include "drake/common/eigen_types.h"
12 #include "drake/common/extract_double.h"
13 #include "drake/math/quaternion.h"
14 #include "drake/math/roll_pitch_yaw.h"
15 
16 namespace drake {
17 namespace maliput {
18 namespace api {
19 
20 class Lane;
21 
22 /// A specific endpoint of a specific Lane.
23 struct LaneEnd {
24  /// Labels for the endpoints of a Lane.
25  /// kStart is the "s == 0" end, and kFinish is the other end.
26  enum Which { kStart, kFinish, };
27 
28  /// An arbitrary strict complete ordering, useful for, e.g., std::map.
29  struct StrictOrder {
30  bool operator()(const LaneEnd& lhs, const LaneEnd& rhs) const {
31  auto as_tuple = [](const LaneEnd& le) {
32  return std::tie(le.lane, le.end);
33  };
34  return as_tuple(lhs) < as_tuple(rhs);
35  }
36  };
37 
38  /// Default constructor.
39  LaneEnd() = default;
40 
41  /// Construct a LaneEnd specifying the @p end of @p lane.
42  LaneEnd(const Lane* _lane, Which _end) : lane(_lane), end(_end) {}
43 
44  const Lane* lane{};
46 };
47 
48 /// Streams a string representation of @p which_end into @p out. Returns
49 /// @p out. This method is provided for the purposes of debugging or
50 /// text-logging. It is not intended for serialization.
51 std::ostream& operator<<(std::ostream& out, const LaneEnd::Which& which_end);
52 
53 /// A 3-dimensional rotation.
54 class Rotation {
55  public:
57 
58  /// Default constructor, creating an identity Rotation.
59  Rotation() : quaternion_(Quaternion<double>::Identity()) {}
60 
61  /// Constructs a Rotation from a quaternion @p quaternion (which will be
62  /// normalized).
63  static Rotation FromQuat(const Quaternion<double>& quaternion) {
64  return Rotation(quaternion.normalized());
65  }
66 
67  /// Constructs a Rotation from @p rpy, a vector of `[roll, pitch, yaw]`,
68  /// expressing a roll around X, followed by pitch around Y,
69  /// followed by yaw around Z (with all angles in radians).
72  }
73 
74  /// Constructs a Rotation expressing a @p roll around X, followed by
75  /// @p pitch around Y, followed by @p yaw around Z (with all angles
76  /// in radians).
77  static Rotation FromRpy(double roll, double pitch, double yaw) {
78  return FromRpy(Vector3<double>(roll, pitch, yaw));
79  }
80 
81  /// Provides a quaternion representation of the rotation.
82  const Quaternion<double>& quat() const { return quaternion_; }
83 
84  /// Sets value from a Quaternion @p quaternion (which will be normalized).
85  void set_quat(const Quaternion<double>& quaternion) {
86  quaternion_ = quaternion.normalized();
87  }
88 
89  /// Provides a rotation matrix representation of the rotation.
90  Matrix3<double> matrix() const { return quaternion_.toRotationMatrix(); }
91 
92  /// Provides a representation of rotation as a vector of angles
93  /// `[roll, pitch, yaw]` (in radians).
94  Vector3<double> rpy() const {
95  return math::QuaternionToSpaceXYZ(to_drake(quaternion_));
96  }
97 
98  // TODO(maddog@tri.global) Deprecate and/or remove roll()/pitch()/yaw(),
99  // since they hide the call to rpy(), and since
100  // most call-sites should probably be using something
101  // else (e.g., quaternion) anyway.
102  /// Returns the roll component of the Rotation (in radians).
103  double roll() const { return rpy().x(); }
104 
105  /// Returns the pitch component of the Rotation (in radians).
106  double pitch() const { return rpy().y(); }
107 
108  /// Returns the yaw component of the Rotation (in radians).
109  double yaw() const { return rpy().z(); }
110 
111  private:
112  explicit Rotation(const Quaternion<double>& quat) : quaternion_(quat) {}
113 
114  // Converts Eigen (x,y,z,w) quaternion to drake's temporary(?) (w,x,y,z).
115  static Vector4<double> to_drake(const Quaternion<double>& quat) {
116  return Vector4<double>(quat.w(), quat.x(), quat.y(), quat.z());
117  }
118 
119  Quaternion<double> quaternion_;
120 };
121 
122 /// Streams a string representation of @p rotation into @p out. Returns
123 /// @p out. This method is provided for the purposes of debugging or
124 /// text-logging. It is not intended for serialization.
125 std::ostream& operator<<(std::ostream& out, const Rotation& rotation);
126 
127 
128 /// A position in 3-dimensional geographical Cartesian space, i.e., in the world
129 /// frame, consisting of three components x, y, and z.
130 ///
131 /// Instantiated templates for the following kinds of T's are provided:
132 /// - double
133 /// - drake::AutoDiffXd
134 /// - drake::symbolic::Expression
135 ///
136 /// They are already available to link against in the containing library.
137 template <typename T>
139  public:
141 
142  /// Default constructor, initializing all components to zero.
143  GeoPositionT() : xyz_(T(0.), T(0.), T(0.)) {}
144 
145  /// Fully parameterized constructor.
146  GeoPositionT(const T& x, const T& y, const T& z) : xyz_(x, y, z) {}
147 
148  /// Constructs a GeoPositionT from a 3-vector @p xyz of the form `[x, y, z]`.
149  static GeoPositionT<T> FromXyz(const Vector3<T>& xyz) {
150  return GeoPositionT<T>(xyz);
151  }
152 
153  /// Returns all components as 3-vector `[x, y, z]`.
154  const Vector3<T>& xyz() const { return xyz_; }
155  /// Sets all components from 3-vector `[x, y, z]`.
156  void set_xyz(const Vector3<T>& xyz) { xyz_ = xyz; }
157 
158  /// @name Getters and Setters
159  //@{
160  /// Gets `x` value.
161  T x() const { return xyz_.x(); }
162  /// Sets `x` value.
163  void set_x(const T& x) { xyz_.x() = x; }
164  /// Gets `y` value.
165  T y() const { return xyz_.y(); }
166  /// Sets `y` value.
167  void set_y(const T& y) { xyz_.y() = y; }
168  /// Gets `z` value.
169  T z() const { return xyz_.z(); }
170  /// Sets `z` value.
171  void set_z(const T& z) { xyz_.z() = z; }
172  //@}
173 
174  /// Constructs a GeoPositionT<double> from other types, producing a clone if
175  /// already double.
177  return {ExtractDoubleOrThrow(xyz_.x()),
180  }
181 
182  private:
183  explicit GeoPositionT(const Vector3<T>& xyz) : xyz_(xyz) {}
184 
186 };
187 
188 // Alias for the double scalar type.
190 
191 /// Streams a string representation of @p geo_position into @p out. Returns
192 /// @p out. This method is provided for the purposes of debugging or
193 /// text-logging. It is not intended for serialization.
194 std::ostream& operator<<(std::ostream& out, const GeoPosition& geo_position);
195 
196 /// GeoPosition overload for the equality operator.
197 template <typename T>
198 auto operator==(const GeoPositionT<T>& lhs, const GeoPositionT<T>& rhs) {
199  return (lhs.xyz() == rhs.xyz());
200 }
201 
202 /// GeoPosition overload for the inequality operator.
203 template <typename T>
204 auto operator!=(const GeoPositionT<T>& lhs, const GeoPositionT<T>& rhs) {
205  return !(lhs.xyz() == rhs.xyz());
206 }
207 
208 /// A 3-dimensional position in a `Lane`-frame, consisting of three components:
209 /// * s is longitudinal position, as arc-length along a Lane's reference line.
210 /// * r is lateral position, perpendicular to the reference line at s.
211 /// * h is height above the road surface.
212 ///
213 /// Instantiated templates for the following kinds of T's are provided:
214 /// - double
215 /// - drake::AutoDiffXd
216 /// - drake::symbolic::Expression
217 ///
218 /// They are already available to link against in the containing library.
219 template <typename T>
221  public:
223 
224  /// Default constructor, initializing all components to zero.
225  LanePositionT() : srh_(T(0.), T(0.), T(0.)) {}
226 
227  /// Fully parameterized constructor.
228  LanePositionT(const T& s, const T& r, const T& h) : srh_(s, r, h) {}
229 
230  /// Constructs a LanePosition from a 3-vector @p srh of the form `[s, r, h]`.
231  static LanePositionT<T> FromSrh(const Vector3<T>& srh) {
232  return LanePositionT<T>(srh);
233  }
234 
235  /// Returns all components as 3-vector `[s, r, h]`.
236  const Vector3<T>& srh() const { return srh_; }
237  /// Sets all components from 3-vector `[s, r, h]`.
238  void set_srh(const Vector3<T>& srh) { srh_ = srh; }
239 
240  /// @name Getters and Setters
241  //@{
242  /// Gets `s` value.
243  T s() const { return srh_.x(); }
244  /// Sets `s` value.
245  void set_s(const T& s) { srh_.x() = s; }
246  /// Gets `r` value.
247  T r() const { return srh_.y(); }
248  /// Sets `r` value.
249  void set_r(const T& r) { srh_.y() = r; }
250  /// Gets `h` value.
251  T h() const { return srh_.z(); }
252  /// Sets `h` value.
253  void set_h(const T& h) { srh_.z() = h; }
254  //@}
255 
256  /// Constructs a LanePositionT<double> from other types, producing a clone if
257  /// already double.
259  return {ExtractDoubleOrThrow(srh_.x()),
260  ExtractDoubleOrThrow(srh_.y()),
261  ExtractDoubleOrThrow(srh_.z())};
262  }
263 
264  private:
265  explicit LanePositionT(const Vector3<T>& srh) : srh_(srh) {}
266 
267  Vector3<T> srh_;
268 };
269 
270 // Alias for the double scalar type.
272 
273 /// Streams a string representation of @p lane_position into @p out. Returns
274 /// @p out. This method is provided for the purposes of debugging or
275 /// text-logging. It is not intended for serialization.
276 std::ostream& operator<<(std::ostream& out, const LanePosition& lane_position);
277 
278 /// Isometric velocity vector in a `Lane`-frame.
279 ///
280 /// sigma_v, rho_v, and eta_v are the components of velocity in a
281 /// (sigma, rho, eta) coordinate system. (sigma, rho, eta) have the same
282 /// orientation as the (s, r, h) at any given point in space, however they
283 /// form an isometric system with a Cartesian distance metric. Hence,
284 /// IsoLaneVelocity represents a "real" physical velocity vector (albeit
285 /// with an orientation relative to the road surface).
287  /// Default constructor.
288  IsoLaneVelocity() = default;
289 
290  /// Fully parameterized constructor.
291  IsoLaneVelocity(double _sigma_v, double _rho_v, double _eta_v)
292  : sigma_v(_sigma_v), rho_v(_rho_v), eta_v(_eta_v) {}
293 
294  double sigma_v{};
295  double rho_v{};
296  double eta_v{};
297 };
298 
299 
300 /// A position in the road network, consisting of a pointer to a specific
301 /// Lane and a `Lane`-frame position in that Lane.
302 struct RoadPosition {
303  /// Default constructor.
304  RoadPosition() = default;
305 
306  /// Fully parameterized constructor.
307  RoadPosition(const Lane* _lane, const LanePosition& _pos)
308  : lane(_lane), pos(_pos) {}
309 
310  const Lane* lane{};
312 };
313 
314 
315 /// Bounds in the lateral dimension (r component) of a `Lane`-frame, consisting
316 /// of a pair of minimum and maximum r value. The bounds must straddle r = 0,
317 /// i.e., the minimum must be <= 0 and the maximum must be >= 0.
318 class RBounds {
319  public:
321 
322  /// Default constructor.
323  RBounds() = default;
324 
325  /// Fully parameterized constructor.
326  /// @throws std::runtime_error When @p min is greater than 0.
327  /// @throws std::runtime_error When @p max is smaller than 0.
328  RBounds(double min, double max) : min_(min), max_(max) {
329  DRAKE_THROW_UNLESS(min <= 0.);
330  DRAKE_THROW_UNLESS(max >= 0.);
331  }
332 
333  /// @name Getters and Setters
334  //@{
335  /// Gets minimum bound.
336  double min() const { return min_; }
337  /// Sets minimum bound.
338  /// @throws std::runtime_error When @p min is greater than 0.
339  void set_min(double min) {
340  DRAKE_THROW_UNLESS(min <= 0.);
341  min_ = min;
342  }
343  /// Gets maximum bound.
344  double max() const { return max_; }
345  /// Sets maximum bound.
346  /// @throws std::runtime_error When @p max is smaller than 0.
347  void set_max(double max) {
348  DRAKE_THROW_UNLESS(max >= 0.);
349  max_ = max;
350  }
351  //@}
352 
353  private:
354  double min_{};
355  double max_{};
356 };
357 
358 
359 /// Bounds in the elevation dimension (`h` component) of a `Lane`-frame,
360 /// consisting of a pair of minimum and maximum `h` value. The bounds
361 /// must straddle `h = 0`, i.e., the minimum must be `<= 0` and the
362 /// maximum must be `>= 0`.
363 class HBounds {
364  public:
366 
367  /// Default constructor.
368  HBounds() = default;
369 
370  /// Fully parameterized constructor.
371  /// @throws std::runtime_error When @p min is greater than 0.
372  /// @throws std::runtime_error When @p max is smaller than 0.
373  HBounds(double min, double max) : min_(min), max_(max) {
374  DRAKE_THROW_UNLESS(min <= 0.);
375  DRAKE_THROW_UNLESS(max >= 0.);
376  }
377 
378  /// @name Getters and Setters
379  //@{
380  /// Gets minimum bound.
381  double min() const { return min_; }
382  /// Sets minimum bound.
383  /// @throws std::runtime_error When @p min is greater than 0.
384  void set_min(double min) {
385  DRAKE_THROW_UNLESS(min <= 0.);
386  min_ = min;
387  }
388  /// Gets maximum bound.
389  double max() const { return max_; }
390  /// Sets maximum bound.
391  /// @throws std::runtime_error When @p max is smaller than 0.
392  void set_max(double max) {
393  DRAKE_THROW_UNLESS(max >= 0.);
394  max_ = max;
395  }
396  //@}
397 
398  private:
399  double min_{};
400  double max_{};
401 };
402 
403 } // namespace api
404 } // namespace maliput
405 } // namespace drake
RBounds(double min, double max)
Fully parameterized constructor.
Definition: lane_data.h:328
auto operator==(const GeoPositionT< T > &lhs, const GeoPositionT< T > &rhs)
GeoPosition overload for the equality operator.
Definition: lane_data.h:198
void set_r(const T &r)
Sets r value.
Definition: lane_data.h:249
void set_xyz(const Vector3< T > &xyz)
Sets all components from 3-vector [x, y, z].
Definition: lane_data.h:156
Eigen::Quaternion< Scalar > Quaternion
A quaternion templated on scalar type.
Definition: eigen_types.h:119
RoadPosition(const Lane *_lane, const LanePosition &_pos)
Fully parameterized constructor.
Definition: lane_data.h:307
static LanePositionT< T > FromSrh(const Vector3< T > &srh)
Constructs a LanePosition from a 3-vector srh of the form [s, r, h].
Definition: lane_data.h:231
static Rotation FromRpy(double roll, double pitch, double yaw)
Constructs a Rotation expressing a roll around X, followed by pitch around Y, followed by yaw around ...
Definition: lane_data.h:77
Eigen::Matrix< Scalar, 4, 1 > Vector4
A column vector of size 4, templated on scalar type.
Definition: eigen_types.h:38
std::vector< Number > x
Definition: ipopt_solver.cc:150
Which end
Definition: lane_data.h:45
A specific endpoint of a specific Lane.
Definition: lane_data.h:23
Definition: automotive_demo.cc:89
double pitch() const
Returns the pitch component of the Rotation (in radians).
Definition: lane_data.h:106
LaneEnd()=default
Default constructor.
void set_x(const T &x)
Sets x value.
Definition: lane_data.h:163
T r() const
Gets r value.
Definition: lane_data.h:247
double yaw() const
Returns the yaw component of the Rotation (in radians).
Definition: lane_data.h:109
GeoPositionT(const T &x, const T &y, const T &z)
Fully parameterized constructor.
Definition: lane_data.h:146
Vector3< double > rpy() const
Provides a representation of rotation as a vector of angles [roll, pitch, yaw] (in radians)...
Definition: lane_data.h:94
const Vector3< T > & srh() const
Returns all components as 3-vector [s, r, h].
Definition: lane_data.h:236
HBounds(double min, double max)
Fully parameterized constructor.
Definition: lane_data.h:373
LanePosition pos
Definition: lane_data.h:311
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
void set_h(const T &h)
Sets h value.
Definition: lane_data.h:253
IsoLaneVelocity(double _sigma_v, double _rho_v, double _eta_v)
Fully parameterized constructor.
Definition: lane_data.h:291
const Lane * lane
Definition: lane_data.h:44
#define DRAKE_THROW_UNLESS(condition)
Evaluates condition and iff the value is false will throw an exception with a message showing at leas...
Definition: drake_throw.h:23
LanePositionT(const T &s, const T &r, const T &h)
Fully parameterized constructor.
Definition: lane_data.h:228
A Lane represents a lane of travel in a road network.
Definition: lane.h:35
double max() const
Gets maximum bound.
Definition: lane_data.h:389
double y
Definition: vtk_util_test.cc:26
T z() const
Gets z value.
Definition: lane_data.h:169
Isometric velocity vector in a Lane-frame.
Definition: lane_data.h:286
double max() const
Gets maximum bound.
Definition: lane_data.h:344
Vector3< typename Derived::Scalar > QuaternionToSpaceXYZ(const Eigen::MatrixBase< Derived > &quaternion)
Computes SpaceXYZ Euler angles from quaternion representation.
Definition: quaternion.h:363
double min() const
Gets minimum bound.
Definition: lane_data.h:381
An arbitrary strict complete ordering, useful for, e.g., std::map.
Definition: lane_data.h:29
static Rotation FromQuat(const Quaternion< double > &quaternion)
Constructs a Rotation from a quaternion quaternion (which will be normalized).
Definition: lane_data.h:63
bool operator()(const LaneEnd &lhs, const LaneEnd &rhs) const
Definition: lane_data.h:30
Eigen::Matrix< Scalar, 3, 1 > Vector3
A column vector of size 3, templated on scalar type.
Definition: eigen_types.h:34
#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
const Vector3< T > & xyz() const
Returns all components as 3-vector [x, y, z].
Definition: lane_data.h:154
Expression max(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:696
std::ostream & operator<<(std::ostream &out, const LaneEnd::Which &which_end)
Streams a string representation of which_end into out.
Definition: lane_data.cc:11
double ExtractDoubleOrThrow(const Eigen::AutoDiffScalar< DerType > &scalar)
Returns the autodiff scalar&#39;s value() as a double.
Definition: autodiff_overloads.h:145
Expression min(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:684
void set_quat(const Quaternion< double > &quaternion)
Sets value from a Quaternion quaternion (which will be normalized).
Definition: lane_data.h:85
const Expression xyz_
Definition: symbolic_polynomial_test.cc:48
Eigen::Vector3d rpy
Definition: rgbd_camera_publish_lcm_example.cc:79
A 3-dimensional position in a Lane-frame, consisting of three components:
Definition: lane_data.h:220
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
void set_min(double min)
Sets minimum bound.
Definition: lane_data.h:339
Definition: lane_data.h:26
double min() const
Gets minimum bound.
Definition: lane_data.h:336
T y() const
Gets y value.
Definition: lane_data.h:165
Definition: lane_data.h:26
void set_s(const T &s)
Sets s value.
Definition: lane_data.h:245
Matrix3< double > matrix() const
Provides a rotation matrix representation of the rotation.
Definition: lane_data.h:90
Quaternion< typename Derived::Scalar > RollPitchYawToQuaternion(const Eigen::MatrixBase< Derived > &rpy)
Computes the Quaternion representation of a rotation given the set of Euler angles describing this ro...
Definition: roll_pitch_yaw_using_quaternion.h:49
Bounds in the elevation dimension (h component) of a Lane-frame, consisting of a pair of minimum and ...
Definition: lane_data.h:363
void set_max(double max)
Sets maximum bound.
Definition: lane_data.h:392
T s() const
Gets s value.
Definition: lane_data.h:243
double roll() const
Returns the roll component of the Rotation (in radians).
Definition: lane_data.h:103
Which
Labels for the endpoints of a Lane.
Definition: lane_data.h:26
double z
Definition: vtk_util_test.cc:27
const Quaternion< double > & quat() const
Provides a quaternion representation of the rotation.
Definition: lane_data.h:82
GeoPositionT< double > MakeDouble() const
Constructs a GeoPositionT<double> from other types, producing a clone if already double.
Definition: lane_data.h:176
void set_min(double min)
Sets minimum bound.
Definition: lane_data.h:384
LanePositionT< double > MakeDouble() const
Constructs a LanePositionT<double> from other types, producing a clone if already double...
Definition: lane_data.h:258
void set_y(const T &y)
Sets y value.
Definition: lane_data.h:167
void set_max(double max)
Sets maximum bound.
Definition: lane_data.h:347
static Rotation FromRpy(const Vector3< double > &rpy)
Constructs a Rotation from rpy, a vector of [roll, pitch, yaw], expressing a roll around X...
Definition: lane_data.h:70
T x() const
Gets x value.
Definition: lane_data.h:161
LaneEnd(const Lane *_lane, Which _end)
Construct a LaneEnd specifying the end of lane.
Definition: lane_data.h:42
T h() const
Gets h value.
Definition: lane_data.h:251
void set_z(const T &z)
Sets z value.
Definition: lane_data.h:171
Eigen::Vector3d pos
Definition: rgbd_camera_publish_lcm_example.cc:78
A position in the road network, consisting of a pointer to a specific Lane and a Lane-frame position ...
Definition: lane_data.h:302
static GeoPositionT< T > FromXyz(const Vector3< T > &xyz)
Constructs a GeoPositionT from a 3-vector xyz of the form [x, y, z].
Definition: lane_data.h:149
auto operator!=(const GeoPositionT< T > &lhs, const GeoPositionT< T > &rhs)
GeoPosition overload for the inequality operator.
Definition: lane_data.h:204
void set_srh(const Vector3< T > &srh)
Sets all components from 3-vector [s, r, h].
Definition: lane_data.h:238