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...
#include <object_collision_estimator.hpp>
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.
◆ ObjectCollisionEstimator()
Construct a new Object Collision Estimator object.
- Parameters
-
[in] | config | configuration struct for the estimator |
[in] | smoother | trajectory smoother to be used during modification of trajectory |
◆ getTrajectoryBoundingBox()
BoundingBoxArray motion::planning::object_collision_estimator::ObjectCollisionEstimator::getTrajectoryBoundingBox |
( |
| ) |
const |
|
inline |
Get the latest bounding box of target trajectory.
- Returns
- The latest bounding box of the target trajectory
◆ updatePlan()
int32_t motion::planning::object_collision_estimator::ObjectCollisionEstimator::updatePlan |
( |
const Trajectory & |
trajectory, |
|
|
Trajectory & |
trajectory_response |
|
) |
| |
|
noexcept |
Perform collision detection given an trajectory.
the list of obstacles should be passed to the estimator with a prior call to updateObstacles. When a collision is detected, the trajectory is modified in place such that the velocity of the ego vehicle becomes 0 before the collision point.
- Parameters
-
[in,out] | trajectory | The transformed trajectory of the ego vehicle. used for collision prediction. |
[in,out] | trajectory_response | The intended trajectory of the ego vehicle. If a collision is detected, this variable is modified in place. |
- Returns
- Collision index
◆ updatePredictedObjects()
void motion::planning::object_collision_estimator::ObjectCollisionEstimator::updatePredictedObjects |
( |
PredictedObjects |
predicted_objects | ) |
|
|
noexcept |
Update the list of obstacles.
The collision estimator stores a list of obstacle. Coordinates should be in the same frame as the the trajectories. When this function is called, the old list is replaced with the new list passed as the parameter. Modify bounding boxes with edges smaller than min_obstacle_dimension_m by computing new corners from their centroid and orientation values. The orientation and centroid of the modified bounding boxes are left unchanged. Assumptions:
- the points are in counterclockwise order
- box.size.x and box.size.y map to the length of the first and second edges respectively
- Parameters
-
[in] | predicted_objects | A array of predicted object representing a list of obstacles |
The documentation for this class was generated from the following files: