From f83e4897d154e1b5c34fcf59bdc6e848969c9c9c Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 25 Jul 2022 07:17:42 +0900 Subject: [PATCH] fix(behavior_velocity_planner): remove unused utils functions (#1383) * fix(behavior_velocity_planner): remove unused util functions Signed-off-by: satoshi-ota * fix(behavior_velocity_planner): use alias Signed-off-by: satoshi-ota * fix(motion_utils): remove path_with_lane_id.hpp Signed-off-by: satoshi-ota --- .../include/motion_utils/motion_utils.hpp | 1 - .../trajectory/path_with_lane_id.hpp | 340 ------- .../src/trajectory/test_path_with_lane_id.cpp | 938 ------------------ .../include/utilization/util.hpp | 111 +-- .../src/utilization/util.cpp | 88 +- 5 files changed, 88 insertions(+), 1390 deletions(-) delete mode 100644 common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp delete mode 100644 common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp diff --git a/common/motion_utils/include/motion_utils/motion_utils.hpp b/common/motion_utils/include/motion_utils/motion_utils.hpp index eb94be5348ff7..6aacd2f3ba8da 100644 --- a/common/motion_utils/include/motion_utils/motion_utils.hpp +++ b/common/motion_utils/include/motion_utils/motion_utils.hpp @@ -16,7 +16,6 @@ #define MOTION_UTILS__MOTION_UTILS_HPP_ #include "motion_utils/marker/marker_helper.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include "motion_utils/trajectory/tmp_conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "motion_utils/vehicle/vehicle_state_checker.hpp" diff --git a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp deleted file mode 100644 index b450a628a4302..0000000000000 --- a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp +++ /dev/null @@ -1,340 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ -#define MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ - -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" - -#include - -#include -#include -#include -#include - -namespace motion_utils -{ -/** - * @brief calculate the point offset from source point along the trajectory (or path) - * @param points points of trajectory, path, ... - * @param src_idx index of source point - * @param offset length of offset from source point - * @return offset point - */ -template <> -inline boost::optional calcLongitudinalOffsetPoint( - const std::vector & points, - const size_t src_idx, const double offset, const bool throw_exception) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; - return {}; - } - - if (points.size() - 1 < src_idx) { - const auto e = std::out_of_range("Invalid source index"); - if (throw_exception) { - throw e; - } - std::cerr << e.what() << std::endl; - return {}; - } - - if (points.size() == 1) { - return {}; - } - - if (src_idx + 1 == points.size() && offset == 0.0) { - return tier4_autoware_utils::getPoint(points.at(src_idx)); - } - - if (offset < 0.0) { - auto reverse_points = points; - std::reverse(reverse_points.begin(), reverse_points.end()); - return calcLongitudinalOffsetPoint( - reverse_points, reverse_points.size() - src_idx - 1, -offset); - } - - double dist_sum = 0.0; - - for (size_t i = src_idx; i < points.size() - 1; ++i) { - const auto & p_front = points.at(i); - const auto & p_back = points.at(i + 1); - - const auto dist_segment = tier4_autoware_utils::calcDistance2d(p_front, p_back); - dist_sum += dist_segment; - - const auto dist_res = offset - dist_sum; - if (dist_res <= 0.0) { - return tier4_autoware_utils::calcInterpolatedPoint( - p_back, p_front, std::abs(dist_res / dist_segment)); - } - } - - // not found (out of range) - return {}; -} - -/** - * @brief calculate the point offset from source point along the trajectory (or path) - * @param points points of trajectory, path, ... - * @param src_point source point - * @param offset length of offset from source point - * @return offset point - */ -template <> -inline boost::optional calcLongitudinalOffsetPoint( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; - return {}; - } - - if (offset < 0.0) { - auto reverse_points = points; - std::reverse(reverse_points.begin(), reverse_points.end()); - return calcLongitudinalOffsetPoint(reverse_points, src_point, -offset); - } - - const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); - const double signed_length_src_offset = - calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); - - return calcLongitudinalOffsetPoint(points, src_seg_idx, offset + signed_length_src_offset); -} - -/** - * @brief calculate the point offset from source point along the trajectory (or path) - * @param points points of trajectory, path, ... - * @param src_idx index of source point - * @param offset length of offset from source point - * @return offset pose - */ -template <> -inline boost::optional calcLongitudinalOffsetPose( - const std::vector & points, - const size_t src_idx, const double offset, const bool throw_exception) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; - return {}; - } - - if (points.size() - 1 < src_idx) { - const auto e = std::out_of_range("Invalid source index"); - if (throw_exception) { - throw e; - } - std::cerr << e.what() << std::endl; - return {}; - } - - if (points.size() == 1) { - return {}; - } - - if (src_idx + 1 == points.size() && offset == 0.0) { - return tier4_autoware_utils::getPose(points.at(src_idx)); - } - - if (offset < 0.0) { - auto reverse_points = points; - std::reverse(reverse_points.begin(), reverse_points.end()); - - double dist_sum = 0.0; - - for (size_t i = reverse_points.size() - src_idx - 1; i < reverse_points.size() - 1; ++i) { - const auto & p_front = reverse_points.at(i); - const auto & p_back = reverse_points.at(i + 1); - - const auto dist_segment = tier4_autoware_utils::calcDistance2d(p_front, p_back); - dist_sum += dist_segment; - - const auto dist_res = -offset - dist_sum; - if (dist_res <= 0.0) { - return tier4_autoware_utils::calcInterpolatedPose( - p_back, p_front, std::abs(dist_res / dist_segment)); - } - } - } else { - double dist_sum = 0.0; - - for (size_t i = src_idx; i < points.size() - 1; ++i) { - const auto & p_front = points.at(i); - const auto & p_back = points.at(i + 1); - - const auto dist_segment = tier4_autoware_utils::calcDistance2d(p_front, p_back); - dist_sum += dist_segment; - - const auto dist_res = offset - dist_sum; - if (dist_res <= 0.0) { - return tier4_autoware_utils::calcInterpolatedPose( - p_front, p_back, 1.0 - std::abs(dist_res / dist_segment)); - } - } - } - - // not found (out of range) - return {}; -} - -/** - * @brief calculate the point offset from source point along the trajectory (or path) - * @param points points of trajectory, path, ... - * @param src_point source point - * @param offset length of offset from source point - * @return offset pase - */ -template <> -inline boost::optional calcLongitudinalOffsetPose( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; - return {}; - } - - const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); - const double signed_length_src_offset = - calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); - - return calcLongitudinalOffsetPose(points, src_seg_idx, offset + signed_length_src_offset); -} - -/** - * @brief calculate the point offset from source point along the trajectory (or path) - * @param seg_idx segment index of point at beginning of length - * @param p_target point to be inserted - * @param points output points of trajectory, path, ... - * @return index of insert point - */ -template <> -inline boost::optional insertTargetPoint( - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; - return {}; - } - - // invalid segment index - if (seg_idx + 1 >= points.size()) { - return {}; - } - - const auto p_front = tier4_autoware_utils::getPoint(points.at(seg_idx)); - const auto p_back = tier4_autoware_utils::getPoint(points.at(seg_idx + 1)); - - try { - validateNonSharpAngle(p_front, p_target, p_back); - } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; - return {}; - } - - const auto overlap_with_front = - tier4_autoware_utils::calcDistance2d(p_target, p_front) < overlap_threshold; - const auto overlap_with_back = - tier4_autoware_utils::calcDistance2d(p_target, p_back) < overlap_threshold; - - geometry_msgs::msg::Pose target_pose; - { - const auto pitch = tier4_autoware_utils::calcElevationAngle(p_target, p_back); - const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_target, p_back); - - target_pose.position = p_target; - target_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); - } - - auto p_insert = points.at(seg_idx); - tier4_autoware_utils::setPose(target_pose, p_insert); - - geometry_msgs::msg::Pose front_pose; - { - const auto pitch = tier4_autoware_utils::calcElevationAngle(p_front, p_target); - const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_front, p_target); - - front_pose.position = tier4_autoware_utils::getPoint(points.at(seg_idx)); - front_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); - } - - if (!overlap_with_front && !overlap_with_back) { - tier4_autoware_utils::setPose(front_pose, points.at(seg_idx)); - points.insert(points.begin() + seg_idx + 1, p_insert); - return seg_idx + 1; - } - - if (overlap_with_back) { - return seg_idx + 1; - } - - return seg_idx; -} - -/** - * @brief calculate the point offset from source point along the trajectory (or path) - * @param insert_point_length length to insert point from the beginning of the points - * @param p_target point to be inserted - * @param points output points of trajectory, path, ... - * @return index of insert point - */ -template <> -inline boost::optional insertTargetPoint( - const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold) -{ - validateNonEmpty(points); - - if (insert_point_length < 0.0) { - return boost::none; - } - - // Get Nearest segment index - boost::optional segment_idx = boost::none; - for (size_t i = 1; i < points.size(); ++i) { - const double length = calcSignedArcLength(points, 0, i); - if (insert_point_length <= length) { - segment_idx = i - 1; - break; - } - } - - if (!segment_idx) { - return boost::none; - } - - return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); -} -} // namespace motion_utils - -#endif // MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ diff --git a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp deleted file mode 100644 index 477c24408efc8..0000000000000 --- a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp +++ /dev/null @@ -1,938 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "motion_utils/trajectory/path_with_lane_id.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" - -#include -#include - -#include -#include - -namespace -{ -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::createQuaternionFromRPY; -using tier4_autoware_utils::transformPoint; - -constexpr double epsilon = 1e-6; - -geometry_msgs::msg::Pose createPose( - double x, double y, double z, double roll, double pitch, double yaw) -{ - geometry_msgs::msg::Pose p; - p.position = createPoint(x, y, z); - p.orientation = createQuaternionFromRPY(roll, pitch, yaw); - return p; -} - -PathWithLaneId generateTestTrajectory( - const size_t num_points, const double point_interval, const double vel = 0.0, - const double init_theta = 0.0, const double delta_theta = 0.0) -{ - PathWithLaneId path_with_lane_id; - for (size_t i = 0; i < num_points; ++i) { - const double theta = init_theta + i * delta_theta; - const double x = i * point_interval * std::cos(theta); - const double y = i * point_interval * std::sin(theta); - - PathPointWithLaneId p; - p.point.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); - p.point.longitudinal_velocity_mps = vel; - path_with_lane_id.points.push_back(p); - } - - return path_with_lane_id; -} -} // namespace - -TEST(trajectory, calcLongitudinalOffsetPointFromIndex_PathWithLaneId) -{ - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPoint; - using motion_utils::calcSignedArcLength; - using tier4_autoware_utils::getPoint; - - const auto traj = generateTestTrajectory(10, 1.0); - const auto total_length = calcArcLength(traj.points); - - // Empty - EXPECT_FALSE(calcLongitudinalOffsetPoint(PathWithLaneId{}.points, {}, {})); - - // Out of range - EXPECT_FALSE(calcLongitudinalOffsetPoint(traj.points, traj.points.size() + 1, 1.0)); - EXPECT_FALSE(calcLongitudinalOffsetPoint(traj.points, -1, 1.0)); - - // Found Pose(forward) - for (size_t i = 0; i < traj.points.size(); ++i) { - double x_ans = getPoint(traj.points.at(i)).x; - - const auto d_back = calcSignedArcLength(traj.points, i, traj.points.size() - 1); - - for (double len = 0.0; len < d_back + epsilon; len += 0.1) { - const auto p_out = calcLongitudinalOffsetPoint(traj.points, i, std::min(len, d_back)); - - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().z, 0.0, epsilon); - - x_ans += 0.1; - } - } - - // Found Pose(backward) - for (size_t i = 0; i < traj.points.size(); ++i) { - double x_ans = getPoint(traj.points.at(i)).x; - - const auto d_front = calcSignedArcLength(traj.points, i, 0); - - for (double len = 0.0; d_front - epsilon < len; len -= 0.1) { - const auto p_out = calcLongitudinalOffsetPoint(traj.points, i, std::max(len, d_front)); - - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().z, 0.0, epsilon); - - x_ans -= 0.1; - } - } - - // No found - { - const auto p_out = calcLongitudinalOffsetPoint(traj.points, 0, total_length + epsilon); - - EXPECT_EQ(p_out, boost::none); - } - - // No found - { - const auto p_out = calcLongitudinalOffsetPoint(traj.points, 9, -total_length - epsilon); - - EXPECT_EQ(p_out, boost::none); - } - - // No found(Trajectory size is 1) - { - const auto one_point_traj = generateTestTrajectory(1, 1.0); - const auto p_out = calcLongitudinalOffsetPoint(one_point_traj.points, 0.0, 0.0); - - EXPECT_EQ(p_out, boost::none); - } -} - -TEST(trajectory, calcLongitudinalOffsetPointFromPoint_PathWithLaneId) -{ - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPoint; - using motion_utils::calcSignedArcLength; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::getPoint; - - const auto traj = generateTestTrajectory(10, 1.0); - const auto total_length = calcArcLength(traj.points); - - // Empty - EXPECT_FALSE(calcLongitudinalOffsetPoint(PathWithLaneId{}.points, {}, {})); - - // Found Pose(forward) - for (double x_start = 0.0; x_start < total_length + epsilon; x_start += 0.1) { - constexpr double lateral_deviation = 0.5; - double x_ans = x_start; - - const auto p_src = createPoint(x_start, lateral_deviation, 0.0); - const auto d_back = calcSignedArcLength(traj.points, p_src, traj.points.size() - 1); - - for (double len = 0.0; len < d_back + epsilon; len += 0.1) { - const auto p_out = calcLongitudinalOffsetPoint(traj.points, p_src, std::min(len, d_back)); - - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().z, 0.0, epsilon); - - x_ans += 0.1; - } - } - - // Found Pose(backward) - for (double x_start = 0.0; x_start < total_length + epsilon; x_start += 0.1) { - constexpr double lateral_deviation = 0.5; - double x_ans = x_start; - - const auto p_src = createPoint(x_start, lateral_deviation, 0.0); - const auto d_front = calcSignedArcLength(traj.points, p_src, 0); - - for (double len = 0.0; d_front - epsilon < len; len -= 0.1) { - const auto p_out = calcLongitudinalOffsetPoint(traj.points, p_src, std::max(len, d_front)); - - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().z, 0.0, epsilon); - - x_ans -= 0.1; - } - } - - // No found - { - const auto p_src = createPoint(0.0, 0.0, 0.0); - const auto p_out = calcLongitudinalOffsetPoint(traj.points, p_src, total_length + 1.0); - - EXPECT_EQ(p_out, boost::none); - } - - // No found - { - const auto p_src = createPoint(9.0, 0.0, 0.0); - const auto p_out = calcLongitudinalOffsetPoint(traj.points, p_src, -total_length - 1.0); - - EXPECT_EQ(p_out, boost::none); - } - - // Out of range(Trajectory size is 1) - { - const auto one_point_traj = generateTestTrajectory(1, 1.0); - EXPECT_FALSE( - calcLongitudinalOffsetPoint(one_point_traj.points, geometry_msgs::msg::Point{}, {})); - } -} - -TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_PathWithLaneId) -{ - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPose; - using motion_utils::calcSignedArcLength; - using tier4_autoware_utils::getPoint; - - const auto traj = generateTestTrajectory(10, 1.0); - const auto total_length = calcArcLength(traj.points); - - // Empty - EXPECT_FALSE(calcLongitudinalOffsetPose(PathWithLaneId{}.points, {}, {})); - - // Out of range - EXPECT_FALSE(calcLongitudinalOffsetPose(traj.points, traj.points.size() + 1, 1.0)); - EXPECT_FALSE(calcLongitudinalOffsetPose(traj.points, -1, 1.0)); - - // Found Pose(forward) - for (size_t i = 0; i < traj.points.size(); ++i) { - double x_ans = getPoint(traj.points.at(i)).x; - - const auto d_back = calcSignedArcLength(traj.points, i, traj.points.size() - 1); - - for (double len = 0.0; len < d_back + epsilon; len += 0.1) { - const auto p_out = calcLongitudinalOffsetPose(traj.points, i, std::min(len, d_back)); - - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().position.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon); - - x_ans += 0.1; - } - } - - // Found Pose(backward) - for (size_t i = 0; i < traj.points.size(); ++i) { - double x_ans = getPoint(traj.points.at(i)).x; - - const auto d_front = calcSignedArcLength(traj.points, i, 0); - - for (double len = 0.0; d_front - epsilon < len; len -= 0.1) { - const auto p_out = calcLongitudinalOffsetPose(traj.points, i, std::max(len, d_front)); - - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().position.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon); - - x_ans -= 0.1; - } - } - - // No found - { - const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, total_length + epsilon); - - EXPECT_EQ(p_out, boost::none); - } - - // No found - { - const auto p_out = calcLongitudinalOffsetPose(traj.points, 9, -total_length - epsilon); - - EXPECT_EQ(p_out, boost::none); - } - - // No found(Trajectory size is 1) - { - const auto one_point_traj = generateTestTrajectory(1, 1.0); - const auto p_out = calcLongitudinalOffsetPose(one_point_traj.points, 0.0, 0.0); - - EXPECT_EQ(p_out, boost::none); - } -} - -TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_PathWithLaneId) -{ - using motion_utils::calcArcLength; - using motion_utils::calcLongitudinalOffsetPose; - using motion_utils::calcSignedArcLength; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::getPoint; - - const auto traj = generateTestTrajectory(10, 1.0); - const auto total_length = calcArcLength(traj.points); - - // Empty - EXPECT_FALSE(calcLongitudinalOffsetPose(PathWithLaneId{}.points, {}, {})); - - // Found Pose(forward) - for (double x_start = 0.0; x_start < total_length + epsilon; x_start += 0.1) { - constexpr double lateral_deviation = 0.5; - double x_ans = x_start; - - const auto p_src = createPoint(x_start, lateral_deviation, 0.0); - const auto d_back = calcSignedArcLength(traj.points, p_src, traj.points.size() - 1); - - for (double len = 0.0; len < d_back + epsilon; len += 0.1) { - const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, std::min(len, d_back)); - - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().position.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon); - - x_ans += 0.1; - } - } - - // Found Pose(backward) - for (double x_start = 0.0; x_start < total_length + epsilon; x_start += 0.1) { - constexpr double lateral_deviation = 0.5; - double x_ans = x_start; - - const auto p_src = createPoint(x_start, lateral_deviation, 0.0); - const auto d_front = calcSignedArcLength(traj.points, p_src, 0); - - for (double len = 0.0; d_front - epsilon < len; len -= 0.1) { - const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, std::max(len, d_front)); - - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().position.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon); - - x_ans -= 0.1; - } - } - - // No found - { - const auto p_src = createPoint(0.0, 0.0, 0.0); - const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, total_length + 1.0); - - EXPECT_EQ(p_out, boost::none); - } - - // No found - { - const auto p_src = createPoint(9.0, 0.0, 0.0); - const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, -total_length - 1.0); - - EXPECT_EQ(p_out, boost::none); - } - - // Out of range(Trajectory size is 1) - { - const auto one_point_traj = generateTestTrajectory(1, 1.0); - EXPECT_FALSE( - calcLongitudinalOffsetPose(one_point_traj.points, geometry_msgs::msg::Point{}, {})); - } -} - -TEST(trajectory, insertTargetPoint_PathWithLaneId) -{ - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertTargetPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; - - const auto traj = generateTestTrajectory(10, 1.0); - const auto total_length = calcArcLength(traj.points); - - // Insert - for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = createPoint(x_start, 0.0, 0.0); - const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); - const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_EQ(p_insert.position.x, p_target.x); - EXPECT_EQ(p_insert.position.y, p_target.y); - EXPECT_EQ(p_insert.position.z, p_target.z); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Insert(Boundary condition) - for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = createPoint(x_start + 1.1e-3, 0.0, 0.0); - const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); - const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_EQ(p_insert.position.x, p_target.x); - EXPECT_EQ(p_insert.position.y, p_target.y); - EXPECT_EQ(p_insert.position.z, p_target.z); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Insert(Quaternion interpolation) - for (double x_start = 0.25; x_start < total_length; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = createPoint(x_start, 0.25 * std::tan(deg2rad(60.0)), 0.0); - const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); - const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(-30.0)); - EXPECT_EQ(p_insert.position.x, p_target.x); - EXPECT_EQ(p_insert.position.y, p_target.y); - EXPECT_EQ(p_insert.position.z, p_target.z); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(60.0)); - EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Not insert(Overlap base_idx point) - for (double x_start = 0.0; x_start < total_length - 1.0 + epsilon; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = createPoint(x_start + 1e-4, 0.0, 0.0); - const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); - const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - } - - // Not insert(Overlap base_idx + 1 point) - for (double x_start = 1.0; x_start < total_length + epsilon; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = createPoint(x_start - 1e-4, 0.0, 0.0); - const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); - const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - } - - // Invalid target point(In front of begin point) - { - testing::internal::CaptureStderr(); - auto traj_out = traj; - - const auto p_target = createPoint(-1.0, 0.0, 0.0); - const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); - const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); - } - - // Invalid target point(Behind of end point) - { - testing::internal::CaptureStderr(); - auto traj_out = traj; - - const auto p_target = createPoint(10.0, 0.0, 0.0); - const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); - const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); - } - - // Invalid target point(Huge lateral offset) - { - testing::internal::CaptureStderr(); - auto traj_out = traj; - - const auto p_target = createPoint(4.0, 10.0, 0.0); - const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); - const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); - } - - // Invalid base index - { - auto traj_out = traj; - - const auto p_target = createPoint(10.0, 0.0, 0.0); - const size_t segment_idx = 9U; - const auto insert_idx = insertTargetPoint(segment_idx, p_target, traj_out.points); - - EXPECT_EQ(insert_idx, boost::none); - } - - // Empty - { - auto empty_traj = generateTestTrajectory(0, 1.0); - const size_t segment_idx = 0; - EXPECT_FALSE(insertTargetPoint(segment_idx, geometry_msgs::msg::Point{}, empty_traj.points)); - } -} - -TEST(trajectory, insertTargetPoint_OverlapThreshold_PathWithLaneId) -{ - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertTargetPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; - - constexpr double overlap_threshold = 1e-4; - const auto traj = generateTestTrajectory(10, 1.0); - const auto total_length = calcArcLength(traj.points); - - // Insert(Boundary condition) - for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = createPoint(x_start + 1.1e-4, 0.0, 0.0); - const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); - const auto insert_idx = - insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold); - - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE( - calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold); - } - - { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_EQ(p_insert.position.x, p_target.x); - EXPECT_EQ(p_insert.position.y, p_target.y); - EXPECT_EQ(p_insert.position.z, p_target.z); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Not insert(Overlap base_idx point) - for (double x_start = 0.0; x_start < total_length - 1.0 + epsilon; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = createPoint(x_start + 1e-5, 0.0, 0.0); - const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); - const auto insert_idx = - insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold); - - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE( - calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold); - } - } - - // Not insert(Overlap base_idx + 1 point) - for (double x_start = 1.0; x_start < total_length + epsilon; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = createPoint(x_start - 1e-5, 0.0, 0.0); - const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); - const auto insert_idx = - insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold); - - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE( - calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold); - } - } -} - -TEST(trajectory, insertTargetPoint_PathWithLaneId_Length) -{ - using motion_utils::calcArcLength; - using motion_utils::findNearestSegmentIndex; - using motion_utils::insertTargetPoint; - using tier4_autoware_utils::calcDistance2d; - using tier4_autoware_utils::createPoint; - using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::getPose; - - const auto traj = generateTestTrajectory(10, 1.0); - const auto total_length = calcArcLength(traj.points); - - // Insert - for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = createPoint(x_start, 0.0, 0.0); - const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); - const auto insert_idx = insertTargetPoint(x_start, p_target, traj_out.points); - - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_EQ(p_insert.position.x, p_target.x); - EXPECT_EQ(p_insert.position.y, p_target.y); - EXPECT_EQ(p_insert.position.z, p_target.z); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Insert(Boundary condition) - for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = createPoint(x_start + 1.1e-3, 0.0, 0.0); - const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); - const auto insert_idx = insertTargetPoint(x_start + 1.1e-3, p_target, traj_out.points); - - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_EQ(p_insert.position.x, p_target.x); - EXPECT_EQ(p_insert.position.y, p_target.y); - EXPECT_EQ(p_insert.position.z, p_target.z); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Right on the terminal point - { - auto traj_out = traj; - - const auto p_target = createPoint(9.0, 0.0, 0.0); - const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); - const auto insert_idx = insertTargetPoint(9.0, p_target, traj_out.points); - - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_EQ(p_insert.position.x, p_target.x); - EXPECT_EQ(p_insert.position.y, p_target.y); - EXPECT_EQ(p_insert.position.z, p_target.z); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Insert(Quaternion interpolation) - for (double x_start = 0.25; x_start < total_length; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = createPoint(x_start, 0.25 * std::tan(deg2rad(60.0)), 0.0); - const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); - const auto insert_idx = insertTargetPoint(x_start, p_target, traj_out.points); - - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(-30.0)); - EXPECT_EQ(p_insert.position.x, p_target.x); - EXPECT_EQ(p_insert.position.y, p_target.y); - EXPECT_EQ(p_insert.position.z, p_target.z); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(60.0)); - EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Not insert(Overlap base_idx point) - for (double x_start = 0.0; x_start < total_length - 1.0 + epsilon; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = createPoint(x_start + 1e-4, 0.0, 0.0); - const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); - const auto insert_idx = insertTargetPoint(x_start + 1e-4, p_target, traj_out.points); - - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - } - - // Not insert(Overlap base_idx + 1 point) - for (double x_start = 1.0; x_start < total_length + epsilon; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = createPoint(x_start - 1e-4, 0.0, 0.0); - const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); - const auto insert_idx = insertTargetPoint(x_start - 1e-4, p_target, traj_out.points); - - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - } - - // Invalid target point(In front of the beginning point) - { - auto traj_out = traj; - - const auto p_target = createPoint(-1.0, 0.0, 0.0); - const auto insert_idx = insertTargetPoint(-1.0, p_target, traj_out.points); - - EXPECT_EQ(insert_idx, boost::none); - } - - // Invalid target point(Behind the end point) - { - auto traj_out = traj; - - const auto p_target = createPoint(10.0, 0.0, 0.0); - const auto insert_idx = insertTargetPoint(10.0, p_target, traj_out.points); - - EXPECT_EQ(insert_idx, boost::none); - } - - // Invalid target point(Huge lateral offset) - { - testing::internal::CaptureStderr(); - auto traj_out = traj; - - const auto p_target = createPoint(4.0, 10.0, 0.0); - const auto insert_idx = insertTargetPoint(4.0, p_target, traj_out.points); - - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); - } - - // Empty - { - auto empty_traj = generateTestTrajectory(0, 1.0); - EXPECT_THROW( - insertTargetPoint(0.0, geometry_msgs::msg::Point{}, empty_traj.points), - std::invalid_argument); - } -} diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp index 2b29474ce5c2d..78ca80f15d12a 100644 --- a/planning/behavior_velocity_planner/include/utilization/util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/util.hpp @@ -76,47 +76,43 @@ struct PointWithSearchRangeIndex SearchRangeIndex index; }; +using BasicPolygons2d = std::vector; +using Polygons2d = std::vector; using Point2d = boost::geometry::model::d2::point_xy; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::Shape; +using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathPoint; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; -using BasicPolygons2d = std::vector; -using Polygons2d = std::vector; +using autoware_auto_planning_msgs::msg::Trajectory; +using motion_utils::calcLongitudinalOffsetToSegment; +using motion_utils::calcSignedArcLength; +using motion_utils::findNearestIndex; +using motion_utils::findNearestSegmentIndex; +using motion_utils::validateNonEmpty; +using tier4_autoware_utils::calcAzimuthAngle; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::calcSquaredDistance2d; +using tier4_autoware_utils::createQuaternionFromYaw; +using tier4_autoware_utils::getPoint; +using tier4_planning_msgs::msg::StopFactor; +using tier4_planning_msgs::msg::StopReason; + namespace planning_utils { using geometry_msgs::msg::Pose; -inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::Point & p) { return p; } -inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::Pose & p) { return p.position; } -inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::PoseStamped & p) -{ - return p.pose.position; -} -inline geometry_msgs::msg::Point getPoint(const autoware_auto_planning_msgs::msg::PathPoint & p) -{ - return p.pose.position; -} -inline geometry_msgs::msg::Point getPoint( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) -{ - return p.point.pose.position; -} -inline geometry_msgs::msg::Point getPoint( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) -{ - return p.pose.position; -} -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_planning_msgs::msg::Path & path, int idx) +inline geometry_msgs::msg::Pose getPose(const Path & path, int idx) { return path.points.at(idx).pose; } -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, int idx) +inline geometry_msgs::msg::Pose getPose(const PathWithLaneId & path, int idx) { return path.points.at(idx).point.pose; } -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_planning_msgs::msg::Trajectory & traj, int idx) +inline geometry_msgs::msg::Pose getPose(const Trajectory & traj, int idx) { return traj.points.at(idx).pose; } @@ -166,8 +162,7 @@ geometry_msgs::msg::Pose transformRelCoordinate2D( const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin); geometry_msgs::msg::Pose transformAbsCoordinate2D( const geometry_msgs::msg::Pose & relative, const geometry_msgs::msg::Pose & origin); -SearchRangeIndex getPathIndexRangeIncludeLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t lane_id); +SearchRangeIndex getPathIndexRangeIncludeLaneId(const PathWithLaneId & path, const int64_t lane_id); /** * @brief find nearest segment index with search range */ @@ -177,13 +172,13 @@ size_t findNearestSegmentIndex(const T & points, const PointWithSearchRangeIndex const auto & index = point_with_index.index; const auto point = point_with_index.point; - motion_utils::validateNonEmpty(points); + validateNonEmpty(points); double min_dist = std::numeric_limits::max(); size_t nearest_idx = 0; for (size_t i = index.min_idx; i <= index.max_idx; ++i) { - const auto dist = tier4_autoware_utils::calcSquaredDistance2d(points.at(i), point); + const auto dist = calcSquaredDistance2d(points.at(i), point); if (dist < min_dist) { min_dist = dist; nearest_idx = i; @@ -197,8 +192,7 @@ size_t findNearestSegmentIndex(const T & points, const PointWithSearchRangeIndex return points.size() - 2; } - const double signed_length = - motion_utils::calcLongitudinalOffsetToSegment(points, nearest_idx, point); + const double signed_length = calcLongitudinalOffsetToSegment(points, nearest_idx, point); if (signed_length <= 0) { return nearest_idx - 1; @@ -213,7 +207,7 @@ template PointWithSearchRangeIndex findFirstNearSearchRangeIndex( const T & points, const geometry_msgs::msg::Point & point, const double distance_thresh = 9.0) { - motion_utils::validateNonEmpty(points); + validateNonEmpty(points); bool min_idx_found = false; bool max_idx_found = false; @@ -242,21 +236,20 @@ double calcSignedArcLengthWithSearchIndex( const T & points, const PointWithSearchRangeIndex & src_point_with_range, const PointWithSearchRangeIndex & dst_point_with_range) { - motion_utils::validateNonEmpty(points); + validateNonEmpty(points); const size_t src_idx = planning_utils::findNearestSegmentIndex(points, src_point_with_range); const size_t dst_idx = planning_utils::findNearestSegmentIndex(points, dst_point_with_range); - const double signed_length = motion_utils::calcSignedArcLength(points, src_idx, dst_idx); + const double signed_length = calcSignedArcLength(points, src_idx, dst_idx); const double signed_length_src_offset = - motion_utils::calcLongitudinalOffsetToSegment(points, src_idx, src_point_with_range.point); + calcLongitudinalOffsetToSegment(points, src_idx, src_point_with_range.point); const double signed_length_dst_offset = - motion_utils::calcLongitudinalOffsetToSegment(points, dst_idx, dst_point_with_range.point); + calcLongitudinalOffsetToSegment(points, dst_idx, dst_point_with_range.point); return signed_length - signed_length_src_offset + signed_length_dst_offset; } -Polygon2d toFootprintPolygon(const autoware_auto_perception_msgs::msg::PredictedObject & object); +Polygon2d toFootprintPolygon(const PredictedObject & object); bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin); Polygon2d generatePathPolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t start_idx, - const size_t end_idx, const double width); + const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width); double calcJudgeLineDistWithAccLimit( const double velocity, const double max_stop_acceleration, const double delay_response_time); @@ -273,14 +266,11 @@ double findReachTime( const double jerk, const double accel, const double velocity, const double distance, const double t_min, const double t_max); -tier4_planning_msgs::msg::StopReason initializeStopReason(const std::string & stop_reason); +StopReason initializeStopReason(const std::string & stop_reason); -void appendStopReason( - const tier4_planning_msgs::msg::StopFactor stop_factor, - tier4_planning_msgs::msg::StopReason * stop_reason); +void appendStopReason(const StopFactor stop_factor, StopReason * stop_reason); -std::vector toRosPoints( - const autoware_auto_perception_msgs::msg::PredictedObjects & object); +std::vector toRosPoints(const PredictedObjects & object); geometry_msgs::msg::Point toRosPoint(const pcl::PointXYZ & pcl_point); geometry_msgs::msg::Point toRosPoint(const Point2d & boost_point, const double z); @@ -298,14 +288,13 @@ std::vector concatVector(const std::vector & vec1, const std::vector & } boost::optional getNearestLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose, - boost::optional & nearest_segment_idx); + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose, boost::optional & nearest_segment_idx); template std::unordered_map, lanelet::ConstLanelet> getRegElemMapOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose) + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose) { std::unordered_map, lanelet::ConstLanelet> reg_elem_map_on_path; @@ -342,8 +331,8 @@ std::unordered_map, lanelet::ConstLanelet> get template std::set getRegElemIdSetOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose) + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose) { std::set reg_elem_id_set; for (const auto & m : getRegElemMapOnPath(path, lanelet_map, current_pose)) { @@ -354,8 +343,8 @@ std::set getRegElemIdSetOnPath( template std::set getLaneletIdSetOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose) + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose) { std::set id_set; for (const auto & m : getRegElemMapOnPath(path, lanelet_map, current_pose)) { @@ -365,12 +354,12 @@ std::set getLaneletIdSetOnPath( } std::vector getLaneletsOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose); + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose); std::set getLaneIdSetOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose); + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose); } // namespace planning_utils } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp index e6e0ec7418376..672c5bee55818 100644 --- a/planning/behavior_velocity_planner/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/src/utilization/util.cpp @@ -25,7 +25,6 @@ namespace planning_utils { Point2d calculateOffsetPoint2d(const Pose & pose, const double offset_x, const double offset_y) { - using tier4_autoware_utils::calcOffsetPose; return to_bg2d(calcOffsetPose(pose, offset_x, offset_y, 0.0)); } @@ -39,8 +38,8 @@ PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, con pose.position.x = lerp(pp0.x, pp1.x, ratio); pose.position.y = lerp(pp0.y, pp1.y, ratio); pose.position.z = lerp(pp0.z, pp1.z, ratio); - const double yaw = tier4_autoware_utils::calcAzimuthAngle(pp0, pp1); - pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + const double yaw = calcAzimuthAngle(pp0, pp1); + pose.orientation = createQuaternionFromYaw(yaw); p.pose = pose; const double v = lerp(p0.longitudinal_velocity_mps, p1.longitudinal_velocity_mps, ratio); p.longitudinal_velocity_mps = v; @@ -68,23 +67,22 @@ bool createDetectionAreaPolygons( const size_t max_index = static_cast(path.points.size() - 1); //! avoid bug with same point polygon const double eps = 1e-3; - auto nearest_idx = motion_utils::findNearestIndex(path.points, pose.position); + auto nearest_idx = findNearestIndex(path.points, pose.position); if (max_index == nearest_idx) return false; // case of path point is not enough size auto p0 = path.points.at(nearest_idx).point; auto first_idx = nearest_idx + 1; // use ego point as start point if same point as ego is not in the path const auto dist_to_nearest = - std::fabs(motion_utils::calcSignedArcLength(path.points, pose.position, nearest_idx)); + std::fabs(calcSignedArcLength(path.points, pose.position, nearest_idx)); if (dist_to_nearest > eps) { const auto nearest_seg_idx = motion_utils::findNearestSegmentIndex(path.points, pose.position); // interpolate ego point const auto & pp = path.points; - const double ds = - tier4_autoware_utils::calcDistance2d(pp.at(nearest_seg_idx), pp.at(nearest_seg_idx + 1)); + const double ds = calcDistance2d(pp.at(nearest_seg_idx), pp.at(nearest_seg_idx + 1)); const double dist_to_nearest_seg = - motion_utils::calcSignedArcLength(path.points, nearest_seg_idx, pose.position); + calcSignedArcLength(path.points, nearest_seg_idx, pose.position); const double ratio = dist_to_nearest_seg / ds; p0 = getLerpPathPointWithLaneId( pp.at(nearest_seg_idx).point, pp.at(nearest_seg_idx + 1).point, ratio); @@ -103,7 +101,7 @@ bool createDetectionAreaPolygons( LineString2d right_outer_bound = {calculateOffsetPoint2d(p0.pose, min_len, -min_dst - eps)}; for (size_t s = first_idx; s <= max_index; s++) { const auto p1 = path.points.at(s).point; - const double ds = tier4_autoware_utils::calcDistance2d(p0, p1); + const double ds = calcDistance2d(p0, p1); dist_sum += ds; length += ds; // calculate the distance that obstacles can move until ego reach the trajectory point @@ -170,8 +168,7 @@ void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons } } -SearchRangeIndex getPathIndexRangeIncludeLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t lane_id) +SearchRangeIndex getPathIndexRangeIncludeLaneId(const PathWithLaneId & path, const int64_t lane_id) { /** * @brief find path index range include given lane_id @@ -214,9 +211,7 @@ void insertVelocity( int max_idx = std::min(static_cast(insert_index + 1), static_cast(path.points.size() - 1)); for (int i = min_idx; i <= max_idx; i++) { - if ( - tier4_autoware_utils::calcDistance2d(path.points.at(static_cast(i)), path_point) < - min_distance) { + if (calcDistance2d(path.points.at(static_cast(i)), path_point) < min_distance) { path.points.at(i).point.longitudinal_velocity_mps = 0; already_has_path_point = true; insert_index = static_cast(i); @@ -232,10 +227,10 @@ void insertVelocity( setVelocityFromIndex(insert_index, v, &path); } -Polygon2d toFootprintPolygon(const autoware_auto_perception_msgs::msg::PredictedObject & object) +Polygon2d toFootprintPolygon(const PredictedObject & object) { Polygon2d obj_footprint; - if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + if (object.shape.type == Shape::POLYGON) { obj_footprint = toBoostPoly(object.shape.footprint); } else { // cylinder type is treated as square-polygon @@ -253,8 +248,7 @@ bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg } Polygon2d generatePathPolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t start_idx, - const size_t end_idx, const double width) + const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width) { Polygon2d ego_area; // open polygon for (size_t i = start_idx; i <= end_idx; ++i) { @@ -326,15 +320,15 @@ bool calcClosestIndex( return closest == -1 ? false : true; } -template bool calcClosestIndex( - const autoware_auto_planning_msgs::msg::Trajectory & path, const geometry_msgs::msg::Pose & pose, - int & closest, double dist_thr, double angle_thr); -template bool calcClosestIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Pose & pose, int & closest, double dist_thr, double angle_thr); -template bool calcClosestIndex( - const autoware_auto_planning_msgs::msg::Path & path, const geometry_msgs::msg::Pose & pose, - int & closest, double dist_thr, double angle_thr); +template bool calcClosestIndex( + const Trajectory & path, const geometry_msgs::msg::Pose & pose, int & closest, double dist_thr, + double angle_thr); +template bool calcClosestIndex( + const PathWithLaneId & path, const geometry_msgs::msg::Pose & pose, int & closest, + double dist_thr, double angle_thr); +template bool calcClosestIndex( + const Path & path, const geometry_msgs::msg::Pose & pose, int & closest, double dist_thr, + double angle_thr); template bool calcClosestIndex( @@ -359,15 +353,13 @@ bool calcClosestIndex( return closest == -1 ? false : true; } -template bool calcClosestIndex( - const autoware_auto_planning_msgs::msg::Trajectory & path, - const geometry_msgs::msg::Point & point, int & closest, double dist_thr); -template bool calcClosestIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & point, int & closest, double dist_thr); -template bool calcClosestIndex( - const autoware_auto_planning_msgs::msg::Path & path, const geometry_msgs::msg::Point & point, - int & closest, double dist_thr); +template bool calcClosestIndex( + const Trajectory & path, const geometry_msgs::msg::Point & point, int & closest, double dist_thr); +template bool calcClosestIndex( + const PathWithLaneId & path, const geometry_msgs::msg::Point & point, int & closest, + double dist_thr); +template bool calcClosestIndex( + const Path & path, const geometry_msgs::msg::Point & point, int & closest, double dist_thr); geometry_msgs::msg::Pose transformRelCoordinate2D( const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin) @@ -539,22 +531,19 @@ double calcDecelerationVelocityFromDistanceToTarget( return current_velocity; } -tier4_planning_msgs::msg::StopReason initializeStopReason(const std::string & stop_reason) +StopReason initializeStopReason(const std::string & stop_reason) { - tier4_planning_msgs::msg::StopReason stop_reason_msg; + StopReason stop_reason_msg; stop_reason_msg.reason = stop_reason; return stop_reason_msg; } -void appendStopReason( - const tier4_planning_msgs::msg::StopFactor stop_factor, - tier4_planning_msgs::msg::StopReason * stop_reason) +void appendStopReason(const StopFactor stop_factor, StopReason * stop_reason) { stop_reason->stop_factors.emplace_back(stop_factor); } -std::vector toRosPoints( - const autoware_auto_perception_msgs::msg::PredictedObjects & object) +std::vector toRosPoints(const PredictedObjects & object) { std::vector points; for (const auto & obj : object.objects) { @@ -593,9 +582,8 @@ LineString2d extendLine( } boost::optional getNearestLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose, - boost::optional & nearest_segment_idx) + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose, boost::optional & nearest_segment_idx) { boost::optional nearest_lane_id; @@ -632,8 +620,8 @@ boost::optional getNearestLaneId( } std::vector getLaneletsOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose) + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose) { boost::optional nearest_segment_idx; const auto nearest_lane_id = @@ -663,8 +651,8 @@ std::vector getLaneletsOnPath( } std::set getLaneIdSetOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose) + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose) { std::set lane_id_set; for (const auto & lane : getLaneletsOnPath(path, lanelet_map, current_pose)) {