perf(bpp): reduce computational cost (autowarefoundation#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>
tier4 · Jan 15, 2024 · 87a45d6 · 87a45d6
1 parent 46c271e
commit 87a45d6
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 17 deletions.
diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/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,

diff --git a/...g/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/...g/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);
   }