From f1965a4628d3978c20dab3be4d18c2e4aa531148 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Mon, 20 May 2024 16:21:22 +0900 Subject: [PATCH] feat(obstacle cruise): reduce computation cost (#7040) (#1306) * reduce computation cost Signed-off-by: Yuki Takagi --- .../obstacle_cruise_planner/polygon_utils.hpp | 3 +-- planning/obstacle_cruise_planner/src/node.cpp | 17 ++++++++++++++--- .../src/polygon_utils.cpp | 6 +++--- 3 files changed, 18 insertions(+), 8 deletions(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp index 1d490abca291a..71d0f84637abc 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp @@ -47,8 +47,7 @@ std::vector getCollisionPoints( const std::vector & traj_points, const std::vector & traj_polygons, const rclcpp::Time & obstacle_stamp, const PredictedPath & predicted_path, const Shape & shape, const rclcpp::Time & current_time, const bool is_driving_forward, - std::vector & collision_index, - const double max_lat_dist = std::numeric_limits::max(), + std::vector & collision_index, const double max_dist = std::numeric_limits::max(), const double max_prediction_time_for_collision_check = std::numeric_limits::max()); } // namespace polygon_utils diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 543653b552ef5..d84dc215987ad 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -1077,7 +1077,11 @@ ObstacleCruisePlannerNode::createCollisionPointsForInsideCruiseObstacle( std::vector collision_index; const auto collision_points = polygon_utils::getCollisionPoints( traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), - is_driving_forward_, collision_index); + is_driving_forward_, collision_index, + calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + + std::hypot( + vehicle_info_.vehicle_length_m, + vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_cruise)); return collision_points; } @@ -1117,7 +1121,10 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle( const auto collision_points = polygon_utils::getCollisionPoints( traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), is_driving_forward_, collision_index, - vehicle_info_.vehicle_width_m + p.max_lat_margin_for_cruise, + calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + + std::hypot( + vehicle_info_.vehicle_length_m, + vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_cruise), p.max_prediction_time_for_collision_check); if (collision_points.empty()) { // Ignore vehicle obstacles outside the trajectory without collision @@ -1193,7 +1200,11 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( std::vector collision_index; const auto collision_points = polygon_utils::getCollisionPoints( traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), - is_driving_forward_, collision_index); + is_driving_forward_, collision_index, + calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + + std::hypot( + vehicle_info_.vehicle_length_m, + vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_stop)); if (collision_points.empty()) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, diff --git a/planning/obstacle_cruise_planner/src/polygon_utils.cpp b/planning/obstacle_cruise_planner/src/polygon_utils.cpp index fa5a96b934f7a..1bd2de0bd985c 100644 --- a/planning/obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/obstacle_cruise_planner/src/polygon_utils.cpp @@ -44,18 +44,18 @@ PointWithStamp calcNearestCollisionPoint( return collision_points.at(min_idx); } -// NOTE: max_lat_dist is used for efficient calculation to suppress boost::geometry's polygon +// NOTE: max_dist is used for efficient calculation to suppress boost::geometry's polygon // calculation. std::optional>> getCollisionIndex( const std::vector & traj_points, const std::vector & traj_polygons, const geometry_msgs::msg::Pose & object_pose, const rclcpp::Time & object_time, - const Shape & object_shape, const double max_lat_dist = std::numeric_limits::max()) + const Shape & object_shape, const double max_dist = std::numeric_limits::max()) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object_pose, object_shape); for (size_t i = 0; i < traj_polygons.size(); ++i) { const double approximated_dist = tier4_autoware_utils::calcDistance2d(traj_points.at(i).pose, object_pose); - if (approximated_dist > max_lat_dist) { + if (approximated_dist > max_dist) { continue; }