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 
24 template <typename T>
25 struct ClosestPose {
26  public:
28  ClosestPose() = default;
29 
31  ClosestPose(const RoadOdometry<T>& odom, const T& dist)
32  : odometry(odom), distance(dist) {}
33 
35  T distance{0.};
36 };
37 
40 enum class AheadOrBehind { kAhead = 0, kBehind = 1 };
41 
51 template <typename T>
52 class PoseSelector {
53  public:
55 
56  PoseSelector() = delete;
57 
84  static std::map<AheadOrBehind, const ClosestPose<T>> FindClosestPair(
85  const maliput::api::Lane* lane,
86  const systems::rendering::PoseVector<T>& ego_pose,
87  const systems::rendering::PoseBundle<T>& traffic_poses,
88  const T& scan_distance);
89 
97  //
98  // TODO(jadecastro): Generalize this function to find and locate cars that are
99  // in lanes that eventually merge with the ego car's default ongoing lane.
100  static ClosestPose<T> FindSingleClosestPose(
101  const maliput::api::Lane* lane,
102  const systems::rendering::PoseVector<T>& ego_pose,
103  const systems::rendering::PoseBundle<T>& traffic_poses,
104  const T& scan_distance, const AheadOrBehind side);
105 
111  static T GetSigmaVelocity(const RoadOdometry<T>& road_odometry);
112 
116  static bool IsWithinDriveable(
117  const maliput::api::LanePosition& lane_position,
118  const maliput::api::Lane* lane);
119 
123  static bool IsWithinLane(const maliput::api::GeoPosition& geo_position,
124  const maliput::api::Lane* lane);
125 
128  static bool IsWithinLane(const maliput::api::LanePosition& lane_position,
129  const maliput::api::Lane* lane);
130 
131  private:
132  // Given a @p lane_direction, returns its default branch and updates @p
133  // lane_direction to match the `lane` and `with_s` of that branch. If there
134  // is no default branch, returns `nullptr` and sets `lane_direction->lane` to
135  // `nullptr`.
136  static std::unique_ptr<maliput::api::LaneEnd> GetDefaultOngoingLane(
137  LaneDirection* lane_direction);
138 
139  // Returns a RoadOdometry that contains infinite positions and zero `r`, `h`,
140  // and velocity values. If @p lane_direction contains `with_s == True`, a
141  // RoadOdometry containing an s-position at positive infinity is returned;
142  // otherwise a negative-intinite position is returned.
143  static RoadOdometry<T> MakeInfiniteOdometry(
144  const LaneDirection& lane_direction);
145 
146  // Returns the distance (along the `s`-coordinate) from an end of a lane to a
147  // @p lane_position in that lane, where the end is determined by the `with_s`
148  // of the provided `lane_direction`. Both `lane` and `with_s` are specified
149  // in @p lane_direction. Throws if any element of @p lane_position is not
150  // within the respective bounds of `lane_direction.lane`.
151  static double CalcLaneProgress(
152  const LaneDirection& lane_direction,
153  const maliput::api::LanePosition& lane_position);
154 
155  // Constructs a LaneDirection structure based on a vehicle's current @p
156  // road_position and @p rotation (in global coordinates), and the @p side of
157  // the car (ahead or behind) that traffic is being observed. Note that
158  // `LaneDirection::with_s` is interpreted as the direction along which targets
159  // are being observed (regardless of the ego car's orientation): it is true if
160  // cars are being observed along the `s`-direction and is false otherwise.
161  static LaneDirection CalcLaneDirection(
162  const maliput::api::RoadPosition& road_position,
163  const Eigen::Quaternion<T>& rotation, AheadOrBehind side);
164 };
165 
166 } // namespace automotive
167 } // namespace drake
PoseSelector is a class that provides the relevant pose or poses with respect to a given ego vehicle ...
Definition: pose_selector.h:52
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
RoadOdometry< T > odometry
Definition: pose_selector.h:34
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 Lane represents a lane of travel in a road network.
Definition: lane.h:32
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
ClosestPose()=default
Default constructor.
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
A 3-dimensional position in a Lane-frame, consisting of three components:
Definition: lane_data.h:187
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:35
A position in the road network, consisting of a pointer to a specific Lane and a Lane-frame position ...
Definition: lane_data.h:258
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