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