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 
10 #include "drake/math/quaternion.h"
12 
13 namespace drake {
14 namespace maliput {
15 namespace api {
16 
17 
18 class Lane;
19 
20 
22 struct LaneEnd {
25  enum Which { kStart, kFinish, };
26 
28  struct StrictOrder {
29  bool operator()(const LaneEnd& lhs, const LaneEnd& rhs) const {
30  auto as_tuple = [](const LaneEnd& le) {
31  return std::tie(le.lane, le.end);
32  };
33  return as_tuple(lhs) < as_tuple(rhs);
34  }
35  };
36 
38  LaneEnd() = default;
39 
41  LaneEnd(const Lane* _lane, Which _end) : lane(_lane), end(_end) {}
42 
43  const Lane* lane{};
45 };
46 
50 std::ostream& operator<<(std::ostream& out, const LaneEnd::Which& which_end);
51 
53 class Rotation {
54  public:
56 
57 
58  Rotation() : quaternion_(Quaternion<double>::Identity()) {}
59 
62  static Rotation FromQuat(const Quaternion<double>& quaternion) {
63  return Rotation(quaternion.normalized());
64  }
65 
71  }
72 
76  static Rotation FromRpy(double roll, double pitch, double yaw) {
77  return FromRpy(Vector3<double>(roll, pitch, yaw));
78  }
79 
81  const Quaternion<double>& quat() const { return quaternion_; }
82 
84  void set_quat(const Quaternion<double>& quaternion) {
85  quaternion_ = quaternion.normalized();
86  }
87 
89  Matrix3<double> matrix() const { return quaternion_.toRotationMatrix(); }
90 
93  Vector3<double> rpy() const {
94  return math::QuaternionToSpaceXYZ(to_drake(quaternion_));
95  }
96 
97  // TODO(maddog@tri.global) Deprecate and/or remove roll()/pitch()/yaw(),
98  // since they hide the call to rpy(), and since
99  // most call-sites should probably be using something
100  // else (e.g., quaternion) anyway.
102  double roll() const { return rpy().x(); }
103 
105  double pitch() const { return rpy().y(); }
106 
108  double yaw() const { return rpy().z(); }
109 
110  private:
111  explicit Rotation(const Quaternion<double>& quat) : quaternion_(quat) {}
112 
113  // Converts Eigen (x,y,z,w) quaternion to drake's temporary(?) (w,x,y,z).
114  static Vector4<double> to_drake(const Quaternion<double>& quat) {
115  return Vector4<double>(quat.w(), quat.x(), quat.y(), quat.z());
116  }
117 
118  Quaternion<double> quaternion_;
119 };
120 
124 std::ostream& operator<<(std::ostream& out, const Rotation& rotation);
125 
128 class GeoPosition {
129  public:
131 
132 
133  GeoPosition() : xyz_(0., 0., 0.) {}
134 
136  GeoPosition(double x, double y, double z) : xyz_(x, y, z) {}
137 
140  explicit GeoPosition(const Vector3<double>& xyz) : xyz_(xyz) {}
141 
143  static GeoPosition FromXyz(const Vector3<double>& xyz) {
144  return GeoPosition(xyz);
145  }
146 
148  const Vector3<double>& xyz() const { return xyz_; }
150  void set_xyz(const Vector3<double>& xyz) { xyz_ = xyz; }
151 
153 
154  double x() const { return xyz_.x(); }
157  void set_x(double x) { xyz_.x() = x; }
159  double y() const { return xyz_.y(); }
161  void set_y(double y) { xyz_.y() = y; }
163  double z() const { return xyz_.z(); }
165  void set_z(double z) { xyz_.z() = z; }
167 
168  private:
170 };
171 
175 std::ostream& operator<<(std::ostream& out, const GeoPosition& geo_position);
176 
178 bool operator==(const GeoPosition& lhs, const GeoPosition& rhs);
179 
181 bool operator!=(const GeoPosition& lhs, const GeoPosition& rhs);
182 
188  public:
190 
191 
192  LanePosition() : srh_(0., 0., 0.) {}
193 
195  LanePosition(double s, double r, double h) : srh_(s, r, h) {}
196 
198  static LanePosition FromSrh(const Vector3<double>& srh) {
199  return LanePosition(srh);
200  }
201 
203  const Vector3<double>& srh() const { return srh_; }
205  void set_srh(const Vector3<double>& srh) { srh_ = srh; }
206 
208 
209  double s() const { return srh_.x(); }
212  void set_s(double s) { srh_.x() = s; }
214  double r() const { return srh_.y(); }
216  void set_r(double r) { srh_.y() = r; }
218  double h() const { return srh_.z(); }
220  void set_h(double h) { srh_.z() = h; }
222 
223  private:
224  Vector3<double> srh_;
225 
226  explicit LanePosition(const Vector3<double>& srh) : srh_(srh) {}
227 };
228 
232 std::ostream& operator<<(std::ostream& out, const LanePosition& lane_position);
233 
244  IsoLaneVelocity() = default;
245 
247  IsoLaneVelocity(double _sigma_v, double _rho_v, double _eta_v)
248  : sigma_v(_sigma_v), rho_v(_rho_v), eta_v(_eta_v) {}
249 
250  double sigma_v{};
251  double rho_v{};
252  double eta_v{};
253 };
254 
255 
258 struct RoadPosition {
260  RoadPosition() = default;
261 
263  RoadPosition(const Lane* _lane, const LanePosition& _pos)
264  : lane(_lane), pos(_pos) {}
265 
266  const Lane* lane{};
268 };
269 
270 
274 class RBounds {
275  public:
277 
278 
279  RBounds() = default;
280 
282  RBounds(double min, double max) : min_(min), max_(max) {
283  DRAKE_DEMAND(min <= 0.);
284  DRAKE_DEMAND(max >= 0.);
285  }
286 
288 
289  double min() const { return min_; }
292  void set_min(double min) { min_ = min; }
294  double max() const { return max_; }
296  void set_max(double max) { max_ = max; }
298 
299  private:
300  double min_{};
301  double max_{};
302 };
303 
304 
309 class HBounds {
310  public:
312 
313 
314  HBounds() = default;
315 
317  HBounds(double min, double max) : min_(min), max_(max) {
318  DRAKE_DEMAND(min <= 0.);
319  DRAKE_DEMAND(max >= 0.);
320  }
321 
323 
324  double min() const { return min_; }
327  void set_min(double min) { min_ = min; }
329  double max() const { return max_; }
331  void set_max(double max) { max_ = max; }
333 
334  private:
335  double min_{};
336  double max_{};
337 };
338 
339 } // namespace api
340 } // namespace maliput
341 } // namespace drake
RBounds(double min, double max)
Fully parameterized constructor.
Definition: lane_data.h:282
Eigen::Quaternion< Scalar > Quaternion
A quaternion templated on scalar type.
Definition: eigen_types.h:98
static GeoPosition FromXyz(const Vector3< double > &xyz)
Constructs a GeoPosition from a 3-vector xyz of the form [x, y, z].
Definition: lane_data.h:143
RoadPosition(const Lane *_lane, const LanePosition &_pos)
Fully parameterized constructor.
Definition: lane_data.h:263
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:76
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
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:44
A specific endpoint of a specific Lane.
Definition: lane_data.h:22
Definition: automotive_demo.cc:88
double pitch() const
Returns the pitch component of the Rotation (in radians).
Definition: lane_data.h:105
LaneEnd()=default
Default constructor.
GeoPosition(double x, double y, double z)
Fully parameterized constructor.
Definition: lane_data.h:136
double yaw() const
Returns the yaw component of the Rotation (in radians).
Definition: lane_data.h:108
const Vector3< double > & srh() const
Returns all components as 3-vector [s, r, h].
Definition: lane_data.h:203
Vector3< double > rpy() const
Provides a representation of rotation as a vector of angles [roll, pitch, yaw] (in radians)...
Definition: lane_data.h:93
HBounds(double min, double max)
Fully parameterized constructor.
Definition: lane_data.h:317
LanePosition pos
Definition: lane_data.h:267
IsoLaneVelocity(double _sigma_v, double _rho_v, double _eta_v)
Fully parameterized constructor.
Definition: lane_data.h:247
const Lane * lane
Definition: lane_data.h:43
double y() const
Gets y value.
Definition: lane_data.h:159
A Lane represents a lane of travel in a road network.
Definition: lane.h:32
double max() const
Gets maximum bound.
Definition: lane_data.h:329
double y
Definition: vtk_util_test.cc:26
Isometric velocity vector in a Lane-frame.
Definition: lane_data.h:242
double max() const
Gets maximum bound.
Definition: lane_data.h:294
Vector3< typename Derived::Scalar > QuaternionToSpaceXYZ(const Eigen::MatrixBase< Derived > &quaternion)
Computes SpaceXYZ Euler angles from quaternion representation.
Definition: quaternion.h:363
An arbitrary strict complete ordering, useful for, e.g., std::map.
Definition: lane_data.h:28
static Rotation FromQuat(const Quaternion< double > &quaternion)
Constructs a Rotation from a quaternion quaternion (which will be normalized).
Definition: lane_data.h:62
bool operator()(const LaneEnd &lhs, const LaneEnd &rhs) const
Definition: lane_data.h:29
void set_xyz(const Vector3< double > &xyz)
Sets all components from 3-vector [x, y, z].
Definition: lane_data.h:150
#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
double z() const
Gets z vaue.
Definition: lane_data.h:163
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:9
bool operator!=(const GeoPosition &lhs, const GeoPosition &rhs)
GeoPosition overload for the inequality operator.
Definition: lane_data.cc:28
#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
LanePosition(double s, double r, double h)
Fully parameterized constructor.
Definition: lane_data.h:195
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:84
void set_x(double x)
Sets x value.
Definition: lane_data.h:157
const Expression xyz_
Definition: symbolic_polynomial_test.cc:44
Eigen::Vector3d rpy
Definition: rgbd_camera_publish_lcm_example.cc:75
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
void set_min(double min)
Sets minimum bound.
Definition: lane_data.h:292
Definition: lane_data.h:25
Definition: lane_data.h:25
void set_s(double s)
Sets s value.
Definition: lane_data.h:212
Matrix3< double > matrix() const
Provides a rotation matrix representation of the rotation.
Definition: lane_data.h:89
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:309
void set_srh(const Vector3< double > &srh)
Sets all components from 3-vector [s, r, h].
Definition: lane_data.h:205
Utilities for arithmetic on quaternions.
void set_max(double max)
Sets maximum bound.
Definition: lane_data.h:331
void set_h(double h)
Sets h value.
Definition: lane_data.h:220
double roll() const
Returns the roll component of the Rotation (in radians).
Definition: lane_data.h:102
Which
Labels for the endpoints of a Lane.
Definition: lane_data.h:25
const Vector3< double > & xyz() const
Returns all components as 3-vector [x, y, z].
Definition: lane_data.h:148
double z
Definition: vtk_util_test.cc:27
A 3-dimensional position in a Lane-frame, consisting of three components:
Definition: lane_data.h:187
void set_y(double y)
Sets y value.
Definition: lane_data.h:161
double r() const
Gets r value.
Definition: lane_data.h:214
void set_z(double z)
Sets z value.
Definition: lane_data.h:165
const Quaternion< double > & quat() const
Provides a quaternion representation of the rotation.
Definition: lane_data.h:81
void set_min(double min)
Sets minimum bound.
Definition: lane_data.h:327
void set_max(double max)
Sets maximum bound.
Definition: lane_data.h:296
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:69
LaneEnd(const Lane *_lane, Which _end)
Construct a LaneEnd specifying the end of lane.
Definition: lane_data.h:41
static LanePosition FromSrh(const Vector3< double > &srh)
Constructs a LanePosition from a 3-vector srh of the form [s, r, h].
Definition: lane_data.h:198
GeoPosition(const Vector3< double > &xyz)
Fully parameterized constructor from a 3-vector xyz of the form [x, y, z].
Definition: lane_data.h:140
int r
Definition: rgbd_camera.cc:89
bool operator==(const GeoPosition &lhs, const GeoPosition &rhs)
GeoPosition overload for the equality operator.
Definition: lane_data.cc:24
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:258
void set_r(double r)
Sets r value.
Definition: lane_data.h:216
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
double h() const
Gets h value.
Definition: lane_data.h:218