Autoware.Auto
|
|
Classes | |
struct | AxisAlignedBoundingBox |
class | ObjectCollisionEstimator |
Given a trajectory and a list of obstacles, detect possible collision points between the ego vehicle and obstacle along the trajectory. Modify the trajectory so that the vehicle stops before the collision. More... | |
struct | ObjectCollisionEstimatorConfig |
struct | PredictedObjectInfo |
Functions | |
BoundingBox | waypointToBox (const TrajectoryPoint &pt, const VehicleConfig &vehicle_param, const float32_t safety_factor) |
Convert a trajectory waypoint into a bounding box representing the volume occupied by the ego vehicle while on this waypoint. More... | |
AxisAlignedBoundingBox | calculateAxisAlignedBoundingBox (const BoundingBox &bbox) |
Converts a bounding box into axis aligned bounding box that covers given box. The goal is to use axis aligned bounding box for optimization as a first step in collision detection. More... | |
bool8_t | isTooFarAway (const AxisAlignedBoundingBox &vehicle_bbox, const AxisAlignedBoundingBox &obstacle_bbox) |
Determine if an obstacle is too far away from a vehicle to collide. More... | |
int32_t | detectCollision (const Trajectory &trajectory, const std::vector< PredictedObjectInfo > &predicted_objects, const VehicleConfig &vehicle_param, const float32_t safety_factor, BoundingBoxArray &waypoint_bboxes) |
Detect possible collision between a trajectory and a list of obstacle bounding boxes. Return the index in the trajectory where the first collision happens. More... | |
int32_t | getStopIndex (const Trajectory &trajectory, const int32_t collision_index, const float32_t stop_margin) noexcept |
Returns the index that vehicle should stop when the object colliding index and stop distance is given. More... | |
AxisAlignedBoundingBox motion::planning::object_collision_estimator::calculateAxisAlignedBoundingBox | ( | const BoundingBox & | bbox | ) |
Converts a bounding box into axis aligned bounding box that covers given box. The goal is to use axis aligned bounding box for optimization as a first step in collision detection.
bbox | A bounding box to convert |
int32_t motion::planning::object_collision_estimator::detectCollision | ( | const Trajectory & | trajectory, |
const std::vector< PredictedObjectInfo > & | predicted_objects, | ||
const VehicleConfig & | vehicle_param, | ||
const float32_t | safety_factor, | ||
BoundingBoxArray & | waypoint_bboxes | ||
) |
Detect possible collision between a trajectory and a list of obstacle bounding boxes. Return the index in the trajectory where the first collision happens.
trajectory | Planned trajectory of ego vehicle. |
predicted_objects | Array of predicted object. |
vehicle_param | Configuration regarding the dimensions of the ego vehicle |
safety_factor | A factor to inflate the size of the vehicle so to avoid getting too close to obstacles. |
waypoint_bboxes | A list of bounding boxes around each waypoint in the trajectory |
|
noexcept |
Returns the index that vehicle should stop when the object colliding index and stop distance is given.
trajectory | Planned trajectory of ego vehicle. |
collision_index | Index of trajectory point that collides with and obstacle |
stop_margin | Distance between the control point of vehicle (CoG or base_link) and obstacle |
bool8_t motion::planning::object_collision_estimator::isTooFarAway | ( | const AxisAlignedBoundingBox & | vehicle_bbox, |
const AxisAlignedBoundingBox & | obstacle_bbox | ||
) |
Determine if an obstacle is too far away from a vehicle to collide.
vehicle_bbox | a bounding box representing the vehicle |
obstacle_bbox | a bounding box representing the obstacle |
BoundingBox motion::planning::object_collision_estimator::waypointToBox | ( | const TrajectoryPoint & | pt, |
const VehicleConfig & | vehicle_param, | ||
const float32_t | safety_factor | ||
) |
Convert a trajectory waypoint into a bounding box representing the volume occupied by the ego vehicle while on this waypoint.
pt | A waypoint of a trajectory |
vehicle_param | Ego vehicle parameter defining its dimensions |
safety_factor | A factor to inflate the size of the vehicle so to avoid getting too close to obstacles. |