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 
17 
18 namespace drake {
19 namespace automotive {
20 
21 /// ClosestPose bundles together the RoadOdometry of a particular target along
22 /// with its distance measure relative to the ego vehicle. Its intended use is
23 /// as the return argument for PoseSelector member functions.
24 template <typename T>
25 struct ClosestPose {
26  public:
27  /// Default constructor.
28  ClosestPose() = default;
29 
30  /// Constructs the ClosestPose via a full parameterization.
31  ClosestPose(const RoadOdometry<T>& odom, const T& dist)
32  : odometry(odom), distance(dist) {}
33 
35  T distance{0.};
36 };
37 
38 /// Specifies whether to assess the cars ahead or behind the ego car at its
39 /// current orientation with respect to its lane.
40 enum class AheadOrBehind { kAhead = 0, kBehind = 1 };
41 
42 /// Specifies whether to check ongoing lanes or both ongoing lanes and confluent
43 /// branches for traffic.
44 enum class ScanStrategy { kBranches, kPath };
45 
46 /// If kCache, configures a planning system (e.g. IdmController, MobilPlanner)
47 /// to declare an abstract state that caches the last-computed RoadPosition. If
48 /// kExhaustiveSearch, then the system will contain no abstract states. Note
49 /// that the kCache option is for performance speedup (at the expense of
50 /// optimizer compatibility) by preventing a potentially sizeable computation
51 /// within RoadGeometry::ToRoadPosition().
53 
54 /// PoseSelector is a class that provides the relevant pose or poses with
55 /// respect to a given ego vehicle driving within a given maliput road geometry.
56 ///
57 /// Instantiated templates for the following kinds of T's are provided:
58 /// - double
59 /// - AutoDiffXd
60 ///
61 /// They are already available to link against in the containing library.
62 ///
63 /// TODO(jadecastro): Enable AutoDiffXd support, and add unit tests.
64 template <typename T>
65 class PoseSelector {
66  public:
68 
69  PoseSelector() = delete;
70 
71  /// Returns the leading and trailing vehicles in a given @p lane or @p road
72  /// that are closest to an ego vehicle along its path, as measured along the
73  /// `s`-coordinate of the ego vehicle's lane. If @p path_or_branches is
74  /// ScanStrategy::kPath, only poses in the same path as the ego (ahead or
75  /// behind) are tracked; if ScanStrategy::kBranches, poses in lanes that
76  /// eventually merge (converge to the same branch point that is on the default
77  /// path of @p lane) are considered in addition to those along the ego car's
78  /// default path.
79  ///
80  /// Users should take heed to the fact that ScanStrategy::kBranches does _not_
81  /// assess the relationship between traffic and ego vehicle velocity when
82  /// selecting poses. Thus, cars within the same lane as the ego but with
83  /// negative net-velocity (approaching the ego car, from the ego's
84  /// point-of-view) could be ignored in favor of a car in a branch with
85  /// positive net-velocity.
86  ///
87  /// The ego vehicle must be within the `driveable_bounds` of @p lane (i.e. the
88  /// road is contiguous with @p lane along the `r`-direction). This function
89  /// is used, for instance, as logic for lane-change planners (e.g. MOBIL).
90  /// The ego car's pose (@p ego_pose) and the poses of the traffic cars (@p
91  /// traffic_poses) are provided. The parameter @p scan_distance determines
92  /// the distance along the sequence of lanes to scan before declaring that no
93  /// traffic car is ahead (resp. behind) the ego car.
94  ///
95  /// @return A map of AheadOrBehind values to vehicle ClosestPoses (containing
96  /// RoadOdometries and closest relative distances). Relative distances are
97  /// always positive, and a distance of positive infinity is returned if no
98  /// traffic cars are found. Note that when no vehicle is detected in front of
99  /// (resp. behind) the ego vehicle, the respective RoadPosition within
100  /// ClosestPoses will contain an `s`-value of positive (resp. negative)
101  /// infinity. Any traffic poses that are redunant with `ego_pose` (i.e. have
102  /// the same RoadPosition as the ego car and thus the same `s` and `r` value)
103  /// are discarded. If no leading/trailing vehicles are seen within
104  /// scan-distance of the ego car, `s`-positions are taken to be at infinite
105  /// distances away from the ego car. Note also that traffic vehicles having
106  /// exactly the same `s`-position as the ego vehicle but with different
107  /// `r`-value or are directly perpendicular in a Lane::to_left(),
108  /// Lane::to_right() lane are taken to be *behind* the ego vehicle. Note that
109  /// this implementation is greatly simplified by considering poses as points
110  /// (i.e. vehicle geometries are ignored).
111  ///
112  /// The RoadGeometry from which @p lane is drawn is required to have default
113  /// branches set for all branches in the road network.
114  static std::map<AheadOrBehind, const ClosestPose<T>> FindClosestPair(
115  const maliput::api::Lane* lane,
116  const systems::rendering::PoseVector<T>& ego_pose,
117  const systems::rendering::PoseBundle<T>& traffic_poses,
118  const T& scan_distance, ScanStrategy path_or_branches);
119 
120  /// Same as PoseSelector::FindClosestPair() except that it returns a single
121  /// ClosestPose for either the vehicle ahead (AheadOrBehind::kAhead) or behind
122  /// (AheadOrBehind::kBehind).
123  ///
124  /// Cars in other lanes are only tracked if they are in confluent lanes to a
125  /// given branch point within the `scan_distance`; i.e. cars in two
126  /// side-by-side lanes that never enter a branch point will not be selected.
127  /// This assumes that the ego car has knowledge of the traffic cars' default
128  /// lane.
129  ///
130  /// Note that when no car is detected in front of the ego car, the returned
131  /// RoadOdometry within ClosestPose will contain an `s`-value of
132  /// `std::numeric_limits<double>::infinity()`.
133  static ClosestPose<T> FindSingleClosestPose(
134  const maliput::api::Lane* lane,
135  const systems::rendering::PoseVector<T>& ego_pose,
136  const systems::rendering::PoseBundle<T>& traffic_poses,
137  const T& scan_distance, const AheadOrBehind side,
138  ScanStrategy path_or_branches);
139 
140  /// Extracts the vehicle's `s`-direction velocity based on its RoadOdometry @p
141  /// road_odometry in the Lane coordinate frame. Assumes the road has zero
142  /// elevation and superelevation. Throws if any element of
143  /// `road_odometry.pos` is not within the respective bounds of
144  /// `road_odometry.lane`.
145  ///
146  /// N.B. This function currently only provides exact derivatives for velocity
147  /// in the `s` direction when the road is straight (no yaw angle variations).
148  //
149  // TODO(jadecastro) Enable AutoDiffXd for
150  // maliput::api::Lane::GetOrientation().
151  static T GetSigmaVelocity(const RoadOdometry<T>& road_odometry);
152 };
153 
154 } // namespace automotive
155 } // namespace drake
PoseSelector is a class that provides the relevant pose or poses with respect to a given ego vehicle ...
Definition: pose_selector.h:65
RoadOdometry< T > odometry
Definition: pose_selector.h:34
Definition: automotive_demo.cc:90
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 Lane represents a lane of travel in a road network.
Definition: lane.h:35
ClosestPose(const RoadOdometry< T > &odom, const T &dist)
Constructs the ClosestPose via a full parameterization.
Definition: pose_selector.h:31
T distance
Definition: pose_selector.h:35
RoadPositionStrategy
If kCache, configures a planning system (e.g.
Definition: pose_selector.h:52
ClosestPose()=default
Default constructor.
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:25
#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
ScanStrategy
Specifies whether to check ongoing lanes or both ongoing lanes and confluent branches for traffic...
Definition: pose_selector.h:44
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:40