Drake
pose_selector.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <map>
4 #include <memory>
5 #include <utility>
6 
7 #include <Eigen/Geometry>
8 
18 
19 namespace drake {
20 namespace automotive {
21 
22 /// ClosestPose bundles together the RoadOdometry of a particular target along
23 /// with its distance measure relative to the ego vehicle. Its intended use is
24 /// as the return argument for PoseSelector member functions.
25 template <typename T>
26 struct ClosestPose {
27  public:
28  /// Default constructor.
29  ClosestPose() = default;
30 
31  /// Constructs the ClosestPose via a full parameterization.
32  ClosestPose(const RoadOdometry<T>& odom, const T& dist)
33  : odometry(odom), distance(dist) {}
34 
36  T distance{0.};
37 };
38 
39 /// Specifies whether to assess the cars ahead or behind the ego car at its
40 /// current orientation with respect to its lane.
41 enum class AheadOrBehind { kAhead = 0, kBehind = 1 };
42 
43 /// PoseSelector is a class that provides the relevant pose or poses with
44 /// respect to a given ego vehicle driving within a given maliput road geometry.
45 ///
46 /// Instantiated templates for the following kinds of T's are provided:
47 /// - double
48 /// - AutoDiffXd
49 ///
50 /// They are already available to link against in the containing library.
51 ///
52 /// TODO(jadecastro): Enable AutoDiffXd support, and add unit tests.
53 template <typename T>
54 class PoseSelector {
55  public:
57 
58  PoseSelector() = delete;
59 
60  /// Returns the leading and trailing vehicles in a given @p lane that are
61  /// closest to an ego vehicle (within @p lane or another lane) as measured
62  /// along the `s`-coordinate of the ego vehicle's lane. The ego vehicle must
63  /// be within the `driveable_bounds` of @p lane (i.e. the road is contiguous
64  /// with @p lane along the `r`-direction). This function is used, for
65  /// instance, as logic for lane-change planners (e.g. MOBIL). The ego car's
66  /// pose (@p ego_pose) and the poses of the traffic cars (@p traffic_poses)
67  /// are provided. The parameter @p scan_distance determines the distance
68  /// along the sequence of lanes to scan before declaring that no traffic car
69  /// is ahead (resp. behind) the ego car. If no leading/trailing vehicles are
70  /// seen within @p traffic_lane, `s`-positions are taken to be at infinite
71  /// distances away from the ego car. Traffic vehicles having exactly the same
72  /// s-position as the ego vehicle but situated in a different (parallel) lane
73  /// are taken to be behind the ego vehicle.
74  ///
75  /// @return A map of AheadOrBehind values to vehicle ClosestPoses (containing
76  /// RoadOdometries and closest relative distances). Relative distances are
77  /// always positive, and a distance of positive infinity is returned if no
78  /// traffic cars are found. Note that when no vehicle is detected in front of
79  /// (resp. behind) the ego vehicle, the respective RoadPosition within
80  /// ClosestPoses will contain an `s`-value of positive (resp. negative)
81  /// infinity. Any traffic poses that are redunant with `ego_pose` (i.e. have
82  /// the same RoadPosition as the ego car) are discarded.
83  ///
84  /// The RoadGeometry from which @p lane is drawn is required to have default
85  /// branches set for all branches in the road network.
86  static std::map<AheadOrBehind, const ClosestPose<T>> FindClosestPair(
87  const maliput::api::Lane* lane,
88  const systems::rendering::PoseVector<T>& ego_pose,
89  const systems::rendering::PoseBundle<T>& traffic_poses,
90  const T& scan_distance);
91 
92  /// Same as PoseSelector::FindClosestPair() except that it returns a single
93  /// ClosestPose for either the vehicle ahead (AheadOrBehind::kAhead) or behind
94  /// (AheadOrBehind::kBehind).
95  ///
96  /// Note that when no car is detected in front of the ego car, the returned
97  /// RoadOdometry within ClosestPose will contain an `s`-value of
98  /// `std::numeric_limits<double>::infinity()`.
99  //
100  // TODO(jadecastro): Generalize this function to find and locate cars that are
101  // in lanes that eventually merge with the ego car's default ongoing lane.
102  static ClosestPose<T> FindSingleClosestPose(
103  const maliput::api::Lane* lane,
104  const systems::rendering::PoseVector<T>& ego_pose,
105  const systems::rendering::PoseBundle<T>& traffic_poses,
106  const T& scan_distance, const AheadOrBehind side);
107 
108  /// Extracts the vehicle's `s`-direction velocity based on its RoadOdometry @p
109  /// road_odometry in the Lane coordinate frame. Assumes the road has zero
110  /// elevation and superelevation. Throws if any element of
111  /// `road_odometry.pos` is not within the respective bounds of
112  /// `road_odometry.lane`.
113  ///
114  /// N.B. This function currently only provides exact derivatives for velocity
115  /// in the `s` direction when the road is straight (no yaw angle variations).
116  //
117  // TODO(jadecastro) Enable AutoDiffXd for
118  // maliput::api::Lane::GetOrientation().
119  static T GetSigmaVelocity(const RoadOdometry<T>& road_odometry);
120 
121  /// Returns `true` if and only if @p lane_position is within the longitudinal
122  /// (s), driveable (r) and elevation (h) bounds of the specified @p lane
123  /// (i.e. within `lane->driveable_bounds()` and `lane->elevation_bounds()`).
124  static bool IsWithinDriveable(
125  const maliput::api::LanePositionT<T>& lane_position,
126  const maliput::api::Lane* lane);
127 
128  /// Returns `true` if and only if @p geo_position is within the longitudinal
129  /// (s), lateral (r) and elevation (h) bounds of the specified @p lane
130  /// (i.e. within `lane->lane_bounds()` and `lane->elevation_bounds()`).
131  static bool IsWithinLane(const maliput::api::GeoPositionT<T>& geo_position,
132  const maliput::api::Lane* lane);
133 
134  /// Returns `true` if and only if @p lane_position is within the driveable
135  /// bounds of @p lane and, in addition, `r` is within its lane bounds.
136  static bool IsWithinLane(const maliput::api::LanePositionT<T>& lane_position,
137  const maliput::api::Lane* lane);
138 
139  private:
140  // Given a @p lane_direction, returns its default branch and updates @p
141  // lane_direction to match the `lane` and `with_s` of that branch. If there
142  // is no default branch, returns `nullopt` and sets `lane_direction->lane` to
143  // `nullptr`.
144  static optional<maliput::api::LaneEnd> GetDefaultOngoingLane(
145  LaneDirection* lane_direction);
146 
147  // Returns a RoadOdometry that contains an infinite `s` position, zero `r` and
148  // `h` positions, and zero velocities. If @p lane_direction contains `with_s
149  // == True`, a RoadOdometry containing an s-position at positive infinity is
150  // returned; otherwise a negative-infinite position is returned. For T =
151  // AutoDiffXd, the derivatives of the returned RoadOdometry are made to be
152  // coherent with respect to @p ego_pose.
153  static RoadOdometry<T> MakeInfiniteOdometry(
154  const LaneDirection& lane_direction,
155  const systems::rendering::PoseVector<T>& ego_pose);
156 
157  // Returns positive infinity. For T = AutoDiffXd, the derivatives of the
158  // the return value are made to be coherent with respect to @p ego_pose.
159  static T MakeInfiniteDistance(
160  const systems::rendering::PoseVector<T>& ego_pose);
161 
162  // Returns the distance (along the `s`-coordinate) from an end of a lane to a
163  // @p lane_position in that lane, where the end is determined by the `with_s`
164  // of the provided `lane_direction`. Both `lane` and `with_s` are specified
165  // in @p lane_direction. Throws if any element of @p lane_position is not
166  // within the respective bounds of `lane_direction.lane`.
167  static T CalcLaneProgress(
168  const LaneDirection& lane_direction,
169  const maliput::api::LanePositionT<T>& lane_position);
170 
171  // Constructs a LaneDirection structure based on a vehicle's current @p lane,
172  // @p lane_position, @p rotation (in global coordinates), and the @p side of
173  // the car (ahead or behind) that traffic is being observed. Note that
174  // `LaneDirection::with_s` in the return argument is interpreted as the
175  // direction along which targets are being observed (regardless of the ego
176  // car's orientation): it is true if cars are being observed along the
177  // `s`-direction and is false otherwise.
178  static LaneDirection CalcLaneDirection(
179  const maliput::api::Lane* lane,
180  const maliput::api::LanePositionT<T>& lane_position,
181  const Eigen::Quaternion<T>& rotation, AheadOrBehind side);
182 };
183 
184 } // namespace automotive
185 } // namespace drake
PoseSelector is a class that provides the relevant pose or poses with respect to a given ego vehicle ...
Definition: pose_selector.h:54
RoadOdometry< T > odometry
Definition: pose_selector.h:35
Definition: automotive_demo.cc:88
RoadOdometry contains the position of the vehicle with respect to a lane in a road, along with its velocity vector in the world frame.
Definition: road_odometry.h:13
PoseBundle is a container for a set of poses, represented by an Isometry3, and corresponding velociti...
Definition: pose_bundle.h:40
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
A Lane represents a lane of travel in a road network.
Definition: lane.h:34
stx::optional< T > optional
Definition: drake_optional.h:14
ClosestPose(const RoadOdometry< T > &odom, const T &dist)
Constructs the ClosestPose via a full parameterization.
Definition: pose_selector.h:32
T distance
Definition: pose_selector.h:36
ClosestPose()=default
Default constructor.
Provides drake::optional as an alias for the appropriate implementation of std::optional or std::expe...
A 3-dimensional position in a Lane-frame, consisting of three components:
Definition: lane_data.h:208
LaneDirection holds the lane that a MaliputRailcar is traversing and the direction in which it is mov...
Definition: lane_direction.h:13
A 7-vector representing the transform of frame A in the world frame, X_WA, in the form {p_WA...
Definition: pose_vector.h:19
ClosestPose bundles together the RoadOdometry of a particular target along with its distance measure ...
Definition: pose_selector.h:26
#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:33
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
AheadOrBehind
Specifies whether to assess the cars ahead or behind the ego car at its current orientation with resp...
Definition: pose_selector.h:41