A collision caused by a vulnerable function in TrajectoryEvaluator

Hi Hatem, when I run the Autoware with openplanner (1.15), I found a collision caused by a vulnerable function in TrajectoryEvaluator. Here are the details of the collision and the corresponding root cause.

Code version：LGSVL+openplanner.1.15
Description about the collision：
- When going through the intersection, an ego collision occurs with the moving truck.
- [Collision video](https://drive.google.com/file/d/1WRU94vtrLG8dnb_c47Y7SCUwrydy8cZP/view?usp=sharing)
Collision analysis：
- Location of vulnerable code: https://github.com/hatem-darweesh/common.git, commit-id:378e2eab6665b3acfd1ac35b547d44372f633f14
  - common/op_planner/src/TrajectoryEvaluator.cpp
  - TrajectoryEvaluator::calculateDistanceCosts
```
for(unsigned int j = 0; j < trajectory_points.size(); j++) //for predictive collision estimation, using the estimated trajectories for other moving objects
        {
            ...
            PlanningHelpers::GetRelativeInfoLimited(roll_outs.at(i), trajectory_points.at(j), info, prev_index);

            double actual_lateral_distance = fabs(info.perp_distance) - 0.05; //add small distance so this never become zero
            double actual_longitudinal_distance = info.from_back_distance + roll_outs.at(i).at(info.iBack).distanceCost - 0.05; //add small distance so this
        }
```
- Here, the function TrajectoryEvaluator::calculateDistanceCosts processes each waypoint of the obtained trajectories in order and calls PlanningHelpers::GetRelativeInfoLimited to predict whether any waypoint would become the position of collision between ego and npc vehicle. However, after dynamic analysis, we found that the waypoints on the obtained trajectories only describe the position of the center point of the obstacle (the size of the obstacle, or in other words, the bounding box of the obstacle is not considered). Therefore, when dealing with large objects like trucks, the planner cannot timely trigger an emergency stop. We also did some simple tests to prove it, for instance, if replace the truck with a small-sized vehicle, the ego collision will not happen (https://drive.google.com/file/d/1syhFXJQSoCaM-007AYsljBNODb0EXV7s/view?usp=sharing).
Mitigation
- When processing the predicted trajectory of obstacles, the bounding box of obstacle at the each trajectory point should be calculated according to the corresponding position and orientation, so as to accurately predict the collision points.

hatem-darweesh / common

A collision caused by a vulnerable function in TrajectoryEvaluator #16