From e1a9339ed89dbd5d1e484922212bdc7a324177e7 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Fri, 11 Nov 2022 10:38:18 +0900 Subject: [PATCH] perf(crosswalk): reduce processing time (#2137) (#175) * perf(crosswalk): use sparse resampled path Signed-off-by: satoshi-ota * perf(crosswalk): create attention area all at once Signed-off-by: satoshi-ota Signed-off-by: satoshi-ota Signed-off-by: tomoya.kimura Signed-off-by: satoshi-ota Signed-off-by: tomoya.kimura Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../crosswalk/scene_crosswalk.hpp | 2 + .../crosswalk/scene_crosswalk.cpp | 91 +++++++++++-------- 2 files changed, 54 insertions(+), 39 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp index 0e2c50c55babc..e17396713949d 100644 --- a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp @@ -117,6 +117,8 @@ class CrosswalkModule : public SceneModuleInterface std::vector getCollisionPoints( const PathWithLaneId & ego_path, const PredictedObject & object, + const boost::geometry::model::polygon> & + attention_area, const std::pair & crosswalk_attention_range); std::pair getAttentionRange(const PathWithLaneId & ego_path); diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp index 9036224c63326..44af5d32c4350 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include @@ -464,13 +465,19 @@ boost::optional> CrosswalkModule::findNea bool found_pedestrians = false; bool found_stuck_vehicle = false; - const auto crosswalk_attention_range = getAttentionRange(ego_path); + PathWithLaneId sparse_resample_path{}; + constexpr double RESAMPLE_INTERVAL = 4.0; + if (!splineInterpolate(ego_path, RESAMPLE_INTERVAL, sparse_resample_path, logger_)) { + return {}; + } + + const auto crosswalk_attention_range = getAttentionRange(sparse_resample_path); const auto & ego_pos = planner_data_->current_pose.pose.position; const auto & objects_ptr = planner_data_->predicted_objects; const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; bool exist_stopline_in_map; - const auto p_stop_line = getStopLine(ego_path, exist_stopline_in_map); + const auto p_stop_line = getStopLine(sparse_resample_path, exist_stopline_in_map); if (!p_stop_line) { return {}; } @@ -482,13 +489,44 @@ boost::optional> CrosswalkModule::findNea const auto ignore_crosswalk = debug_data_.ignore_crosswalk = isRedSignalForPedestrians(); + const auto ego_polygon = createVehiclePolygon(planner_data_->vehicle_info_); + + Polygon attention_area; + for (size_t j = 0; j < sparse_resample_path.points.size() - 1; ++j) { + const auto & p_ego_front = sparse_resample_path.points.at(j).point.pose; + const auto & p_ego_back = sparse_resample_path.points.at(j + 1).point.pose; + const auto front_length = + calcSignedArcLength(sparse_resample_path.points, ego_pos, p_ego_front.position); + const auto back_length = + calcSignedArcLength(sparse_resample_path.points, ego_pos, p_ego_back.position); + + if (back_length < crosswalk_attention_range.first) { + continue; + } + + if (crosswalk_attention_range.second < front_length) { + break; + } + + const auto ego_one_step_polygon = createOneStepPolygon(p_ego_front, p_ego_back, ego_polygon); + + debug_data_.ego_polygons.push_back(toMsg(ego_one_step_polygon, ego_pos.z)); + + std::vector unions; + bg::union_(attention_area, ego_one_step_polygon, unions); + if (!unions.empty()) { + attention_area = unions.front(); + bg::correct(attention_area); + } + } + for (const auto & object : objects_ptr->objects) { const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear; const auto obj_uuid = toHexString(object.object_id); if (isVehicle(object)) { - found_stuck_vehicle = found_stuck_vehicle || isStuckVehicle(ego_path, object); + found_stuck_vehicle = found_stuck_vehicle || isStuckVehicle(sparse_resample_path, object); } if (!isTargetType(object)) { @@ -499,7 +537,8 @@ boost::optional> CrosswalkModule::findNea continue; } - for (auto & cp : getCollisionPoints(ego_path, object, crosswalk_attention_range)) { + for (auto & cp : getCollisionPoints( + sparse_resample_path, object, attention_area, crosswalk_attention_range)) { const auto is_ignore_object = ignore_objects_.count(obj_uuid) != 0; if (is_ignore_object) { cp.state = CollisionPointState::IGNORE; @@ -521,8 +560,9 @@ boost::optional> CrosswalkModule::findNea found_pedestrians = true; stop_factor.stop_factor_points.push_back(obj_pos); - const auto dist_ego2cp = calcSignedArcLength(ego_path.points, ego_pos, cp.collision_point) - - planner_param_.stop_margin; + const auto dist_ego2cp = + calcSignedArcLength(sparse_resample_path.points, ego_pos, cp.collision_point) - + planner_param_.stop_margin; if (dist_ego2cp < minimum_stop_dist) { first_stop_point = cp.collision_point; @@ -577,12 +617,13 @@ boost::optional> CrosswalkModule::findNea const auto & p_stop = stop_at_stop_line ? p_stop_line.get().second : first_stop_point; const auto stop_line_distance = exist_stopline_in_map ? 0.0 : planner_param_.stop_line_distance; const auto margin = stop_at_stop_line ? stop_line_distance : planner_param_.stop_margin; - - const size_t base_idx = findNearestSegmentIndex(ego_path.points, p_stop); - const auto residual_length = calcLongitudinalOffsetToSegment(ego_path.points, base_idx, p_stop); + const size_t base_idx = findNearestSegmentIndex(sparse_resample_path.points, p_stop); + const auto residual_length = + calcLongitudinalOffsetToSegment(sparse_resample_path.points, base_idx, p_stop); const auto update_margin = margin - residual_length + base_link2front; - const auto stop_point = getBackwardInsertPointFromBasePoint(base_idx, ego_path, update_margin); + const auto stop_point = + getBackwardInsertPointFromBasePoint(base_idx, sparse_resample_path, update_margin); stop_factor.stop_pose = stop_point.get().second.point.pose; return stop_point; @@ -802,7 +843,7 @@ void CrosswalkModule::clampAttentionRangeByNeighborCrosswalks( } std::vector CrosswalkModule::getCollisionPoints( - const PathWithLaneId & ego_path, const PredictedObject & object, + const PathWithLaneId & ego_path, const PredictedObject & object, const Polygon & attention_area, const std::pair & crosswalk_attention_range) { stop_watch_.tic(__func__); @@ -814,37 +855,9 @@ std::vector CrosswalkModule::getCollisionPoints( const auto & ego_pos = planner_data_->current_pose.pose.position; const auto & ego_vel = planner_data_->current_velocity->twist.linear; - const auto ego_polygon = createVehiclePolygon(planner_data_->vehicle_info_); const auto obj_polygon = createObjectPolygon(object.shape.dimensions.x, object.shape.dimensions.y); - Polygon attention_area; - for (size_t j = 0; j < ego_path.points.size() - 1; ++j) { - const auto & p_ego_front = ego_path.points.at(j).point.pose; - const auto & p_ego_back = ego_path.points.at(j + 1).point.pose; - const auto front_length = calcSignedArcLength(ego_path.points, ego_pos, p_ego_front.position); - const auto back_length = calcSignedArcLength(ego_path.points, ego_pos, p_ego_back.position); - - if (back_length < crosswalk_attention_range.first) { - continue; - } - - if (crosswalk_attention_range.second < front_length) { - break; - } - - const auto ego_one_step_polygon = createOneStepPolygon(p_ego_front, p_ego_back, ego_polygon); - - debug_data_.ego_polygons.push_back(toMsg(ego_one_step_polygon, ego_pos.z)); - - std::vector unions; - bg::union_(attention_area, ego_one_step_polygon, unions); - if (!unions.empty()) { - attention_area = unions.front(); - bg::correct(attention_area); - } - } - for (const auto & obj_path : object.kinematics.predicted_paths) { for (size_t i = 0; i < obj_path.path.size() - 1; ++i) { const auto p_obj_front = obj_path.path.at(i);