perf(bpp): reduce computational cost (#6054)

* pref(avoidance): don't use boost union_

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* perf(avoidance): reduce frequency to call calcSignedArcLength

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* perf(turn_signal): reduce frequency to call calcSignedArcLength

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* perf(static_drivable_area_expansion): don't use calcDistance2d

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(static_drivable_area_expansion): rename and fix return value consistency

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -588,7 +588,7 @@ struct ShiftLineData
  */
 struct DebugData
 {
-  geometry_msgs::msg::Polygon detection_area;
+  std::vector<geometry_msgs::msg::Polygon> detection_areas;
 
   lanelet::ConstLineStrings3d bounds;
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

@@ -112,13 +112,12 @@ class AvoidanceModule : public SceneModuleInterface
    */
   void updateRegisteredRTCStatus(const PathWithLaneId & path)
   {
-    const Point ego_position = planner_data_->self_odometry->pose.pose.position;
+    const auto ego_idx = planner_data_->findEgoIndex(path.points);
 
     for (const auto & left_shift : left_shift_array_) {
       const double start_distance =
-        calcSignedArcLength(path.points, ego_position, left_shift.start_pose.position);
-      const double finish_distance =
-        calcSignedArcLength(path.points, ego_position, left_shift.finish_pose.position);
+        calcSignedArcLength(path.points, ego_idx, left_shift.start_pose.position);
+      const double finish_distance = start_distance + left_shift.relative_longitudinal;
       rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
         left_shift.uuid, true, start_distance, finish_distance, clock_->now());
       if (finish_distance > -1.0e-03) {
@@ -130,9 +129,8 @@ class AvoidanceModule : public SceneModuleInterface
 
     for (const auto & right_shift : right_shift_array_) {
       const double start_distance =
-        calcSignedArcLength(path.points, ego_position, right_shift.start_pose.position);
-      const double finish_distance =
-        calcSignedArcLength(path.points, ego_position, right_shift.finish_pose.position);
+        calcSignedArcLength(path.points, ego_idx, right_shift.start_pose.position);
+      const double finish_distance = start_distance + right_shift.relative_longitudinal;
       rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
         right_shift.uuid, true, start_distance, finish_distance, clock_->now());
       if (finish_distance > -1.0e-03) {
@@ -399,6 +397,7 @@ class AvoidanceModule : public SceneModuleInterface
     UUID uuid;
     Pose start_pose;
     Pose finish_pose;
+    double relative_longitudinal{0.0};
   };
 
   using RegisteredShiftLineArray = std::vector<RegisteredShiftLine>;

planning/behavior_path_avoidance_module/src/debug.cpp

@@ -608,10 +608,15 @@ MarkerArray createDebugMarkerArray(
     addShiftGrad(debug.total_backward_grad, debug.total_shift, "grad_backward", 0.4, 0.2, 0.9);
   }
 
+  // detection area
+  size_t i = 0;
+  for (const auto & detection_area : debug.detection_areas) {
+    add(createPolygonMarkerArray(detection_area, "detection_area", i++, 0.16, 1.0, 0.69, 0.1));
+  }
+
   // misc
   {
     add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5));
-    add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1));
     add(createDrivableBounds(data, "drivable_bound", 1.0, 0.0, 0.42));
     add(laneletsAsTriangleMarkerArray(
       "drivable_lanes", transformToLanelets(data.drivable_lanes),

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -703,18 +703,6 @@ bool AvoidanceModule::isSafePath(
     return true;  // if safety check is disabled, it always return safe.
   }
 
-  const bool limit_to_max_velocity = false;
-  const auto ego_predicted_path_params =
-    std::make_shared<utils::path_safety_checker::EgoPredictedPathParams>(
-      parameters_->ego_predicted_path_params);
-  const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(shifted_path.path.points);
-  const auto ego_predicted_path_for_front_object = utils::path_safety_checker::createPredictedPath(
-    ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx,
-    true, limit_to_max_velocity);
-  const auto ego_predicted_path_for_rear_object = utils::path_safety_checker::createPredictedPath(
-    ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx,
-    false, limit_to_max_velocity);
-
   const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points);
   const auto is_right_shift = [&]() -> std::optional<bool> {
     for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) {
@@ -741,6 +729,22 @@ bool AvoidanceModule::isSafePath(
   const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects(
     avoid_data_, planner_data_, parameters_, is_right_shift.value(), debug);
 
+  if (safety_check_target_objects.empty()) {
+    return true;
+  }
+
+  const bool limit_to_max_velocity = false;
+  const auto ego_predicted_path_params =
+    std::make_shared<utils::path_safety_checker::EgoPredictedPathParams>(
+      parameters_->ego_predicted_path_params);
+  const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(shifted_path.path.points);
+  const auto ego_predicted_path_for_front_object = utils::path_safety_checker::createPredictedPath(
+    ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx,
+    true, limit_to_max_velocity);
+  const auto ego_predicted_path_for_rear_object = utils::path_safety_checker::createPredictedPath(
+    ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx,
+    false, limit_to_max_velocity);
+
   for (const auto & object : safety_check_target_objects) {
     auto current_debug_data = utils::path_safety_checker::createObjectDebug(object);
 
@@ -793,15 +797,16 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig
 
   const auto longest_dist_to_shift_point = [&]() {
     double max_dist = 0.0;
-    for (const auto & pnt : path_shifter_.getShiftLines()) {
-      max_dist = std::max(
-        max_dist, calcSignedArcLength(previous_path.points, pnt.start.position, getEgoPosition()));
+    auto lines = path_shifter_.getShiftLines();
+    if (lines.empty()) {
+      return max_dist;
     }
-    for (const auto & sp : generator_.getRawRegisteredShiftLine()) {
-      max_dist = std::max(
-        max_dist, calcSignedArcLength(previous_path.points, sp.start.position, getEgoPosition()));
-    }
-    return max_dist;
+    std::sort(lines.begin(), lines.end(), [](const auto & a, const auto & b) {
+      return a.start_idx < b.start_idx;
+    });
+    return std::max(
+      max_dist,
+      calcSignedArcLength(previous_path.points, lines.front().start.position, getEgoPosition()));
   }();
 
   const auto extra_margin = 10.0;  // Since distance does not consider arclength, but just line.
@@ -1029,11 +1034,14 @@ void AvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines)
   const auto sl = helper_->getMainShiftLine(shift_lines);
   const auto sl_front = shift_lines.front();
   const auto sl_back = shift_lines.back();
+  const auto relative_longitudinal = sl_back.end_longitudinal - sl_front.start_longitudinal;
 
   if (helper_->getRelativeShiftToPath(sl) > 0.0) {
-    left_shift_array_.push_back({uuid_map_.at("left"), sl_front.start, sl_back.end});
+    left_shift_array_.push_back(
+      {uuid_map_.at("left"), sl_front.start, sl_back.end, relative_longitudinal});
   } else if (helper_->getRelativeShiftToPath(sl) < 0.0) {
-    right_shift_array_.push_back({uuid_map_.at("right"), sl_front.start, sl_back.end});
+    right_shift_array_.push_back(
+      {uuid_map_.at("right"), sl_front.start, sl_back.end, relative_longitudinal});
   }
 
   uuid_map_.at("left") = generateUUID();
@@ -1150,15 +1158,19 @@ bool AvoidanceModule::isValidShiftLine(
     const size_t start_idx = shift_lines.front().start_idx;
     const size_t end_idx = shift_lines.back().end_idx;
 
+    const auto path = shifter_for_validate.getReferencePath();
+    const auto left_bound = lanelet::utils::to2D(avoid_data_.left_bound);
+    const auto right_bound = lanelet::utils::to2D(avoid_data_.right_bound);
     for (size_t i = start_idx; i <= end_idx; ++i) {
-      const auto p = getPoint(shifter_for_validate.getReferencePath().points.at(i));
+      const auto p = getPoint(path.points.at(i));
       lanelet::BasicPoint2d basic_point{p.x, p.y};
 
       const auto shift_length = proposed_shift_path.shift_length.at(i);
-      const auto bound = shift_length > 0.0 ? avoid_data_.left_bound : avoid_data_.right_bound;
       const auto THRESHOLD = minimum_distance + std::abs(shift_length);
 
-      if (boost::geometry::distance(basic_point, lanelet::utils::to2D(bound)) < THRESHOLD) {
+      if (
+        boost::geometry::distance(basic_point, (shift_length > 0.0 ? left_bound : right_bound)) <
+        THRESHOLD) {
         RCLCPP_DEBUG_THROTTLE(
           getLogger(), *clock_, 1000,
           "following latest new shift line may cause deviation from drivable area.");

planning/behavior_path_avoidance_module/src/utils.cpp

@@ -1892,11 +1892,12 @@ std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
     max_offset = std::max(max_offset, offset);
   }
 
+  const double MARGIN = is_running ? 1.0 : 0.0;  // [m]
   const auto detection_area =
-    createVehiclePolygon(planner_data->parameters.vehicle_info, max_offset);
+    createVehiclePolygon(planner_data->parameters.vehicle_info, max_offset + MARGIN);
   const auto ego_idx = planner_data->findEgoIndex(path.points);
 
-  Polygon2d attention_area;
+  std::vector<Polygon2d> detection_areas;
   for (size_t i = 0; i < path.points.size() - 1; ++i) {
     const auto & p_ego_front = path.points.at(i).point.pose;
     const auto & p_ego_back = path.points.at(i + 1).point.pose;
@@ -1906,41 +1907,27 @@ std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
       break;
     }
 
-    const auto ego_one_step_polygon = createOneStepPolygon(p_ego_front, p_ego_back, detection_area);
-
-    std::vector<Polygon2d> unions;
-    boost::geometry::union_(attention_area, ego_one_step_polygon, unions);
-    if (!unions.empty()) {
-      attention_area = unions.front();
-      boost::geometry::correct(attention_area);
-    }
+    detection_areas.push_back(createOneStepPolygon(p_ego_front, p_ego_back, detection_area));
   }
 
-  // expand detection area width only when the module is running.
-  if (is_running) {
-    constexpr int PER_CIRCLE = 36;
-    constexpr double MARGIN = 1.0;  // [m]
-    boost::geometry::strategy::buffer::distance_symmetric<double> distance_strategy(MARGIN);
-    boost::geometry::strategy::buffer::join_round join_strategy(PER_CIRCLE);
-    boost::geometry::strategy::buffer::end_round end_strategy(PER_CIRCLE);
-    boost::geometry::strategy::buffer::point_circle circle_strategy(PER_CIRCLE);
-    boost::geometry::strategy::buffer::side_straight side_strategy;
-    boost::geometry::model::multi_polygon<Polygon2d> result;
-    // Create the buffer of a multi polygon
-    boost::geometry::buffer(
-      attention_area, result, distance_strategy, side_strategy, join_strategy, end_strategy,
-      circle_strategy);
-    if (!result.empty()) {
-      attention_area = result.front();
+  std::for_each(detection_areas.begin(), detection_areas.end(), [&](const auto & detection_area) {
+    debug.detection_areas.push_back(toMsg(detection_area, data.reference_pose.position.z));
+  });
+
+  const auto within_detection_area = [&](const auto & obj_polygon) {
+    for (const auto & detection_area : detection_areas) {
+      if (!boost::geometry::disjoint(obj_polygon, detection_area)) {
+        return true;
+      }
     }
-  }
 
-  debug.detection_area = toMsg(attention_area, data.reference_pose.position.z);
+    return false;
+  };
 
   const auto objects = planner_data->dynamic_object->objects;
   std::for_each(objects.begin(), objects.end(), [&](const auto & object) {
     const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object);
-    if (boost::geometry::disjoint(obj_polygon, attention_area)) {
+    if (!within_detection_area(obj_polygon)) {
       other_objects.objects.push_back(object);
     } else {
       target_objects.objects.push_back(object);

planning/behavior_path_planner_common/src/turn_signal_decider.cpp

@@ -415,15 +415,7 @@ TurnSignalInfo TurnSignalDecider::use_prior_turn_signal(
 
   const double dist_to_original_desired_start =
     get_distance(original_desired_start_point) - base_link2front_;
-  const double dist_to_original_desired_end = get_distance(original_desired_end_point);
-  const double dist_to_original_required_start =
-    get_distance(original_required_start_point) - base_link2front_;
-  const double dist_to_original_required_end = get_distance(original_required_end_point);
   const double dist_to_new_desired_start = get_distance(new_desired_start_point) - base_link2front_;
-  const double dist_to_new_desired_end = get_distance(new_desired_end_point);
-  const double dist_to_new_required_start =
-    get_distance(new_required_start_point) - base_link2front_;
-  const double dist_to_new_required_end = get_distance(new_required_end_point);
 
   // If we still do not reach the desired front point we ignore it
   if (dist_to_original_desired_start > 0.0 && dist_to_new_desired_start > 0.0) {
@@ -435,6 +427,9 @@ TurnSignalInfo TurnSignalDecider::use_prior_turn_signal(
     return original_signal;
   }
 
+  const double dist_to_original_desired_end = get_distance(original_desired_end_point);
+  const double dist_to_new_desired_end = get_distance(new_desired_end_point);
+
   // If we already passed the desired end point, return the other signal
   if (dist_to_original_desired_end < 0.0 && dist_to_new_desired_end < 0.0) {
     TurnSignalInfo empty_signal_info;
@@ -445,6 +440,13 @@ TurnSignalInfo TurnSignalDecider::use_prior_turn_signal(
     return original_signal;
   }
 
+  const double dist_to_original_required_start =
+    get_distance(original_required_start_point) - base_link2front_;
+  const double dist_to_original_required_end = get_distance(original_required_end_point);
+  const double dist_to_new_required_start =
+    get_distance(new_required_start_point) - base_link2front_;
+  const double dist_to_new_required_end = get_distance(new_required_end_point);
+
   if (dist_to_original_desired_start <= dist_to_new_desired_start) {
     const auto enable_prior = use_prior_turn_signal(
       dist_to_original_required_start, dist_to_original_required_end, dist_to_new_required_start,

planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

@@ -208,26 +208,25 @@ std::optional<std::pair<size_t, geometry_msgs::msg::Point>> intersectBound(
   return std::nullopt;
 }
 
-double calcDistanceFromPointToSegment(
+double calcSquaredDistanceFromPointToSegment(
   const geometry_msgs::msg::Point & segment_start_point,
   const geometry_msgs::msg::Point & segment_end_point,
   const geometry_msgs::msg::Point & target_point)
 {
+  using tier4_autoware_utils::calcSquaredDistance2d;
+
   const auto & a = segment_start_point;
   const auto & b = segment_end_point;
   const auto & p = target_point;
 
   const double dot_val = (b.x - a.x) * (p.x - a.x) + (b.y - a.y) * (p.y - a.y);
-  const double squared_segment_length = tier4_autoware_utils::calcSquaredDistance2d(a, b);
+  const double squared_segment_length = calcSquaredDistance2d(a, b);
   if (0 <= dot_val && dot_val <= squared_segment_length) {
-    const double numerator = std::abs((p.x - a.x) * (a.y - b.y) - (p.y - a.y) * (a.x - b.x));
-    const double denominator = std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2));
-    return numerator / denominator;
+    return calcSquaredDistance2d(p, a) - dot_val * dot_val / squared_segment_length;
   }
 
   // target_point is outside the segment.
-  return std::min(
-    tier4_autoware_utils::calcDistance2d(a, p), tier4_autoware_utils::calcDistance2d(b, p));
+  return std::min(calcSquaredDistance2d(a, p), calcSquaredDistance2d(b, p));
 }
 
 PolygonPoint transformBoundFrenetCoordinate(
@@ -238,8 +237,8 @@ PolygonPoint transformBoundFrenetCoordinate(
   // find wrong nearest index.
   std::vector<double> dist_to_bound_segment_vec;
   for (size_t i = 0; i < bound_points.size() - 1; ++i) {
-    const double dist_to_bound_segment =
-      calcDistanceFromPointToSegment(bound_points.at(i), bound_points.at(i + 1), target_point);
+    const double dist_to_bound_segment = calcSquaredDistanceFromPointToSegment(
+      bound_points.at(i), bound_points.at(i + 1), target_point);
     dist_to_bound_segment_vec.push_back(dist_to_bound_segment);
   }