Trim a single prediction trajectory, keep the portion that is not in junction.
68 {
69 if (!adc_trajectory_container.IsProtected()) {
70 ADEBUG <<
"Not in protection mode.";
71 return false;
72 }
73 if (obstacle == nullptr || obstacle->history_size() == 0) {
74 AERROR <<
"Invalid obstacle.";
75 return false;
76 }
77 int num_of_point = trajectory->trajectory_point_size();
78 if (num_of_point == 0) {
79 return false;
80 }
81 const Feature& feature = obstacle->latest_feature();
82 double vehicle_length = feature.length();
83 double vehicle_heading = feature.velocity_heading();
84 double forward_length =
85 std::fmax(vehicle_length / 2.0 - FLAGS_distance_beyond_junction, 0.0);
86
87 double front_x = trajectory->trajectory_point(0).path_point().x() +
88 forward_length * std::cos(vehicle_heading);
89 double front_y = trajectory->trajectory_point(0).path_point().y() +
90 forward_length * std::sin(vehicle_heading);
91 PathPoint front_point;
92 front_point.set_x(front_x);
93 front_point.set_y(front_y);
94 bool front_in_junction =
95 adc_trajectory_container.IsPointInJunction(front_point);
96
97 const PathPoint& start_point = trajectory->trajectory_point(0).path_point();
98 bool start_in_junction =
99 adc_trajectory_container.IsPointInJunction(start_point);
100
101 if (front_in_junction || start_in_junction) {
102 return false;
103 }
104
105 int index = 0;
106 while (index < num_of_point) {
107 const PathPoint& point = trajectory->trajectory_point(index).path_point();
108 if (adc_trajectory_container.IsPointInJunction(point)) {
109 break;
110 }
111 ++index;
112 }
113
114
115 if (index == num_of_point) {
116 return false;
117 }
118
119 for (int i = index; i < num_of_point; ++i) {
120 trajectory->mutable_trajectory_point()->RemoveLast();
121 }
122 return true;
123}