feat(behavior_path_planner): define class of SafetyChaecker

planning/behavior_path_planner/CMakeLists.txt

@@ -39,6 +39,7 @@ set(common_src
   src/utils/start_planner/shift_pull_out.cpp
   src/utils/start_planner/geometric_pull_out.cpp
   src/utils/path_shifter/path_shifter.cpp
+  src/utils/safety_checker/safety_checker.cpp
   src/utils/drivable_area_expansion/drivable_area_expansion.cpp
   src/utils/drivable_area_expansion/map_utils.cpp
   src/utils/drivable_area_expansion/footprints.cpp

planning/behavior_path_planner/docs/safety_checker.jp.md

@@ -0,0 +1,57 @@
+### SafetyChecker
+
+SafetyCheckerは、与えられたパスが動的障害物と衝突するかどうかを確認する安全性チェッククラスです。
+
+## Purpose / Role
+
+行動パスプランナーにおいて、特定のモジュール（例えば、レーンチェンジ）は自己車両の安全なナビゲーションを確保するために、動的障害物との衝突チェックを行う必要があります。このユーティリティクラスは、他の道路利用者との安全性チェックを行う際に使うことのできる汎用性の高いクラスの定義を目指します。
+
+### Assumptions
+
+SafetyCheckerモジュールは、以下の前提条件に基づいています：
+
+ユーザーは、自己車両と動的障害物の位置、速度、形状をユーティリティ関数に提供する必要があります。
+自己車両と動的障害物の予測パスの各点のヨー角は、パスの次の点を指すべきです。
+SafetyCheckerモジュールはRSS距離を用いて、他のオブジェクトとの潜在的な衝突の安全性を判断します。
+
+### Limitations
+
+現在のところ、動的障害物の予測パスの各点のヨー角は次の点を指していません。そのため、SafetyChecker関数は一部のエッジケースで不正確な結果を返す可能性があります。
+
+### Inner working / Algorithm
+
+SafetyCheckerのアルゴリズムの流れは、以下の説明で述べられています。
+
+safety_check_flow
+
+isPathSafe: パスが安全であるかどうかを判断するメソッド。パスと自己車両のオドメトリを引数として受け取り、パスが安全であれば true、そうでなければ false を返します。
+
+createPredictedPath: 予測パスを生成するメソッド。自己車両の現在の速度、目標速度、加速度、自己車両の姿勢、時間分解能、停止時間に基づいて予測パスを作成します。
+
+getBackwardLanelets: 後方のlaneletsを取得するメソッド。ルートハンドラー、ターゲットレーン、自己車両の姿勢を引数として受け取り、後方のlaneletsを返します。安全確認の対象となるエリアを取得することを目的として利用します。
+
+filterObjectIndices: オブジェクトのインデックスをフィルタリングするメソッド。ロードオブジェクトのリスト、フィルタ関数を引数として受け取り、インデックスのリストを返します。
+
+getEgoExpectedPoseAndConvertToPolygon: 自己車両の予想姿勢を生成し、それを多角形に変換するメソッド。この多角形は、後続のメソッドで衝突チェックに使用されます。
+
+isSafeInLaneletCollisionCheck: lanelet間での衝突をチェックするメソッド。衝突がなければ true を返し、あれば false を返します。
+
+isObjectIndexIncluded: 指定したインデックスが動的オブジェクトのリストのインデックスに含まれているかどうかを確認するメソッド。
+
+isTargetObjectFront: 指定したオブジェクトが自己車両の前方にあるかどうかを判断するメソッド。前方にあれば true を返し、そうでなければ false を返します。
+
+これらのメソッドを使用して、自己車両の予測パスが他の道路利用者と衝突する可能性があるかどうかを判断することができます。
+
+### How to use
+
+SafetyCheckerのインスタンスを生成する際には、SafetyCheckParamsおよびSafetyCheckDataという構造体を定義して、それらを引数に与える必要があります。
+
+SafetyCheckParams
+この構造体は、安全性チェックのパラメータを保持します。具体的なパラメータは、チェックの対象となる動的障害物の形状やサイズ、自己車両の速度や位置、予測パスの生成に関する設定などを含みます。
+
+SafetyCheckData
+この構造体は、安全性チェックに必要なデータを保持します。具体的なデータは、チェックの対象となる動的障害物の現在の状態、自己車両の現在の状態、ルートハンドラー、その他の関連データなどを含みます。
+
+### Future wotk
+
+このクラスの汎用性を高めるために他のモジュールで実装されている安全確認機能をシンプルに実現する方法を検討する方法がある。

planning/behavior_path_planner/docs/safety_checker.md

@@ -0,0 +1,84 @@
+# SafetyChecker
+
+SafetyChecker is a safety check class that verifies if a given path collides with dynamic obstacles.
+
+## Purpose / Role
+
+In a behavior path planner, specific modules, such as lane change, need to perform collision checks with dynamic obstacles to ensure the safe navigation of the ego vehicle. The purpose of this utility class is to provide a versatile definition of a class that can be used for safety checks with other road users.
+
+### Assumptions
+
+The SafetyChecker module is based on the following assumptions:
+
+- The user needs to provide the position, velocity, and shape of the ego vehicle and dynamic obstacles to the utility functions.
+- The yaw angle of each point on the predicted path of the ego vehicle and dynamic obstacles should point to the next point on the path.
+- The SafetyChecker module determines the safety of potential collisions with other objects using RSS distance.
+
+### Limitations
+
+Currently, the yaw angle of each point on the predicted path of dynamic obstacles does not point to the next point. Therefore, the SafetyChecker function may return inaccurate results in some edge cases.
+
+### Inner working / Algorithm
+
+The algorithm flow of SafetyChecker is described as follows:
+
+```plantuml
+@startuml
+skinparam monochrome true
+
+title SafetyChecker: isSafeInLaneletCollisionCheck
+start
+
+:calculate path to safety check;
+:calculate predicted ego poses;
+
+partition Check Collision {
+  while (has next target object) is (yes)
+    :calculate target object path;
+    if (isSafeInLaneletCollisionCheck) then (yes)
+        :continue checking;
+    else (no)
+        :return false;
+        stop
+    endif
+  endwhile (no)
+  :return true;
+  stop
+}
+
+@enduml
+```
+
+- `isPathSafe`: This method determines if a path is safe. It takes the path and ego vehicle odometry as arguments and returns `true` if the path is safe, `false` otherwise.
+
+- `createPredictedPath`: This method generates a predicted path. It creates a predicted path based on the current velocity, target velocity, acceleration, ego vehicle's pose, time resolution, and stop time.
+
+- `getBackwardLanelets`: This method retrieves the backward lanelets. It takes the route handler, target lane, and ego vehicle's pose as arguments and returns the backward lanelets. It is used to obtain the area of interest for safety checks.
+
+- `filterObjectIndices`: This method filters the object indices. It takes a list of road objects and a filter function as arguments, and returns a list of indices.
+
+- `getEgoExpectedPoseAndConvertToPolygon`: This method generates the expected pose of the ego vehicle and converts it to a polygon. This polygon is used in subsequent methods for collision checks.
+
+- `isSafeInLaneletCollisionCheck`: This method checks for collisions between lanelets. It returns `true` if there is no collision, `false` otherwise.
+
+- `isObjectIndexIncluded`: This method checks if the specified index is included in the list of dynamic object indices.
+
+- `isTargetObjectFront`: This method determines if the specified object is in front of the ego vehicle. It returns `true` if it is in front, `false` otherwise.
+
+By using these methods, it is possible to determine if the predicted path of the ego vehicle has the potential to collide with other road users.
+
+### How to use
+
+When creating an instance of SafetyChecker, you need to define the structures SafetyCheckParams and SafetyCheckData and provide them as arguments.
+
+#### SafetyCheckParams
+
+This structure holds the parameters for safety checks. Specific parameters include the shape and size of dynamic obstacles to be checked, the velocity and position of the ego vehicle, and settings for generating predicted paths.
+
+#### SafetyCheckData
+
+This structure holds the data required for safety checks. Specific data includes the current state of dynamic obstacles, the current state of the ego vehicle, route handler, and other related data.
+
+### Future Work
+
+There is a possibility of exploring ways to simplify the implementation of safety check features implemented in other modules to enhance the versatility of this class.

planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp

@@ -35,6 +35,7 @@
 
 namespace behavior_path_planner::utils
 {
+using autoware_auto_perception_msgs::msg::PredictedPath;
 using autoware_auto_planning_msgs::msg::Path;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
@@ -58,6 +59,26 @@ PathWithLaneId resamplePathWithSpline(
   const PathWithLaneId & path, const double interval, const bool keep_input_points = false,
   const std::pair<double, double> target_section = {0.0, std::numeric_limits<double>::max()});
 
+/**
+ * @brief Create a predicted path from the current velocity to a target velocity
+ * @param [in] following_trajectory_points The trajectory points that the vehicle is supposed to
+ * follow
+ * @param [in] current_velocity The current velocity of the vehicle
+ * @param [in] target_velocity The desired velocity the vehicle should reach
+ * @param [in] acc_till_target_velocity The acceleration of the vehicle until it reaches the target
+ * velocity(constant)
+ * @param [in] pose The current pose of the vehicle, including its position and orientation
+ * @param [in] resolution The time resolution for the path prediction, affecting the granularity of
+ * the generated path
+ * @param [in] stopping_time The time required for starting to run
+ * @return An object of type PredictedPath that contains the predicted path of the vehicle
+ */
+PredictedPath createPredictedPathFromTargetVelocity(
+  const std::vector<PathPointWithLaneId> & following_trajectory_points,
+  const double current_velocity, const double target_velocity,
+  const double acc_till_target_velocity, const Pose & pose, const double resolution,
+  const double stopping_time);
+
 Path toPath(const PathWithLaneId & input);
 
 size_t getIdxByArclength(