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