Fix the absence of the objects' bound in prediction.

Hello, the maintainers of OpenPlanner.

As discussed in https://github.com/hatem-darweesh/autoware.ai.openplanner/issues/7, there has been a problem that the op_trajectory_evaluator would not consider the bounding-box along the predicted trajectories of objects. Hence, this PR aims at solving this problem without hurting performance.

Root Cause of the Problem

While calculating the distance cost of rollouts, the trajectory evaluator will consider the predicted trajectories of detected objects through increasing the lateral cost of rollouts those are close to a waypoint of predicted trajectories, as shown in the following code snippet (from op_planner):

    if(actual_longitudinal_distance > params.pathDensity && actual_longitudinal_distance < params.minFollowingDistance && actual_lateral_distance < g_lateral_skip_value && !info.bAfter && !info.bBefore && t_diff < eval_params_.collision_time_)
    {
        trajectory_costs.at(i).longitudinal_cost  += 1.0/actual_longitudinal_distance;

        //std::cout << info.bAfter << ", " << info.bBefore << ", " << actual_lateral_distance << ", " << actual_longitudinal_distance <<", " << t_diff <<", " << a_diff <<" ," << traj_prob <<std::endl;

        if(actual_lateral_distance < c_lateral_d  && t_diff < eval_params_.collision_time_) // collision point
        {
            trajectory_costs.at(i).lateral_cost += 2.0; // use half meter fixed critical distance as contact cost for all collision points in the range
            collision_points.push_back(info.perp_point);
            if(actual_longitudinal_distance < params.minFollowingDistance)
            {
                trajectory_costs.at(i).bBlocked = true;
            }

            if(trajectory_costs.at(i).closest_obj_distance > actual_longitudinal_distance)
            {
                trajectory_costs.at(i).closest_obj_distance = actual_longitudinal_distance;
                trajectory_costs.at(i).closest_obj_velocity = trajectory_points.at(j).v;
            }
        }
        else
        {
            trajectory_costs.at(i).lateral_cost += 1.0/actual_lateral_distance;
        }
    }

However, in the comparing condition actual_lateral_distance < c_lateral_d, c_lateral_d is defined as:

double critical_lateral_distance = car_info.width / 2.0 + params.horizontalSafetyDistancel

which only considers the width of the ego, but ignores the bound of the detected objects. Therefore, if the object is large (e.g., a truck), critical_lateral_distance will be too small, resulting in collisions (see video-1). Hence, the bound of the object should also be considered into the calculation of critical_lateral_distance.

However, as pointed out by @hatem-darweesh, if bounding boxes of all predicted trajectory waypoints are calculated, the performance will be hurt a lot (about 20% more CPU clocks, measured by perf). To avoid this, I use the circumcircle of the bounding box instead. The calculation of a circumcircle is easier since it does not need to calculate the orientation while the bounding box requires it.

The final solution is easy and does not increase the code size (see File changed of the PR) as well as the performance overhead (see Performance Evaluation). And it can actually be safer, demonstrated by video-1 and video-2.

Video demonstrations of the solution

The videos are larger than 10MB, so I put them on youtube. video-1: https://youtu.be/LYpQKPATKYk. A collision caused by ignoring the bound of objects. video-2: https://youtu.be/FzZDTIinmxo. After patching the problem, the collision would not happen, and the ego will intendedly try to overtake the truck by changing the driving lane.

Performance Evaluation

I use perf(https://perf.wiki.kernel.org/index.php/Main_Page) to measure the performance. The performance of before-patch and after-patch are both measured on a driving scenario with 14 NPCs, using the LGSVL simulator. As the following figures show, the performance does not increase from the aspect of CPU clocks. Before-patch takes 43.05% CPU clocks and After-patch takes 32.39% CPU clocks. After-patch has lower CPU clocks percentage which I think is because the scenario last longer since the collision did not happen.

Before patch:

After patch:

hatem-darweesh / common