- Generated by
|
delphyne
|
TrafficPoseSelector is a class that provides the relevant pose or poses with respect to a given ego vehicle driving within a given maliput road geometry.
Instantiated templates for the following kinds of T's are provided:
They are already available to link against in the containing library.
TODO(jadecastro): Enable AutoDiffXd support, and add unit tests.
#include <src/systems/traffic_pose_selector.h>
Public Member Functions | |
| TrafficPoseSelector ()=delete | |
Static Public Member Functions | |
| static std::map< AheadOrBehind, const ClosestPose< T > > | FindClosestPair (const maliput::api::Lane *lane, const drake::systems::rendering::PoseVector< T > &ego_pose, const drake::systems::rendering::PoseBundle< T > &traffic_poses, const T &scan_distance, ScanStrategy path_or_branches) |
Returns the leading and trailing vehicles in a given lane or road that are closest to an ego vehicle along its path, as measured along the s-coordinate of the ego vehicle's lane. More... | |
| static ClosestPose< T > | FindSingleClosestPose (const maliput::api::Lane *lane, const drake::systems::rendering::PoseVector< T > &ego_pose, const drake::systems::rendering::PoseBundle< T > &traffic_poses, const T &scan_distance, const AheadOrBehind side, ScanStrategy path_or_branches) |
| Same as TrafficPoseSelector::FindClosestPair() except that it returns a single ClosestPose for either the vehicle ahead (AheadOrBehind::kAhead) or behind (AheadOrBehind::kBehind). More... | |
| static T | GetSigmaVelocity (const RoadOdometry< T > &road_odometry) |
Extracts the vehicle's s-direction velocity based on its RoadOdometry road_odometry in the Lane coordinate frame. More... | |
|
delete |
|
static |
Returns the leading and trailing vehicles in a given lane or road that are closest to an ego vehicle along its path, as measured along the s-coordinate of the ego vehicle's lane.
If path_or_branches is ScanStrategy::kPath, only poses in the same path as the ego (ahead or behind) are tracked; if ScanStrategy::kBranches, poses in lanes that eventually merge (converge to the same branch point that is on the default path of lane) are considered in addition to those along the ego car's default path.
Users should take heed to the fact that ScanStrategy::kBranches does not assess the relationship between traffic and ego vehicle velocity when selecting poses. Thus, cars within the same lane as the ego but with negative net-velocity (approaching the ego car, from the ego's point-of-view) could be ignored in favor of a car in a branch with positive net-velocity.
The ego vehicle must be within the segment_bounds of lane (i.e. the road is contiguous with lane along the r-direction). This function is used, for instance, as logic for lane-change planners (e.g. MOBIL). The ego car's pose (ego_pose) and the poses of the traffic cars (traffic_poses) are provided. The parameter scan_distance determines the distance along the sequence of lanes to scan before declaring that no traffic car is ahead (resp. behind) the ego car.
s-value of positive (resp. negative) infinity. Any traffic poses that are redunant with ego_pose (i.e. have the same RoadPosition as the ego car and thus the same s and r value) are discarded. If no leading/trailing vehicles are seen within scan-distance of the ego car, s-positions are taken to be at infinite distances away from the ego car. Note also that traffic vehicles having exactly the same s-position as the ego vehicle but with different r-value or are directly perpendicular in a Lane::to_left(), Lane::to_right() lane are taken to be behind the ego vehicle. Note that this implementation is greatly simplified by considering poses as points (i.e. vehicle geometries are ignored).The RoadGeometry from which lane is drawn is required to have default branches set for all branches in the road network.
|
static |
Same as TrafficPoseSelector::FindClosestPair() except that it returns a single ClosestPose for either the vehicle ahead (AheadOrBehind::kAhead) or behind (AheadOrBehind::kBehind).
Cars in other lanes are only tracked if they are in confluent lanes to a given branch point within the scan_distance; i.e. cars in two side-by-side lanes that never enter a branch point will not be selected. This assumes that the ego car has knowledge of the traffic cars' default lane.
Note that when no car is detected in front of the ego car, the returned RoadOdometry within ClosestPose will contain an s-value of std::numeric_limits<double>::infinity().
|
static |
Extracts the vehicle's s-direction velocity based on its RoadOdometry road_odometry in the Lane coordinate frame.
Assumes the road has zero elevation and superelevation. Throws if any element of road_odometry.pos is not within the respective bounds of road_odometry.lane.
N.B. This function currently only provides exact derivatives for velocity in the s direction when the road is straight (no yaw angle variations).