feat(run_out_module): add tests to run out (autowarefoundation#9222)

* WIP add tests for utils and path_utils Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add tests for utils and fix test path utils Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * dynamic obstacles Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * new tests and add function declarations Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add points for test of extractObstaclePointsWithinPolygon Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add state machine tests and other tests for dynamic obstacle Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * remove unused test checks Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * remove unused tests Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * remove unwanted semicolons Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * test Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add comments Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * solve cpp-check limitation issue by removing namespaces Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> --------- Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
1222-takeshi · Nov 14, 2024 · 4e12cb0 · 4e12cb0
1 parent 7528c47
commit 4e12cb0
Show file tree

Hide file tree

Showing 11 changed files with 962 additions and 15 deletions.
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/CMakeLists.txt
@@ -15,4 +15,19 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/path_utils.cpp
 )
 
+if(BUILD_TESTING)
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
+    tests/test_dynamic_obstacle.cpp
+    tests/test_path_utils.cpp
+    tests/test_utils.cpp
+    tests/test_state_machine.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    autoware_behavior_velocity_run_out_module
+  )
+  target_include_directories(test_${PROJECT_NAME} PRIVATE src)
+endif()
+
+
+
 ament_auto_package(INSTALL_TO_SHARE config)
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml
@@ -41,6 +41,7 @@
   <depend>tf2_ros</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>
 

diff --git a/...avior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/...avior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp
@@ -32,8 +32,7 @@
 
 namespace autoware::behavior_velocity_planner
 {
-namespace
-{
+
 // create quaternion facing to the nearest trajectory point
 geometry_msgs::msg::Quaternion createQuaternionFacingToTrajectory(
   const PathPointsWithLaneId & path_points, const geometry_msgs::msg::Point & point)
@@ -114,10 +113,7 @@ pcl::PointCloud<pcl::PointXYZ> applyVoxelGridFilter(
 bool isAheadOf(
   const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Pose & base_pose)
 {
-  const auto longitudinal_deviation =
-    autoware::universe_utils::calcLongitudinalDeviation(base_pose, target_point);
-  const bool is_ahead = longitudinal_deviation > 0;
-  return is_ahead;
+  return autoware::universe_utils::calcLongitudinalDeviation(base_pose, target_point) > 0;
 }
 
 pcl::PointCloud<pcl::PointXYZ> extractObstaclePointsWithinPolygon(
@@ -349,8 +345,6 @@ std::vector<geometry_msgs::msg::Pose> createPathToPredictionTime(
   return path_to_prediction_time;
 }
 
-}  // namespace
-
 DynamicObstacleCreatorForObject::DynamicObstacleCreatorForObject(
   rclcpp::Node & node, std::shared_ptr<RunOutDebug> & debug_ptr, const DynamicObstacleParam & param)
 : DynamicObstacleCreator(node, debug_ptr, param)

diff --git a/...avior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp b/...avior_velocity_planner/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp
@@ -40,6 +40,7 @@
 #include <tf2_ros/transform_listener.h>
 
 #include <memory>
+#include <string>
 #include <vector>
 
 namespace autoware::behavior_velocity_planner
@@ -171,6 +172,58 @@ class DynamicObstacleCreatorForPoints : public DynamicObstacleCreator
   pcl::PointCloud<pcl::PointXYZ> obstacle_points_map_filtered_;
 };
 
+geometry_msgs::msg::Quaternion createQuaternionFacingToTrajectory(
+  const PathPointsWithLaneId & path_points, const geometry_msgs::msg::Point & point);
+
+std::vector<geometry_msgs::msg::Pose> createPredictedPath(
+  const geometry_msgs::msg::Pose & initial_pose, const float time_step,
+  const float max_velocity_mps, const float max_prediction_time);
+
+pcl::PointCloud<pcl::PointXYZ> applyVoxelGridFilter(
+  const pcl::PointCloud<pcl::PointXYZ> & input_points);
+
+pcl::PointCloud<pcl::PointXYZ> applyVoxelGridFilter(
+  const sensor_msgs::msg::PointCloud2 & input_points);
+
+bool isAheadOf(
+  const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Pose & base_pose);
+
+pcl::PointCloud<pcl::PointXYZ> extractObstaclePointsWithinPolygon(
+  const pcl::PointCloud<pcl::PointXYZ> & input_points, const Polygons2d & polys);
+
+std::vector<pcl::PointCloud<pcl::PointXYZ>> groupPointsWithNearestSegmentIndex(
+  const pcl::PointCloud<pcl::PointXYZ> & input_points, const PathPointsWithLaneId & path_points);
+
+pcl::PointXYZ calculateLateralNearestPoint(
+  const pcl::PointCloud<pcl::PointXYZ> & input_points, const geometry_msgs::msg::Pose & base_pose);
+
+pcl::PointCloud<pcl::PointXYZ> selectLateralNearestPoints(
+  const std::vector<pcl::PointCloud<pcl::PointXYZ>> & points_with_index,
+  const PathPointsWithLaneId & path_points);
+
+pcl::PointCloud<pcl::PointXYZ> extractLateralNearestPoints(
+  const pcl::PointCloud<pcl::PointXYZ> & input_points, const PathWithLaneId & path,
+  const float interval);
+
+std::optional<Eigen::Affine3f> getTransformMatrix(
+  const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id,
+  const std::string & source_frame_id, const builtin_interfaces::msg::Time & stamp);
+
+pcl::PointCloud<pcl::PointXYZ> transformPointCloud(
+  const PointCloud2 & input_pointcloud, const Eigen::Affine3f & transform_matrix);
+
+PointCloud2 concatPointCloud(
+  const pcl::PointCloud<pcl::PointXYZ> & cloud1, const pcl::PointCloud<pcl::PointXYZ> & cloud2);
+
+void calculateMinAndMaxVelFromCovariance(
+  const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance,
+  const double std_dev_multiplier, run_out_utils::DynamicObstacle & dynamic_obstacle);
+
+double convertDurationToDouble(const builtin_interfaces::msg::Duration & duration);
+
+std::vector<geometry_msgs::msg::Pose> createPathToPredictionTime(
+  const autoware_perception_msgs::msg::PredictedPath & predicted_path, double prediction_time);
+
 }  // namespace autoware::behavior_velocity_planner
 
 #endif  // DYNAMIC_OBSTACLE_HPP_
diff --git a/...ng/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.cpp b/...ng/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/path_utils.cpp
@@ -17,6 +17,9 @@
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 namespace autoware::behavior_velocity_planner::run_out_utils
 {
+/**
+ * This function returns the point with the smallest (signed) longitudinal distance
+ */
 geometry_msgs::msg::Point findLongitudinalNearestPoint(
   const std::vector<tier4_planning_msgs::msg::PathPointWithLaneId> & points,
   const geometry_msgs::msg::Point & src_point,

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp
@@ -127,11 +127,7 @@ std::vector<geometry_msgs::msg::Point> findLateralSameSidePoints(
 
 bool isSamePoint(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2)
 {
-  if (autoware::universe_utils::calcDistance2d(p1, p2) < std::numeric_limits<float>::epsilon()) {
-    return true;
-  }
-
-  return false;
+  return (autoware::universe_utils::calcDistance2d(p1, p2) < std::numeric_limits<float>::epsilon());
 }
 
 // insert path velocity which doesn't exceed original velocity
@@ -230,7 +226,7 @@ PathPointsWithLaneId decimatePathPoints(
   const PathPointsWithLaneId & input_path_points, const float step)
 {
   if (input_path_points.empty()) {
-    return PathPointsWithLaneId();
+    return {};
   }
 
   float dist_sum = 0.0;

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp
@@ -255,7 +255,8 @@ std::optional<std::vector<geometry_msgs::msg::Point>> createDetectionAreaPolygon
 
 // extend path to the pose of goal
 PathWithLaneId extendPath(const PathWithLaneId & input, const double extend_distance);
-PathPoint createExtendPathPoint(const double extend_distance, const PathPoint & goal_point);
+PathPointWithLaneId createExtendPathPoint(
+  const double extend_distance, const PathPointWithLaneId & goal_point);
 
 DetectionMethod toEnum(const std::string & detection_method);