fix(out_of_lane): lateral distance calculation for predicted path (au…

…towarefoundation#5145) * fix(out_of_lane): lateral distance calculation for predicted path Signed-off-by: Takamasa Horibe <[email protected]> * remove overlap points in the object_time_to_range function Signed-off-by: Maxime CLEMENT <[email protected]> --------- Signed-off-by: Takamasa Horibe <[email protected]> Signed-off-by: Maxime CLEMENT <[email protected]> Co-authored-by: Maxime CLEMENT <[email protected]>
tier4 · Sep 29, 2023 · 410bfe8 · 410bfe8
1 parent af72ece
commit 410bfe8
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 10 deletions.
diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp
@@ -70,31 +70,32 @@ std::optional<std::pair<double, double>> object_time_to_range(
   auto worst_exit_time = std::optional<double>();
 
   for (const auto & predicted_path : object.kinematics.predicted_paths) {
+    const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path);
     const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds();
     const auto enter_point =
       geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y());
     const auto enter_segment_idx =
-      motion_utils::findNearestSegmentIndex(predicted_path.path, enter_point);
+      motion_utils::findNearestSegmentIndex(unique_path_points, enter_point);
     const auto enter_offset = motion_utils::calcLongitudinalOffsetToSegment(
-      predicted_path.path, enter_segment_idx, enter_point);
-    const auto enter_lat_dist = std::abs(
-      motion_utils::calcLateralOffset(predicted_path.path, enter_point, enter_segment_idx));
+      unique_path_points, enter_segment_idx, enter_point);
+    const auto enter_lat_dist =
+      std::abs(motion_utils::calcLateralOffset(unique_path_points, enter_point, enter_segment_idx));
     const auto enter_segment_length = tier4_autoware_utils::calcDistance2d(
-      predicted_path.path[enter_segment_idx], predicted_path.path[enter_segment_idx + 1]);
+      unique_path_points[enter_segment_idx], unique_path_points[enter_segment_idx + 1]);
     const auto enter_offset_ratio = enter_offset / enter_segment_length;
     const auto enter_time =
       static_cast<double>(enter_segment_idx) * time_step + enter_offset_ratio * time_step;
 
     const auto exit_point =
       geometry_msgs::msg::Point().set__x(range.exiting_point.x()).set__y(range.exiting_point.y());
     const auto exit_segment_idx =
-      motion_utils::findNearestSegmentIndex(predicted_path.path, exit_point);
+      motion_utils::findNearestSegmentIndex(unique_path_points, exit_point);
     const auto exit_offset = motion_utils::calcLongitudinalOffsetToSegment(
-      predicted_path.path, exit_segment_idx, exit_point);
+      unique_path_points, exit_segment_idx, exit_point);
     const auto exit_lat_dist =
-      std::abs(motion_utils::calcLateralOffset(predicted_path.path, exit_point, exit_segment_idx));
+      std::abs(motion_utils::calcLateralOffset(unique_path_points, exit_point, exit_segment_idx));
     const auto exit_segment_length = tier4_autoware_utils::calcDistance2d(
-      predicted_path.path[exit_segment_idx], predicted_path.path[exit_segment_idx + 1]);
+      unique_path_points[exit_segment_idx], unique_path_points[exit_segment_idx + 1]);
     const auto exit_offset_ratio = exit_offset / static_cast<double>(exit_segment_length);
     const auto exit_time =
       static_cast<double>(exit_segment_idx) * time_step + exit_offset_ratio * time_step;

diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp
@@ -21,6 +21,9 @@
 
 namespace behavior_velocity_planner::out_of_lane
 {
+using motion_utils::calcLateralOffset;
+using tier4_autoware_utils::calcDistance2d;
+
 /// @brief filter predicted objects and their predicted paths
 /// @param [in] objects predicted objects to filter
 /// @param [in] ego_data ego data
@@ -42,8 +45,13 @@ autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects(
     auto filtered_object = object;
     const auto is_invalid_predicted_path = [&](const auto & predicted_path) {
       const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence;
+      const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path);
+
+      // Note: lateral offset can not be calculated for path with less than 2 unique points
       const auto lat_offset_to_current_ego =
-        std::abs(motion_utils::calcLateralOffset(predicted_path.path, ego_data.pose.position));
+        unique_path_points.size() > 1
+          ? std::abs(calcLateralOffset(unique_path_points, ego_data.pose.position))
+          : calcDistance2d(unique_path_points.front(), ego_data.pose.position);
       const auto is_crossing_ego =
         lat_offset_to_current_ego <=
         object.shape.dimensions.y / 2.0 + std::max(