refactor(behavior_path_planner): remove unnecessary functions (autowa…

…refoundation#3271) * refactor(behavior_path_planner): remove unnecessary functions Signed-off-by: yutaka <purewater0901@gmail.com> * update Signed-off-by: yutaka <purewater0901@gmail.com> --------- Signed-off-by: yutaka <purewater0901@gmail.com>
ktro2828 · Apr 7, 2023 · 04e9437 · 04e9437
1 parent 59c9db3
commit 04e9437
Show file tree

Hide file tree

Showing 4 changed files with 4 additions and 48 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp
@@ -102,10 +102,6 @@ void getProjectedDistancePointFromPolygons(
   Pose & point_on_object);
 // data conversions
 
-std::vector<Pose> convertToPoseArray(const PathWithLaneId & path);
-
-PoseArray convertToGeometryPoseArray(const PathWithLaneId & path);
-
 PredictedPath convertToPredictedPath(
   const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & pose,
   const double nearest_seg_idx, const double duration, const double resolution,

diff --git a/.../behavior_path_planner/src/util/geometric_parallel_parking/geometric_parallel_parking.cpp b/.../behavior_path_planner/src/util/geometric_parallel_parking/geometric_parallel_parking.cpp
@@ -38,7 +38,6 @@
 #include <vector>
 
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
-using behavior_path_planner::util::convertToGeometryPoseArray;
 using behavior_path_planner::util::removeOverlappingPoints;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;

diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp
@@ -122,33 +122,11 @@ using geometry_msgs::msg::PoseWithCovarianceStamped;
 using tf2::fromMsg;
 using tier4_autoware_utils::Point2d;
 
-std::vector<Pose> convertToPoseArray(const PathWithLaneId & path)
-{
-  std::vector<Pose> pose_array;
-  pose_array.reserve(path.points.size());
-  for (const auto & pt : path.points) {
-    pose_array.push_back(pt.point.pose);
-  }
-  return pose_array;
-}
-
 double l2Norm(const Vector3 vector)
 {
   return std::sqrt(std::pow(vector.x, 2) + std::pow(vector.y, 2) + std::pow(vector.z, 2));
 }
 
-PoseArray convertToGeometryPoseArray(const PathWithLaneId & path)
-{
-  PoseArray converted_array;
-  converted_array.header = path.header;
-
-  converted_array.poses.reserve(path.points.size());
-  for (const auto & point_with_id : path.points) {
-    converted_array.poses.push_back(point_with_id.point.pose);
-  }
-  return converted_array;
-}
-
 PredictedPath convertToPredictedPath(
   const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & vehicle_pose,
   const double nearest_seg_idx, const double duration, const double resolution,
@@ -1387,19 +1365,6 @@ std::vector<uint64_t> getIds(const lanelet::ConstLanelets & lanelets)
   return ids;
 }
 
-Path convertToPathFromPathWithLaneId(const PathWithLaneId & path_with_lane_id)
-{
-  Path path;
-  path.header = path_with_lane_id.header;
-  path.left_bound = path_with_lane_id.left_bound;
-  path.right_bound = path_with_lane_id.right_bound;
-  path.points.reserve(path_with_lane_id.points.size());
-  for (const auto & pt_with_lane_id : path_with_lane_id.points) {
-    path.points.push_back(pt_with_lane_id.point);
-  }
-  return path;
-}
-
 lanelet::Polygon3d getVehiclePolygon(
   const Pose & vehicle_pose, const double vehicle_width, const double base_link2front)
 {

diff --git a/planning/behavior_path_planner/test/test_utilities.cpp b/planning/behavior_path_planner/test/test_utilities.cpp
@@ -29,14 +29,12 @@ TEST(BehaviorPathPlanningUtilitiesBehaviorTest, vehiclePoseToFrenetOnStraightLin
 {
   PathWithLaneId path =
     behavior_path_planner::generateStraightSamplePathWithLaneId(0.0f, 1.0f, 10u);
-  std::vector<geometry_msgs::msg::Pose> geometry_points =
-    behavior_path_planner::util::convertToPoseArray(path);
   Pose vehicle_pose = behavior_path_planner::generateEgoSamplePose(10.7f, -1.7f, 0.0);
 
   const size_t vehicle_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
-    geometry_points, vehicle_pose, 3.0, 1.0);
+    path.points, vehicle_pose, 3.0, 1.0);
   const auto vehicle_pose_frenet = behavior_path_planner::util::convertToFrenetPoint(
-    geometry_points, vehicle_pose.position, vehicle_seg_idx);
+    path.points, vehicle_pose.position, vehicle_seg_idx);
 
   EXPECT_NEAR(vehicle_pose_frenet.distance, -1.7f, 1e-3);
   EXPECT_NEAR(vehicle_pose_frenet.length, 10.7f, 1e-3);
@@ -46,14 +44,12 @@ TEST(BehaviorPathPlanningUtilitiesBehaviorTest, vehiclePoseToFrenetOnDiagonalLin
 {
   PathWithLaneId path =
     behavior_path_planner::generateDiagonalSamplePathWithLaneId(0.0f, 1.0f, 10u);
-  std::vector<geometry_msgs::msg::Pose> geometry_points =
-    behavior_path_planner::util::convertToPoseArray(path);
   Pose vehicle_pose = behavior_path_planner::generateEgoSamplePose(0.1f, 0.1f, 0.0);
 
   const size_t vehicle_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
-    geometry_points, vehicle_pose, 3.0, 1.0);
+    path.points, vehicle_pose, 3.0, 1.0);
   const auto vehicle_pose_frenet = behavior_path_planner::util::convertToFrenetPoint(
-    geometry_points, vehicle_pose.position, vehicle_seg_idx);
+    path.points, vehicle_pose.position, vehicle_seg_idx);
 
   EXPECT_NEAR(vehicle_pose_frenet.distance, 0, 1e-2);
   EXPECT_NEAR(vehicle_pose_frenet.length, 0.1414f, 1e-2);