feat(obstacle cruise): reduce computation cost (autowarefoundation#7040…

…) (#1306) * reduce computation cost Signed-off-by: Yuki Takagi <[email protected]>
tier4 · May 20, 2024 · f1965a4 · f1965a4
1 parent 9bae39d
commit f1965a4
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 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
@@ -47,8 +47,7 @@ std::vector<PointWithStamp> getCollisionPoints(
   const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & 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<size_t> & collision_index,
-  const double max_lat_dist = std::numeric_limits<double>::max(),
+  std::vector<size_t> & collision_index, const double max_dist = std::numeric_limits<double>::max(),
   const double max_prediction_time_for_collision_check = std::numeric_limits<double>::max());
 
 }  // namespace polygon_utils

diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp
@@ -1077,7 +1077,11 @@ ObstacleCruisePlannerNode::createCollisionPointsForInsideCruiseObstacle(
   std::vector<size_t> 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<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
     std::vector<size_t> 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
@@ -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<std::pair<size_t, std::vector<PointWithStamp>>> getCollisionIndex(
   const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & 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<double>::max())
+  const Shape & object_shape, const double max_dist = std::numeric_limits<double>::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;
     }