From 410bfe8ac1872cb92a4e496f8d9d4ca6b74d3096 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 27 Sep 2023 17:45:29 +0900 Subject: [PATCH] fix(out_of_lane): lateral distance calculation for predicted path (#5145) * fix(out_of_lane): lateral distance calculation for predicted path Signed-off-by: Takamasa Horibe * remove overlap points in the object_time_to_range function Signed-off-by: Maxime CLEMENT --------- Signed-off-by: Takamasa Horibe Signed-off-by: Maxime CLEMENT Co-authored-by: Maxime CLEMENT --- .../src/decisions.cpp | 19 ++++++++++--------- .../src/filter_predicted_objects.hpp | 10 +++++++++- 2 files changed, 19 insertions(+), 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 index 3d8c6b7eb663b..339ac35eb4444 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -70,17 +70,18 @@ std::optional> object_time_to_range( auto worst_exit_time = std::optional(); 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(enter_segment_idx) * time_step + enter_offset_ratio * time_step; @@ -88,13 +89,13 @@ std::optional> object_time_to_range( 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(exit_segment_length); const auto exit_time = static_cast(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 index 48869e5e3926d..0d26a29b6fab5 100644 --- 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(