feat: improve lc computing performance (#294)

* fix(lane_change): extend target lane range in safety check (autowarefoundation#2776) * fix(lane_change): extend target lane range in safety check Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix both safety check in lane change module Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Refactoring Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * also made for external request Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * get all lanes within 100 meters Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Change logging level Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Fix target lane empty Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * check current pose Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fixed the wrong lane parameter Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix spell check Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * make code similar between lane change and external request Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix collision not working Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Rename parameters Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix remove some argument from function parameter Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * style(pre-commit): autofix --------- Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * refactor(lane_change): compute obj indices return if safe path found (autowarefoundation#2825) Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * perf(lane_change): only compute interpolate ego once (autowarefoundation#2839) * fix(lane_change): improvement on isLaneChangePathSafe computation Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * re-add logic to ignore parked vehicle Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix the duration issues and some code editing Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> --------- Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> --------- Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
tier4 · Feb 28, 2023 · cd65202 · cd65202
1 parent 7ba9c41
commit cd65202
Show file tree

Hide file tree

Showing 9 changed files with 426 additions and 350 deletions.
diff --git a/...lanner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp b/...lanner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp
@@ -70,6 +70,13 @@ struct LaneChangePhaseInfo
 
   [[nodiscard]] double sum() const { return prepare + lane_changing; }
 };
+
+struct LaneChangeTargetObjectIndices
+{
+  std::vector<size_t> current_lane{};
+  std::vector<size_t> target_lane{};
+  std::vector<size_t> other_lane{};
+};
 }  // namespace behavior_path_planner
 
 #endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_
diff --git a/...ing/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/...ing/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp
@@ -69,35 +69,22 @@ std::optional<LaneChangePath> constructCandidatePath(
   const double prepare_speed, const double lane_change_distance, const double lane_changing_speed,
   const BehaviorPathPlannerParameters & params, const LaneChangeParameters & lane_change_param);
 
-LaneChangePaths getLaneChangePaths(
+std::pair<bool, bool> getLaneChangePaths(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets,
   const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist,
-  const BehaviorPathPlannerParameters & common_parameter,
-  const behavior_path_planner::LaneChangeParameters & parameter);
-
-LaneChangePaths selectValidPaths(
-  const LaneChangePaths & paths, const lanelet::ConstLanelets & current_lanes,
-  const lanelet::ConstLanelets & target_lanes, const RouteHandler & route_handler,
-  const Pose & current_pose, const Pose & goal_pose, const double minimum_lane_change_length);
-
-bool selectSafePath(
-  const LaneChangePaths & paths, const lanelet::ConstLanelets & current_lanes,
-  const lanelet::ConstLanelets & target_lanes,
-  const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose,
-  const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters,
-  const behavior_path_planner::LaneChangeParameters & ros_parameters,
-  LaneChangePath * selected_path,
-  std::unordered_map<std::string, CollisionCheckDebug> & debug_data);
+  const PredictedObjects::ConstSharedPtr dynamic_objects,
+  const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter,
+  const double check_distance, LaneChangePaths * candidate_paths,
+  std::unordered_map<std::string, CollisionCheckDebug> * debug_data);
 
 bool isLaneChangePathSafe(
-  const LaneChangePath & lane_change_path, const lanelet::ConstLanelets & current_lanes,
-  const lanelet::ConstLanelets & target_lanes,
-  const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose,
+  const LaneChangePath & lane_change_path, const PredictedObjects::ConstSharedPtr dynamic_objects,
+  const LaneChangeTargetObjectIndices & dynamic_object_indices, const Pose & current_pose,
   const size_t current_seg_idx, const Twist & current_twist,
   const BehaviorPathPlannerParameters & common_parameters,
   const behavior_path_planner::LaneChangeParameters & lane_change_parameters,
   const double front_decel, const double rear_decel, Pose & ego_pose_before_collision,
-  std::unordered_map<std::string, CollisionCheckDebug> & debug_data, const bool use_buffer = true,
+  std::unordered_map<std::string, CollisionCheckDebug> & debug_data,
   const double acceleration = 0.0);
 
 bool hasEnoughDistance(
@@ -148,6 +135,18 @@ bool hasEnoughDistanceToLaneChangeAfterAbort(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
   const Pose & curent_pose, const double abort_return_dist,
   const BehaviorPathPlannerParameters & common_param);
-}  // namespace behavior_path_planner::lane_change_utils
 
+lanelet::ConstLanelets getExtendedTargetLanesForCollisionCheck(
+  const RouteHandler & route_handler, const lanelet::ConstLanelet & target_lanes,
+  const Pose & current_pose, const double backward_length);
+
+LaneChangeTargetObjectIndices filterObjectIndices(
+  const LaneChangePaths & lane_change_paths, const PredictedObjects & objects,
+  const lanelet::ConstLanelets & target_backward_lanes, const Pose & current_pose,
+  const double forward_path_length, const double filter_width,
+  const bool ignore_unknown_obj = false);
+
+double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0);
+
+}  // namespace behavior_path_planner::lane_change_utils
 #endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__UTIL_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp
@@ -17,6 +17,7 @@
 
 #include "behavior_path_planner/data_manager.hpp"
 #include "behavior_path_planner/debug_utilities.hpp"
+#include "behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp"
 #include "behavior_path_planner/scene_module/pull_out/pull_out_path.hpp"
 
 #include <opencv2/opencv.hpp>
@@ -513,19 +514,20 @@ bool isLateralDistanceEnough(
   const double & relative_lateral_distance, const double & lateral_distance_threshold);
 
 bool isSafeInLaneletCollisionCheck(
-  const Pose & ego_current_pose, const Twist & ego_current_twist,
-  const PredictedPath & ego_predicted_path, const VehicleInfo & ego_info,
-  const double check_start_time, const double check_end_time, const double check_time_resolution,
-  const PredictedObject & target_object, const PredictedPath & target_object_path,
-  const BehaviorPathPlannerParameters & common_parameters, const double front_decel,
+  const std::vector<std::pair<Pose, tier4_autoware_utils::Polygon2d>> & interpolated_ego,
+  const Twist & ego_current_twist, const std::vector<double> & check_duration,
+  const double prepare_duration, const PredictedObject & target_object,
+  const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters,
+  const double prepare_phase_ignore_target_speed_thresh, const double front_decel,
   const double rear_decel, Pose & ego_pose_before_collision, CollisionCheckDebug & debug);
 
 bool isSafeInFreeSpaceCollisionCheck(
-  const Pose & ego_current_pose, const Twist & ego_current_twist,
-  const PredictedPath & ego_predicted_path, const VehicleInfo & ego_info,
-  const double check_start_time, const double check_end_time, const double check_time_resolution,
-  const PredictedObject & target_object, const BehaviorPathPlannerParameters & common_parameters,
-  const double front_decel, const double rear_decel, CollisionCheckDebug & debug);
+  const std::vector<std::pair<Pose, tier4_autoware_utils::Polygon2d>> & interpolated_ego,
+  const Twist & ego_current_twist, const std::vector<double> & check_duration,
+  const double prepare_duration, const PredictedObject & target_object,
+  const BehaviorPathPlannerParameters & common_parameters,
+  const double prepare_phase_ignore_target_speed_thresh, const double front_decel,
+  const double rear_decel, CollisionCheckDebug & debug);
 
 bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold);
 

diff --git a/...ehavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp b/...ehavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp
@@ -371,50 +371,36 @@ std::pair<bool, bool> ExternalRequestLaneChangeModule::getSafePath(
 
   const auto current_lanes = util::getCurrentLanes(planner_data_);
 
-  if (!lane_change_lanes.empty()) {
-    // find candidate paths
-    const auto lane_change_paths = lane_change_utils::getLaneChangePaths(
-      *route_handler, current_lanes, lane_change_lanes, current_pose, current_twist,
-      common_parameters, *parameters_);
-
-    // get lanes used for detection
-    lanelet::ConstLanelets check_lanes;
-    if (!lane_change_paths.empty()) {
-      const auto & longest_path = lane_change_paths.front();
-      // we want to see check_distance [m] behind vehicle so add lane changing length
-      const double check_distance_with_path = check_distance + longest_path.length.sum();
-      check_lanes = route_handler->getCheckTargetLanesFromPath(
-        longest_path.path, lane_change_lanes, check_distance_with_path);
-    }
+  if (lane_change_lanes.empty()) {
+    return std::make_pair(false, false);
+  }
 
-    // select valid path
-    const LaneChangePaths valid_paths = lane_change_utils::selectValidPaths(
-      lane_change_paths, current_lanes, check_lanes, *route_handler, current_pose,
-      route_handler->getGoalPose(),
-      common_parameters.minimum_lane_change_length +
-        common_parameters.backward_length_buffer_for_end_of_lane +
-        parameters_->lane_change_finish_judge_buffer);
+  // find candidate paths
+  LaneChangePaths valid_paths;
+  const auto [found_valid_path, found_safe_path] = lane_change_utils::getLaneChangePaths(
+    *route_handler, current_lanes, lane_change_lanes, current_pose, current_twist,
+    planner_data_->dynamic_object, common_parameters, *parameters_, check_distance, &valid_paths,
+    &object_debug_);
+  debug_valid_path_ = valid_paths;
 
-    if (valid_paths.empty()) {
-      return std::make_pair(false, false);
-    }
-    debug_valid_path_ = valid_paths;
-
-    // select safe path
-    const bool found_safe_path = lane_change_utils::selectSafePath(
-      valid_paths, current_lanes, check_lanes, planner_data_->dynamic_object, current_pose,
-      current_twist, common_parameters, *parameters_, &safe_path, object_debug_);
+  if (parameters_->publish_debug_marker) {
+    setObjectDebugVisualization();
+  } else {
+    debug_marker_.markers.clear();
+  }
 
-    if (parameters_->publish_debug_marker) {
-      setObjectDebugVisualization();
-    } else {
-      debug_marker_.markers.clear();
-    }
+  if (!found_valid_path) {
+    return {false, false};
+  }
 
-    return std::make_pair(true, found_safe_path);
+  if (found_safe_path) {
+    safe_path = valid_paths.back();
+  } else {
+    // force candidate
+    safe_path = valid_paths.front();
   }
 
-  return std::make_pair(false, false);
+  return {found_valid_path, found_safe_path};
 }
 
 bool ExternalRequestLaneChangeModule::isSafe() const { return status_.is_safe; }
@@ -669,27 +655,32 @@ bool ExternalRequestLaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_
   const auto current_pose = getEgoPose();
   const auto current_twist = getEgoTwist();
   const auto & dynamic_objects = planner_data_->dynamic_object;
-  const auto & current_lanes = status_.current_lanes;
   const auto & common_parameters = planner_data_->parameters;
   const auto & route_handler = planner_data_->route_handler;
   const auto path = status_.lane_change_path;
 
   constexpr double check_distance = 100.0;
   // get lanes used for detection
-  const double check_distance_with_path = check_distance + path.length.sum();
-  const auto check_lanes = route_handler->getCheckTargetLanesFromPath(
-    path.path, status_.lane_change_lanes, check_distance_with_path);
+  const auto check_lanes = lane_change_utils::getExtendedTargetLanesForCollisionCheck(
+    *route_handler, path.target_lanelets.front(), current_pose, check_distance);
 
   std::unordered_map<std::string, CollisionCheckDebug> debug_data;
 
+  constexpr auto ignore_unknown{true};
+  const auto lateral_buffer =
+    lane_change_utils::calcLateralBufferForFiltering(common_parameters.vehicle_width);
+  const auto dynamic_object_indices = lane_change_utils::filterObjectIndices(
+    {path}, *dynamic_objects, check_lanes, current_pose, common_parameters.forward_path_length,
+    lateral_buffer, ignore_unknown);
+
   const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
     path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold,
     common_parameters.ego_nearest_yaw_threshold);
   return lane_change_utils::isLaneChangePathSafe(
-    path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist,
+    path, dynamic_objects, dynamic_object_indices, current_pose, current_seg_idx, current_twist,
     common_parameters, *parameters_, common_parameters.expected_front_deceleration_for_abort,
     common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data,
-    false, status_.lane_change_path.acceleration);
+    status_.lane_change_path.acceleration);
 }
 
 void ExternalRequestLaneChangeModule::updateOutputTurnSignal(BehaviorModuleOutput & output)

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp
@@ -371,50 +371,36 @@ std::pair<bool, bool> LaneChangeModule::getSafePath(
 
   const auto current_lanes = util::getCurrentLanes(planner_data_);
 
-  if (!lane_change_lanes.empty()) {
-    // find candidate paths
-    const auto lane_change_paths = lane_change_utils::getLaneChangePaths(
-      *route_handler, current_lanes, lane_change_lanes, current_pose, current_twist,
-      common_parameters, *parameters_);
-
-    // get lanes used for detection
-    lanelet::ConstLanelets check_lanes;
-    if (!lane_change_paths.empty()) {
-      const auto & longest_path = lane_change_paths.front();
-      // we want to see check_distance [m] behind vehicle so add lane changing length
-      const double check_distance_with_path = check_distance + longest_path.length.sum();
-      check_lanes = route_handler->getCheckTargetLanesFromPath(
-        longest_path.path, lane_change_lanes, check_distance_with_path);
-    }
+  if (lane_change_lanes.empty()) {
+    return std::make_pair(false, false);
+  }
 
-    // select valid path
-    const LaneChangePaths valid_paths = lane_change_utils::selectValidPaths(
-      lane_change_paths, current_lanes, check_lanes, *route_handler, current_pose,
-      route_handler->getGoalPose(),
-      common_parameters.minimum_lane_change_length +
-        common_parameters.backward_length_buffer_for_end_of_lane +
-        parameters_->lane_change_finish_judge_buffer);
+  // find candidate paths
+  LaneChangePaths valid_paths;
+  const auto [found_valid_path, found_safe_path] = lane_change_utils::getLaneChangePaths(
+    *route_handler, current_lanes, lane_change_lanes, current_pose, current_twist,
+    planner_data_->dynamic_object, common_parameters, *parameters_, check_distance, &valid_paths,
+    &object_debug_);
+  debug_valid_path_ = valid_paths;
 
-    if (valid_paths.empty()) {
-      return std::make_pair(false, false);
-    }
-    debug_valid_path_ = valid_paths;
-
-    // select safe path
-    const bool found_safe_path = lane_change_utils::selectSafePath(
-      valid_paths, current_lanes, check_lanes, planner_data_->dynamic_object, current_pose,
-      current_twist, common_parameters, *parameters_, &safe_path, object_debug_);
+  if (parameters_->publish_debug_marker) {
+    setObjectDebugVisualization();
+  } else {
+    debug_marker_.markers.clear();
+  }
 
-    if (parameters_->publish_debug_marker) {
-      setObjectDebugVisualization();
-    } else {
-      debug_marker_.markers.clear();
-    }
+  if (!found_valid_path) {
+    return {false, false};
+  }
 
-    return std::make_pair(true, found_safe_path);
+  if (found_safe_path) {
+    safe_path = valid_paths.back();
+  } else {
+    // force candidate
+    safe_path = valid_paths.front();
   }
 
-  return std::make_pair(false, false);
+  return {found_valid_path, found_safe_path};
 }
 
 bool LaneChangeModule::isSafe() const { return status_.is_safe; }
@@ -669,27 +655,30 @@ bool LaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) cons
   const auto current_pose = getEgoPose();
   const auto current_twist = getEgoTwist();
   const auto & dynamic_objects = planner_data_->dynamic_object;
-  const auto & current_lanes = status_.current_lanes;
   const auto & common_parameters = planner_data_->parameters;
   const auto & route_handler = planner_data_->route_handler;
   const auto & path = status_.lane_change_path;
 
-  constexpr double check_distance = 100.0;
   // get lanes used for detection
-  const double check_distance_with_path = check_distance + path.length.sum();
-  const auto check_lanes = route_handler->getCheckTargetLanesFromPath(
-    path.path, status_.lane_change_lanes, check_distance_with_path);
+  const auto check_lanes = lane_change_utils::getExtendedTargetLanesForCollisionCheck(
+    *route_handler, path.target_lanelets.front(), current_pose, check_distance_);
 
   std::unordered_map<std::string, CollisionCheckDebug> debug_data;
+  constexpr auto ignore_unknown{true};
+  const auto lateral_buffer =
+    lane_change_utils::calcLateralBufferForFiltering(common_parameters.vehicle_width);
+  const auto dynamic_object_indices = lane_change_utils::filterObjectIndices(
+    {path}, *dynamic_objects, check_lanes, current_pose, common_parameters.forward_path_length,
+    lateral_buffer, ignore_unknown);
 
   const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
     path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold,
     common_parameters.ego_nearest_yaw_threshold);
   return lane_change_utils::isLaneChangePathSafe(
-    path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist,
+    path, dynamic_objects, dynamic_object_indices, current_pose, current_seg_idx, current_twist,
     common_parameters, *parameters_, common_parameters.expected_front_deceleration_for_abort,
     common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data,
-    false, status_.lane_change_path.acceleration);
+    status_.lane_change_path.acceleration);
 }
 
 void LaneChangeModule::updateOutputTurnSignal(BehaviorModuleOutput & output)