fix(autoware_path_optimizer): fix unusedFunction (autowarefoundation#…

…8644) fix:unusedFunction Signed-off-by: kobayu858 <yutaro.kobayashi@tier4.jp>
a-maumau · Sep 2, 2024 · 164e64d · 164e64d
1 parent aecae38
commit 164e64d
Show file tree

Hide file tree

Showing 2 changed files with 0 additions and 27 deletions.
diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp
@@ -132,10 +132,6 @@ std::vector<ReferencePoint> convertToReferencePoints(
 
 std::vector<ReferencePoint> sanitizePoints(const std::vector<ReferencePoint> & points);
 
-void compensateLastPose(
-  const PathPoint & last_path_point, std::vector<TrajectoryPoint> & traj_points,
-  const double delta_dist_threshold, const double delta_yaw_threshold);
-
 geometry_msgs::msg::Point getNearestPosition(
   const std::vector<ReferencePoint> & points, const int target_idx, const double offset);
 

diff --git a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp
@@ -97,29 +97,6 @@ std::vector<ReferencePoint> sanitizePoints(const std::vector<ReferencePoint> & p
   return output;
 }
 
-void compensateLastPose(
-  const PathPoint & last_path_point, std::vector<TrajectoryPoint> & traj_points,
-  const double delta_dist_threshold, const double delta_yaw_threshold)
-{
-  if (traj_points.empty()) {
-    traj_points.push_back(convertToTrajectoryPoint(last_path_point));
-    return;
-  }
-
-  const geometry_msgs::msg::Pose last_traj_pose = traj_points.back().pose;
-
-  const double dist = autoware::universe_utils::calcDistance2d(
-    last_path_point.pose.position, last_traj_pose.position);
-  const double norm_diff_yaw = [&]() {
-    const double diff_yaw =
-      tf2::getYaw(last_path_point.pose.orientation) - tf2::getYaw(last_traj_pose.orientation);
-    return autoware::universe_utils::normalizeRadian(diff_yaw);
-  }();
-  if (dist > delta_dist_threshold || std::fabs(norm_diff_yaw) > delta_yaw_threshold) {
-    traj_points.push_back(convertToTrajectoryPoint(last_path_point));
-  }
-}
-
 geometry_msgs::msg::Point getNearestPosition(
   const std::vector<ReferencePoint> & points, const int target_idx, const double offset)
 {