test(dynamic_obstacle_stop): add tests and do some refactoring (#8250)

Signed-off-by: Maxime CLEMENT <[email protected]>

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/CMakeLists.txt

@@ -9,4 +9,15 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   DIRECTORY src
 )
 
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
+    test/test_object_filtering.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    ${PROJECT_NAME}
+  )
+endif()
+
 ament_auto_package(INSTALL_TO_SHARE config)

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml

@@ -27,6 +27,7 @@
   <depend>tier4_planning_msgs</depend>
   <depend>visualization_msgs</depend>
 
+  <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp

@@ -1,4 +1,4 @@
-// Copyright 2023 TIER IV, Inc.
+// Copyright 2023-2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -18,75 +18,97 @@
 
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
+
 #include <algorithm>
 #include <vector>
 
 namespace autoware::motion_velocity_planner::dynamic_obstacle_stop
 {
 
-/// @brief filter the given predicted objects
-/// @param objects predicted objects
-/// @param ego_data ego data, including its trajectory and pose
-/// @param params parameters
-/// @param hysteresis [m] extra distance threshold used for filtering
-/// @return filtered predicted objects
+bool is_vehicle(const autoware_perception_msgs::msg::PredictedObject & object)
+{
+  constexpr auto is_vehicle_class = [](const auto & c) {
+    return c.label == autoware_perception_msgs::msg::ObjectClassification::CAR ||
+           c.label == autoware_perception_msgs::msg::ObjectClassification::BUS ||
+           c.label == autoware_perception_msgs::msg::ObjectClassification::TRUCK ||
+           c.label == autoware_perception_msgs::msg::ObjectClassification::TRAILER ||
+           c.label == autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE ||
+           c.label == autoware_perception_msgs::msg::ObjectClassification::BICYCLE;
+  };
+  return std::find_if(
+           object.classification.begin(), object.classification.end(), is_vehicle_class) !=
+         object.classification.end();
+}
+
+bool is_in_range(
+  const autoware_perception_msgs::msg::PredictedObject & object,
+  const TrajectoryPoints & ego_trajectory, const PlannerParam & params, const double hysteresis)
+{
+  const auto distance = std::abs(motion_utils::calcLateralOffset(
+    ego_trajectory, object.kinematics.initial_pose_with_covariance.pose.position));
+  return distance <= params.minimum_object_distance_from_ego_trajectory +
+                       params.ego_lateral_offset + object.shape.dimensions.y / 2.0 + hysteresis;
+};
+
+bool is_not_too_close(
+  const autoware_perception_msgs::msg::PredictedObject & object, const EgoData & ego_data,
+  const double & ego_longitudinal_offset)
+{
+  const auto obj_arc_length = motion_utils::calcSignedArcLength(
+    ego_data.trajectory, ego_data.pose.position,
+    object.kinematics.initial_pose_with_covariance.pose.position);
+  return std::abs(obj_arc_length) > ego_data.longitudinal_offset_to_first_trajectory_idx +
+                                      ego_longitudinal_offset + object.shape.dimensions.x / 2.0;
+};
+
+bool is_unavoidable(
+  const autoware_perception_msgs::msg::PredictedObject & object,
+  const geometry_msgs::msg::Pose & ego_pose,
+  const std::optional<geometry_msgs::msg::Pose> & ego_earliest_stop_pose,
+  const PlannerParam & params)
+{
+  constexpr auto same_direction_diff_threshold = M_PI_2 + M_PI_4;
+  const auto & o_pose = object.kinematics.initial_pose_with_covariance.pose;
+  const auto o_yaw = tf2::getYaw(o_pose.orientation);
+  const auto ego_yaw = tf2::getYaw(ego_pose.orientation);
+  const auto yaw_diff = std::abs(universe_utils::normalizeRadian(o_yaw - ego_yaw));
+  const auto opposite_heading = yaw_diff > same_direction_diff_threshold;
+  const auto collision_distance_threshold =
+    params.ego_lateral_offset + object.shape.dimensions.y / 2.0 + params.hysteresis;
+  const auto lat_distance =
+    std::abs(universe_utils::calcLateralDeviation(o_pose, ego_pose.position));
+  auto has_collision = opposite_heading && lat_distance <= collision_distance_threshold;
+  if (ego_earliest_stop_pose) {
+    const auto direction_yaw = std::atan2(
+      o_pose.position.y - ego_earliest_stop_pose->position.y,
+      o_pose.position.x - ego_earliest_stop_pose->position.x);
+    const auto yaw_diff_at_earliest_stop_pose =
+      std::abs(universe_utils::normalizeRadian(o_yaw - direction_yaw));
+    const auto lat_distance_at_earliest_stop_pose =
+      std::abs(universe_utils::calcLateralDeviation(o_pose, ego_earliest_stop_pose->position));
+    const auto collision_at_earliest_stop_pose =
+      yaw_diff_at_earliest_stop_pose > same_direction_diff_threshold &&
+      lat_distance_at_earliest_stop_pose <= collision_distance_threshold;
+    has_collision |= collision_at_earliest_stop_pose;
+  }
+  return has_collision;
+};
+
 std::vector<autoware_perception_msgs::msg::PredictedObject> filter_predicted_objects(
   const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data,
   const PlannerParam & params, const double hysteresis)
 {
   std::vector<autoware_perception_msgs::msg::PredictedObject> filtered_objects;
-  const auto is_vehicle = [](const auto & o) {
-    return std::find_if(o.classification.begin(), o.classification.end(), [](const auto & c) {
-             return c.label == autoware_perception_msgs::msg::ObjectClassification::CAR ||
-                    c.label == autoware_perception_msgs::msg::ObjectClassification::BUS ||
-                    c.label == autoware_perception_msgs::msg::ObjectClassification::TRUCK ||
-                    c.label == autoware_perception_msgs::msg::ObjectClassification::TRAILER ||
-                    c.label == autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE ||
-                    c.label == autoware_perception_msgs::msg::ObjectClassification::BICYCLE;
-           }) != o.classification.end();
-  };
-  const auto is_in_range = [&](const auto & o) {
-    const auto distance = std::abs(autoware::motion_utils::calcLateralOffset(
-      ego_data.trajectory, o.kinematics.initial_pose_with_covariance.pose.position));
-    return distance <= params.minimum_object_distance_from_ego_trajectory +
-                         params.ego_lateral_offset + o.shape.dimensions.y / 2.0 + hysteresis;
-  };
-  const auto is_not_too_close = [&](const auto & o) {
-    const auto obj_arc_length = autoware::motion_utils::calcSignedArcLength(
-      ego_data.trajectory, ego_data.pose.position,
-      o.kinematics.initial_pose_with_covariance.pose.position);
-    return obj_arc_length > ego_data.longitudinal_offset_to_first_trajectory_idx +
-                              params.ego_longitudinal_offset + o.shape.dimensions.x / 2.0;
-  };
-  const auto is_unavoidable = [&](const auto & o) {
-    const auto & o_pose = o.kinematics.initial_pose_with_covariance.pose;
-    const auto o_yaw = tf2::getYaw(o_pose.orientation);
-    const auto ego_yaw = tf2::getYaw(ego_data.pose.orientation);
-    const auto yaw_diff = std::abs(autoware::universe_utils::normalizeRadian(o_yaw - ego_yaw));
-    const auto opposite_heading = yaw_diff > M_PI_2 + M_PI_4;
-    const auto collision_distance_threshold =
-      params.ego_lateral_offset + o.shape.dimensions.y / 2.0 + params.hysteresis;
-    // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_point_and_angle
-    const auto lat_distance = std::abs(
-      (o_pose.position.y - ego_data.pose.position.y) * std::cos(o_yaw) -
-      (o_pose.position.x - ego_data.pose.position.x) * std::sin(o_yaw));
-    auto has_collision = lat_distance <= collision_distance_threshold;
-    if (ego_data.earliest_stop_pose) {
-      const auto collision_at_earliest_stop_pose =
-        std::abs(
-          (o_pose.position.y - ego_data.earliest_stop_pose->position.y) * std::cos(o_yaw) -
-          (o_pose.position.x - ego_data.earliest_stop_pose->position.x) * std::sin(o_yaw)) <=
-        collision_distance_threshold;
-      has_collision |= collision_at_earliest_stop_pose;
-    }
-    return opposite_heading && has_collision;
-  };
   for (const auto & object : objects.objects) {
     const auto is_not_too_slow = object.kinematics.initial_twist_with_covariance.twist.linear.x >=
                                  params.minimum_object_velocity;
     if (
-      is_vehicle(object) && is_not_too_slow && is_in_range(object) && is_not_too_close(object) &&
-      (!params.ignore_unavoidable_collisions || !is_unavoidable(object)))
+      is_vehicle(object) && is_not_too_slow &&
+      is_in_range(object, ego_data.trajectory, params, hysteresis) &&
+      is_not_too_close(object, ego_data, params.ego_longitudinal_offset) &&
+      (!params.ignore_unavoidable_collisions ||
+       !is_unavoidable(object, ego_data.pose, ego_data.earliest_stop_pose, params)))
       filtered_objects.push_back(object);
   }
   return filtered_objects;

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp

@@ -1,4 +1,4 @@
-// Copyright 2023 TIER IV, Inc.
+// Copyright 2023-2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -23,6 +23,49 @@
 
 namespace autoware::motion_velocity_planner::dynamic_obstacle_stop
 {
+/// @brief check if the object is a vehicle
+/// @details the object classification labels are used to determine if the object is a vehicle
+/// (classification probabilities are ignored)
+/// @param object a predicted object with a label to check
+/// @return true if the object is a vehicle
+bool is_vehicle(const autoware_perception_msgs::msg::PredictedObject & object);
+
+/// @brief check if the object is within the lateral range of the ego trajectory
+/// @details the lateral offset of the object position to the ego trajectory is compared to the ego
+/// + object width + minimum_object_distance_from_ego_trajectory parameter + hysteresis
+/// @param object a predicted object with its pose and the lateral offset of its shape
+/// @param ego_trajectory trajectory of the ego vehicle
+/// @param params parameters with the minimum lateral range and the ego lateral offset
+/// @param hysteresis [m] currently used hysteresis distance
+/// @return true if the object is in range of the ego trajectory
+bool is_in_range(
+  const autoware_perception_msgs::msg::PredictedObject & object,
+  const TrajectoryPoints & ego_trajectory, const PlannerParam & params, const double hysteresis);
+
+/// @brief check if an object is not too close to the ego vehicle
+/// @details the arc length difference of the ego and of the object position on the ego trajectory
+/// are compared with the ego and object length
+/// @param object a predicted object with its pose and shape
+/// @param ego_data ego data with its trajectory, pose, and arc length of the pose on the trajectory
+/// @param ego_earliest_stop_pose pose the ego vehicle would reach if it were to stop now
+/// @param params parameters with the lateral ego offset distance and the hysteresis distance
+/// @return true if the object is in range of the ego vehicle
+bool is_not_too_close(
+  const autoware_perception_msgs::msg::PredictedObject & object, const EgoData & ego_data,
+  const double & ego_longitudinal_offset);
+
+/// @brief check if a collision would occur with the object, even if ego were stopping now
+/// @details we roughly check for a collision at the current ego pose or the earliest stop pose
+/// @param object a predicted object with a label to check
+/// @param ego_pose pose of the ego vehicle
+/// @param ego_earliest_stop_pose pose the ego vehicle would reach if it were to stop now
+/// @param params parameters with the lateral ego offset distance and the hysteresis distance
+/// @return true if the object is in range of the ego vehicle
+bool is_unavoidable(
+  const autoware_perception_msgs::msg::PredictedObject & object,
+  const geometry_msgs::msg::Pose & ego_pose,
+  const std::optional<geometry_msgs::msg::Pose> & ego_earliest_stop_pose,
+  const PlannerParam & params);
 
 /// @brief filter the given predicted objects
 /// @param objects predicted objects

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp

@@ -38,22 +38,22 @@ using Rtree = boost::geometry::index::rtree<BoxIndexPair, boost::geometry::index
 /// @brief parameters for the "out of lane" module
 struct PlannerParam
 {
-  double extra_object_width;
-  double minimum_object_velocity;
-  double stop_distance_buffer;
-  double time_horizon;
-  double hysteresis;
-  double add_duration_buffer;
-  double remove_duration_buffer;
-  double ego_longitudinal_offset;
-  double ego_lateral_offset;
-  double minimum_object_distance_from_ego_trajectory;
-  bool ignore_unavoidable_collisions;
+  double extra_object_width{};
+  double minimum_object_velocity{};
+  double stop_distance_buffer{};
+  double time_horizon{};
+  double hysteresis{};
+  double add_duration_buffer{};
+  double remove_duration_buffer{};
+  double ego_longitudinal_offset{};
+  double ego_lateral_offset{};
+  double minimum_object_distance_from_ego_trajectory{};
+  bool ignore_unavoidable_collisions{};
 };
 
 struct EgoData
 {
-  TrajectoryPoints trajectory{};
+  TrajectoryPoints trajectory;
   size_t first_trajectory_idx{};
   double longitudinal_offset_to_first_trajectory_idx;  // [m]
   geometry_msgs::msg::Pose pose;
@@ -65,11 +65,11 @@ struct EgoData
 /// @brief debug data
 struct DebugData
 {
-  autoware::universe_utils::MultiPolygon2d obstacle_footprints{};
+  autoware::universe_utils::MultiPolygon2d obstacle_footprints;
   size_t prev_dynamic_obstacles_nb{};
-  autoware::universe_utils::MultiPolygon2d ego_footprints{};
+  autoware::universe_utils::MultiPolygon2d ego_footprints;
   size_t prev_ego_footprints_nb{};
-  std::optional<geometry_msgs::msg::Pose> stop_pose{};
+  std::optional<geometry_msgs::msg::Pose> stop_pose;
   size_t prev_collisions_nb{};
   double z{};
   void reset_data()