motion::planning::object_collision_estimator Namespace Reference

Classes
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

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...
bool8_t	isTooFarAway (const TrajectoryPoint way_point, const BoundingBox obstacle_bbox, const float32_t distance_threshold)
	Determine if a obstacle is too far away from a way point given a distance threshold. More...
int32_t	detectCollision (const Trajectory &trajectory, const BoundingBoxArray &obstacles, 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

detectCollision()

int32_t motion::planning::object_collision_estimator::detectCollision	(	const Trajectory &	trajectory,
		const BoundingBoxArray &	obstacles,
		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

trajectory	Planned trajectory of ego vehicle.
obstacles	Array of bounding boxes of detected obstacles.
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

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

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

Returns

int32_t The index into the trajectory points where vehicle should stop.

isTooFarAway()

bool8_t motion::planning::object_collision_estimator::isTooFarAway	(	const TrajectoryPoint	way_point,
		const BoundingBox	obstacle_bbox,
		const float32_t	distance_threshold
	)

Determine if a obstacle is too far away from a way point given a distance threshold.

Parameters

way_point	a trajectory way point
obstacle_bbox	a bounding box with 4 corners representing the obstacle
distance_threshold	the minimum distance for a obstacle to be determined too far away

Returns

bool8_t Return true if the bounding box of the obstacle is at least distance_threshold away from the way point.

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

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.

Returns

BoundingBox The box bounding the ego vehicle at the waypoint.