test(bpp_common): add test for path utils (autowarefoundation#9122)

* add test file for path utils

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

* fix

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

* add tests for map irrelevant function

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

* add test for getUnshiftedEgoPose

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

* add docstring and remove unneccesary function

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

---------

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

Author: go-sakayori

planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt

@@ -34,7 +34,8 @@ target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC
 
 if(BUILD_TESTING)
   ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_utilities
-  test/test_utils.cpp
+    test/test_utils.cpp
+    test/test_path_utils.cpp
   )
 
   target_link_libraries(test_${PROJECT_NAME}_utilities

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_utils.hpp

@@ -58,34 +58,76 @@ PathWithLaneId resamplePathWithSpline(
   const PathWithLaneId & path, const double interval, const bool keep_input_points = false,
   const std::pair<double, double> target_section = {0.0, std::numeric_limits<double>::max()});
 
+/**
+ * @brief Gets index based on target index and arc length.
+ * @param [in] path Path.
+ * @param [in] target_idx Target index.
+ * @param [in] signed_arc Signed arc length. (Positive is forward)
+ * @return Index
+ */
 size_t getIdxByArclength(
   const PathWithLaneId & path, const size_t target_idx, const double signed_arc);
 
+/**
+ * @brief Clips path according to the target index and length.
+ * @param [in] path Path.
+ * @param [in] target_idx Target index.
+ * @param [in] forward Forward distance from target index.
+ * @param [in] backward Backward distance from target index.
+ * @return Index
+ */
 void clipPathLength(
   PathWithLaneId & path, const size_t target_idx, const double forward, const double backward);
 
-void clipPathLength(
-  PathWithLaneId & path, const size_t target_idx, const BehaviorPathPlannerParameters & params);
-
 PathWithLaneId convertWayPointsToPathWithLaneId(
   const autoware::freespace_planning_algorithms::PlannerWaypoints & waypoints,
   const double velocity, const lanelet::ConstLanelets & lanelets);
 
+/**
+ * @brief Get indices where the driving direction would reverse.
+ * @param [in] path Original path.
+ * @return Indices.
+ */
 std::vector<size_t> getReversingIndices(const PathWithLaneId & path);
 
+/**
+ * @brief Divide path by given indices.
+ * @param [in] path Original path.
+ * @param [in] indices Where to split the path.
+ * @return Divided paths.
+ */
 std::vector<PathWithLaneId> dividePath(
   const PathWithLaneId & path, const std::vector<size_t> & indices);
 
+/**
+ * @brief Corrects the velocity implemented in the trajectory by judging the vehicle driving
+ * direction.
+ * @param [in] divided_paths Multiple path with lane ID.
+ * @return
+ */
 void correctDividedPathVelocity(std::vector<PathWithLaneId> & divided_paths);
 
 // only two points is supported
-std::vector<double> splineTwoPoints(
+std::vector<double> spline_two_points(
   const std::vector<double> & base_s, const std::vector<double> & base_x, const double begin_diff,
   const double end_diff, const std::vector<double> & new_s);
 
+/**
+ * @brief Interpolates pose between 2 pose.
+ * @param [in] start_pose Start pose.
+ * @param [in] end_pose End pose.
+ * @param [in] resample_interval Resampling interval.
+ * @return Array of pose
+ */
 std::vector<Pose> interpolatePose(
   const Pose & start_pose, const Pose & end_pose, const double resample_interval);
 
+/**
+ * @brief Get ego pose which is not shifted.
+ * @param [in] ego_pose Ego pose.
+ * @param [in] prev_path Previous path with lane ID.
+ * @return Unshifted pose.
+ */
 geometry_msgs::msg::Pose getUnshiftedEgoPose(
   const geometry_msgs::msg::Pose & ego_pose, const ShiftedPath & prev_path);
 
@@ -94,6 +136,12 @@ PathWithLaneId calcCenterLinePath(
   const double longest_dist_to_shift_line,
   const std::optional<PathWithLaneId> & prev_module_path = std::nullopt);
 
+/**
+ * @brief Combines 2 path which do not overlap.
+ * @param [in] path1 First path.
+ * @param [in] path2 Second path.
+ * @return Combined path.
+ */
 PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & path2);
 
 BehaviorModuleOutput getReferencePath(

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp

@@ -27,7 +27,6 @@
 #include <tf2/utils.h>
 
 #include <algorithm>
-#include <limits>
 #include <optional>
 #include <utility>
 #include <vector>
@@ -102,6 +101,7 @@ PathWithLaneId resamplePathWithSpline(
     transformed_path, 0, transformed_path.size());
   for (size_t i = 0; i < path.points.size(); ++i) {
     const double s = s_vec.at(i);
+
     for (const auto & lane_id : path.points.at(i).lane_ids) {
       if (!keep_input_points && (unique_lane_ids.find(lane_id) != unique_lane_ids.end())) {
         continue;
@@ -202,13 +202,6 @@ void clipPathLength(
   path.points = clipped_points;
 }
 
-// TODO(murooka) This function should be replaced with autoware::motion_utils::cropPoints
-void clipPathLength(
-  PathWithLaneId & path, const size_t target_idx, const BehaviorPathPlannerParameters & params)
-{
-  clipPathLength(path, target_idx, params.forward_path_length, params.backward_path_length);
-}
-
 PathWithLaneId convertWayPointsToPathWithLaneId(
   const autoware::freespace_planning_algorithms::PlannerWaypoints & waypoints,
   const double velocity, const lanelet::ConstLanelets & lanelets)
@@ -311,7 +304,7 @@ void correctDividedPathVelocity(std::vector<PathWithLaneId> & divided_paths)
 }
 
 // only two points is supported
-std::vector<double> splineTwoPoints(
+std::vector<double> spline_two_points(
   const std::vector<double> & base_s, const std::vector<double> & base_x, const double begin_diff,
   const double end_diff, const std::vector<double> & new_s)
 {
@@ -350,10 +343,10 @@ std::vector<Pose> interpolatePose(
     new_s.push_back(s);
   }
 
-  const std::vector<double> interpolated_x = splineTwoPoints(
+  const std::vector<double> interpolated_x = spline_two_points(
     base_s, base_x, std::cos(tf2::getYaw(start_pose.orientation)),
     std::cos(tf2::getYaw(end_pose.orientation)), new_s);
-  const std::vector<double> interpolated_y = splineTwoPoints(
+  const std::vector<double> interpolated_y = spline_two_points(
     base_s, base_y, std::sin(tf2::getYaw(start_pose.orientation)),
     std::sin(tf2::getYaw(end_pose.orientation)), new_s);
   for (size_t i = 0; i < interpolated_x.size(); ++i) {