Autoware.Auto
motion::planning::object_collision_estimator Namespace Reference

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...
 

Function Documentation

◆ calculateAxisAlignedBoundingBox()

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.

Parameters
bboxA bounding box to convert
Returns
AxisAlignedBoundingBox The axis aligned bounding box

◆ detectCollision()

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.

Parameters
trajectoryPlanned trajectory of ego vehicle.
predicted_objectsArray of predicted object.
vehicle_paramConfiguration regarding the dimensions of the ego vehicle
safety_factorA factor to inflate the size of the vehicle so to avoid getting too close to obstacles.
waypoint_bboxesA list of bounding boxes around each waypoint in the trajectory
Returns
int32_t The index into the trajectory points where the first collision happens. If no collision is detected, -1 is returned.

◆ getStopIndex()

int32_t motion::planning::object_collision_estimator::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.

Parameters
trajectoryPlanned trajectory of ego vehicle.
collision_indexIndex of trajectory point that collides with and obstacle
stop_marginDistance between the control point of vehicle (CoG or base_link) and obstacle
Returns
int32_t The index into the trajectory points where vehicle should stop.

◆ isTooFarAway()

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.

Parameters
vehicle_bboxa bounding box representing the vehicle
obstacle_bboxa bounding box representing the obstacle
Returns
bool8_t Return true if an obstacle is too far away from a vehicle to collide.

◆ waypointToBox()

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.

Parameters
ptA waypoint of a trajectory
vehicle_paramEgo vehicle parameter defining its dimensions
safety_factorA factor to inflate the size of the vehicle so to avoid getting too close to obstacles.
Returns
BoundingBox The box bounding the ego vehicle at the waypoint.