feat(behavior_path_planner): revise lane change module (autowarefound…

…ation#1139) * feat(behavior_path_planner): revise lane change module Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Refactoring and renaming Remove some of the unused variables Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Add longitudinal threshold and modify default param Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> fix error after rebase Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * refactor and copy predicted if not empty Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Include abort parameters and reorganize parameters declaration Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> feat(behavior_path_planner): lane change revised visualization (autowarefoundation#1140) Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> change force lane change path color Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> visualize ego polygon with respect to object using same color Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Add valid path's visualization Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> fix conflicts Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> fix(behavior_path_planner): lane change ego and target polygon pose (autowarefoundation#1786) * fix(behavior_path_planner): lane change ego and target polygon pose Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * reduce lane changing duration Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * refactor(behavior_path_planner): lane change refactoring Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com> Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> fix(behavior_path_planner): fix lane change logic on the edge case Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> fix pose init and add failed reasons Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Ignore prepare distance if object is static Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> revert the lerp by time stamp Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> feat(behavior_path_planner): debug message for lane change Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
zulfaqar-azmi-t4 · Dec 16, 2022 · fd5ac60 · fd5ac60
1 parent 4ef1b82
commit fd5ac60
Show file tree

Hide file tree

Showing 43 changed files with 3,207 additions and 722 deletions.
diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
@@ -1012,6 +1012,112 @@ inline boost::optional<size_t> insertStopPoint(
 
   return stop_idx;
 }
+
+template <class T>
+size_t findFirstNearestIndexWithSoftConstraints(
+  const T & points, const geometry_msgs::msg::Pose & pose,
+  const double dist_threshold = std::numeric_limits<double>::max(),
+  const double yaw_threshold = std::numeric_limits<double>::max())
+{
+  validateNonEmpty(points);
+
+  {  // with dist and yaw thresholds
+    const double squared_dist_threshold = dist_threshold * dist_threshold;
+    double min_squared_dist = std::numeric_limits<double>::max();
+    size_t min_idx = 0;
+    bool is_within_constraints = false;
+    for (size_t i = 0; i < points.size(); ++i) {
+      const auto squared_dist =
+        tier4_autoware_utils::calcSquaredDistance2d(points.at(i), pose.position);
+      const auto yaw =
+        tier4_autoware_utils::calcYawDeviation(tier4_autoware_utils::getPose(points.at(i)), pose);
+
+      if (squared_dist_threshold < squared_dist || yaw_threshold < std::abs(yaw)) {
+        if (is_within_constraints) {
+          break;
+        } else {
+          continue;
+        }
+      }
+
+      if (min_squared_dist <= squared_dist) {
+        continue;
+      }
+
+      min_squared_dist = squared_dist;
+      min_idx = i;
+      is_within_constraints = true;
+    }
+
+    // nearest index is found
+    if (is_within_constraints) {
+      return min_idx;
+    }
+  }
+
+  {  // with dist threshold
+    const double squared_dist_threshold = dist_threshold * dist_threshold;
+    double min_squared_dist = std::numeric_limits<double>::max();
+    size_t min_idx = 0;
+    bool is_within_constraints = false;
+    for (size_t i = 0; i < points.size(); ++i) {
+      const auto squared_dist =
+        tier4_autoware_utils::calcSquaredDistance2d(points.at(i), pose.position);
+
+      if (squared_dist_threshold < squared_dist) {
+        if (is_within_constraints) {
+          break;
+        } else {
+          continue;
+        }
+      }
+
+      if (min_squared_dist <= squared_dist) {
+        continue;
+      }
+
+      min_squared_dist = squared_dist;
+      min_idx = i;
+      is_within_constraints = true;
+    }
+
+    // nearest index is found
+    if (is_within_constraints) {
+      return min_idx;
+    }
+  }
+
+  // without any threshold
+  return findNearestIndex(points, pose.position);
+}
+
+template <class T>
+size_t findFirstNearestSegmentIndexWithSoftConstraints(
+  const T & points, const geometry_msgs::msg::Pose & pose,
+  const double dist_threshold = std::numeric_limits<double>::max(),
+  const double yaw_threshold = std::numeric_limits<double>::max())
+{
+  // find first nearest index with soft constraints (not segment index)
+  const size_t nearest_idx =
+    findFirstNearestIndexWithSoftConstraints(points, pose, dist_threshold, yaw_threshold);
+
+  // calculate segment index
+  if (nearest_idx == 0) {
+    return 0;
+  }
+  if (nearest_idx == points.size() - 1) {
+    return points.size() - 2;
+  }
+
+  const double signed_length = calcLongitudinalOffsetToSegment(points, nearest_idx, pose.position);
+
+  if (signed_length <= 0) {
+    return nearest_idx - 1;
+  }
+
+  return nearest_idx;
+}
+
 }  // namespace motion_utils
 
 #endif  // MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_
diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt
@@ -22,13 +22,15 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/scene_module/lane_following/lane_following_module.cpp
   src/scene_module/lane_change/lane_change_module.cpp
   src/scene_module/lane_change/util.cpp
+  src/scene_module/lane_change/debug.cpp
   src/scene_module/pull_over/pull_over_module.cpp
   src/scene_module/pull_over/util.cpp
   src/scene_module/pull_out/pull_out_module.cpp
   src/scene_module/pull_out/util.cpp
   src/scene_module/utils/geometric_parallel_parking.cpp
   src/scene_module/utils/occupancy_grid_based_collision_detector.cpp
   src/scene_module/utils/path_shifter.cpp
+  src/scene_module/scene_module_visitor.cpp
 )
 
 target_include_directories(behavior_path_planner_node SYSTEM PUBLIC