test(detection_area): refactor and add unit tests (#9087)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

Author: maxime-clem

planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt

@@ -6,9 +6,16 @@ autoware_package()
 pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml)
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
-  src/debug.cpp
-  src/manager.cpp
-  src/scene.cpp
+  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_utils.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME})
+endif()
+
 ament_auto_package(INSTALL_TO_SHARE config)

planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml

@@ -33,6 +33,7 @@
   <depend>tf2_geometry_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/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp

@@ -14,32 +14,28 @@
 
 #include "scene.hpp"
 
+#include "utils.hpp"
+
 #include <autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp>
 #include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
-#ifdef ROS_DISTRO_GALACTIC
-#include <tf2_eigen/tf2_eigen.h>
-#else
 #include <tf2_eigen/tf2_eigen.hpp>
-#endif
 
 #include <lanelet2_core/geometry/Polygon.h>
 
-#include <algorithm>
 #include <memory>
 #include <utility>
 #include <vector>
 
 namespace autoware::behavior_velocity_planner
 {
-namespace bg = boost::geometry;
 using autoware::motion_utils::calcLongitudinalOffsetPose;
 using autoware::motion_utils::calcSignedArcLength;
 
 DetectionAreaModule::DetectionAreaModule(
   const int64_t module_id, const int64_t lane_id,
   const lanelet::autoware::DetectionArea & detection_area_reg_elem,
-  const PlannerParam & planner_param, const rclcpp::Logger logger,
+  const PlannerParam & planner_param, const rclcpp::Logger & logger,
   const rclcpp::Clock::SharedPtr clock)
 : SceneModuleInterface(module_id, logger, clock),
   lane_id_(lane_id),
@@ -51,13 +47,6 @@ DetectionAreaModule::DetectionAreaModule(
   velocity_factor_.init(PlanningBehavior::USER_DEFINED_DETECTION_AREA);
 }
 
-LineString2d DetectionAreaModule::getStopLineGeometry2d() const
-{
-  const lanelet::ConstLineString3d stop_line = detection_area_reg_elem_.stopLine();
-  return planning_utils::extendLine(
-    stop_line[0], stop_line[1], planner_data_->stop_line_extend_length);
-}
-
 bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)
 {
   // Store original path
@@ -69,14 +58,16 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   *stop_reason = planning_utils::initializeStopReason(StopReason::DETECTION_AREA);
 
   // Find obstacles in detection area
-  const auto obstacle_points = getObstaclePoints();
+  const auto obstacle_points = detection_area::get_obstacle_points(
+    detection_area_reg_elem_.detectionAreas(), *planner_data_->no_ground_pointcloud);
   debug_data_.obstacle_points = obstacle_points;
   if (!obstacle_points.empty()) {
     last_obstacle_found_time_ = std::make_shared<const rclcpp::Time>(clock_->now());
   }
 
   // Get stop line geometry
-  const auto stop_line = getStopLineGeometry2d();
+  const auto stop_line = detection_area::get_stop_line_geometry2d(
+    detection_area_reg_elem_, planner_data_->stop_line_extend_length);
 
   // Get self pose
   const auto & self_pose = planner_data_->current_odometry->pose;
@@ -118,7 +109,8 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   setDistance(stop_dist);
 
   // Check state
-  setSafe(canClearStopState());
+  setSafe(detection_area::can_clear_stop_state(
+    last_obstacle_found_time_, clock_->now(), planner_param_.state_clear_time));
   if (isActivated()) {
     state_ = State::GO;
     last_obstacle_found_time_ = {};
@@ -165,7 +157,14 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
 
   // Ignore objects if braking distance is not enough
   if (planner_param_.use_pass_judge_line) {
-    if (state_ != State::STOP && !hasEnoughBrakingDistance(self_pose, stop_point->second)) {
+    const auto current_velocity = planner_data_->current_velocity->twist.linear.x;
+    const double pass_judge_line_distance = planning_utils::calcJudgeLineDistWithAccLimit(
+      current_velocity, planner_data_->current_acceleration->accel.accel.linear.x,
+      planner_data_->delay_response_time);
+    if (
+      state_ != State::STOP &&
+      !detection_area::has_enough_braking_distance(
+        self_pose, stop_point->second, pass_judge_line_distance, current_velocity)) {
       RCLCPP_WARN_THROTTLE(
         logger_, *clock_, std::chrono::milliseconds(1000).count(),
         "[detection_area] vehicle is over stop border");
@@ -206,138 +205,4 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
 
   return true;
 }
-
-// calc smallest enclosing circle with average O(N) algorithm
-// reference:
-// https://erickimphotography.com/blog/wp-content/uploads/2018/09/Computational-Geometry-Algorithms-and-Applications-3rd-Ed.pdf
-std::pair<lanelet::BasicPoint2d, double> calcSmallestEnclosingCircle(
-  const lanelet::ConstPolygon2d & poly)
-{
-  // The `eps` is used to avoid precision bugs in circle inclusion checks.
-  // If the value of `eps` is too small, this function doesn't work well. More than 1e-10 is
-  // recommended.
-  const double eps = 1e-5;
-  lanelet::BasicPoint2d center(0.0, 0.0);
-  double radius_squared = 0.0;
-
-  auto cross = [](const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2) -> double {
-    return p1.x() * p2.y() - p1.y() * p2.x();
-  };
-
-  auto make_circle_3 = [&](
-                         const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2,
-                         const lanelet::BasicPoint2d & p3) -> void {
-    // reference for circumcenter vector https://en.wikipedia.org/wiki/Circumscribed_circle
-    const double A = (p2 - p3).squaredNorm();
-    const double B = (p3 - p1).squaredNorm();
-    const double C = (p1 - p2).squaredNorm();
-    const double S = cross(p2 - p1, p3 - p1);
-    if (std::abs(S) < eps) return;
-    center = (A * (B + C - A) * p1 + B * (C + A - B) * p2 + C * (A + B - C) * p3) / (4 * S * S);
-    radius_squared = (center - p1).squaredNorm() + eps;
-  };
-
-  auto make_circle_2 =
-    [&](const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2) -> void {
-    center = (p1 + p2) * 0.5;
-    radius_squared = (center - p1).squaredNorm() + eps;
-  };
-
-  auto in_circle = [&](const lanelet::BasicPoint2d & p) -> bool {
-    return (center - p).squaredNorm() <= radius_squared;
-  };
-
-  // mini disc
-  for (size_t i = 1; i < poly.size(); i++) {
-    const auto p1 = poly[i].basicPoint2d();
-    if (in_circle(p1)) continue;
-
-    // mini disc with point
-    const auto p0 = poly[0].basicPoint2d();
-    make_circle_2(p0, p1);
-    for (size_t j = 0; j < i; j++) {
-      const auto p2 = poly[j].basicPoint2d();
-      if (in_circle(p2)) continue;
-
-      // mini disc with two points
-      make_circle_2(p1, p2);
-      for (size_t k = 0; k < j; k++) {
-        const auto p3 = poly[k].basicPoint2d();
-        if (in_circle(p3)) continue;
-
-        // mini disc with tree points
-        make_circle_3(p1, p2, p3);
-      }
-    }
-  }
-
-  return std::make_pair(center, radius_squared);
-}
-
-std::vector<geometry_msgs::msg::Point> DetectionAreaModule::getObstaclePoints() const
-{
-  std::vector<geometry_msgs::msg::Point> obstacle_points;
-
-  const auto detection_areas = detection_area_reg_elem_.detectionAreas();
-  const auto & points = *(planner_data_->no_ground_pointcloud);
-
-  for (const auto & detection_area : detection_areas) {
-    const auto poly = lanelet::utils::to2D(detection_area);
-    const auto circle = calcSmallestEnclosingCircle(poly);
-    for (const auto p : points) {
-      const double squared_dist = (circle.first.x() - p.x) * (circle.first.x() - p.x) +
-                                  (circle.first.y() - p.y) * (circle.first.y() - p.y);
-      if (squared_dist <= circle.second) {
-        if (bg::within(Point2d{p.x, p.y}, poly.basicPolygon())) {
-          obstacle_points.push_back(autoware::universe_utils::createPoint(p.x, p.y, p.z));
-          // get all obstacle point becomes high computation cost so skip if any point is found
-          break;
-        }
-      }
-    }
-  }
-
-  return obstacle_points;
-}
-
-bool DetectionAreaModule::canClearStopState() const
-{
-  // vehicle can clear stop state if the obstacle has never appeared in detection area
-  if (!last_obstacle_found_time_) {
-    return true;
-  }
-
-  // vehicle can clear stop state if the certain time has passed since the obstacle disappeared
-  const auto elapsed_time = clock_->now() - *last_obstacle_found_time_;
-  if (elapsed_time.seconds() >= planner_param_.state_clear_time) {
-    return true;
-  }
-
-  // rollback in simulation mode
-  if (elapsed_time.seconds() < 0.0) {
-    return true;
-  }
-
-  return false;
-}
-
-bool DetectionAreaModule::hasEnoughBrakingDistance(
-  const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const
-{
-  // get vehicle info and compute pass_judge_line_distance
-  const auto current_velocity = planner_data_->current_velocity->twist.linear.x;
-  const double max_acc = planner_data_->max_stop_acceleration_threshold;
-  const double delay_response_time = planner_data_->delay_response_time;
-  const double pass_judge_line_distance =
-    planning_utils::calcJudgeLineDistWithAccLimit(current_velocity, max_acc, delay_response_time);
-
-  // prevent from being judged as not having enough distance when the current velocity is zero
-  // and the vehicle crosses the stop line
-  if (current_velocity < 1e-3) {
-    return true;
-  }
-
-  return arc_lane_utils::calcSignedDistance(self_pose, line_pose.position) >
-         pass_judge_line_distance;
-}
 }  // namespace autoware::behavior_velocity_planner

planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp

@@ -63,11 +63,10 @@ class DetectionAreaModule : public SceneModuleInterface
     double distance_to_judge_over_stop_line;
   };
 
-public:
   DetectionAreaModule(
     const int64_t module_id, const int64_t lane_id,
     const lanelet::autoware::DetectionArea & detection_area_reg_elem,
-    const PlannerParam & planner_param, const rclcpp::Logger logger,
+    const PlannerParam & planner_param, const rclcpp::Logger & logger,
     const rclcpp::Clock::SharedPtr clock);
 
   bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override;
@@ -76,15 +75,6 @@ class DetectionAreaModule : public SceneModuleInterface
   autoware::motion_utils::VirtualWalls createVirtualWalls() override;
 
 private:
-  LineString2d getStopLineGeometry2d() const;
-
-  std::vector<geometry_msgs::msg::Point> getObstaclePoints() const;
-
-  bool canClearStopState() const;
-
-  bool hasEnoughBrakingDistance(
-    const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const;
-
   // Lane id
   int64_t lane_id_;