diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp index 3688b8cd243fa..1911eb8441894 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp @@ -114,6 +114,39 @@ inline geometry_msgs::msg::Pose getPose(const autoware_auto_planning_msgs::msg:: return p.pose; } +template +void setPose(const geometry_msgs::msg::Pose & pose, [[maybe_unused]] T & p) +{ + static_assert(sizeof(T) == 0, "Only specializations of getPose can be used."); + throw std::logic_error("Only specializations of getPose can be used."); +} + +template <> +inline void setPose(const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Pose & p) +{ + p = pose; +} + +template <> +inline void setPose(const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::PoseStamped & p) +{ + p.pose = pose; +} + +template <> +inline void setPose( + const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::PathPoint & p) +{ + p.pose = pose; +} + +template <> +inline void setPose( + const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::TrajectoryPoint & p) +{ + p.pose = pose; +} + inline geometry_msgs::msg::Point createPoint(const double x, const double y, const double z) { geometry_msgs::msg::Point p; diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp new file mode 100644 index 0000000000000..019e88e4ce1a2 --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp @@ -0,0 +1,46 @@ +// 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 TIER4_AUTOWARE_UTILS__GEOMETRY__PATH_WITH_LANE_ID_GEOMETRY_HPP_ +#define TIER4_AUTOWARE_UTILS__GEOMETRY__PATH_WITH_LANE_ID_GEOMETRY_HPP_ + +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + +namespace tier4_autoware_utils +{ +template <> +inline geometry_msgs::msg::Point getPoint( + const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +{ + return p.point.pose.position; +} + +template <> +inline geometry_msgs::msg::Pose getPose( + const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +{ + return p.point.pose; +} + +template <> +inline void setPose( + const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +{ + p.point.pose = pose; +} +} // namespace tier4_autoware_utils + +#endif // TIER4_AUTOWARE_UTILS__GEOMETRY__PATH_WITH_LANE_ID_GEOMETRY_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp index d12c906a0203e..cd84d80356e55 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp @@ -17,6 +17,7 @@ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" #include "tier4_autoware_utils/geometry/pose_deviation.hpp" #include "tier4_autoware_utils/math/constants.hpp" #include "tier4_autoware_utils/math/normalization.hpp" @@ -32,6 +33,7 @@ #include "tier4_autoware_utils/ros/update_param.hpp" #include "tier4_autoware_utils/ros/wait_for_param.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" +#include "tier4_autoware_utils/trajectory/path_with_lane_id.hpp" #include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/vehicle/vehicle_state_checker.hpp" diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.hpp new file mode 100644 index 0000000000000..7b7feefd364ba --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.hpp @@ -0,0 +1,258 @@ +// 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 TIER4_AUTOWARE_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ +#define TIER4_AUTOWARE_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ + +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" +#include "tier4_autoware_utils/trajectory/trajectory.hpp" + +#include + +#include +#include +#include +#include + +namespace tier4_autoware_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) +{ + validateNonEmpty(points); + + if (points.size() - 1 < src_idx) { + throw std::out_of_range("Invalid source index"); + } + + if (points.size() == 1) { + return {}; + } + + if (src_idx + 1 == points.size() && offset == 0.0) { + return 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 = calcDistance2d(p_front, p_back); + dist_sum += dist_segment; + + const auto dist_res = offset - dist_sum; + if (dist_res <= 0.0) { + return 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) +{ + validateNonEmpty(points); + + 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) +{ + validateNonEmpty(points); + + if (points.size() - 1 < src_idx) { + throw std::out_of_range("Invalid source index"); + } + + if (points.size() == 1) { + return {}; + } + + if (src_idx + 1 == points.size() && offset == 0.0) { + return 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 = calcDistance2d(p_front, p_back); + dist_sum += dist_segment; + + const auto dist_res = -offset - dist_sum; + if (dist_res <= 0.0) { + return 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 = calcDistance2d(p_front, p_back); + dist_sum += dist_segment; + + const auto dist_res = offset - dist_sum; + if (dist_res <= 0.0) { + return 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) +{ + validateNonEmpty(points); + + 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 size_t insertTargetPoint( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold) +{ + validateNonEmpty(points); + + const auto p_front = getPoint(points.at(seg_idx)); + const auto p_back = 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; + } + + const auto overlap_with_front = calcDistance2d(p_target, p_front) < overlap_threshold; + const auto overlap_with_back = calcDistance2d(p_target, p_back) < overlap_threshold; + + geometry_msgs::msg::Pose target_pose; + { + const auto pitch = calcElevationAngle(p_target, p_back); + const auto yaw = calcAzimuthAngle(p_target, p_back); + + target_pose.position = p_target; + target_pose.orientation = createQuaternionFromRPY(0.0, pitch, yaw); + } + + auto p_insert = points.at(seg_idx); + setPose(target_pose, p_insert); + + geometry_msgs::msg::Pose front_pose; + { + const auto pitch = calcElevationAngle(p_front, p_target); + const auto yaw = calcAzimuthAngle(p_front, p_target); + + front_pose.position = getPoint(points.at(seg_idx)); + front_pose.orientation = createQuaternionFromRPY(0.0, pitch, yaw); + } + + if (!overlap_with_front && !overlap_with_back) { + 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; +} +} // namespace tier4_autoware_utils + +#endif // TIER4_AUTOWARE_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp index e5372c47c8997..316b171928d2c 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -17,12 +17,14 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/geometry/pose_deviation.hpp" +#include "tier4_autoware_utils/math/constants.hpp" #include #include #include #include +#include namespace tier4_autoware_utils { @@ -34,6 +36,27 @@ void validateNonEmpty(const T & points) } } +template +void validateNonSharpAngle( + const T & point1, const T & point2, const T & point3, const double angle_threshold = pi / 4) +{ + const auto p1 = getPoint(point1); + const auto p2 = getPoint(point2); + const auto p3 = getPoint(point3); + + const std::vector vec_1to2 = {p2.x - p1.x, p2.y - p1.y, p2.z - p1.z}; + const std::vector vec_3to2 = {p2.x - p3.x, p2.y - p3.y, p2.z - p3.z}; + const auto product = std::inner_product(vec_1to2.begin(), vec_1to2.end(), vec_3to2.begin(), 0.0); + + const auto dist_1to2 = calcDistance3d(p1, p2); + const auto dist_3to2 = calcDistance3d(p3, p2); + + constexpr double epsilon = 1e-3; + if (std::cos(angle_threshold) < product / dist_1to2 / dist_3to2 + epsilon) { + throw std::invalid_argument("Sharp angle."); + } +} + template boost::optional searchZeroVelocityIndex( const T & points_with_twist, const size_t src_idx, const size_t dst_idx) @@ -417,6 +440,227 @@ boost::optional calcDistanceToForwardStopPoint( return std::max(0.0, *closest_stop_dist); } + +/** + * @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 T & points, const size_t src_idx, const double offset) +{ + validateNonEmpty(points); + + if (points.size() - 1 < src_idx) { + throw std::out_of_range("Invalid source index"); + } + + if (points.size() == 1) { + return {}; + } + + if (src_idx + 1 == points.size() && offset == 0.0) { + return 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 = calcDistance2d(p_front, p_back); + dist_sum += dist_segment; + + const auto dist_res = offset - dist_sum; + if (dist_res <= 0.0) { + return 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 T & points, const geometry_msgs::msg::Point & src_point, const double offset) +{ + validateNonEmpty(points); + + 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 T & points, const size_t src_idx, const double offset) +{ + validateNonEmpty(points); + + if (points.size() - 1 < src_idx) { + throw std::out_of_range("Invalid source index"); + } + + if (points.size() == 1) { + return {}; + } + + if (src_idx + 1 == points.size() && offset == 0.0) { + return 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 = calcDistance2d(p_front, p_back); + dist_sum += dist_segment; + + const auto dist_res = -offset - dist_sum; + if (dist_res <= 0.0) { + return 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 = calcDistance2d(p_front, p_back); + dist_sum += dist_segment; + + const auto dist_res = offset - dist_sum; + if (dist_res <= 0.0) { + return 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 T & points, const geometry_msgs::msg::Point & src_point, const double offset) +{ + validateNonEmpty(points); + + 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 size_t insertTargetPoint( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, T & points, + const double overlap_threshold = 1e-3) +{ + validateNonEmpty(points); + + const auto p_front = getPoint(points.at(seg_idx)); + const auto p_back = 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; + } + + const auto overlap_with_front = calcDistance2d(p_target, p_front) < overlap_threshold; + const auto overlap_with_back = calcDistance2d(p_target, p_back) < overlap_threshold; + + geometry_msgs::msg::Pose target_pose; + { + const auto pitch = calcElevationAngle(p_target, p_back); + const auto yaw = calcAzimuthAngle(p_target, p_back); + + target_pose.position = p_target; + target_pose.orientation = createQuaternionFromRPY(0.0, pitch, yaw); + } + + auto p_insert = points.at(seg_idx); + setPose(target_pose, p_insert); + + geometry_msgs::msg::Pose front_pose; + { + const auto pitch = calcElevationAngle(p_front, p_target); + const auto yaw = calcAzimuthAngle(p_front, p_target); + + front_pose.position = getPoint(points.at(seg_idx)); + front_pose.orientation = createQuaternionFromRPY(0.0, pitch, yaw); + } + + if (!overlap_with_front && !overlap_with_back) { + 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; +} } // namespace tier4_autoware_utils #endif // TIER4_AUTOWARE_UTILS__TRAJECTORY__TRAJECTORY_HPP_ diff --git a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp index 6d18af534e776..42f906c6172e2 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp @@ -199,6 +199,76 @@ TEST(geometry, getPose) } } +TEST(geometry, setPose) +{ + using tier4_autoware_utils::setPose; + + const double x_ans = 1.0; + const double y_ans = 2.0; + const double z_ans = 3.0; + const double q_x_ans = 0.1; + const double q_y_ans = 0.2; + const double q_z_ans = 0.3; + const double q_w_ans = 0.4; + + geometry_msgs::msg::Pose p; + p.position.x = x_ans; + p.position.y = y_ans; + p.position.z = z_ans; + p.orientation.x = q_x_ans; + p.orientation.y = q_y_ans; + p.orientation.z = q_z_ans; + p.orientation.w = q_w_ans; + + { + geometry_msgs::msg::Pose p_out{}; + setPose(p, p_out); + EXPECT_DOUBLE_EQ(p_out.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans); + } + + { + geometry_msgs::msg::PoseStamped p_out{}; + setPose(p, p_out); + EXPECT_DOUBLE_EQ(p_out.pose.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.pose.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.pose.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.w, q_w_ans); + } + + { + autoware_auto_planning_msgs::msg::PathPoint p_out{}; + setPose(p, p_out); + EXPECT_DOUBLE_EQ(p_out.pose.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.pose.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.pose.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.w, q_w_ans); + } + + { + autoware_auto_planning_msgs::msg::TrajectoryPoint p_out{}; + setPose(p, p_out); + EXPECT_DOUBLE_EQ(p_out.pose.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.pose.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.pose.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.pose.orientation.w, q_w_ans); + } +} + TEST(geometry, createPoint) { using tier4_autoware_utils::createPoint; diff --git a/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp b/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp new file mode 100644 index 0000000000000..b62589bc5650c --- /dev/null +++ b/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp @@ -0,0 +1,97 @@ +// 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 "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" + +#include + +TEST(geometry, getPoint_PathWithLaneId) +{ + using tier4_autoware_utils::getPoint; + + const double x_ans = 1.0; + const double y_ans = 2.0; + const double z_ans = 3.0; + + autoware_auto_planning_msgs::msg::PathPointWithLaneId p; + p.point.pose.position.x = x_ans; + p.point.pose.position.y = y_ans; + p.point.pose.position.z = z_ans; + const geometry_msgs::msg::Point p_out = getPoint(p); + EXPECT_DOUBLE_EQ(p_out.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.z, z_ans); +} + +TEST(geometry, getPose_PathWithLaneId) +{ + using tier4_autoware_utils::getPose; + + const double x_ans = 1.0; + const double y_ans = 2.0; + const double z_ans = 3.0; + const double q_x_ans = 0.1; + const double q_y_ans = 0.2; + const double q_z_ans = 0.3; + const double q_w_ans = 0.4; + + autoware_auto_planning_msgs::msg::PathPointWithLaneId p; + p.point.pose.position.x = x_ans; + p.point.pose.position.y = y_ans; + p.point.pose.position.z = z_ans; + p.point.pose.orientation.x = q_x_ans; + p.point.pose.orientation.y = q_y_ans; + p.point.pose.orientation.z = q_z_ans; + p.point.pose.orientation.w = q_w_ans; + const geometry_msgs::msg::Pose p_out = getPose(p); + EXPECT_DOUBLE_EQ(p_out.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans); +} + +TEST(geometry, setPose_PathWithLaneId) +{ + using tier4_autoware_utils::setPose; + + const double x_ans = 1.0; + const double y_ans = 2.0; + const double z_ans = 3.0; + const double q_x_ans = 0.1; + const double q_y_ans = 0.2; + const double q_z_ans = 0.3; + const double q_w_ans = 0.4; + + geometry_msgs::msg::Pose p; + p.position.x = x_ans; + p.position.y = y_ans; + p.position.z = z_ans; + p.orientation.x = q_x_ans; + p.orientation.y = q_y_ans; + p.orientation.z = q_z_ans; + p.orientation.w = q_w_ans; + + autoware_auto_planning_msgs::msg::PathPointWithLaneId p_out{}; + setPose(p, p_out); + EXPECT_DOUBLE_EQ(p_out.point.pose.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.point.pose.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.point.pose.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.point.pose.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.point.pose.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.point.pose.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.point.pose.orientation.w, q_w_ans); +} diff --git a/common/tier4_autoware_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/tier4_autoware_utils/test/src/trajectory/test_path_with_lane_id.cpp new file mode 100644 index 0000000000000..6c56ed700d6a8 --- /dev/null +++ b/common/tier4_autoware_utils/test/src/trajectory/test_path_with_lane_id.cpp @@ -0,0 +1,698 @@ +// 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 "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/trajectory/path_with_lane_id.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 tier4_autoware_utils::calcArcLength; + using tier4_autoware_utils::calcLongitudinalOffsetPoint; + using tier4_autoware_utils::calcSignedArcLength; + using tier4_autoware_utils::getPoint; + + const auto traj = generateTestTrajectory(10, 1.0); + const auto total_length = calcArcLength(traj.points); + + // Empty + EXPECT_THROW(calcLongitudinalOffsetPoint(PathWithLaneId{}.points, {}, {}), std::invalid_argument); + + // Out of range + EXPECT_THROW( + calcLongitudinalOffsetPoint(traj.points, traj.points.size() + 1, 1.0), std::out_of_range); + EXPECT_THROW(calcLongitudinalOffsetPoint(traj.points, -1, 1.0), std::out_of_range); + + // 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 tier4_autoware_utils::calcArcLength; + using tier4_autoware_utils::calcLongitudinalOffsetPoint; + using tier4_autoware_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_THROW(calcLongitudinalOffsetPoint(PathWithLaneId{}.points, {}, {}), std::invalid_argument); + + // 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_THROW( + calcLongitudinalOffsetPoint(one_point_traj.points, geometry_msgs::msg::Point{}, {}), + std::out_of_range); + } +} + +TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_PathWithLaneId) +{ + using tier4_autoware_utils::calcArcLength; + using tier4_autoware_utils::calcLongitudinalOffsetPose; + using tier4_autoware_utils::calcSignedArcLength; + using tier4_autoware_utils::getPoint; + + const auto traj = generateTestTrajectory(10, 1.0); + const auto total_length = calcArcLength(traj.points); + + // Empty + EXPECT_THROW(calcLongitudinalOffsetPose(PathWithLaneId{}.points, {}, {}), std::invalid_argument); + + // Out of range + EXPECT_THROW( + calcLongitudinalOffsetPose(traj.points, traj.points.size() + 1, 1.0), std::out_of_range); + EXPECT_THROW(calcLongitudinalOffsetPose(traj.points, -1, 1.0), std::out_of_range); + + // 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 tier4_autoware_utils::calcArcLength; + using tier4_autoware_utils::calcLongitudinalOffsetPose; + using tier4_autoware_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_THROW(calcLongitudinalOffsetPose(PathWithLaneId{}.points, {}, {}), std::invalid_argument); + + // 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_THROW( + calcLongitudinalOffsetPose(one_point_traj.points, geometry_msgs::msg::Point{}, {}), + std::out_of_range); + } +} + +TEST(trajectory, insertTargetPoint_PathWithLaneId) +{ + using tier4_autoware_utils::calcArcLength; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::createPoint; + using tier4_autoware_utils::deg2rad; + using tier4_autoware_utils::findNearestSegmentIndex; + using tier4_autoware_utils::getPose; + using tier4_autoware_utils::insertTargetPoint; + + 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_EQ(insert_idx, 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)); + 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_EQ(insert_idx, 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)); + 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_EQ(insert_idx, 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)); + 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_EQ(insert_idx, 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_EQ(insert_idx, 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, 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); + } + } + + // 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, 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); + } + } + + // 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, 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); + } + } + + // Empty + { + auto empty_traj = generateTestTrajectory(0, 1.0); + EXPECT_THROW( + insertTargetPoint({}, geometry_msgs::msg::Point{}, empty_traj.points), std::invalid_argument); + } +} + +TEST(trajectory, insertTargetPoint_OverlapThreshold_PathWithLaneId) +{ + using tier4_autoware_utils::calcArcLength; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::createPoint; + using tier4_autoware_utils::deg2rad; + using tier4_autoware_utils::findNearestSegmentIndex; + using tier4_autoware_utils::getPose; + using tier4_autoware_utils::insertTargetPoint; + + 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_EQ(insert_idx, 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)); + 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_EQ(insert_idx, 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_EQ(insert_idx, 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); + } + } +} diff --git a/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp b/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp index 9e481e26bcf94..8fee29d0524e1 100644 --- a/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -18,6 +18,7 @@ #include "tier4_autoware_utils/trajectory/trajectory.hpp" #include +#include #include #include @@ -103,6 +104,65 @@ TEST(trajectory, validateNonEmpty) EXPECT_NO_THROW(validateNonEmpty(traj.points)); } +TEST(trajectory, validateNonSharpAngle_DefaultThreshold) +{ + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using tier4_autoware_utils::validateNonSharpAngle; + + TrajectoryPoint p1; + p1.pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + p1.longitudinal_velocity_mps = 0.0; + + TrajectoryPoint p2; + p2.pose = createPose(1.0, 1.0, 0.0, 0.0, 0.0, 0.0); + p2.longitudinal_velocity_mps = 0.0; + + TrajectoryPoint p3; + p3.pose = createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0); + p3.longitudinal_velocity_mps = 0.0; + + // Non sharp angle + { + EXPECT_NO_THROW(validateNonSharpAngle(p1, p2, p3)); + } + + // Sharp angle + { + EXPECT_THROW(validateNonSharpAngle(p2, p1, p3), std::invalid_argument); + EXPECT_THROW(validateNonSharpAngle(p1, p3, p2), std::invalid_argument); + } +} + +TEST(trajectory, validateNonSharpAngle_SetThreshold) +{ + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using tier4_autoware_utils::pi; + using tier4_autoware_utils::validateNonSharpAngle; + + TrajectoryPoint p1; + p1.pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + p1.longitudinal_velocity_mps = 0.0; + + TrajectoryPoint p2; + p2.pose = createPose(1.73205080756887729, 0.0, 0.0, 0.0, 0.0, 0.0); + p2.longitudinal_velocity_mps = 0.0; + + TrajectoryPoint p3; + p3.pose = createPose(1.73205080756887729, 1.0, 0.0, 0.0, 0.0, 0.0); + p3.longitudinal_velocity_mps = 0.0; + + // Non sharp angle + { + EXPECT_NO_THROW(validateNonSharpAngle(p1, p2, p3, pi / 6)); + EXPECT_NO_THROW(validateNonSharpAngle(p2, p3, p1, pi / 6)); + } + + // Sharp angle + { + EXPECT_THROW(validateNonSharpAngle(p3, p1, p2, pi / 6), std::invalid_argument); + } +} + TEST(trajectory, searchZeroVelocityIndex) { using tier4_autoware_utils::searchZeroVelocityIndex; @@ -923,3 +983,799 @@ TEST(trajectory, calcDistanceToForwardStopPoint_YawThreshold) } } } + +TEST(trajectory, calcLongitudinalOffsetPointFromIndex) +{ + using tier4_autoware_utils::calcArcLength; + using tier4_autoware_utils::calcLongitudinalOffsetPoint; + using tier4_autoware_utils::calcSignedArcLength; + using tier4_autoware_utils::getPoint; + + const auto traj = generateTestTrajectory(10, 1.0); + const auto total_length = calcArcLength(traj.points); + + // Empty + EXPECT_THROW(calcLongitudinalOffsetPoint(Trajectory{}.points, {}, {}), std::invalid_argument); + + // Out of range + EXPECT_THROW( + calcLongitudinalOffsetPoint(traj.points, traj.points.size() + 1, 1.0), std::out_of_range); + EXPECT_THROW(calcLongitudinalOffsetPoint(traj.points, -1, 1.0), std::out_of_range); + + // 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) +{ + using tier4_autoware_utils::calcArcLength; + using tier4_autoware_utils::calcLongitudinalOffsetPoint; + using tier4_autoware_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_THROW(calcLongitudinalOffsetPoint(Trajectory{}.points, {}, {}), std::invalid_argument); + + // 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_THROW( + calcLongitudinalOffsetPoint(one_point_traj.points, geometry_msgs::msg::Point{}, {}), + std::out_of_range); + } +} + +TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) +{ + using tier4_autoware_utils::calcArcLength; + using tier4_autoware_utils::calcLongitudinalOffsetPose; + using tier4_autoware_utils::calcSignedArcLength; + using tier4_autoware_utils::getPoint; + + const auto traj = generateTestTrajectory(10, 1.0); + const auto total_length = calcArcLength(traj.points); + + // Empty + EXPECT_THROW(calcLongitudinalOffsetPose(Trajectory{}.points, {}, {}), std::invalid_argument); + + // Out of range + EXPECT_THROW( + calcLongitudinalOffsetPose(traj.points, traj.points.size() + 1, 1.0), std::out_of_range); + EXPECT_THROW(calcLongitudinalOffsetPose(traj.points, -1, 1.0), std::out_of_range); + + // 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, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) +{ + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using tier4_autoware_utils::calcArcLength; + using tier4_autoware_utils::calcLongitudinalOffsetPose; + using tier4_autoware_utils::deg2rad; + + Trajectory traj{}; + + { + TrajectoryPoint p; + p.pose = createPose(0.0, 0.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); + p.longitudinal_velocity_mps = 0.0; + traj.points.push_back(p); + } + + { + TrajectoryPoint p; + p.pose = createPose(1.0, 1.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + p.longitudinal_velocity_mps = 0.0; + traj.points.push_back(p); + } + + const auto total_length = calcArcLength(traj.points); + + // Found pose(forward) + for (double len = 0.0; len < total_length; len += 0.1) { + const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, len); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); + + EXPECT_NE(p_out, boost::none); + EXPECT_NEAR(p_out.get().position.x, len * std::cos(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.get().position.y, len * std::sin(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + } + + // Found pose(backward) + for (double len = total_length; 0.0 < len; len -= 0.1) { + const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, -len); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); + + EXPECT_NE(p_out, boost::none); + EXPECT_NEAR(p_out.get().position.x, 1.0 - len * std::cos(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.get().position.y, 1.0 - len * std::sin(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + } + + // Boundary condition + { + const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, total_length); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + + EXPECT_NE(p_out, boost::none); + EXPECT_NEAR(p_out.get().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.get().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + } + + // Boundary condition + { + const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, 0.0); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + + EXPECT_NE(p_out, boost::none); + EXPECT_NEAR(p_out.get().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.get().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + } +} + +TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) +{ + using tier4_autoware_utils::calcArcLength; + using tier4_autoware_utils::calcLongitudinalOffsetPose; + using tier4_autoware_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_THROW(calcLongitudinalOffsetPose(Trajectory{}.points, {}, {}), std::invalid_argument); + + // 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_THROW( + calcLongitudinalOffsetPose(one_point_traj.points, geometry_msgs::msg::Point{}, {}), + std::out_of_range); + } +} + +TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) +{ + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using tier4_autoware_utils::calcArcLength; + using tier4_autoware_utils::calcLongitudinalOffsetPose; + using tier4_autoware_utils::calcLongitudinalOffsetToSegment; + using tier4_autoware_utils::createPoint; + using tier4_autoware_utils::deg2rad; + + Trajectory traj{}; + + { + TrajectoryPoint p; + p.pose = createPose(0.0, 0.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); + p.longitudinal_velocity_mps = 0.0; + traj.points.push_back(p); + } + + { + TrajectoryPoint p; + p.pose = createPose(1.0, 1.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + p.longitudinal_velocity_mps = 0.0; + traj.points.push_back(p); + } + + const auto total_length = calcArcLength(traj.points); + + // Found pose + for (double len_start = 0.0; len_start < total_length; len_start += 0.1) { + constexpr double deviation = 0.1; + + const auto p_src = createPoint( + len_start * std::cos(deg2rad(45.0)) + deviation, + len_start * std::sin(deg2rad(45.0)) - deviation, 0.0); + const auto src_offset = calcLongitudinalOffsetToSegment(traj.points, 0, p_src); + + for (double len = -src_offset; len < total_length - src_offset; len += 0.1) { + const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, len); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); + + EXPECT_NE(p_out, boost::none); + EXPECT_NEAR( + p_out.get().position.x, p_src.x + len * std::cos(deg2rad(45.0)) - deviation, epsilon); + EXPECT_NEAR( + p_out.get().position.y, p_src.y + len * std::sin(deg2rad(45.0)) + deviation, epsilon); + EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary condition + { + constexpr double deviation = 0.1; + + const auto p_src = createPoint(1.0 + deviation, 1.0 - deviation, 0.0); + const auto src_offset = calcLongitudinalOffsetToSegment(traj.points, 0, p_src); + + const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, total_length - src_offset); + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + + EXPECT_NE(p_out, boost::none); + EXPECT_NEAR(p_out.get().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.get().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + } +} + +TEST(trajectory, insertTargetPoint) +{ + using tier4_autoware_utils::calcArcLength; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::createPoint; + using tier4_autoware_utils::deg2rad; + using tier4_autoware_utils::findNearestSegmentIndex; + using tier4_autoware_utils::getPose; + using tier4_autoware_utils::insertTargetPoint; + + 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_EQ(insert_idx, 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)); + 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_EQ(insert_idx, 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)); + 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_EQ(insert_idx, 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)); + 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_EQ(insert_idx, 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_EQ(insert_idx, 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, 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); + } + } + + // 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, 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); + } + } + + // 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, 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); + } + } + + // Empty + { + auto empty_traj = generateTestTrajectory(0, 1.0); + EXPECT_THROW( + insertTargetPoint({}, geometry_msgs::msg::Point{}, empty_traj.points), std::invalid_argument); + } +} + +TEST(trajectory, insertTargetPoint_OverlapThreshold) +{ + using tier4_autoware_utils::calcArcLength; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::createPoint; + using tier4_autoware_utils::deg2rad; + using tier4_autoware_utils::findNearestSegmentIndex; + using tier4_autoware_utils::getPose; + using tier4_autoware_utils::insertTargetPoint; + + 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_EQ(insert_idx, 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)); + 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_EQ(insert_idx, 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_EQ(insert_idx, 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); + } + } +} diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 427393cc14f78..1ac5c0bb26668 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -57,23 +57,6 @@ #include #include -namespace tier4_autoware_utils -{ -template <> -inline geometry_msgs::msg::Point getPoint( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) -{ - return p.point.pose.position; -} - -template <> -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) -{ - return p.point.pose; -} -} // namespace tier4_autoware_utils - namespace tf2 { inline void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped & out) diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp index c7f86477ff404..80409f3ec92f4 100644 --- a/planning/behavior_velocity_planner/include/utilization/util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/util.hpp @@ -16,8 +16,7 @@ #define UTILIZATION__UTIL_HPP_ #include -#include -#include +#include #include #include @@ -53,22 +52,6 @@ #include #include -namespace tier4_autoware_utils -{ -template <> -inline geometry_msgs::msg::Point getPoint( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) -{ - return p.point.pose.position; -} -template <> -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) -{ - return p.point.pose; -} -} // namespace tier4_autoware_utils - namespace behavior_velocity_planner { struct SearchRangeIndex