add test for generate_stop_line

Signed-off-by: Maxime CLEMENT <[email protected]>
autowarefoundation · Oct 3, 2024 · 9ba045e · 9ba045e
1 parent 5eced7c
commit 9ba045e
Show file tree

Hide file tree

Showing 3 changed files with 73 additions and 6 deletions.
diff --git a/...ehavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp b/...ehavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp
@@ -69,7 +69,6 @@ std::optional<LineString2d> generate_stop_line(
   LineString2d stop_line;
   for (const auto & no_stopping_area : no_stopping_areas) {
     const auto & area_poly = lanelet::utils::to2D(no_stopping_area).basicPolygon();
-    lanelet::BasicLineString2d path_line;
     for (size_t i = 0; i < path.points.size() - 1; ++i) {
       const auto p0 = path.points.at(i).point.pose.position;
       const auto p1 = path.points.at(i + 1).point.pose.position;

diff --git a/...ehavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp b/...ehavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp
@@ -68,11 +68,15 @@ void insert_stop_point(
   tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point);
 
 /**
- * @brief auto gen no stopping area stop line from area polygon if stop line is not set
- *        ---------------
- * ------col-------------|--> ego path
- *        |     Area     |
- *        ---------------
+ * @brief generate stop line from no stopping area polygons
+ *          ________________
+ * ------|--|--------------|--> ego path
+ *  stop |  |     Area     |
+ *  line |  L______________/
+ * @param path input path
+ * @param no_stopping_areas no stopping area polygons
+ * @param ego_width [m] width of ego
+ * @param stop_line_margin [m] margin to keep between the stop line and the no stopping areas
  **/
 std::optional<universe_utils::LineString2d> generate_stop_line(
   const tier4_planning_msgs::msg::PathWithLaneId & path,

diff --git a/...r_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp b/...r_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp
@@ -21,6 +21,9 @@
 #include <tier4_planning_msgs/msg/detail/path_point_with_lane_id__struct.hpp>
 
 #include <gtest/gtest.h>
+#include <lanelet2_core/Forward.h>
+#include <lanelet2_core/primitives/Point.h>
+#include <lanelet2_core/primitives/Polygon.h>
 
 #include <stdexcept>
 
@@ -90,3 +93,64 @@ TEST(NoStoppingAreaTest, insertStopPoint)
   stop_point.second = path.points.back().point.pose;
   EXPECT_THROW(insert_stop_point(path, stop_point), std::out_of_range);
 }
+
+TEST(NoStoppingAreaTest, generateStopLine)
+{
+  using autoware::behavior_velocity_planner::no_stopping_area::generate_stop_line;
+  tier4_planning_msgs::msg::PathWithLaneId path;
+  constexpr auto nb_points = 10;
+  for (auto x = 0; x < nb_points; ++x) {
+    tier4_planning_msgs::msg::PathPointWithLaneId p;
+    p.point.pose.position.x = 1.0 * x;
+    p.point.pose.position.y = 0.0;
+    path.points.push_back(p);
+  }
+  lanelet::ConstPolygons3d no_stopping_areas;
+  double ego_width = 0.0;
+  double stop_line_margin = 0.0;
+  // no areas, expect no stop line
+  auto stop_line = generate_stop_line(path, no_stopping_areas, ego_width, stop_line_margin);
+  EXPECT_FALSE(stop_line.has_value());
+
+  // area outside of the path
+  lanelet::Polygon3d poly;
+  poly.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, -2.0));
+  poly.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, -0.5));
+  poly.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, -0.5));
+  poly.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, -2.0));
+  no_stopping_areas.push_back(poly);
+  stop_line = generate_stop_line(path, no_stopping_areas, ego_width, stop_line_margin);
+  EXPECT_FALSE(stop_line.has_value());
+  // area on the path, 0 margin and 0 width
+  poly.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, -1.0));
+  poly.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, 1.0));
+  poly.push_back(lanelet::Point3d(lanelet::InvalId, 6.0, 1.0));
+  poly.push_back(lanelet::Point3d(lanelet::InvalId, 6.0, -1.0));
+  no_stopping_areas.push_back(poly);
+  stop_line = generate_stop_line(path, no_stopping_areas, ego_width, stop_line_margin);
+  EXPECT_TRUE(stop_line.has_value());
+  ASSERT_EQ(stop_line->size(), 2UL);
+  EXPECT_EQ(stop_line->front().x(), 5.0);
+  EXPECT_EQ(stop_line->front().y(), 0.0);
+  EXPECT_EQ(stop_line->back().x(), 5.0);
+  EXPECT_EQ(stop_line->back().y(), 0.0);
+  // set a margin
+  stop_line_margin = 1.0;
+  stop_line = generate_stop_line(path, no_stopping_areas, ego_width, stop_line_margin);
+  EXPECT_TRUE(stop_line.has_value());
+  ASSERT_EQ(stop_line->size(), 2UL);
+  EXPECT_EQ(stop_line->front().x(), 4.0);
+  EXPECT_EQ(stop_line->front().y(), 0.0);
+  EXPECT_EQ(stop_line->back().x(), 4.0);
+  EXPECT_EQ(stop_line->back().y(), 0.0);
+  // set a width
+  ego_width = 2.0;
+  stop_line = generate_stop_line(path, no_stopping_areas, ego_width, stop_line_margin);
+  EXPECT_TRUE(stop_line.has_value());
+  ASSERT_EQ(stop_line->size(), 2UL);
+  // TODO(anyone): if we stop at this stop line ego overlaps the 1st area, is this okay ?
+  EXPECT_EQ(stop_line->front().x(), 4.0);
+  EXPECT_EQ(stop_line->front().y(), 2.0);
+  EXPECT_EQ(stop_line->back().x(), 4.0);
+  EXPECT_EQ(stop_line->back().y(), -2.0);
+}