From 70659bb11a7ab7121a80a5c7030278b5deb86e82 Mon Sep 17 00:00:00 2001 From: kobayu858 Date: Wed, 28 Aug 2024 16:07:12 +0900 Subject: [PATCH] fix:unusedFunction Signed-off-by: kobayu858 --- .../behavior_path_side_shift_module/utils.hpp | 8 ---- .../src/utils.cpp | 40 ------------------- 2 files changed, 48 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/utils.hpp index a36eaabcf928c..d5888ab6a79d1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/utils.hpp @@ -29,16 +29,8 @@ using tier4_planning_msgs::msg::PathWithLaneId; void setOrientation(PathWithLaneId * path); -bool getStartAvoidPose( - const PathWithLaneId & path, const double start_distance, const size_t nearest_idx, - Pose * start_avoid_pose); - bool isAlmostZero(double v); -Point transformToGrid( - const Point & pt, const double longitudinal_offset, const double lateral_offset, const double yaw, - const TransformStamped & geom_tf); - } // namespace autoware::behavior_path_planner #endif // AUTOWARE__BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/utils.cpp index 60fc5f74a3e1a..f30895c10d64d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/utils.cpp @@ -53,49 +53,9 @@ void setOrientation(PathWithLaneId * path) } } -bool getStartAvoidPose( - const PathWithLaneId & path, const double start_distance, const size_t nearest_idx, - Pose * start_avoid_pose) -{ - if (!start_avoid_pose) { - return false; - } - if (nearest_idx >= path.points.size()) { - return false; - } - - double arclength = 0.0; - for (size_t idx = nearest_idx + 1; idx < path.points.size(); ++idx) { - const auto pt = path.points.at(idx).point; - const auto pt_prev = path.points.at(idx - 1).point; - const double dx = pt.pose.position.x - pt_prev.pose.position.x; - const double dy = pt.pose.position.y - pt_prev.pose.position.y; - arclength += std::hypot(dx, dy); - - if (arclength > start_distance) { - *start_avoid_pose = pt.pose; - return true; - } - } - - return false; -} - bool isAlmostZero(double v) { return std::fabs(v) < 1.0e-4; } -Point transformToGrid( - const Point & pt, const double longitudinal_offset, const double lateral_offset, const double yaw, - const TransformStamped & geom_tf) -{ - Point offset_pt, grid_pt; - offset_pt = pt; - offset_pt.x += longitudinal_offset * cos(yaw) - lateral_offset * sin(yaw); - offset_pt.y += longitudinal_offset * sin(yaw) + lateral_offset * cos(yaw); - tf2::doTransform(offset_pt, grid_pt, geom_tf); - return grid_pt; -} - } // namespace autoware::behavior_path_planner