test(bpp_common): add unit test for safety check (#9223)

* add test for object collision

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

* add test for more functions

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

* add docstring

Signed-off-by: Go Sakayori <gsakayori@gmail.com>

* fix lane change

Signed-off-by: Go Sakayori <gsakayori@gmail.com>

---------

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>
Signed-off-by: Go Sakayori <gsakayori@gmail.com>

Author: go-sakayori

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -1940,10 +1940,10 @@ bool NormalLaneChange::is_collided(
   const auto & bpp_param = *common_data_ptr_->bpp_param_ptr;
 
   for (const auto & obj_path : obj_predicted_paths) {
-    const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons(
-      lane_change_path, ego_predicted_path, obj, obj_path, bpp_param, selected_rss_param,
-      hysteresis_factor, safety_check_max_vel, collision_check_yaw_diff_threshold,
-      current_debug_data.second);
+    const auto collided_polygons = utils::path_safety_checker::get_collided_polygons(
+      lane_change_path, ego_predicted_path, obj, obj_path, bpp_param.vehicle_info,
+      selected_rss_param, hysteresis_factor, safety_check_max_vel,
+      collision_check_yaw_diff_threshold, current_debug_data.second);
 
     if (collided_polygons.empty()) {
       utils::path_safety_checker::updateCollisionCheckDebugMap(

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp

@@ -45,16 +45,27 @@ using autoware_perception_msgs::msg::Shape;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
 
+/**
+ * @brief Checks if the object is coming toward the ego vehicle judging by yaw deviation
+ * @param vehicle_pose Ego vehicle pose.
+ * @param object_pose Object pose.
+ * @param angle_threshold Angle threshold.
+ * @return True if the object vehicle is coming towards the ego vehicle
+ */
 bool isTargetObjectOncoming(
   const geometry_msgs::msg::Pose & vehicle_pose, const geometry_msgs::msg::Pose & object_pose,
   const double angle_threshold = M_PI_2);
 
+/**
+ * @brief Checks if the object is in front of the ego vehicle.
+ * @param ego_pose Ego vehicle pose.
+ * @param obj_polygon Polygon of object.
+ * @param base_to_front Base link to vehicle front.
+ * @return True if object is in front.
+ */
 bool isTargetObjectFront(
   const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon,
-  const autoware::vehicle_info_utils::VehicleInfo & vehicle_info);
-bool isTargetObjectFront(
-  const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose,
-  const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon);
+  const double base_to_front);
 
 Polygon2d createExtendedPolygon(
   const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info,
@@ -72,7 +83,15 @@ double calcRssDistance(
   const double front_object_velocity, const double rear_object_velocity,
   const RSSparams & rss_params);
 
-double calcMinimumLongitudinalLength(
+/**
+ * @brief Calculates the minimum longitudinal length using rss parameter. The object is either ego
+ * vehicle or target object.
+ * @param front_object_velocity Front object velocity.
+ * @param rear_object_velocity Front object velocity.
+ * @param RSSparams RSS parameters
+ * @return Maximum distance.
+ */
+double calc_minimum_longitudinal_length(
   const double front_object_velocity, const double rear_object_velocity,
   const RSSparams & rss_params);
 
@@ -84,15 +103,19 @@ double calcMinimumLongitudinalLength(
  *         it contains the interpolated pose, velocity, and time. If the interpolation fails
  *         (e.g., empty path, negative time, or time beyond the path), it returns std::nullopt.
  */
-std::optional<PoseWithVelocityStamped> calcInterpolatedPoseWithVelocity(
+std::optional<PoseWithVelocityStamped> calc_interpolated_pose_with_velocity(
   const std::vector<PoseWithVelocityStamped> & path, const double relative_time);
 
-std::optional<PoseWithVelocityAndPolygonStamped> getInterpolatedPoseWithVelocityAndPolygonStamped(
+std::optional<PoseWithVelocityAndPolygonStamped>
+get_interpolated_pose_with_velocity_and_polygon_stamped(
   const std::vector<PoseWithVelocityStamped> & pred_path, const double current_time,
   const VehicleInfo & ego_info);
-std::optional<PoseWithVelocityAndPolygonStamped> getInterpolatedPoseWithVelocityAndPolygonStamped(
+
+std::optional<PoseWithVelocityAndPolygonStamped>
+get_interpolated_pose_with_velocity_and_polygon_stamped(
   const std::vector<PoseWithVelocityStamped> & pred_path, const double current_time,
   const Shape & shape);
+
 template <typename T, typename F>
 std::vector<T> filterPredictedPathByTimeHorizon(
   const std::vector<T> & path, const double time_horizon, const F & interpolateFunc);
@@ -151,14 +174,13 @@ bool checkCollision(
  * @param debug The debug information for collision checking.
  * @return a list of polygon when collision is expected.
  */
-std::vector<Polygon2d> getCollidedPolygons(
+std::vector<Polygon2d> get_collided_polygons(
   const PathWithLaneId & planned_path,
   const std::vector<PoseWithVelocityStamped> & predicted_ego_path,
   const ExtendedPredictedObject & target_object,
-  const PredictedPathWithPolygon & target_object_path,
-  const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
-  const double hysteresis_factor, const double max_velocity_limit, const double yaw_difference_th,
-  CollisionCheckDebug & debug);
+  const PredictedPathWithPolygon & target_object_path, const VehicleInfo & vehicle_info,
+  const RSSparams & rss_parameters, const double hysteresis_factor, const double max_velocity_limit,
+  const double yaw_difference_th, CollisionCheckDebug & debug);
 
 bool checkPolygonsIntersects(
   const std::vector<Polygon2d> & polys_1, const std::vector<Polygon2d> & polys_2);
@@ -167,8 +189,30 @@ bool checkSafetyWithIntegralPredictedPolygon(
   const ExtendedPredictedObjects & objects, const bool check_all_predicted_path,
   const IntegralPredictedPolygonParams & params, CollisionCheckDebugMap & debug_map);
 
-double calcObstacleMinLength(const Shape & shape);
-double calcObstacleMaxLength(const Shape & shape);
+/**
+ * @brief Calculates the minimum length from obstacle centroid to outer point.
+ * @param shape Object shape.
+ * @return Minimum distance.
+ */
+double calc_obstacle_min_length(const Shape & shape);
+
+/**
+ * @brief Calculates the maximum length from obstacle centroid to outer point.
+ * @param shape Object shape.
+ * @return Maximum distance.
+ */
+double calc_obstacle_max_length(const Shape & shape);
+
+/**
+ * @brief Calculate collision roughly by comparing minimum/maximum distance with margin.
+ * @param path The path of the ego vehicle.
+ * @param objects The predicted objects.
+ * @param margin Distance margin to judge collision.
+ * @param parameters The common parameters used in behavior path planner.
+ * @param use_offset_ego_point If true, the closest point to the object is calculated by
+ * interpolating the path points.
+ * @return Collision (rough) between minimum distance and maximum distance
+ */
 std::pair<bool, bool> checkObjectsCollisionRough(
   const PathWithLaneId & path, const PredictedObjects & objects, const double margin,
   const BehaviorPathPlannerParameters & parameters, const bool use_offset_ego_point);