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