diff --git a/modules/prediction/container/obstacles/obstacle.cc b/modules/prediction/container/obstacles/obstacle.cc index e0e409575769026113584cfbae1875f1528edb28..5d9c5e24ca86b57e798de1b90d52d317f1c93e37 100644 --- a/modules/prediction/container/obstacles/obstacle.cc +++ b/modules/prediction/container/obstacles/obstacle.cc @@ -873,6 +873,7 @@ void Obstacle::SetLaneGraphFeature(Feature* feature) { if (feature->has_lane() && feature->lane().has_lane_graph()) { SetLanePoints(feature); + SetLaneSequencePath(feature->mutable_lane()->mutable_lane_graph()); } ADEBUG << "Obstacle [" << id_ << "] set lane graph features."; @@ -960,6 +961,19 @@ void Obstacle::SetLaneSequencePath(LaneGraph* const lane_graph) { } lane_segment_s += lane_segment->total_length(); } + int num_path_point = lane_sequence->path_point_size(); + if (num_path_point <= 0) { + continue; + } + for (int j = 0; j + 1 < num_path_point; ++j) { + PathPoint* first_point = lane_sequence->mutable_path_point(j); + PathPoint* second_point = lane_sequence->mutable_path_point(j + 1); + double delta_theta = second_point->theta() - first_point->theta(); + double delta_s = second_point->s() - first_point->s(); + double kappa = std::abs(delta_theta / (delta_s + FLAGS_double_precision)); + lane_sequence->mutable_path_point(j)->set_kappa(kappa); + } + lane_sequence->mutable_path_point(num_path_point - 1)->set_kappa(0.0); } }