From f9793347debfc9be11e93b68503160e20a9d86b3 Mon Sep 17 00:00:00 2001 From: Yuma Nihei Date: Thu, 22 Jun 2023 15:33:44 +0900 Subject: [PATCH] feat(tier4_autoware_utils): add trajectory func (#600) * feat(tier4_autoware_utils): add trajectory func from 514fea4 Signed-off-by: Yuma Nihei * feat: add path_with_lane_id.hpp from 7ef1bee Signed-off-by: Yuma Nihei * ci(pre-commit): autofix * fix: replace motion_utils to tier4_autoware_utils * fix: replace motion_utils to tier4_autoware_utils * ci(pre-commit): autofix * fix: replace motion_utils to tier4_autoware_utils * feat: add include path_with_lane_id.hpp Signed-off-by: Yuma Nihei --------- Signed-off-by: Yuma Nihei Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../tier4_autoware_utils.hpp | 1 + .../trajectory/path_with_lane_id.hpp | 113 +++++++ .../trajectory/trajectory.hpp | 289 ++++++++++++++++++ 3 files changed, 403 insertions(+) create mode 100644 common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.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 4d9282b28647..95c981935337 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 @@ -34,6 +34,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" #endif // TIER4_AUTOWARE_UTILS__TIER4_AUTOWARE_UTILS_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 000000000000..3946c68aa0b0 --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.hpp @@ -0,0 +1,113 @@ +// 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/path_with_lane_id_geometry.hpp" +#include "tier4_autoware_utils/trajectory/trajectory.hpp" + +#include + +#include +#include +#include + +namespace tier4_autoware_utils +{ +inline boost::optional> getPathIndexRangeWithLaneId( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) +{ + size_t start_idx = 0; // NOTE: to prevent from maybe-uninitialized error + size_t end_idx = 0; // NOTE: to prevent from maybe-uninitialized error + + bool found_first_idx = false; + for (size_t i = 0; i < path.points.size(); ++i) { + const auto & p = path.points.at(i); + for (const auto & id : p.lane_ids) { + if (id == target_lane_id) { + if (!found_first_idx) { + start_idx = i; + found_first_idx = true; + } + end_idx = i; + } + } + } + + if (found_first_idx) { + // NOTE: In order to fully cover lanes with target_lane_id, start_idx and end_idx are expanded. + start_idx = static_cast(std::max(0, static_cast(start_idx) - 1)); + end_idx = std::min(path.points.size() - 1, end_idx + 1); + + return std::make_pair(start_idx, end_idx); + } + + return {}; +} + +inline size_t findNearestIndexFromLaneId( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & pos, const int64_t lane_id) +{ + const auto opt_range = getPathIndexRangeWithLaneId(path, lane_id); + if (opt_range) { + const size_t start_idx = opt_range->first; + const size_t end_idx = opt_range->second; + + validateNonEmpty(path.points); + + const auto sub_points = std::vector{ + path.points.begin() + start_idx, path.points.begin() + end_idx + 1}; + validateNonEmpty(sub_points); + + return start_idx + findNearestIndex(sub_points, pos); + } + + return findNearestIndex(path.points, pos); +} + +inline size_t findNearestSegmentIndexFromLaneId( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & pos, const int64_t lane_id) +{ + const size_t nearest_idx = findNearestIndexFromLaneId(path, pos, lane_id); + + if (nearest_idx == 0) { + return 0; + } + if (nearest_idx == path.points.size() - 1) { + return path.points.size() - 2; + } + + const double signed_length = calcLongitudinalOffsetToSegment(path.points, nearest_idx, pos); + + if (signed_length <= 0) { + return nearest_idx - 1; + } + + return nearest_idx; +} + +// @brief Calculates the path to be followed by the rear wheel center in order to make the vehicle +// center follow the input path +// @param [in] path with position to be followed by the center of the vehicle +// @param [out] PathWithLaneId to be followed by the rear wheel center follow to make the vehicle +// center follow the input path NOTE: rear_to_cog is supposed to be positive +autoware_auto_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, + const bool enable_last_point_compensation = true); +} // 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 180a9980c46d..041b08ecdfe2 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 @@ -66,6 +66,78 @@ void validateNonEmpty(const T & points) } } +/** + * @brief validate a point is in a non-sharp angle between two points or not + * @param point1 front point + * @param point2 point to be validated + * @param point3 back point + */ +template +void validateNonSharpAngle( + const T & point1, const T & point2, const T & point3, + const double angle_threshold = tier4_autoware_utils::pi / 4) +{ + const auto p1 = tier4_autoware_utils::getPoint(point1); + const auto p2 = tier4_autoware_utils::getPoint(point2); + const auto p3 = tier4_autoware_utils::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 = tier4_autoware_utils::calcDistance3d(p1, p2); + const auto dist_3to2 = tier4_autoware_utils::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."); + } +} + +/** + * @brief checks whether a path of trajectory has forward driving direction + * @param points points of trajectory, path, ... + * @return (forward / backward) driving (true / false) + */ +template +boost::optional isDrivingForward(const T points) +{ + if (points.size() < 2) { + return boost::none; + } + + // check the first point direction + const auto & first_pose = tier4_autoware_utils::getPose(points.at(0)); + const auto & second_pose = tier4_autoware_utils::getPose(points.at(1)); + + return tier4_autoware_utils::isDrivingForward(first_pose, second_pose); +} + +/** + * @brief checks whether a path of trajectory has forward driving direction using its longitudinal + * velocity + * @param points_with_twist points of trajectory, path, ... (with velocity) + * @return (forward / backward) driving (true, false, none "if velocity is zero") + */ +template +boost::optional isDrivingForwardWithTwist(const T points_with_twist) +{ + if (points_with_twist.empty()) { + return boost::none; + } + if (points_with_twist.size() == 1) { + if (0.0 < tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) { + return true; + } else if (0.0 > tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) { + return false; + } else { + return boost::none; + } + } + + return isDrivingForward(points_with_twist); +} + template boost::optional searchZeroVelocityIndex( const T & points_with_twist, const size_t src_idx, const size_t dst_idx) @@ -570,6 +642,223 @@ inline boost::optional calcLongitudinalOffsetPose( set_orientation_from_position_direction); } +/** + * @brief insert a point in points container (trajectory, path, ...) using segment id + * @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, ... + * @param overlap_threshold distance threshold, used to check if the inserted point is between start + * and end of nominated segment to be added in. + * @return index of segment id, where point is inserted + */ +template +inline boost::optional insertTargetPoint( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, T & points, + const double overlap_threshold = 1e-3) +{ + 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; + + const auto is_driving_forward = isDrivingForward(points); + if (!is_driving_forward) { + return {}; + } + + geometry_msgs::msg::Pose target_pose; + { + const auto p_base = is_driving_forward.get() ? p_back : p_front; + const auto pitch = tier4_autoware_utils::calcElevationAngle(p_target, p_base); + const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_target, p_base); + + 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 base_pose; + { + const auto p_base = is_driving_forward.get() ? p_front : p_back; + const auto pitch = tier4_autoware_utils::calcElevationAngle(p_base, p_target); + const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_base, p_target); + + base_pose.position = tier4_autoware_utils::getPoint(p_base); + base_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); + } + + if (!overlap_with_front && !overlap_with_back) { + if (is_driving_forward.get()) { + tier4_autoware_utils::setPose(base_pose, points.at(seg_idx)); + } else { + tier4_autoware_utils::setPose(base_pose, points.at(seg_idx + 1)); + } + 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 insert a point in points container (trajectory, path, ...) using length of point to be + * inserted + * @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, ... + * @param overlap_threshold distance threshold, used to check if the inserted point is between start + * and end of nominated segment to be added in. + * @return index of segment id, where point is inserted + */ +template +inline boost::optional insertTargetPoint( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, T & points, + const double overlap_threshold = 1e-3) +{ + 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); +} + +/** + * @brief insert a point in points container (trajectory, path, ...) using segment index and length + * of point to be inserted + * @param src_segment_idx source segment index on the trajectory + * @param insert_point_length length to insert point from the beginning of the points + * @param points output points of trajectory, path, ... + * @param overlap_threshold distance threshold, used to check if the inserted point is between start + * and end of nominated segment to be added in. + * @return index of insert point + */ +template +inline boost::optional insertTargetPoint( + const size_t src_segment_idx, const double insert_point_length, T & points, + const double overlap_threshold = 1e-3) +{ + validateNonEmpty(points); + + if (src_segment_idx >= points.size() - 1) { + return boost::none; + } + + // Get Nearest segment index + boost::optional segment_idx = boost::none; + if (0.0 <= insert_point_length) { + for (size_t i = src_segment_idx + 1; i < points.size(); ++i) { + const double length = calcSignedArcLength(points, src_segment_idx, i); + if (insert_point_length <= length) { + segment_idx = i - 1; + break; + } + } + } else { + for (int i = src_segment_idx - 1; 0 <= i; --i) { + const double length = calcSignedArcLength(points, src_segment_idx, i); + if (length <= insert_point_length) { + segment_idx = i; + break; + } + } + } + + if (!segment_idx) { + return boost::none; + } + + // Get Target Point + const double segment_length = calcSignedArcLength(points, *segment_idx, *segment_idx + 1); + const double target_length = + insert_point_length - calcSignedArcLength(points, src_segment_idx, *segment_idx); + const double ratio = std::clamp(target_length / segment_length, 0.0, 1.0); + const auto p_target = tier4_autoware_utils::calcInterpolatedPoint( + tier4_autoware_utils::getPoint(points.at(*segment_idx)), + tier4_autoware_utils::getPoint(points.at(*segment_idx + 1)), ratio); + + return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); +} + +/** + * @brief Insert a target point from a source pose on the trajectory + * @param src_pose source pose on the trajectory + * @param insert_point_length length to insert point from the beginning of the points + * @param points output points of trajectory, path, ... + * @param max_dist max distance, used to search for nearest segment index in points container to the + * given source pose + * @param max_yaw max yaw, used to search for nearest segment index in points container to the given + * source pose + * @param overlap_threshold distance threshold, used to check if the inserted point is between start + * and end of nominated segment to be added in. + * @return index of insert point + */ +template +inline boost::optional insertTargetPoint( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, T & points, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3) +{ + validateNonEmpty(points); + + if (insert_point_length < 0.0) { + return boost::none; + } + + const auto nearest_segment_idx = findNearestSegmentIndex(points, src_pose, max_dist, max_yaw); + if (!nearest_segment_idx) { + return boost::none; + } + + const double offset_length = + calcLongitudinalOffsetToSegment(points, *nearest_segment_idx, src_pose.position); + + return insertTargetPoint( + *nearest_segment_idx, insert_point_length + offset_length, points, overlap_threshold); +} } // namespace tier4_autoware_utils #endif // TIER4_AUTOWARE_UTILS__TRAJECTORY__TRAJECTORY_HPP_