diff --git a/common/motion_utils/CMakeLists.txt b/common/motion_utils/CMakeLists.txt index 3b8f78a7d3917..eac15d4cf476e 100644 --- a/common/motion_utils/CMakeLists.txt +++ b/common/motion_utils/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(Boost REQUIRED) ament_auto_add_library(motion_utils SHARED src/motion_utils.cpp src/marker/marker_helper.cpp + src/vehicle/vehicle_state_checker.cpp ) if(BUILD_TESTING) diff --git a/common/motion_utils/include/motion_utils/motion_utils.hpp b/common/motion_utils/include/motion_utils/motion_utils.hpp index 796ae8c1674fb..eb94be5348ff7 100644 --- a/common/motion_utils/include/motion_utils/motion_utils.hpp +++ b/common/motion_utils/include/motion_utils/motion_utils.hpp @@ -16,5 +16,9 @@ #define MOTION_UTILS__MOTION_UTILS_HPP_ #include "motion_utils/marker/marker_helper.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "motion_utils/vehicle/vehicle_state_checker.hpp" #endif // MOTION_UTILS__MOTION_UTILS_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.hpp b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp similarity index 80% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.hpp rename to common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp index bd2174f011022..b450a628a4302 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp @@ -12,12 +12,12 @@ // 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_ +#ifndef MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ +#define MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ +#include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" -#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include @@ -26,7 +26,7 @@ #include #include -namespace tier4_autoware_utils +namespace motion_utils { /** * @brief calculate the point offset from source point along the trajectory (or path) @@ -61,7 +61,7 @@ inline boost::optional calcLongitudinalOffsetPoint( } if (src_idx + 1 == points.size() && offset == 0.0) { - return getPoint(points.at(src_idx)); + return tier4_autoware_utils::getPoint(points.at(src_idx)); } if (offset < 0.0) { @@ -77,12 +77,13 @@ inline boost::optional calcLongitudinalOffsetPoint( const auto & p_front = points.at(i); const auto & p_back = points.at(i + 1); - const auto dist_segment = calcDistance2d(p_front, p_back); + const auto dist_segment = tier4_autoware_utils::calcDistance2d(p_front, p_back); dist_sum += dist_segment; const auto dist_res = offset - dist_sum; if (dist_res <= 0.0) { - return calcInterpolatedPoint(p_back, p_front, std::abs(dist_res / dist_segment)); + return tier4_autoware_utils::calcInterpolatedPoint( + p_back, p_front, std::abs(dist_res / dist_segment)); } } @@ -155,7 +156,7 @@ inline boost::optional calcLongitudinalOffsetPose( } if (src_idx + 1 == points.size() && offset == 0.0) { - return getPose(points.at(src_idx)); + return tier4_autoware_utils::getPose(points.at(src_idx)); } if (offset < 0.0) { @@ -168,12 +169,13 @@ inline boost::optional calcLongitudinalOffsetPose( 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); + const auto dist_segment = tier4_autoware_utils::calcDistance2d(p_front, p_back); dist_sum += dist_segment; const auto dist_res = -offset - dist_sum; if (dist_res <= 0.0) { - return calcInterpolatedPose(p_back, p_front, std::abs(dist_res / dist_segment)); + return tier4_autoware_utils::calcInterpolatedPose( + p_back, p_front, std::abs(dist_res / dist_segment)); } } } else { @@ -183,12 +185,13 @@ inline boost::optional calcLongitudinalOffsetPose( const auto & p_front = points.at(i); const auto & p_back = points.at(i + 1); - const auto dist_segment = calcDistance2d(p_front, p_back); + const auto dist_segment = tier4_autoware_utils::calcDistance2d(p_front, p_back); dist_sum += dist_segment; const auto dist_res = offset - dist_sum; if (dist_res <= 0.0) { - return calcInterpolatedPose(p_front, p_back, 1.0 - std::abs(dist_res / dist_segment)); + return tier4_autoware_utils::calcInterpolatedPose( + p_front, p_back, 1.0 - std::abs(dist_res / dist_segment)); } } } @@ -248,8 +251,8 @@ inline boost::optional insertTargetPoint( return {}; } - const auto p_front = getPoint(points.at(seg_idx)); - const auto p_back = getPoint(points.at(seg_idx + 1)); + 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); @@ -258,32 +261,34 @@ inline boost::optional insertTargetPoint( return {}; } - const auto overlap_with_front = calcDistance2d(p_target, p_front) < overlap_threshold; - const auto overlap_with_back = calcDistance2d(p_target, p_back) < overlap_threshold; + const auto overlap_with_front = + tier4_autoware_utils::calcDistance2d(p_target, p_front) < overlap_threshold; + const auto overlap_with_back = + tier4_autoware_utils::calcDistance2d(p_target, p_back) < overlap_threshold; geometry_msgs::msg::Pose target_pose; { - const auto pitch = calcElevationAngle(p_target, p_back); - const auto yaw = calcAzimuthAngle(p_target, p_back); + const auto pitch = tier4_autoware_utils::calcElevationAngle(p_target, p_back); + const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_target, p_back); target_pose.position = p_target; - target_pose.orientation = createQuaternionFromRPY(0.0, pitch, yaw); + target_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); } auto p_insert = points.at(seg_idx); - setPose(target_pose, p_insert); + tier4_autoware_utils::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); + const auto pitch = tier4_autoware_utils::calcElevationAngle(p_front, p_target); + const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_front, p_target); - front_pose.position = getPoint(points.at(seg_idx)); - front_pose.orientation = createQuaternionFromRPY(0.0, pitch, yaw); + front_pose.position = tier4_autoware_utils::getPoint(points.at(seg_idx)); + front_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); } if (!overlap_with_front && !overlap_with_back) { - setPose(front_pose, points.at(seg_idx)); + tier4_autoware_utils::setPose(front_pose, points.at(seg_idx)); points.insert(points.begin() + seg_idx + 1, p_insert); return seg_idx + 1; } @@ -330,6 +335,6 @@ inline boost::optional insertTargetPoint( return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); } -} // namespace tier4_autoware_utils +} // namespace motion_utils -#endif // TIER4_AUTOWARE_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ +#endif // MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/tmp_conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp similarity index 84% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/tmp_conversion.hpp rename to common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp index b699a579c3a63..3bf06fa241315 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/tmp_conversion.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ -#define TIER4_AUTOWARE_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ +#ifndef MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ +#define MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/geometry/pose_deviation.hpp" @@ -26,7 +26,7 @@ #include #include -namespace tier4_autoware_utils +namespace motion_utils { /** * @brief Convert std::vector to @@ -39,7 +39,7 @@ namespace tier4_autoware_utils * @todo Decide how to handle the situation that we need to use the trajectory with the size of * points larger than the capacity. (Tier IV) */ -autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( +inline autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( const std::vector & trajectory) { autoware_auto_planning_msgs::msg::Trajectory output{}; @@ -56,7 +56,7 @@ autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to * std::vector. */ -std::vector convertToTrajectoryPointArray( +inline std::vector convertToTrajectoryPointArray( const autoware_auto_planning_msgs::msg::Trajectory & trajectory) { std::vector output(trajectory.points.size()); @@ -64,6 +64,6 @@ std::vector convertToTrajecto return output; } -} // namespace tier4_autoware_utils +} // namespace motion_utils -#endif // TIER4_AUTOWARE_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ +#endif // MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp similarity index 86% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp rename to common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index ec0ad9a895a01..32d6c4241c744 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__TRAJECTORY__TRAJECTORY_HPP_ -#define TIER4_AUTOWARE_UTILS__TRAJECTORY__TRAJECTORY_HPP_ +#ifndef MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ +#define MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/geometry/pose_deviation.hpp" @@ -57,7 +57,7 @@ std::vector removeOverlapPoints(const T & points, con } } // namespace -namespace tier4_autoware_utils +namespace motion_utils { template void validateNonEmpty(const T & points) @@ -69,18 +69,19 @@ void validateNonEmpty(const T & points) template void validateNonSharpAngle( - const T & point1, const T & point2, const T & point3, const double angle_threshold = pi / 4) + const T & point1, const T & point2, const T & point3, + const double angle_threshold = tier4_autoware_utils::pi / 4) { - const auto p1 = getPoint(point1); - const auto p2 = getPoint(point2); - const auto p3 = getPoint(point3); + 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 = calcDistance3d(p1, p2); - const auto dist_3to2 = calcDistance3d(p3, p2); + 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) { @@ -124,7 +125,7 @@ size_t findNearestIndex(const T & points, const geometry_msgs::msg::Point & poin size_t min_idx = 0; for (size_t i = 0; i < points.size(); ++i) { - const auto dist = calcSquaredDistance2d(points.at(i), point); + const auto dist = tier4_autoware_utils::calcSquaredDistance2d(points.at(i), point); if (dist < min_dist) { min_dist = dist; min_idx = i; @@ -153,12 +154,13 @@ boost::optional findNearestIndex( size_t min_idx = 0; for (size_t i = 0; i < points.size(); ++i) { - const auto squared_dist = calcSquaredDistance2d(points.at(i), pose); + const auto squared_dist = tier4_autoware_utils::calcSquaredDistance2d(points.at(i), pose); if (squared_dist > max_squared_dist) { continue; } - const auto yaw = calcYawDeviation(getPose(points.at(i)), pose); + const auto yaw = + tier4_autoware_utils::calcYawDeviation(tier4_autoware_utils::getPose(points.at(i)), pose); if (std::fabs(yaw) > max_yaw) { continue; } @@ -218,8 +220,8 @@ double calcLongitudinalOffsetToSegment( return std::nan(""); } - const auto p_front = getPoint(overlap_removed_points.at(seg_idx)); - const auto p_back = getPoint(overlap_removed_points.at(seg_idx + 1)); + const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx)); + const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx + 1)); const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0}; const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0}; @@ -329,8 +331,8 @@ double calcLateralOffset( const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, p_target); - const auto p_front = getPoint(overlap_removed_points.at(seg_idx)); - const auto p_back = getPoint(overlap_removed_points.at(seg_idx + 1)); + const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx)); + const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx + 1)); const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0}; @@ -358,7 +360,7 @@ double calcSignedArcLength(const T & points, const size_t src_idx, const size_t double dist_sum = 0.0; for (size_t i = src_idx; i < dst_idx; ++i) { - dist_sum += calcDistance2d(points.at(i), points.at(i + 1)); + dist_sum += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); } return dist_sum; } @@ -547,21 +549,21 @@ boost::optional calcDistanceToForwardStopPoint( } const auto nearest_segment_idx = - tier4_autoware_utils::findNearestSegmentIndex(points_with_twist, pose, max_dist, max_yaw); + motion_utils::findNearestSegmentIndex(points_with_twist, pose, max_dist, max_yaw); if (!nearest_segment_idx) { return boost::none; } - const auto stop_idx = tier4_autoware_utils::searchZeroVelocityIndex( + const auto stop_idx = motion_utils::searchZeroVelocityIndex( points_with_twist, *nearest_segment_idx + 1, points_with_twist.size()); if (!stop_idx) { return boost::none; } - const auto closest_stop_dist = tier4_autoware_utils::calcSignedArcLength( - points_with_twist, pose, *stop_idx, max_dist, max_yaw); + const auto closest_stop_dist = + motion_utils::calcSignedArcLength(points_with_twist, pose, *stop_idx, max_dist, max_yaw); if (!closest_stop_dist) { return boost::none; @@ -602,7 +604,7 @@ inline boost::optional calcLongitudinalOffsetPoint( } if (src_idx + 1 == points.size() && offset == 0.0) { - return getPoint(points.at(src_idx)); + return tier4_autoware_utils::getPoint(points.at(src_idx)); } if (offset < 0.0) { @@ -618,12 +620,13 @@ inline boost::optional calcLongitudinalOffsetPoint( const auto & p_front = points.at(i); const auto & p_back = points.at(i + 1); - const auto dist_segment = calcDistance2d(p_front, p_back); + const auto dist_segment = tier4_autoware_utils::calcDistance2d(p_front, p_back); dist_sum += dist_segment; const auto dist_res = offset - dist_sum; if (dist_res <= 0.0) { - return calcInterpolatedPoint(p_back, p_front, std::abs(dist_res / dist_segment)); + return tier4_autoware_utils::calcInterpolatedPoint( + p_back, p_front, std::abs(dist_res / dist_segment)); } } @@ -694,7 +697,7 @@ inline boost::optional calcLongitudinalOffsetPose( } if (src_idx + 1 == points.size() && offset == 0.0) { - return getPose(points.at(src_idx)); + return tier4_autoware_utils::getPose(points.at(src_idx)); } if (offset < 0.0) { @@ -707,12 +710,13 @@ inline boost::optional calcLongitudinalOffsetPose( 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); + const auto dist_segment = tier4_autoware_utils::calcDistance2d(p_front, p_back); dist_sum += dist_segment; const auto dist_res = -offset - dist_sum; if (dist_res <= 0.0) { - return calcInterpolatedPose(p_back, p_front, std::abs(dist_res / dist_segment)); + return tier4_autoware_utils::calcInterpolatedPose( + p_back, p_front, std::abs(dist_res / dist_segment)); } } } else { @@ -722,12 +726,13 @@ inline boost::optional calcLongitudinalOffsetPose( const auto & p_front = points.at(i); const auto & p_back = points.at(i + 1); - const auto dist_segment = calcDistance2d(p_front, p_back); + const auto dist_segment = tier4_autoware_utils::calcDistance2d(p_front, p_back); dist_sum += dist_segment; const auto dist_res = offset - dist_sum; if (dist_res <= 0.0) { - return calcInterpolatedPose(p_front, p_back, 1.0 - std::abs(dist_res / dist_segment)); + return tier4_autoware_utils::calcInterpolatedPose( + p_front, p_back, 1.0 - std::abs(dist_res / dist_segment)); } } } @@ -785,8 +790,8 @@ inline boost::optional insertTargetPoint( return {}; } - const auto p_front = getPoint(points.at(seg_idx)); - const auto p_back = getPoint(points.at(seg_idx + 1)); + 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); @@ -795,32 +800,34 @@ inline boost::optional insertTargetPoint( return {}; } - const auto overlap_with_front = calcDistance2d(p_target, p_front) < overlap_threshold; - const auto overlap_with_back = calcDistance2d(p_target, p_back) < overlap_threshold; + const auto overlap_with_front = + tier4_autoware_utils::calcDistance2d(p_target, p_front) < overlap_threshold; + const auto overlap_with_back = + tier4_autoware_utils::calcDistance2d(p_target, p_back) < overlap_threshold; geometry_msgs::msg::Pose target_pose; { - const auto pitch = calcElevationAngle(p_target, p_back); - const auto yaw = calcAzimuthAngle(p_target, p_back); + const auto pitch = tier4_autoware_utils::calcElevationAngle(p_target, p_back); + const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_target, p_back); target_pose.position = p_target; - target_pose.orientation = createQuaternionFromRPY(0.0, pitch, yaw); + target_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); } auto p_insert = points.at(seg_idx); - setPose(target_pose, p_insert); + tier4_autoware_utils::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); + const auto pitch = tier4_autoware_utils::calcElevationAngle(p_front, p_target); + const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_front, p_target); - front_pose.position = getPoint(points.at(seg_idx)); - front_pose.orientation = createQuaternionFromRPY(0.0, pitch, yaw); + front_pose.position = tier4_autoware_utils::getPoint(points.at(seg_idx)); + front_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); } if (!overlap_with_front && !overlap_with_back) { - setPose(front_pose, points.at(seg_idx)); + tier4_autoware_utils::setPose(front_pose, points.at(seg_idx)); points.insert(points.begin() + seg_idx + 1, p_insert); return seg_idx + 1; } @@ -903,11 +910,12 @@ inline boost::optional insertTargetPoint( const double target_length = std::max( 0.0, insert_point_length - calcSignedArcLength(points, start_segment_idx, *segment_idx)); const double ratio = std::clamp(target_length / segment_length, 0.0, 1.0); - const auto p_target = calcInterpolatedPoint( - getPoint(points.at(*segment_idx)), getPoint(points.at(*segment_idx + 1)), ratio); + 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); } -} // namespace tier4_autoware_utils +} // namespace motion_utils -#endif // TIER4_AUTOWARE_UTILS__TRAJECTORY__TRAJECTORY_HPP_ +#endif // MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/vehicle/vehicle_state_checker.hpp b/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp similarity index 87% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/vehicle/vehicle_state_checker.hpp rename to common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp index 47df469399170..873adadadd2c6 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/vehicle/vehicle_state_checker.hpp +++ b/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_ -#define TIER4_AUTOWARE_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_ +#ifndef MOTION_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_ +#define MOTION_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_ #include @@ -24,7 +24,7 @@ #include #include -namespace tier4_autoware_utils +namespace motion_utils { using autoware_auto_planning_msgs::msg::Trajectory; @@ -71,6 +71,6 @@ class VehicleArrivalChecker : public VehicleStopChecker void onTrajectory(const Trajectory::SharedPtr msg); }; -} // namespace tier4_autoware_utils +} // namespace motion_utils -#endif // TIER4_AUTOWARE_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_ +#endif // MOTION_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_ diff --git a/common/motion_utils/package.xml b/common/motion_utils/package.xml index 4d5cc2867a699..a1cbfbf0645b1 100644 --- a/common/motion_utils/package.xml +++ b/common/motion_utils/package.xml @@ -12,6 +12,7 @@ autoware_cmake autoware_auto_planning_msgs + autoware_auto_vehicle_msgs builtin_interfaces geometry_msgs libboost-dev diff --git a/common/tier4_autoware_utils/src/vehicle/vehicle_state_checker.cpp b/common/motion_utils/src/vehicle/vehicle_state_checker.cpp similarity index 89% rename from common/tier4_autoware_utils/src/vehicle/vehicle_state_checker.cpp rename to common/motion_utils/src/vehicle/vehicle_state_checker.cpp index 95d07fa865435..724840a104b9f 100644 --- a/common/tier4_autoware_utils/src/vehicle/vehicle_state_checker.cpp +++ b/common/motion_utils/src/vehicle/vehicle_state_checker.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/vehicle/vehicle_state_checker.hpp" +#include "motion_utils/vehicle/vehicle_state_checker.hpp" -#include "tier4_autoware_utils/trajectory/trajectory.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include -namespace tier4_autoware_utils +namespace motion_utils { VehicleStopChecker::VehicleStopChecker(rclcpp::Node * node) : clock_(node->get_clock()), logger_(node->get_logger()) @@ -104,15 +104,15 @@ bool VehicleArrivalChecker::isVehicleStoppedAtStopPoint(const double stop_durati } const auto & p = odometry_ptr_->pose.pose.position; - const auto idx = searchZeroVelocityIndex(trajectory_ptr_->points); + const auto idx = motion_utils::searchZeroVelocityIndex(trajectory_ptr_->points); if (!idx) { return false; } - return std::abs(calcSignedArcLength(trajectory_ptr_->points, p, idx.get())) < + return std::abs(motion_utils::calcSignedArcLength(trajectory_ptr_->points, p, idx.get())) < th_arrived_distance_m; } void VehicleArrivalChecker::onTrajectory(const Trajectory::SharedPtr msg) { trajectory_ptr_ = msg; } -} // namespace tier4_autoware_utils +} // namespace motion_utils diff --git a/common/tier4_autoware_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp similarity index 96% rename from common/tier4_autoware_utils/test/src/trajectory/test_path_with_lane_id.cpp rename to common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp index 419e816f328ad..477c24408efc8 100644 --- a/common/tier4_autoware_utils/test/src/trajectory/test_path_with_lane_id.cpp +++ b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" -#include "tier4_autoware_utils/trajectory/path_with_lane_id.hpp" #include #include @@ -63,9 +63,9 @@ PathWithLaneId generateTestTrajectory( TEST(trajectory, calcLongitudinalOffsetPointFromIndex_PathWithLaneId) { - using tier4_autoware_utils::calcArcLength; - using tier4_autoware_utils::calcLongitudinalOffsetPoint; - using tier4_autoware_utils::calcSignedArcLength; + using motion_utils::calcArcLength; + using motion_utils::calcLongitudinalOffsetPoint; + using motion_utils::calcSignedArcLength; using tier4_autoware_utils::getPoint; const auto traj = generateTestTrajectory(10, 1.0); @@ -139,9 +139,9 @@ TEST(trajectory, calcLongitudinalOffsetPointFromIndex_PathWithLaneId) TEST(trajectory, calcLongitudinalOffsetPointFromPoint_PathWithLaneId) { - using tier4_autoware_utils::calcArcLength; - using tier4_autoware_utils::calcLongitudinalOffsetPoint; - using tier4_autoware_utils::calcSignedArcLength; + using motion_utils::calcArcLength; + using motion_utils::calcLongitudinalOffsetPoint; + using motion_utils::calcSignedArcLength; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::getPoint; @@ -217,9 +217,9 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint_PathWithLaneId) TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_PathWithLaneId) { - using tier4_autoware_utils::calcArcLength; - using tier4_autoware_utils::calcLongitudinalOffsetPose; - using tier4_autoware_utils::calcSignedArcLength; + using motion_utils::calcArcLength; + using motion_utils::calcLongitudinalOffsetPose; + using motion_utils::calcSignedArcLength; using tier4_autoware_utils::getPoint; const auto traj = generateTestTrajectory(10, 1.0); @@ -301,9 +301,9 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_PathWithLaneId) TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_PathWithLaneId) { - using tier4_autoware_utils::calcArcLength; - using tier4_autoware_utils::calcLongitudinalOffsetPose; - using tier4_autoware_utils::calcSignedArcLength; + using motion_utils::calcArcLength; + using motion_utils::calcLongitudinalOffsetPose; + using motion_utils::calcSignedArcLength; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::getPoint; @@ -387,13 +387,13 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_PathWithLaneId) TEST(trajectory, insertTargetPoint_PathWithLaneId) { - using tier4_autoware_utils::calcArcLength; + using motion_utils::calcArcLength; + using motion_utils::findNearestSegmentIndex; + using motion_utils::insertTargetPoint; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::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); @@ -606,13 +606,13 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId) TEST(trajectory, insertTargetPoint_OverlapThreshold_PathWithLaneId) { - using tier4_autoware_utils::calcArcLength; + using motion_utils::calcArcLength; + using motion_utils::findNearestSegmentIndex; + using motion_utils::insertTargetPoint; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::findNearestSegmentIndex; using tier4_autoware_utils::getPose; - using tier4_autoware_utils::insertTargetPoint; constexpr double overlap_threshold = 1e-4; const auto traj = generateTestTrajectory(10, 1.0); @@ -699,13 +699,13 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold_PathWithLaneId) TEST(trajectory, insertTargetPoint_PathWithLaneId_Length) { - using tier4_autoware_utils::calcArcLength; + using motion_utils::calcArcLength; + using motion_utils::findNearestSegmentIndex; + using motion_utils::insertTargetPoint; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::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); diff --git a/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp similarity index 96% rename from common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp rename to common/motion_utils/test/src/trajectory/test_trajectory.cpp index 6d38e6fe42913..2ca7b6790aa04 100644 --- a/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" -#include "tier4_autoware_utils/trajectory/tmp_conversion.hpp" -#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include #include @@ -94,7 +94,7 @@ void updateTrajectoryVelocityAt(T & points, const size_t idx, const double vel) TEST(trajectory, validateNonEmpty) { - using tier4_autoware_utils::validateNonEmpty; + using motion_utils::validateNonEmpty; // Empty EXPECT_THROW(validateNonEmpty(Trajectory{}.points), std::invalid_argument); @@ -107,7 +107,7 @@ TEST(trajectory, validateNonEmpty) TEST(trajectory, validateNonSharpAngle_DefaultThreshold) { using autoware_auto_planning_msgs::msg::TrajectoryPoint; - using tier4_autoware_utils::validateNonSharpAngle; + using motion_utils::validateNonSharpAngle; TrajectoryPoint p1; p1.pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); @@ -136,8 +136,8 @@ TEST(trajectory, validateNonSharpAngle_DefaultThreshold) TEST(trajectory, validateNonSharpAngle_SetThreshold) { using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using motion_utils::validateNonSharpAngle; 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); @@ -165,7 +165,7 @@ TEST(trajectory, validateNonSharpAngle_SetThreshold) TEST(trajectory, searchZeroVelocityIndex) { - using tier4_autoware_utils::searchZeroVelocityIndex; + using motion_utils::searchZeroVelocityIndex; // Empty EXPECT_FALSE(searchZeroVelocityIndex(Trajectory{}.points)); @@ -244,7 +244,7 @@ TEST(trajectory, searchZeroVelocityIndex) TEST(trajectory, findNearestIndex_Pos_StraightTrajectory) { - using tier4_autoware_utils::findNearestIndex; + using motion_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0); @@ -275,7 +275,7 @@ TEST(trajectory, findNearestIndex_Pos_StraightTrajectory) TEST(trajectory, findNearestIndex_Pos_CurvedTrajectory) { - using tier4_autoware_utils::findNearestIndex; + using motion_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); @@ -285,7 +285,7 @@ TEST(trajectory, findNearestIndex_Pos_CurvedTrajectory) TEST(trajectory, findNearestIndex_Pose_NoThreshold) { - using tier4_autoware_utils::findNearestIndex; + using motion_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0); @@ -311,7 +311,7 @@ TEST(trajectory, findNearestIndex_Pose_NoThreshold) TEST(trajectory, findNearestIndex_Pose_DistThreshold) { - using tier4_autoware_utils::findNearestIndex; + using motion_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0); @@ -327,7 +327,7 @@ TEST(trajectory, findNearestIndex_Pose_DistThreshold) TEST(trajectory, findNearestIndex_Pose_YawThreshold) { - using tier4_autoware_utils::findNearestIndex; + using motion_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0); const auto max_d = std::numeric_limits::max(); @@ -346,7 +346,7 @@ TEST(trajectory, findNearestIndex_Pose_YawThreshold) TEST(trajectory, findNearestIndex_Pose_DistAndYawThreshold) { - using tier4_autoware_utils::findNearestIndex; + using motion_utils::findNearestIndex; const auto traj = generateTestTrajectory(10, 1.0); @@ -360,7 +360,7 @@ TEST(trajectory, findNearestIndex_Pose_DistAndYawThreshold) TEST(trajectory, findNearestSegmentIndex) { - using tier4_autoware_utils::findNearestSegmentIndex; + using motion_utils::findNearestSegmentIndex; const auto traj = generateTestTrajectory(10, 1.0); @@ -400,7 +400,7 @@ TEST(trajectory, findNearestSegmentIndex) TEST(trajectory, calcLongitudinalOffsetToSegment_StraightTrajectory) { - using tier4_autoware_utils::calcLongitudinalOffsetToSegment; + using motion_utils::calcLongitudinalOffsetToSegment; const auto traj = generateTestTrajectory(10, 1.0); const bool throw_exception = true; @@ -450,7 +450,7 @@ TEST(trajectory, calcLongitudinalOffsetToSegment_StraightTrajectory) TEST(trajectory, calcLongitudinalOffsetToSegment_CurveTrajectory) { - using tier4_autoware_utils::calcLongitudinalOffsetToSegment; + using motion_utils::calcLongitudinalOffsetToSegment; const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); @@ -462,7 +462,7 @@ TEST(trajectory, calcLongitudinalOffsetToSegment_CurveTrajectory) TEST(trajectory, calcLateralOffset) { - using tier4_autoware_utils::calcLateralOffset; + using motion_utils::calcLateralOffset; const auto traj = generateTestTrajectory(10, 1.0); const bool throw_exception = true; @@ -503,7 +503,7 @@ TEST(trajectory, calcLateralOffset) TEST(trajectory, calcLateralOffset_CurveTrajectory) { - using tier4_autoware_utils::calcLateralOffset; + using motion_utils::calcLateralOffset; const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); @@ -514,7 +514,7 @@ TEST(trajectory, calcLateralOffset_CurveTrajectory) TEST(trajectory, calcSignedArcLengthFromIndexToIndex) { - using tier4_autoware_utils::calcSignedArcLength; + using motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -537,7 +537,7 @@ TEST(trajectory, calcSignedArcLengthFromIndexToIndex) TEST(trajectory, calcSignedArcLengthFromPointToIndex) { - using tier4_autoware_utils::calcSignedArcLength; + using motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -566,7 +566,7 @@ TEST(trajectory, calcSignedArcLengthFromPointToIndex) TEST(trajectory, calcSignedArcLengthFromPoseToIndex_DistThreshold) { - using tier4_autoware_utils::calcSignedArcLength; + using motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -586,7 +586,7 @@ TEST(trajectory, calcSignedArcLengthFromPoseToIndex_DistThreshold) TEST(trajectory, calcSignedArcLengthFromPoseToIndex_YawThreshold) { - using tier4_autoware_utils::calcSignedArcLength; + using motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); const auto max_d = std::numeric_limits::max(); @@ -608,7 +608,7 @@ TEST(trajectory, calcSignedArcLengthFromPoseToIndex_YawThreshold) TEST(trajectory, calcSignedArcLengthFromIndexToPoint) { - using tier4_autoware_utils::calcSignedArcLength; + using motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -637,7 +637,7 @@ TEST(trajectory, calcSignedArcLengthFromIndexToPoint) TEST(trajectory, calcSignedArcLengthFromPointToPoint) { - using tier4_autoware_utils::calcSignedArcLength; + using motion_utils::calcSignedArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -700,7 +700,7 @@ TEST(trajectory, calcSignedArcLengthFromPointToPoint) TEST(trajectory, calcArcLength) { - using tier4_autoware_utils::calcArcLength; + using motion_utils::calcArcLength; const auto traj = generateTestTrajectory(10, 1.0); @@ -713,7 +713,7 @@ TEST(trajectory, calcArcLength) TEST(trajectory, convertToTrajectory) { - using tier4_autoware_utils::convertToTrajectory; + using motion_utils::convertToTrajectory; // Size check { @@ -736,7 +736,7 @@ TEST(trajectory, convertToTrajectory) TEST(trajectory, convertToTrajectoryPointArray) { - using tier4_autoware_utils::convertToTrajectoryPointArray; + using motion_utils::convertToTrajectoryPointArray; const auto traj_input = generateTestTrajectory(100, 1.0); const auto traj = convertToTrajectoryPointArray(traj_input); @@ -752,7 +752,7 @@ TEST(trajectory, convertToTrajectoryPointArray) TEST(trajectory, calcDistanceToForwardStopPointFromIndex) { - using tier4_autoware_utils::calcDistanceToForwardStopPoint; + using motion_utils::calcDistanceToForwardStopPoint; auto traj_input = generateTestTrajectory(100, 1.0, 3.0); traj_input.points.at(50).longitudinal_velocity_mps = 0.0; @@ -810,7 +810,7 @@ TEST(trajectory, calcDistanceToForwardStopPointFromIndex) TEST(trajectory, calcDistanceToForwardStopPointFromPose) { - using tier4_autoware_utils::calcDistanceToForwardStopPoint; + using motion_utils::calcDistanceToForwardStopPoint; auto traj_input = generateTestTrajectory(100, 1.0, 3.0); traj_input.points.at(50).longitudinal_velocity_mps = 0.0; @@ -894,7 +894,7 @@ TEST(trajectory, calcDistanceToForwardStopPointFromPose) TEST(trajectory, calcDistanceToForwardStopPoint_DistThreshold) { - using tier4_autoware_utils::calcDistanceToForwardStopPoint; + using motion_utils::calcDistanceToForwardStopPoint; auto traj_input = generateTestTrajectory(100, 1.0, 3.0); traj_input.points.at(50).longitudinal_velocity_mps = 0.0; @@ -938,7 +938,7 @@ TEST(trajectory, calcDistanceToForwardStopPoint_DistThreshold) TEST(trajectory, calcDistanceToForwardStopPoint_YawThreshold) { - using tier4_autoware_utils::calcDistanceToForwardStopPoint; + using motion_utils::calcDistanceToForwardStopPoint; using tier4_autoware_utils::deg2rad; const auto max_d = std::numeric_limits::max(); @@ -990,9 +990,9 @@ TEST(trajectory, calcDistanceToForwardStopPoint_YawThreshold) TEST(trajectory, calcLongitudinalOffsetPointFromIndex) { - using tier4_autoware_utils::calcArcLength; - using tier4_autoware_utils::calcLongitudinalOffsetPoint; - using tier4_autoware_utils::calcSignedArcLength; + using motion_utils::calcArcLength; + using motion_utils::calcLongitudinalOffsetPoint; + using motion_utils::calcSignedArcLength; using tier4_autoware_utils::getPoint; const auto traj = generateTestTrajectory(10, 1.0); @@ -1066,9 +1066,9 @@ TEST(trajectory, calcLongitudinalOffsetPointFromIndex) TEST(trajectory, calcLongitudinalOffsetPointFromPoint) { - using tier4_autoware_utils::calcArcLength; - using tier4_autoware_utils::calcLongitudinalOffsetPoint; - using tier4_autoware_utils::calcSignedArcLength; + using motion_utils::calcArcLength; + using motion_utils::calcLongitudinalOffsetPoint; + using motion_utils::calcSignedArcLength; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::getPoint; @@ -1144,9 +1144,9 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint) TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) { - using tier4_autoware_utils::calcArcLength; - using tier4_autoware_utils::calcLongitudinalOffsetPose; - using tier4_autoware_utils::calcSignedArcLength; + using motion_utils::calcArcLength; + using motion_utils::calcLongitudinalOffsetPose; + using motion_utils::calcSignedArcLength; using tier4_autoware_utils::getPoint; const auto traj = generateTestTrajectory(10, 1.0); @@ -1229,8 +1229,8 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) { using autoware_auto_planning_msgs::msg::TrajectoryPoint; - using tier4_autoware_utils::calcArcLength; - using tier4_autoware_utils::calcLongitudinalOffsetPose; + using motion_utils::calcArcLength; + using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::deg2rad; Trajectory traj{}; @@ -1314,9 +1314,9 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) { - using tier4_autoware_utils::calcArcLength; - using tier4_autoware_utils::calcLongitudinalOffsetPose; - using tier4_autoware_utils::calcSignedArcLength; + using motion_utils::calcArcLength; + using motion_utils::calcLongitudinalOffsetPose; + using motion_utils::calcSignedArcLength; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::getPoint; @@ -1401,9 +1401,9 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) 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 motion_utils::calcArcLength; + using motion_utils::calcLongitudinalOffsetPose; + using motion_utils::calcLongitudinalOffsetToSegment; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::deg2rad; @@ -1474,13 +1474,13 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) TEST(trajectory, insertTargetPoint) { - using tier4_autoware_utils::calcArcLength; + using motion_utils::calcArcLength; + using motion_utils::findNearestSegmentIndex; + using motion_utils::insertTargetPoint; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::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); @@ -1693,13 +1693,13 @@ TEST(trajectory, insertTargetPoint) TEST(trajectory, insertTargetPoint_OverlapThreshold) { - using tier4_autoware_utils::calcArcLength; + using motion_utils::calcArcLength; + using motion_utils::findNearestSegmentIndex; + using motion_utils::insertTargetPoint; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::findNearestSegmentIndex; using tier4_autoware_utils::getPose; - using tier4_autoware_utils::insertTargetPoint; constexpr double overlap_threshold = 1e-4; const auto traj = generateTestTrajectory(10, 1.0); @@ -1786,13 +1786,13 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold) TEST(trajectory, insertTargetPoint_Length) { - using tier4_autoware_utils::calcArcLength; + using motion_utils::calcArcLength; + using motion_utils::findNearestSegmentIndex; + using motion_utils::insertTargetPoint; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::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); @@ -2026,13 +2026,13 @@ TEST(trajectory, insertTargetPoint_Length) TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) { - using tier4_autoware_utils::calcArcLength; + using motion_utils::calcArcLength; + using motion_utils::findNearestSegmentIndex; + using motion_utils::insertTargetPoint; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::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); @@ -2212,13 +2212,13 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Idx) { - using tier4_autoware_utils::calcArcLength; + using motion_utils::calcArcLength; + using motion_utils::findNearestSegmentIndex; + using motion_utils::insertTargetPoint; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::deg2rad; - using tier4_autoware_utils::findNearestSegmentIndex; using tier4_autoware_utils::getPose; - using tier4_autoware_utils::insertTargetPoint; const auto traj = generateTestTrajectory(10, 1.0); diff --git a/common/tier4_autoware_utils/test/src/vehicle/test_vehicle_state_checker.cpp b/common/motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp similarity index 99% rename from common/tier4_autoware_utils/test/src/vehicle/test_vehicle_state_checker.cpp rename to common/motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp index f25d478dd9930..a41906a153b79 100644 --- a/common/tier4_autoware_utils/test/src/vehicle/test_vehicle_state_checker.cpp +++ b/common/motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "motion_utils/vehicle/vehicle_state_checker.hpp" #include "test_vehicle_state_checker_helper.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/vehicle/vehicle_state_checker.hpp" #include @@ -32,12 +32,12 @@ constexpr double STOP_DURATION_THRESHOLD_600_MS = 0.6; constexpr double STOP_DURATION_THRESHOLD_800_MS = 0.8; constexpr double STOP_DURATION_THRESHOLD_1000_MS = 1.0; +using motion_utils::VehicleArrivalChecker; +using motion_utils::VehicleStopChecker; using nav_msgs::msg::Odometry; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternion; using tier4_autoware_utils::createTranslation; -using tier4_autoware_utils::VehicleArrivalChecker; -using tier4_autoware_utils::VehicleStopChecker; class CheckerNode : public rclcpp::Node { diff --git a/common/tier4_autoware_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp b/common/motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp similarity index 100% rename from common/tier4_autoware_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp rename to common/motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp diff --git a/common/path_distance_calculator/package.xml b/common/path_distance_calculator/package.xml index 49d93ba161e63..e2d4864eeb306 100644 --- a/common/path_distance_calculator/package.xml +++ b/common/path_distance_calculator/package.xml @@ -12,6 +12,7 @@ autoware_cmake autoware_auto_planning_msgs + motion_utils rclcpp rclcpp_components tier4_autoware_utils diff --git a/common/path_distance_calculator/src/path_distance_calculator.cpp b/common/path_distance_calculator/src/path_distance_calculator.cpp index 71c7719c9f536..5e4e080393950 100644 --- a/common/path_distance_calculator/src/path_distance_calculator.cpp +++ b/common/path_distance_calculator/src/path_distance_calculator.cpp @@ -14,6 +14,7 @@ #include "path_distance_calculator.hpp" +#include #include #include @@ -51,8 +52,8 @@ PathDistanceCalculator::PathDistanceCalculator(const rclcpp::NodeOptions & optio RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "too short path"); } - const double distance = tier4_autoware_utils::calcSignedArcLength( - path->points, pose->pose.position, path->points.size() - 1); + const double distance = + motion_utils::calcSignedArcLength(path->points, pose->pose.position, path->points.size() - 1); tier4_debug_msgs::msg::Float64Stamped msg; msg.stamp = pose->header.stamp; diff --git a/common/tier4_autoware_utils/CMakeLists.txt b/common/tier4_autoware_utils/CMakeLists.txt index d13d88b73ed7e..b8b5fb6bb1894 100644 --- a/common/tier4_autoware_utils/CMakeLists.txt +++ b/common/tier4_autoware_utils/CMakeLists.txt @@ -8,7 +8,6 @@ find_package(Boost REQUIRED) ament_auto_add_library(tier4_autoware_utils SHARED src/tier4_autoware_utils.cpp - src/vehicle/vehicle_state_checker.cpp ) if(BUILD_TESTING) 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 c66d4fcaddba2..51ad4e74065aa 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 @@ -32,8 +32,5 @@ #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" #endif // TIER4_AUTOWARE_UTILS__TIER4_AUTOWARE_UTILS_HPP_ diff --git a/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp b/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp index 890b7ea45d94f..c2ff8cfca9281 100644 --- a/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp +++ b/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp @@ -19,8 +19,8 @@ #include #include +#include #include -#include #include #include diff --git a/common/tier4_debug_tools/package.xml b/common/tier4_debug_tools/package.xml index 3351b15ecea15..d0a6487db0744 100644 --- a/common/tier4_debug_tools/package.xml +++ b/common/tier4_debug_tools/package.xml @@ -13,6 +13,7 @@ autoware_auto_planning_msgs geometry_msgs + motion_utils rclcpp rclcpp_components tf2_ros diff --git a/common/tier4_debug_tools/src/lateral_error_publisher.cpp b/common/tier4_debug_tools/src/lateral_error_publisher.cpp index 3d7a0de7e6946..18c97a47aa55a 100644 --- a/common/tier4_debug_tools/src/lateral_error_publisher.cpp +++ b/common/tier4_debug_tools/src/lateral_error_publisher.cpp @@ -74,7 +74,7 @@ void LateralErrorPublisher::onGroundTruthPose( } // Search closest trajectory point with vehicle pose - const auto closest_index = tier4_autoware_utils::findNearestIndex( + const auto closest_index = motion_utils::findNearestIndex( current_trajectory_ptr_->points, current_vehicle_pose_ptr_->pose.pose, std::numeric_limits::max(), yaw_threshold_to_search_closest_); if (!closest_index) { diff --git a/control/lane_departure_checker/package.xml b/control/lane_departure_checker/package.xml index 1cf5fda88657f..6658dcc61bd57 100644 --- a/control/lane_departure_checker/package.xml +++ b/control/lane_departure_checker/package.xml @@ -18,6 +18,7 @@ eigen geometry_msgs lanelet2_extension + motion_utils nav_msgs rclcpp rclcpp_components diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index a4bf3b7578ea4..8cee01856e154 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -16,6 +16,7 @@ #include "lane_departure_checker/util/create_vehicle_footprint.hpp" +#include #include #include #include @@ -59,11 +60,10 @@ bool isInAnyLane(const lanelet::ConstLanelets & candidate_lanelets, const Point2 size_t findNearestIndexWithSoftYawConstraints( const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double yaw_threshold) { - const auto nearest_idx_optional = tier4_autoware_utils::findNearestIndex( + const auto nearest_idx_optional = motion_utils::findNearestIndex( trajectory.points, pose, std::numeric_limits::max(), yaw_threshold); - return nearest_idx_optional - ? *nearest_idx_optional - : tier4_autoware_utils::findNearestIndex(trajectory.points, pose.position); + return nearest_idx_optional ? *nearest_idx_optional + : motion_utils::findNearestIndex(trajectory.points, pose.position); } LinearRing2d createHullFromFootprints(const std::vector & footprints) diff --git a/control/operation_mode_transition_manager/package.xml b/control/operation_mode_transition_manager/package.xml index 73d6f8cb3eb79..d469750163547 100644 --- a/control/operation_mode_transition_manager/package.xml +++ b/control/operation_mode_transition_manager/package.xml @@ -12,6 +12,7 @@ autoware_auto_system_msgs autoware_auto_vehicle_msgs geometry_msgs + motion_utils rclcpp rclcpp_components std_srvs diff --git a/control/operation_mode_transition_manager/src/operation_mode_transition_manager.cpp b/control/operation_mode_transition_manager/src/operation_mode_transition_manager.cpp index 323122b5ff7d1..5a941b8adab44 100644 --- a/control/operation_mode_transition_manager/src/operation_mode_transition_manager.cpp +++ b/control/operation_mode_transition_manager/src/operation_mode_transition_manager.cpp @@ -14,6 +14,7 @@ #include "operation_mode_transition_manager/operation_mode_transition_manager.hpp" +#include "motion_utils/motion_utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include @@ -22,9 +23,9 @@ namespace operation_mode_transition_manager { +using motion_utils::findNearestIndex; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcYawDeviation; -using tier4_autoware_utils::findNearestIndex; OperationModeTransitionManager::OperationModeTransitionManager(const rclcpp::NodeOptions & options) : Node("operation_mode_transition_manager", options) diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index 374c80946bd3a..9a38c45c35c0d 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -14,6 +14,7 @@ #include "operation_mode_transition_manager/state.hpp" +#include "motion_utils/motion_utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include @@ -22,9 +23,9 @@ namespace operation_mode_transition_manager { +using motion_utils::findNearestIndex; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcYawDeviation; -using tier4_autoware_utils::findNearestIndex; EngageStateBase::EngageStateBase(const State state, rclcpp::Node * node) : logger_(node->get_logger()), clock_(node->get_clock()), state_(state) diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp index 6cd729841fe77..75f70defc6420 100644 --- a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp +++ b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp @@ -21,8 +21,8 @@ #include "geometry/common_2d.hpp" #include "motion_common/motion_common.hpp" #include "motion_common/trajectory_common.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" -#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "trajectory_follower/visibility_control.hpp" #include // NOLINT @@ -112,13 +112,13 @@ TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint lerpTrajectoryPoint( const T & points, const Pose & pose, const float64_t max_dist, const float64_t max_yaw) { TrajectoryPoint interpolated_point; - auto seg_idx = tier4_autoware_utils::findNearestSegmentIndex(points, pose, max_dist, max_yaw); + auto seg_idx = motion_utils::findNearestSegmentIndex(points, pose, max_dist, max_yaw); if (!seg_idx) { // if not fund idx - seg_idx = tier4_autoware_utils::findNearestSegmentIndex(points, pose); + seg_idx = motion_utils::findNearestSegmentIndex(points, pose); } const float64_t len_to_interpolated = - tier4_autoware_utils::calcLongitudinalOffsetToSegment(points, *seg_idx, pose.position); + motion_utils::calcLongitudinalOffsetToSegment(points, *seg_idx, pose.position); const float64_t len_segment = trajectory_common::calcSignedArcLength(points, *seg_idx, *seg_idx + 1); const float64_t interpolate_ratio = std::clamp(len_to_interpolated / len_segment, 0.0, 1.0); diff --git a/control/trajectory_follower/package.xml b/control/trajectory_follower/package.xml index 974c066d7b81c..296c7f7f6c59a 100644 --- a/control/trajectory_follower/package.xml +++ b/control/trajectory_follower/package.xml @@ -21,6 +21,7 @@ geometry_msgs interpolation motion_common + motion_utils osqp_interface rclcpp rclcpp_components diff --git a/control/trajectory_follower/src/longitudinal_controller_utils.cpp b/control/trajectory_follower/src/longitudinal_controller_utils.cpp index a60ac9c2da2a2..3269c7766c645 100644 --- a/control/trajectory_follower/src/longitudinal_controller_utils.cpp +++ b/control/trajectory_follower/src/longitudinal_controller_utils.cpp @@ -70,12 +70,12 @@ float64_t calcStopDistance( trajectory_common::searchZeroVelocityIndex(traj.points); auto seg_idx = - tier4_autoware_utils::findNearestSegmentIndex(traj.points, current_pose, max_dist, max_yaw); + motion_utils::findNearestSegmentIndex(traj.points, current_pose, max_dist, max_yaw); if (!seg_idx) { // if not fund idx - seg_idx = tier4_autoware_utils::findNearestSegmentIndex(traj.points, current_pose); + seg_idx = motion_utils::findNearestSegmentIndex(traj.points, current_pose); } - const float64_t signed_length_src_offset = tier4_autoware_utils::calcLongitudinalOffsetToSegment( - traj.points, *seg_idx, current_pose.position); + const float64_t signed_length_src_offset = + motion_utils::calcLongitudinalOffsetToSegment(traj.points, *seg_idx, current_pose.position); if (std::isnan(signed_length_src_offset)) { return 0.0; @@ -84,12 +84,12 @@ float64_t calcStopDistance( // If no zero velocity point, return the length between current_pose to the end of trajectory. if (!stop_idx_opt) { float64_t signed_length_on_traj = - tier4_autoware_utils::calcSignedArcLength(traj.points, *seg_idx, traj.points.size() - 1); + motion_utils::calcSignedArcLength(traj.points, *seg_idx, traj.points.size() - 1); return signed_length_on_traj - signed_length_src_offset; } float64_t signed_length_on_traj = - tier4_autoware_utils::calcSignedArcLength(traj.points, *seg_idx, *stop_idx_opt); + motion_utils::calcSignedArcLength(traj.points, *seg_idx, *stop_idx_opt); return signed_length_on_traj - signed_length_src_offset; } diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp index 2d7e2550ebfee..1a6f3601090b6 100644 --- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp +++ b/control/trajectory_follower/src/pid_longitudinal_controller.cpp @@ -794,7 +794,7 @@ PidLongitudinalController::Motion PidLongitudinalController::keepBrakeBeforeStop if (m_enable_brake_keeping_before_stop == false) { return output_motion; } - // const auto stop_idx = tier4_autoware_utils::searchZeroVelocityIndex(traj.points); + // const auto stop_idx = motion_utils::searchZeroVelocityIndex(traj.points); const auto stop_idx = motion_common::searchZeroVelocityIndex(traj.points); if (!stop_idx) { return output_motion; diff --git a/control/trajectory_follower_nodes/package.xml b/control/trajectory_follower_nodes/package.xml index 7c9ffde6b898c..b3a982b0a6b9b 100644 --- a/control/trajectory_follower_nodes/package.xml +++ b/control/trajectory_follower_nodes/package.xml @@ -15,6 +15,7 @@ autoware_auto_planning_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs + motion_utils pure_pursuit rclcpp rclcpp_components diff --git a/control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp b/control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp index db3c2e246f9ab..ecaa12398a68e 100644 --- a/control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp +++ b/control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp @@ -14,6 +14,7 @@ #include "trajectory_follower_nodes/simple_trajectory_follower.hpp" +#include #include #include @@ -21,9 +22,9 @@ namespace simple_trajectory_follower { +using motion_utils::findNearestIndex; using tier4_autoware_utils::calcLateralDeviation; using tier4_autoware_utils::calcYawDeviation; -using tier4_autoware_utils::findNearestIndex; SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & options) : Node("simple_trajectory_follower", options) diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 3740f47a4fd91..3bda53beb588a 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml index 531d3e967b8ba..98e7e125f78af 100644 --- a/perception/map_based_prediction/package.xml +++ b/perception/map_based_prediction/package.xml @@ -14,6 +14,7 @@ autoware_auto_perception_msgs interpolation lanelet2_extension + motion_utils rclcpp rclcpp_components tf2 diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 993a621684e54..c760abdbfae5f 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -771,7 +771,7 @@ float MapBasedPredictionNode::calculateLocalLikelihood( converted_centerline.push_back(converted_p); } const double lat_dist = - std::fabs(tier4_autoware_utils::calcLateralOffset(converted_centerline, obj_point)); + std::fabs(motion_utils::calcLateralOffset(converted_centerline, obj_point)); // Compute Chi-squared distributed (Equation (8) in the paper) const double sigma_d = sigma_lateral_offset_; // Standard Deviation for lateral position @@ -1055,7 +1055,7 @@ double MapBasedPredictionNode::calcRightLateralOffset( boundary_path[i] = tier4_autoware_utils::createPoint(x, y, 0.0); } - return std::fabs(tier4_autoware_utils::calcLateralOffset(boundary_path, search_pose.position)); + return std::fabs(motion_utils::calcLateralOffset(boundary_path, search_pose.position)); } double MapBasedPredictionNode::calcLeftLateralOffset( @@ -1235,7 +1235,7 @@ PosePath MapBasedPredictionNode::resamplePath(const PosePath & base_path) const base_x.at(i) = base_path.at(i).position.x; base_y.at(i) = base_path.at(i).position.y; base_z.at(i) = base_path.at(i).position.z; - base_s.at(i) = tier4_autoware_utils::calcSignedArcLength(base_path, 0, i); + base_s.at(i) = motion_utils::calcSignedArcLength(base_path, 0, i); } const double base_path_len = base_s.back(); diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 4b143e9cd8a29..4d594f46c8af8 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -15,6 +15,7 @@ #include "map_based_prediction/path_generator.hpp" #include +#include #include #include @@ -170,7 +171,7 @@ PredictedPath PathGenerator::generatePolynomialPath( const TrackedObject & object, const PosePath & ref_path) { // Get current Frenet Point - const double ref_path_len = tier4_autoware_utils::calcArcLength(ref_path); + const double ref_path_len = motion_utils::calcArcLength(ref_path); const auto current_point = getFrenetPoint(object, ref_path); // Step1. Set Target Frenet Point @@ -296,7 +297,7 @@ PosePath PathGenerator::interpolateReferencePath( base_path_x.push_back(base_path.at(i).position.x); base_path_y.push_back(base_path.at(i).position.y); base_path_z.push_back(base_path.at(i).position.z); - base_path_s.push_back(tier4_autoware_utils::calcSignedArcLength(base_path, 0, i)); + base_path_s.push_back(motion_utils::calcSignedArcLength(base_path, 0, i)); } std::vector resampled_s(frenet_predicted_path.size()); @@ -369,10 +370,9 @@ FrenetPoint PathGenerator::getFrenetPoint(const TrackedObject & object, const Po FrenetPoint frenet_point; const auto obj_point = object.kinematics.pose_with_covariance.pose.position; - const size_t nearest_segment_idx = - tier4_autoware_utils::findNearestSegmentIndex(ref_path, obj_point); + const size_t nearest_segment_idx = motion_utils::findNearestSegmentIndex(ref_path, obj_point); const double l = - tier4_autoware_utils::calcLongitudinalOffsetToSegment(ref_path, nearest_segment_idx, obj_point); + motion_utils::calcLongitudinalOffsetToSegment(ref_path, nearest_segment_idx, obj_point); const float vx = static_cast(object.kinematics.twist_with_covariance.twist.linear.x); const float vy = static_cast(object.kinematics.twist_with_covariance.twist.linear.y); const float obj_yaw = @@ -381,8 +381,8 @@ FrenetPoint PathGenerator::getFrenetPoint(const TrackedObject & object, const Po static_cast(tf2::getYaw(ref_path.at(nearest_segment_idx).orientation)); const float delta_yaw = obj_yaw - lane_yaw; - frenet_point.s = tier4_autoware_utils::calcSignedArcLength(ref_path, 0, nearest_segment_idx) + l; - frenet_point.d = tier4_autoware_utils::calcLateralOffset(ref_path, obj_point); + frenet_point.s = motion_utils::calcSignedArcLength(ref_path, 0, nearest_segment_idx) + l; + frenet_point.d = motion_utils::calcLateralOffset(ref_path, obj_point); frenet_point.s_vel = vx * std::cos(delta_yaw) - vy * std::sin(delta_yaw); frenet_point.d_vel = vx * std::sin(delta_yaw) + vy * std::cos(delta_yaw); frenet_point.s_acc = 0.0; diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index cbb1cf0a612a0..85dccb3dc2c0a 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -25,6 +25,7 @@ lanelet2_extension libboost-dev libopencv-dev + motion_utils rclcpp rclcpp_components route_handler diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp index b5a97435a623b..a8ed63359b9ea 100644 --- a/planning/behavior_path_planner/src/path_utilities.cpp +++ b/planning/behavior_path_planner/src/path_utilities.cpp @@ -183,13 +183,13 @@ size_t getIdxByArclength(const PathWithLaneId & path, const Pose & origin, const throw std::runtime_error("[getIdxByArclength] path points must be > 0"); } - const auto boost_closest_idx = tier4_autoware_utils::findNearestIndex( + const auto boost_closest_idx = motion_utils::findNearestIndex( path.points, origin, std::numeric_limits::max(), M_PI / 4.0); // If the nearest index search with angle limit fails, search again without angle limit. size_t closest_idx = boost_closest_idx ? *boost_closest_idx - : tier4_autoware_utils::findNearestIndex(path.points, origin.position); + : motion_utils::findNearestIndex(path.points, origin.position); using tier4_autoware_utils::calcDistance2d; double sum_length = 0.0; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 0f1dfb992eb6f..4a4456ccac553 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -40,11 +40,11 @@ namespace behavior_path_planner { +using motion_utils::calcSignedArcLength; +using motion_utils::findNearestIndex; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcLateralDeviation; -using tier4_autoware_utils::calcSignedArcLength; using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::findNearestIndex; AvoidanceModule::AvoidanceModule( const std::string & name, rclcpp::Node & node, const AvoidanceParameters & parameters) @@ -1988,7 +1988,7 @@ PathWithLaneId AvoidanceModule::calcCenterLinePath( // for debug: check if the path backward distance is same as the desired length. // { - // const auto back_to_ego = tier4_autoware_utils::calcSignedArcLength( + // const auto back_to_ego = motion_utils::calcSignedArcLength( // centerline_path.points, centerline_path.points.front().point.pose.position, // getEgoPosition()); // RCLCPP_INFO(getLogger(), "actual back_to_ego distance = %f", back_to_ego); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp index 801dff7b7cd27..f3acfd78088a3 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp @@ -146,14 +146,13 @@ void fillLongitudinalAndLengthByClosestFootprint( tier4_autoware_utils::Polygon2d object_poly{}; util::calcObjectPolygon(object, &object_poly); - const double distance = tier4_autoware_utils::calcSignedArcLength( + const double distance = motion_utils::calcSignedArcLength( path.points, ego_pos, object.kinematics.initial_pose_with_covariance.pose.position); double min_distance = distance; double max_distance = distance; for (const auto & p : object_poly.outer()) { const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); - const double arc_length = - tier4_autoware_utils::calcSignedArcLength(path.points, ego_pos, point); + const double arc_length = motion_utils::calcSignedArcLength(path.points, ego_pos, point); min_distance = std::min(min_distance, arc_length); max_distance = std::max(max_distance, arc_length); } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index ac9bae1851eb1..b4eddfa2a5bc1 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -194,7 +194,7 @@ CandidateOutput LaneChangeModule::planCandidate() const output.path_candidate = selected_path.path; output.lateral_shift = selected_path.shifted_path.shift_length.at(end_idx) - selected_path.shifted_path.shift_length.at(start_idx); - output.distance_to_path_change = tier4_autoware_utils::calcSignedArcLength( + output.distance_to_path_change = motion_utils::calcSignedArcLength( selected_path.path.points, planner_data_->self_pose->pose.position, selected_path.shift_point.start.position); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 2ba50833ec166..250390eafdd67 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -184,9 +184,9 @@ std::vector getLaneChangePaths( shift_point.length = lane_change_start_on_self_lane_arc.distance; shift_point.start = lane_change_start_on_self_lane; shift_point.end = lane_change_end_on_target_lane; - shift_point.start_idx = tier4_autoware_utils::findNearestIndex( + shift_point.start_idx = motion_utils::findNearestIndex( target_lane_reference_path.points, lane_change_start_on_self_lane.position); - shift_point.end_idx = tier4_autoware_utils::findNearestIndex( + shift_point.end_idx = motion_utils::findNearestIndex( target_lane_reference_path.points, lane_change_end_on_target_lane.position); } @@ -202,7 +202,7 @@ std::vector getLaneChangePaths( rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), "failed to generate shifted path."); } - const auto lanechange_end_idx = tier4_autoware_utils::findNearestIndex( + const auto lanechange_end_idx = motion_utils::findNearestIndex( shifted_path.path.points, reference_path2.points.front().point.pose); if (lanechange_end_idx) { for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { @@ -224,7 +224,7 @@ std::vector getLaneChangePaths( static_cast( std::max(lane_change_distance / lane_changing_duration, minimum_lane_change_velocity))); const auto nearest_idx = - tier4_autoware_utils::findNearestIndex(reference_path2.points, point.point.pose); + motion_utils::findNearestIndex(reference_path2.points, point.point.pose); point.lane_ids = reference_path2.points.at(*nearest_idx).lane_ids; } diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 054cf7e29208b..5d3ab62dca23b 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -199,7 +199,7 @@ CandidateOutput PullOutModule::planCandidate() const selected_retreat_path.pull_out_path.path.header = planner_data_->route_handler->getRouteHeader(); CandidateOutput output_retreat(selected_retreat_path.pull_out_path.path); - output_retreat.distance_to_path_change = tier4_autoware_utils::calcSignedArcLength( + output_retreat.distance_to_path_change = motion_utils::calcSignedArcLength( selected_retreat_path.pull_out_path.path.points, current_pose.position, selected_retreat_path.backed_pose.position); return output_retreat; @@ -210,7 +210,7 @@ CandidateOutput PullOutModule::planCandidate() const selected_path.path.header = planner_data_->route_handler->getRouteHeader(); CandidateOutput output(selected_path.path); - output.distance_to_path_change = tier4_autoware_utils::calcSignedArcLength( + output.distance_to_path_change = motion_utils::calcSignedArcLength( selected_path.path.points, current_pose.position, selected_path.shift_point.start.position); return output; } @@ -482,7 +482,7 @@ std::pair PullOutModule::getSafeRetreatPath( const auto shoulder_line_path = route_handler->getCenterLinePath( pull_out_lanes, arc_position_pose.length - pull_out_lane_length_, arc_position_pose.length + pull_out_lane_length_); - const auto idx = tier4_autoware_utils::findNearestIndex(shoulder_line_path.points, current_pose); + const auto idx = motion_utils::findNearestIndex(shoulder_line_path.points, current_pose); const auto yaw_shoulder_lane = tf2::getYaw(shoulder_line_path.points.at(*idx).point.pose.orientation); @@ -559,8 +559,7 @@ bool PullOutModule::getBackDistance( const auto shoulder_line_path = route_handler->getCenterLinePath( pull_out_lanes, arc_position_pose.length - pull_out_lane_length_, arc_position_pose.length + pull_out_lane_length_); - const auto idx = - tier4_autoware_utils::findNearestIndex(shoulder_line_path.points, current_pose); + const auto idx = motion_utils::findNearestIndex(shoulder_line_path.points, current_pose); yaw_shoulder_lane = tf2::getYaw(shoulder_line_path.points.at(*idx).point.pose.orientation); } diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp index 5991c6f60e713..4d1220b63a58a 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp @@ -194,11 +194,11 @@ std::vector getPullOutPaths( continue; } - const auto pull_out_end_idx = tier4_autoware_utils::findNearestIndex( + const auto pull_out_end_idx = motion_utils::findNearestIndex( shifted_path.path.points, reference_path2.points.front().point.pose); const auto goal_idx = - tier4_autoware_utils::findNearestIndex(shifted_path.path.points, route_handler.getGoalPose()); + motion_utils::findNearestIndex(shifted_path.path.points, route_handler.getGoalPose()); if (pull_out_end_idx && goal_idx) { const auto distance_pull_out_end_to_goal = tier4_autoware_utils::calcDistance2d( diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 725b2b369887b..f3bde34014ead 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -33,17 +34,17 @@ #include #include +using motion_utils::calcLongitudinalOffsetPose; +using motion_utils::calcSignedArcLength; +using motion_utils::findNearestIndex; using nav_msgs::msg::OccupancyGrid; using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::calcSignedArcLength; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromYaw; -using tier4_autoware_utils::findNearestIndex; using tier4_autoware_utils::inverseTransformPose; using tier4_autoware_utils::transformPose; diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp index 8c21542372831..98078deee0bd7 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp @@ -221,10 +221,9 @@ std::vector getShiftParkingPaths( } const auto shift_end_idx = - tier4_autoware_utils::findNearestIndex(shifted_path.path.points, shift_end_point.point.pose); + motion_utils::findNearestIndex(shifted_path.path.points, shift_end_point.point.pose); - const auto goal_idx = - tier4_autoware_utils::findNearestIndex(shifted_path.path.points, goal_pose); + const auto goal_idx = motion_utils::findNearestIndex(shifted_path.path.points, goal_pose); if (shift_end_idx && goal_idx) { for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { @@ -266,15 +265,15 @@ std::vector getShiftParkingPaths( goal_path_point.lane_ids = shifted_path.path.points.back().lane_ids; candidate_path.path.points.push_back(goal_path_point); - const auto shift_start_idx = tier4_autoware_utils::findNearestIndex( - candidate_path.path.points, shift_point.start.position); + const auto shift_start_idx = + motion_utils::findNearestIndex(candidate_path.path.points, shift_point.start.position); for (size_t i = shift_start_idx; i < candidate_path.path.points.size(); i++) { candidate_path.shifted_path.path.points.push_back(candidate_path.path.points.at(i)); } - shift_point.start_idx = tier4_autoware_utils::findNearestIndex( + shift_point.start_idx = motion_utils::findNearestIndex( candidate_path.shifted_path.path.points, shift_point.start.position); - shift_point.end_idx = tier4_autoware_utils::findNearestIndex( + shift_point.end_idx = motion_utils::findNearestIndex( candidate_path.shifted_path.path.points, shift_point.end.position); // todo: shift_length size dose not match path size due to resample, // so sume fuctions (like getTurnInfo()) can not be used with this shifted_point. diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index feb3b769f3144..a42c825622de4 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -207,11 +207,11 @@ bool SideShiftModule::addShiftPoint() auto shift_points = path_shifter_.getShiftPoints(); const auto calcLongitudinal_to_shift_start = [this](const auto & sp) { - return tier4_autoware_utils::calcSignedArcLength( + return motion_utils::calcSignedArcLength( reference_path_->points, getEgoPose().pose.position, sp.start.position); }; const auto calcLongitudinal_to_shift_end = [this](const auto & sp) { - return tier4_autoware_utils::calcSignedArcLength( + return motion_utils::calcSignedArcLength( reference_path_->points, getEgoPose().pose.position, sp.end.position); }; @@ -406,7 +406,7 @@ double SideShiftModule::getClosestShiftLength() const } const auto ego_point = planner_data_->self_pose->pose.position; - const auto closest = tier4_autoware_utils::findNearestIndex(prev_output_.path.points, ego_point); + const auto closest = motion_utils::findNearestIndex(prev_output_.path.points, ego_point); return prev_output_.shift_length.at(closest); } @@ -439,7 +439,7 @@ PoseStamped SideShiftModule::getUnshiftedEgoPose(const ShiftedPath & prev_path) // un-shifted fot current ideal pose const auto closest = - tier4_autoware_utils::findNearestIndex(prev_path.path.points, ego_pose.pose.position); + motion_utils::findNearestIndex(prev_path.path.points, ego_pose.pose.position); PoseStamped unshifted_pose = ego_pose; diff --git a/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp b/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp index b3cfddf50fbc9..7301c07e686a5 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp @@ -260,7 +260,7 @@ bool PathShifter::calcShiftPointFromArcLength( return false; } - const auto origin_idx = tier4_autoware_utils::findNearestIndex(path.points, origin); + const auto origin_idx = motion_utils::findNearestIndex(path.points, origin); const auto arclength_from_origin = util::calcPathArcLengthArray(path, origin_idx); if (dist_to_end > arclength_from_origin.back()) { @@ -311,8 +311,8 @@ bool PathShifter::calcShiftPointFromArcLength( void PathShifter::updateShiftPointIndices() { for (auto & p : shift_points_) { - p.start_idx = tier4_autoware_utils::findNearestIndex(reference_path_.points, p.start.position); - p.end_idx = tier4_autoware_utils::findNearestIndex(reference_path_.points, p.end.position); + p.start_idx = motion_utils::findNearestIndex(reference_path_.points, p.start.position); + p.end_idx = motion_utils::findNearestIndex(reference_path_.points, p.end.position); } is_index_aligned_ = true; } @@ -381,7 +381,7 @@ bool PathShifter::sortShiftPointsAlongPath([[maybe_unused]] const PathWithLaneId void PathShifter::removeBehindShiftPointAndSetBaseOffset(const Point & base_point) { - const auto base_idx = tier4_autoware_utils::findNearestIndex(reference_path_.points, base_point); + const auto base_idx = motion_utils::findNearestIndex(reference_path_.points, base_point); // If shift_point.end is behind the base_point, remove the shift_point and // set its shift_length to the base_offset. diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 993565c7c90a6..58a685fe88cf6 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -101,11 +101,11 @@ std::array getLaneletScope( points.push_back(p); } const size_t nearest_segment_idx = - tier4_autoware_utils::findNearestSegmentIndex(points, current_pose.position); + motion_utils::findNearestSegmentIndex(points, current_pose.position); // forward lanelet const auto forward_offset_length = - tier4_autoware_utils::calcSignedArcLength(points, current_pose.position, nearest_segment_idx); + motion_utils::calcSignedArcLength(points, current_pose.position, nearest_segment_idx); double sum_length = std::min(forward_offset_length, 0.0); size_t current_lane_idx = nearest_lane_idx; auto current_lane = lanes.at(current_lane_idx); @@ -152,8 +152,8 @@ std::array getLaneletScope( // backward lanelet current_point_idx = nearest_segment_idx + 1; - const auto backward_offset_length = tier4_autoware_utils::calcSignedArcLength( - points, nearest_segment_idx + 1, current_pose.position); + const auto backward_offset_length = + motion_utils::calcSignedArcLength(points, nearest_segment_idx + 1, current_pose.position); sum_length = std::min(backward_offset_length, 0.0); current_lane_idx = nearest_lane_idx; current_lane = lanes.at(current_lane_idx); @@ -236,14 +236,12 @@ FrenetCoordinate3d convertToFrenetCoordinate3d( FrenetCoordinate3d frenet_coordinate; const size_t nearest_segment_idx = - tier4_autoware_utils::findNearestSegmentIndex(linestring, search_point_geom); - const double longitudinal_length = tier4_autoware_utils::calcLongitudinalOffsetToSegment( + motion_utils::findNearestSegmentIndex(linestring, search_point_geom); + const double longitudinal_length = motion_utils::calcLongitudinalOffsetToSegment( linestring, nearest_segment_idx, search_point_geom); frenet_coordinate.length = - tier4_autoware_utils::calcSignedArcLength(linestring, 0, nearest_segment_idx) + - longitudinal_length; - frenet_coordinate.distance = - tier4_autoware_utils::calcLateralOffset(linestring, search_point_geom); + motion_utils::calcSignedArcLength(linestring, 0, nearest_segment_idx) + longitudinal_length; + frenet_coordinate.distance = motion_utils::calcLateralOffset(linestring, search_point_geom); return frenet_coordinate; } @@ -864,12 +862,12 @@ bool setGoal( PathPointWithLaneId refined_goal{}; { // NOTE: goal does not have valid z, that will be calculated by interpolation here const size_t closest_seg_idx = - tier4_autoware_utils::findNearestSegmentIndex(input.points, goal.position); - const double closest_to_goal_dist = tier4_autoware_utils::calcSignedArcLength( + motion_utils::findNearestSegmentIndex(input.points, goal.position); + const double closest_to_goal_dist = motion_utils::calcSignedArcLength( input.points, input.points.at(closest_seg_idx).point.pose.position, goal.position); // TODO(murooka) implement calcSignedArcLength(points, idx, point) - const double seg_dist = tier4_autoware_utils::calcSignedArcLength( - input.points, closest_seg_idx, closest_seg_idx + 1); + const double seg_dist = + motion_utils::calcSignedArcLength(input.points, closest_seg_idx, closest_seg_idx + 1); const double closest_z = input.points.at(closest_seg_idx).point.pose.position.z; const double next_z = input.points.at(closest_seg_idx + 1).point.pose.position.z; const double goal_z = std::abs(seg_dist) < 1e-6 @@ -898,12 +896,12 @@ bool setGoal( pre_refined_goal.point.pose.orientation = goal.orientation; { // NOTE: interpolate z and velocity of pre_refined_goal - const size_t closest_seg_idx = tier4_autoware_utils::findNearestSegmentIndex( - input.points, pre_refined_goal.point.pose.position); - const double closest_to_pre_goal_dist = tier4_autoware_utils::calcSignedArcLength( + const size_t closest_seg_idx = + motion_utils::findNearestSegmentIndex(input.points, pre_refined_goal.point.pose.position); + const double closest_to_pre_goal_dist = motion_utils::calcSignedArcLength( input.points, input.points.at(closest_seg_idx).point.pose.position, pre_refined_goal.point.pose.position); - const double seg_dist = tier4_autoware_utils::calcSignedArcLength( + const double seg_dist = motion_utils::calcSignedArcLength( input.points, closest_seg_idx, closest_seg_idx + 1); // TODO(murooka) implement calcSignedArcLength(points, idx, point) @@ -1830,7 +1828,7 @@ PathWithLaneId setDecelerationVelocity( for (auto & point : reference_path.points) { const auto arclength_to_target = std::max( - 0.0, tier4_autoware_utils::calcSignedArcLength( + 0.0, motion_utils::calcSignedArcLength( reference_path.points, point.point.pose.position, target_pose.position) + buffer); if (arclength_to_target > deceleration_interval) continue; @@ -1843,8 +1841,7 @@ PathWithLaneId setDecelerationVelocity( } const auto stop_point_length = - tier4_autoware_utils::calcSignedArcLength(reference_path.points, 0, target_pose.position) + - buffer; + motion_utils::calcSignedArcLength(reference_path.points, 0, target_pose.position) + buffer; if (target_velocity == 0.0 && stop_point_length > 0) { const auto stop_point = util::insertStopPoint(stop_point_length, &reference_path); } @@ -1859,7 +1856,7 @@ PathWithLaneId setDecelerationVelocityForTurnSignal( for (auto & point : reference_path.points) { const auto arclength_to_target = std::max( - 0.0, tier4_autoware_utils::calcSignedArcLength( + 0.0, motion_utils::calcSignedArcLength( reference_path.points, point.point.pose.position, target_pose.position)); point.point.longitudinal_velocity_mps = std::min( point.point.longitudinal_velocity_mps, @@ -1867,7 +1864,7 @@ PathWithLaneId setDecelerationVelocityForTurnSignal( } const auto stop_point_length = - tier4_autoware_utils::calcSignedArcLength(reference_path.points, 0, target_pose.position); + motion_utils::calcSignedArcLength(reference_path.points, 0, target_pose.position); if (stop_point_length > 0) { const auto stop_point = util::insertStopPoint(stop_point_length, &reference_path); } diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp index d8ac7802b7b72..c407d2ad8168f 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp @@ -18,8 +18,8 @@ #include #include #include +#include #include -#include #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp index 1a769e3139164..0b8f2b4ef444f 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp @@ -38,7 +38,7 @@ geometry_msgs::msg::Point findLongitudinalNearestPoint( geometry_msgs::msg::Point min_dist_point{}; for (const auto & p : target_points) { - const float dist = tier4_autoware_utils::calcSignedArcLength(points, src_point, p); + const float dist = motion_utils::calcSignedArcLength(points, src_point, p); if (dist < min_dist) { min_dist = dist; min_dist_point = p; @@ -52,14 +52,13 @@ template size_t calcIndexByLength( const T & points, const geometry_msgs::msg::Pose & current_pose, const double target_length) { - const size_t nearest_index = - tier4_autoware_utils::findNearestIndex(points, current_pose.position); + const size_t nearest_index = motion_utils::findNearestIndex(points, current_pose.position); if (target_length < 0) { return nearest_index; } for (size_t i = nearest_index; i < points.size(); i++) { - double length_sum = tier4_autoware_utils::calcSignedArcLength(points, current_pose.position, i); + double length_sum = motion_utils::calcSignedArcLength(points, current_pose.position, i); if (length_sum > target_length) { return i; } @@ -73,14 +72,13 @@ template size_t calcIndexByLengthReverse( const T & points, const geometry_msgs::msg::Point & src_point, const float target_length) { - const auto nearest_seg_idx = tier4_autoware_utils::findNearestSegmentIndex(points, src_point); + const auto nearest_seg_idx = motion_utils::findNearestSegmentIndex(points, src_point); if (nearest_seg_idx == 0) { return 0; } for (size_t i = nearest_seg_idx; i > 0; i--) { - const auto length_sum = - std::abs(tier4_autoware_utils::calcSignedArcLength(points, src_point, i)); + const auto length_sum = std::abs(motion_utils::calcSignedArcLength(points, src_point, i)); if (length_sum > target_length) { return i + 1; } diff --git a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp index 609b8115d6756..cc3e5054cf0dc 100644 --- a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp @@ -17,9 +17,9 @@ #include #include +#include #include #include -#include #include #include @@ -95,11 +95,11 @@ boost::optional getLerpTrajectoryPointWithIdx( const T & points, const geometry_msgs::msg::Point & point) { TrajectoryPoint interpolated_point; - const size_t nearest_seg_idx = tier4_autoware_utils::findNearestSegmentIndex(points, point); + const size_t nearest_seg_idx = motion_utils::findNearestSegmentIndex(points, point); const double len_to_interpolated = - tier4_autoware_utils::calcLongitudinalOffsetToSegment(points, nearest_seg_idx, point); + motion_utils::calcLongitudinalOffsetToSegment(points, nearest_seg_idx, point); const double len_segment = - tier4_autoware_utils::calcSignedArcLength(points, nearest_seg_idx, nearest_seg_idx + 1); + motion_utils::calcSignedArcLength(points, nearest_seg_idx, nearest_seg_idx + 1); const double ratio = len_to_interpolated / len_segment; if (ratio <= 0.0 || 1.0 <= ratio) return boost::none; const double interpolate_ratio = std::clamp(ratio, 0.0, 1.0); @@ -130,7 +130,7 @@ inline bool smoothPath( const PathWithLaneId & in_path, PathWithLaneId & out_path, const std::shared_ptr & planner_data) { - using tier4_autoware_utils::findNearestIndex; + using motion_utils::findNearestIndex; const geometry_msgs::msg::Pose current_pose = planner_data->current_pose.pose; const double v0 = planner_data->current_velocity->twist.linear.x; const double a0 = planner_data->current_accel.get(); @@ -145,7 +145,7 @@ inline bool smoothPath( } const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); auto nearest_idx = - tier4_autoware_utils::findNearestIndex(*traj_lateral_acc_filtered, current_pose.position); + motion_utils::findNearestIndex(*traj_lateral_acc_filtered, current_pose.position); // Resample trajectory with ego-velocity based interval distances auto traj_resampled = smoother->resampleTrajectory(*traj_lateral_acc_filtered, v0, nearest_idx); diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp index ce539be3e2050..2b29474ce5c2d 100644 --- a/planning/behavior_velocity_planner/include/utilization/util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/util.hpp @@ -16,6 +16,7 @@ #define UTILIZATION__UTIL_HPP_ #include +#include #include #include @@ -176,7 +177,7 @@ size_t findNearestSegmentIndex(const T & points, const PointWithSearchRangeIndex const auto & index = point_with_index.index; const auto point = point_with_index.point; - tier4_autoware_utils::validateNonEmpty(points); + motion_utils::validateNonEmpty(points); double min_dist = std::numeric_limits::max(); size_t nearest_idx = 0; @@ -197,7 +198,7 @@ size_t findNearestSegmentIndex(const T & points, const PointWithSearchRangeIndex } const double signed_length = - tier4_autoware_utils::calcLongitudinalOffsetToSegment(points, nearest_idx, point); + motion_utils::calcLongitudinalOffsetToSegment(points, nearest_idx, point); if (signed_length <= 0) { return nearest_idx - 1; @@ -212,7 +213,7 @@ template PointWithSearchRangeIndex findFirstNearSearchRangeIndex( const T & points, const geometry_msgs::msg::Point & point, const double distance_thresh = 9.0) { - tier4_autoware_utils::validateNonEmpty(points); + motion_utils::validateNonEmpty(points); bool min_idx_found = false; bool max_idx_found = false; @@ -241,14 +242,14 @@ double calcSignedArcLengthWithSearchIndex( const T & points, const PointWithSearchRangeIndex & src_point_with_range, const PointWithSearchRangeIndex & dst_point_with_range) { - tier4_autoware_utils::validateNonEmpty(points); + motion_utils::validateNonEmpty(points); const size_t src_idx = planning_utils::findNearestSegmentIndex(points, src_point_with_range); const size_t dst_idx = planning_utils::findNearestSegmentIndex(points, dst_point_with_range); - const double signed_length = tier4_autoware_utils::calcSignedArcLength(points, src_idx, dst_idx); - const double signed_length_src_offset = tier4_autoware_utils::calcLongitudinalOffsetToSegment( - points, src_idx, src_point_with_range.point); - const double signed_length_dst_offset = tier4_autoware_utils::calcLongitudinalOffsetToSegment( - points, dst_idx, dst_point_with_range.point); + const double signed_length = motion_utils::calcSignedArcLength(points, src_idx, dst_idx); + const double signed_length_src_offset = + motion_utils::calcLongitudinalOffsetToSegment(points, src_idx, src_point_with_range.point); + const double signed_length_dst_offset = + motion_utils::calcLongitudinalOffsetToSegment(points, dst_idx, dst_point_with_range.point); return signed_length - signed_length_src_offset + signed_length_dst_offset; } Polygon2d toFootprintPolygon(const autoware_auto_perception_msgs::msg::PredictedObject & object); diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp index b7c042abba433..0838ace064772 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp @@ -142,7 +142,7 @@ bool BlindSpotModule::modifyPathVelocity( /* set stop speed */ setSafe(state_machine_.getState() != State::STOP); - setDistance(tier4_autoware_utils::calcSignedArcLength( + setDistance(motion_utils::calcSignedArcLength( path->points, current_pose.pose.position, path->points.at(stop_line_idx).point.pose.position)); if (!isActivated()) { constexpr double stop_vel = 0.0; diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp index 43b2c331217ff..bc01cbcc58eb2 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp @@ -30,7 +30,7 @@ std::vector getCrosswalksOnPath( { std::vector crosswalks; - auto nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex( + auto nearest_segment_idx = motion_utils::findNearestSegmentIndex( path.points, current_pose, std::numeric_limits::max(), M_PI_2); // Add current lane id diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp index c70a25ca6173c..8661c6964fec7 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp @@ -28,14 +28,14 @@ namespace bg = boost::geometry; using Point = bg::model::d2::point_xy; using Polygon = bg::model::polygon; using Line = bg::model::linestring; -using tier4_autoware_utils::calcArcLength; +using motion_utils::calcArcLength; +using motion_utils::calcLateralOffset; +using motion_utils::calcLongitudinalOffsetToSegment; +using motion_utils::calcSignedArcLength; +using motion_utils::findNearestIndex; +using motion_utils::findNearestSegmentIndex; using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcLateralOffset; -using tier4_autoware_utils::calcLongitudinalOffsetToSegment; -using tier4_autoware_utils::calcSignedArcLength; using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::findNearestIndex; -using tier4_autoware_utils::findNearestSegmentIndex; using tier4_autoware_utils::getPoint; using tier4_autoware_utils::pose2transform; diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp index 42b012057801a..09dc622d91fbc 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include #include #include @@ -81,7 +81,7 @@ bool WalkwayModule::modifyPathVelocity( planning_utils::appendStopReason(stop_factor, stop_reason); // use arc length to identify if ego vehicle is in front of walkway stop or not. - const double signed_arc_dist_to_stop_point = tier4_autoware_utils::calcSignedArcLength( + const double signed_arc_dist_to_stop_point = motion_utils::calcSignedArcLength( path->points, planner_data_->current_pose.pose.position, debug_data_.first_stop_pose.position); const double distance_threshold = 1.0; diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp index 1d8e885866713..241d9405b5514 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp @@ -45,7 +45,7 @@ namespace bg = boost::geometry; using Point = bg::model::d2::point_xy; using Polygon = bg::model::polygon; using Line = bg::model::linestring; -using tier4_autoware_utils::findNearestSegmentIndex; +using motion_utils::findNearestSegmentIndex; bool getBackwardPointFromBasePoint( const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp index 99c0659d8fa8a..61e32190a077d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include #include #ifdef ROS_DISTRO_GALACTIC @@ -242,8 +242,8 @@ bool DetectionAreaModule::modifyPathVelocity( const auto & stop_pose = stop_point->second; - setDistance(tier4_autoware_utils::calcSignedArcLength( - path->points, self_pose.position, stop_pose.position)); + setDistance( + motion_utils::calcSignedArcLength(path->points, self_pose.position, stop_pose.position)); // Check state setSafe(canClearStopState()); @@ -363,8 +363,7 @@ bool DetectionAreaModule::isOverLine( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const { - return tier4_autoware_utils::calcSignedArcLength( - path.points, self_pose.position, line_pose.position) < 0; + return motion_utils::calcSignedArcLength(path.points, self_pose.position, line_pose.position) < 0; } bool DetectionAreaModule::hasEnoughBrakingDistance( diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index efeecc57607cd..d0ae2ce6e530a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -157,7 +157,7 @@ bool IntersectionModule::modifyPathVelocity( RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); RCLCPP_DEBUG(logger_, "===== plan end ====="); setSafe(true); - setDistance(tier4_autoware_utils::calcSignedArcLength( + setDistance(motion_utils::calcSignedArcLength( path->points, planner_data_->current_pose.pose.position, path->points.at(stop_line_idx).point.pose.position)); return true; // no plan needed. @@ -183,7 +183,7 @@ bool IntersectionModule::modifyPathVelocity( const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; setSafe(!is_entry_prohibited); - setDistance(tier4_autoware_utils::calcSignedArcLength( + setDistance(motion_utils::calcSignedArcLength( path->points, planner_data_->current_pose.pose.position, path->points.at(stop_line_idx).point.pose.position)); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index eb4984eecc49d..bf1d09f455519 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -119,7 +119,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( stop_factor.stop_factor_points.emplace_back(debug_data_.first_collision_point); planning_utils::appendStopReason(stop_factor, stop_reason); - const double signed_arc_dist_to_stop_point = tier4_autoware_utils::calcSignedArcLength( + const double signed_arc_dist_to_stop_point = motion_utils::calcSignedArcLength( path->points, current_pose.pose.position, path->points.at(stop_line_idx).point.pose.position); constexpr double distance_threshold = 2.0; diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp index ea9568a42ac3e..ac465a9565195 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp @@ -134,7 +134,7 @@ bool NoStoppingAreaModule::modifyPathVelocity( return true; } const auto & stop_pose = stop_point->second; - setDistance(tier4_autoware_utils::calcSignedArcLength( + setDistance(motion_utils::calcSignedArcLength( original_path.points, current_pose.pose.position, stop_pose.position)); if (isOverDeadLine(original_path, current_pose.pose, stop_pose)) { // ego can't stop in front of no stopping area -> GO or OR @@ -360,8 +360,7 @@ bool NoStoppingAreaModule::isOverDeadLine( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const { - return tier4_autoware_utils::calcSignedArcLength( - path.points, self_pose.position, line_pose.position) + + return motion_utils::calcSignedArcLength(path.points, self_pose.position, line_pose.position) + planner_param_.dead_line_margin < 0.0; } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index cb2b98eff5a6c..5fbd213c34d0f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -107,7 +107,7 @@ bool OcclusionSpotModule::modifyPathVelocity( //! never change this interpolation interval(will affect module accuracy) splineInterpolate(clipped_path, 1.0, &path_interpolated, logger_); const geometry_msgs::msg::Point start_point = path_interpolated.points.at(0).point.pose.position; - const auto offset = tier4_autoware_utils::calcSignedArcLength( + const auto offset = motion_utils::calcSignedArcLength( path_interpolated.points, ego_pose, start_point, param_.dist_thr, param_.angle_thr); if (offset == boost::none) return true; const double offset_from_start_to_ego = -offset.get(); diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp index b62aff7f3a98c..1763ef3c66445 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp @@ -24,7 +24,7 @@ namespace geometry_msgs::msg::Quaternion createQuaternionFacingToTrajectory( const PathPointsWithLaneId & path_points, const geometry_msgs::msg::Point & point) { - const auto nearest_idx = tier4_autoware_utils::findNearestIndex(path_points, point); + const auto nearest_idx = motion_utils::findNearestIndex(path_points, point); const auto & nearest_pose = path_points.at(nearest_idx).point.pose; const auto longitudinal_offset = diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp index de81e32c2e4e0..d398bb42fdac4 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp @@ -241,10 +241,10 @@ boost::optional RunOutModule::findNearestCollisionObstacle( std::sort( dynamic_obstacles.begin(), dynamic_obstacles.end(), [&path, &base_pose](const auto & lhs, const auto & rhs) -> bool { - const auto dist_lhs = tier4_autoware_utils::calcSignedArcLength( - path.points, base_pose.position, lhs.pose.position); - const auto dist_rhs = tier4_autoware_utils::calcSignedArcLength( - path.points, base_pose.position, rhs.pose.position); + const auto dist_lhs = + motion_utils::calcSignedArcLength(path.points, base_pose.position, lhs.pose.position); + const auto dist_rhs = + motion_utils::calcSignedArcLength(path.points, base_pose.position, rhs.pose.position); return dist_lhs < dist_rhs; }); @@ -551,7 +551,7 @@ boost::optional RunOutModule::calcStopPoint( } // calculate distance to collision with the obstacle - const float dist_to_collision_point = tier4_autoware_utils::calcSignedArcLength( + const float dist_to_collision_point = motion_utils::calcSignedArcLength( path.points, current_pose.position, dynamic_obstacle->nearest_collision_point); const float dist_to_collision = dist_to_collision_point - planner_param_.vehicle_param.base_to_front; @@ -641,7 +641,7 @@ void RunOutModule::insertStopPoint( // find nearest point index behind the stop point const auto nearest_seg_idx = - tier4_autoware_utils::findNearestSegmentIndex(path.points, stop_point->position); + motion_utils::findNearestSegmentIndex(path.points, stop_point->position); auto insert_idx = nearest_seg_idx + 1; // if stop point is ahead of the end of the path, don't insert @@ -671,7 +671,7 @@ void RunOutModule::insertVelocity( } const auto longitudinal_offset_to_collision_point = - tier4_autoware_utils::calcSignedArcLength( + motion_utils::calcSignedArcLength( smoothed_path.points, current_pose.position, dynamic_obstacle->nearest_collision_point) - planner_param_.vehicle_param.base_to_front; // enough distance to the obstacle @@ -740,7 +740,7 @@ void RunOutModule::insertApproachingVelocity( { // insert slow down velocity from nearest segment point const auto nearest_seg_idx = - tier4_autoware_utils::findNearestSegmentIndex(output_path.points, current_pose.position); + motion_utils::findNearestSegmentIndex(output_path.points, current_pose.position); run_out_utils::insertPathVelocityFromIndexLimited( nearest_seg_idx, approaching_vel, output_path.points); @@ -760,7 +760,7 @@ void RunOutModule::insertApproachingVelocity( stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); const auto nearest_seg_idx_stop = - tier4_autoware_utils::findNearestSegmentIndex(output_path.points, stop_point.position); + motion_utils::findNearestSegmentIndex(output_path.points, stop_point.position); auto insert_idx_stop = nearest_seg_idx_stop + 1; // to PathPointWithLaneId @@ -783,7 +783,7 @@ void RunOutModule::applyMaxJerkLimit( const auto stop_point = path.points.at(stop_point_idx.get()).point.pose.position; const auto dist_to_stop_point = - tier4_autoware_utils::calcSignedArcLength(path.points, current_pose.position, stop_point); + motion_utils::calcSignedArcLength(path.points, current_pose.position, stop_point); // calculate desired velocity with limited jerk const auto jerk_limited_vel = planning_utils::calcDecelerationVelocityFromDistanceToTarget( @@ -828,15 +828,15 @@ void RunOutModule::publishDebugValue( const geometry_msgs::msg::Pose & current_pose) const { if (dynamic_obstacle) { - const auto lateral_dist = std::abs(tier4_autoware_utils::calcLateralOffset( - path.points, dynamic_obstacle->pose.position)) - - planner_param_.vehicle_param.width / 2.0; + const auto lateral_dist = + std::abs(motion_utils::calcLateralOffset(path.points, dynamic_obstacle->pose.position)) - + planner_param_.vehicle_param.width / 2.0; const auto longitudinal_dist_to_obstacle = - tier4_autoware_utils::calcSignedArcLength( + motion_utils::calcSignedArcLength( path.points, current_pose.position, dynamic_obstacle->pose.position) - planner_param_.vehicle_param.base_to_front; - const float dist_to_collision_point = tier4_autoware_utils::calcSignedArcLength( + const float dist_to_collision_point = motion_utils::calcSignedArcLength( path.points, current_pose.position, dynamic_obstacle->nearest_collision_point); const auto dist_to_collision = dist_to_collision_point - planner_param_.vehicle_param.base_to_front; diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp index ace39ced61a1d..2c035705afd05 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp @@ -340,7 +340,7 @@ std::vector excludeObstaclesOutSideOfLine( std::vector extracted_dynamic_obstacle; for (const auto & obstacle : dynamic_obstacles) { const auto obstacle_nearest_idx = - tier4_autoware_utils::findNearestIndex(path_points, obstacle.pose.position); + motion_utils::findNearestIndex(path_points, obstacle.pose.position); const auto & obstacle_nearest_path_point = path_points.at(obstacle_nearest_idx).point.pose.position; @@ -394,8 +394,7 @@ PathWithLaneId trimPathFromSelfPose( const PathWithLaneId & input, const geometry_msgs::msg::Pose & self_pose, const double trim_distance) { - const size_t nearest_idx = - tier4_autoware_utils::findNearestIndex(input.points, self_pose.position); + const size_t nearest_idx = motion_utils::findNearestIndex(input.points, self_pose.position); PathWithLaneId output{}; output.header = input.header; diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp index b784a3da277cb..3cd08f1f5b6e2 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include #include #include diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp index 3910febedb6fd..fe3090f42ab68 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include #include #include // To be replaced by std::optional in C++17 @@ -235,7 +235,7 @@ bool TrafficLightModule::modifyPathVelocity( geometry_msgs::msg::Point stop_line_point_msg; stop_line_point_msg.x = stop_line_point.x(); stop_line_point_msg.y = stop_line_point.y(); - const double signed_arc_length_to_stop_point = tier4_autoware_utils::calcSignedArcLength( + const double signed_arc_length_to_stop_point = motion_utils::calcSignedArcLength( input_path.points, self_pose.pose.position, stop_line_point_msg); setDistance(signed_arc_length_to_stop_point); diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp index c84a591b4dd79..02bb52b240ebc 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include #include #include @@ -257,8 +257,8 @@ size_t insertStopVelocityAtCollision( const SegmentIndexWithPoint & collision, const double offset, autoware_auto_planning_msgs::msg::PathWithLaneId * path) { - const auto collision_offset = tier4_autoware_utils::calcLongitudinalOffsetToSegment( - path->points, collision.index, collision.point); + const auto collision_offset = + motion_utils::calcLongitudinalOffsetToSegment(path->points, collision.index, collision.point); const auto offset_segment = findOffsetSegment(*path, collision.index, offset + collision_offset); const auto interpolated_pose = @@ -461,7 +461,7 @@ bool VirtualTrafficLightModule::isBeforeStartLine() } const double max_dist = std::numeric_limits::max(); - const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength( + const auto signed_arc_length = motion_utils::calcSignedArcLength( module_data_.path.points, module_data_.head_pose, collision->point, max_dist, planner_param_.max_yaw_deviation_rad); @@ -479,7 +479,7 @@ bool VirtualTrafficLightModule::isBeforeStopLine() } const double max_dist = std::numeric_limits::max(); - const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength( + const auto signed_arc_length = motion_utils::calcSignedArcLength( module_data_.path.points, module_data_.head_pose, collision->point, max_dist, planner_param_.max_yaw_deviation_rad); @@ -502,7 +502,7 @@ bool VirtualTrafficLightModule::isAfterAnyEndLine() } const double max_dist = std::numeric_limits::max(); - const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength( + const auto signed_arc_length = motion_utils::calcSignedArcLength( module_data_.path.points, module_data_.head_pose, collision->point, max_dist, planner_param_.max_yaw_deviation_rad); @@ -518,7 +518,7 @@ bool VirtualTrafficLightModule::isNearAnyEndLine() } const double max_dist = std::numeric_limits::max(); - const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength( + const auto signed_arc_length = motion_utils::calcSignedArcLength( module_data_.path.points, module_data_.head_pose, collision->point, max_dist, planner_param_.max_yaw_deviation_rad); diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp index acbb6773c23ad..e6e0ec7418376 100644 --- a/planning/behavior_velocity_planner/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/src/utilization/util.cpp @@ -68,24 +68,23 @@ bool createDetectionAreaPolygons( const size_t max_index = static_cast(path.points.size() - 1); //! avoid bug with same point polygon const double eps = 1e-3; - auto nearest_idx = tier4_autoware_utils::findNearestIndex(path.points, pose.position); + auto nearest_idx = motion_utils::findNearestIndex(path.points, pose.position); if (max_index == nearest_idx) return false; // case of path point is not enough size auto p0 = path.points.at(nearest_idx).point; auto first_idx = nearest_idx + 1; // use ego point as start point if same point as ego is not in the path const auto dist_to_nearest = - std::fabs(tier4_autoware_utils::calcSignedArcLength(path.points, pose.position, nearest_idx)); + std::fabs(motion_utils::calcSignedArcLength(path.points, pose.position, nearest_idx)); if (dist_to_nearest > eps) { - const auto nearest_seg_idx = - tier4_autoware_utils::findNearestSegmentIndex(path.points, pose.position); + const auto nearest_seg_idx = motion_utils::findNearestSegmentIndex(path.points, pose.position); // interpolate ego point const auto & pp = path.points; const double ds = tier4_autoware_utils::calcDistance2d(pp.at(nearest_seg_idx), pp.at(nearest_seg_idx + 1)); const double dist_to_nearest_seg = - tier4_autoware_utils::calcSignedArcLength(path.points, nearest_seg_idx, pose.position); + motion_utils::calcSignedArcLength(path.points, nearest_seg_idx, pose.position); const double ratio = dist_to_nearest_seg / ds; p0 = getLerpPathPointWithLaneId( pp.at(nearest_seg_idx).point, pp.at(nearest_seg_idx + 1).point, ratio); @@ -600,7 +599,7 @@ boost::optional getNearestLaneId( { boost::optional nearest_lane_id; - nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex( + nearest_segment_idx = motion_utils::findNearestSegmentIndex( path.points, current_pose, std::numeric_limits::max(), M_PI_2); if (!nearest_segment_idx) { diff --git a/planning/behavior_velocity_planner/test/src/test_utilization.cpp b/planning/behavior_velocity_planner/test/src/test_utilization.cpp index f362ee2b30f80..1a9d4cdbf4aed 100644 --- a/planning/behavior_velocity_planner/test/src/test_utilization.cpp +++ b/planning/behavior_velocity_planner/test/src/test_utilization.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/trajectory/trajectory.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "utilization/path_utilization.hpp" #include "utils.hpp" @@ -93,8 +93,8 @@ TEST(specialInterpolation, specialInterpolation) using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathPoint; using behavior_velocity_planner::interpolatePath; - using tier4_autoware_utils::calcSignedArcLength; - using tier4_autoware_utils::searchZeroVelocityIndex; + using motion_utils::calcSignedArcLength; + using motion_utils::searchZeroVelocityIndex; const auto genPath = [](const auto p, const auto v) { if (p.size() != v.size()) throw std::invalid_argument("different size is not expected"); diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp index a3668c3eb58da..ce87a7281d49c 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -138,7 +138,7 @@ Trajectory getPartialTrajectory( double calcDistance2d(const Trajectory & trajectory, const Pose & pose) { - const auto idx = tier4_autoware_utils::findNearestIndex(trajectory.points, pose.position); + const auto idx = motion_utils::findNearestIndex(trajectory.points, pose.position); return tier4_autoware_utils::calcDistance2d(trajectory.points.at(idx), pose); } @@ -351,8 +351,8 @@ bool FreespacePlannerNode::isPlanRequired() if (node_param_.replan_when_obstacle_found) { algo_->setMap(*occupancy_grid_); - const size_t nearest_index_partial = tier4_autoware_utils::findNearestIndex( - partial_trajectory_.points, current_pose_.pose.position); + const size_t nearest_index_partial = + motion_utils::findNearestIndex(partial_trajectory_.points, current_pose_.pose.position); const size_t end_index_partial = partial_trajectory_.points.size() - 1; const auto forward_trajectory = diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp index dccabd683b141..27bcef0855e37 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp @@ -15,6 +15,8 @@ #ifndef MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_ #define MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_ +#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/resample.hpp" #include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" #include "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp" @@ -30,8 +32,6 @@ #include "tier4_autoware_utils/math/unit_conversion.hpp" #include "tier4_autoware_utils/ros/self_pose_listener.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" -#include "tier4_autoware_utils/trajectory/tmp_conversion.hpp" -#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp index ac510efc4df85..7301ae49f412b 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp @@ -15,8 +15,8 @@ #ifndef MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_ #define MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_ +#include "motion_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/trajectory_utils.hpp" -#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index f3ff57eb68434..0cbc7f3ed17d6 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -15,11 +15,11 @@ #ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT #define MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT +#include "motion_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" #include "motion_velocity_smoother/smoother/smoother_base.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" -#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose.hpp" diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp index cea252f991f75..2d595e9c1a1d6 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp @@ -15,9 +15,9 @@ #ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT #define MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT +#include "motion_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/linear_interpolation.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp index 9246c198befd9..3138716359c45 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -15,10 +15,10 @@ #ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ #define MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ +#include "motion_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index a5159ff42efce..5ab6befb5b676 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -15,10 +15,10 @@ #ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ #define MOTION_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ +#include "motion_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index 0604d913ebfc3..53c112ddf90aa 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -15,10 +15,10 @@ #ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ #define MOTION_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ +#include "motion_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp index cb07212afe04d..6650c418267c1 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp @@ -15,11 +15,11 @@ #ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ #define MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ +#include "motion_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/resample.hpp" #include "motion_velocity_smoother/trajectory_utils.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp index 8a930bd9244ca..f7d77c5ceb4c0 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp @@ -15,8 +15,8 @@ #ifndef MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ #define MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ +#include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose.hpp" diff --git a/planning/motion_velocity_smoother/package.xml b/planning/motion_velocity_smoother/package.xml index fbef021da0fed..a634752ba84c3 100644 --- a/planning/motion_velocity_smoother/package.xml +++ b/planning/motion_velocity_smoother/package.xml @@ -18,6 +18,7 @@ geometry_msgs interpolation libboost-dev + motion_utils nav_msgs osqp_interface rclcpp diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index cb51e52254bb1..a9395e82668a1 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -260,7 +260,7 @@ void MotionVelocitySmootherNode::initCommonParam() void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const { - Trajectory publishing_trajectory = tier4_autoware_utils::convertToTrajectory(trajectory); + Trajectory publishing_trajectory = motion_utils::convertToTrajectory(trajectory); publishing_trajectory.header = base_traj_raw_ptr_->header; pub_trajectory_->publish(publishing_trajectory); } @@ -365,15 +365,15 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar } // calculate trajectory velocity - TrajectoryPoints output = calcTrajectoryVelocity( - tier4_autoware_utils::convertToTrajectoryPointArray(*base_traj_raw_ptr_)); + TrajectoryPoints output = + calcTrajectoryVelocity(motion_utils::convertToTrajectoryPointArray(*base_traj_raw_ptr_)); if (output.empty()) { RCLCPP_WARN(get_logger(), "Output Point is empty"); return; } // Get the nearest point - const auto output_closest_idx = tier4_autoware_utils::findNearestIndex( + const auto output_closest_idx = motion_utils::findNearestIndex( output, current_pose_ptr_->pose, std::numeric_limits::max(), node_param_.delta_yaw_threshold); const auto output_closest_point = @@ -423,7 +423,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( TrajectoryPoints output{}; // velocity is optimized by qp solver // Extract trajectory around self-position with desired forward-backward length - const auto input_closest = tier4_autoware_utils::findNearestIndex( + const auto input_closest = motion_utils::findNearestIndex( traj_input, current_pose_ptr_->pose, std::numeric_limits::max(), node_param_.delta_yaw_threshold); if (!input_closest) { @@ -461,7 +461,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( applyExternalVelocityLimit(*traj_extracted); // Change trajectory velocity to zero when current_velocity == 0 & stop_dist is close - const auto traj_extracted_closest = tier4_autoware_utils::findNearestIndex( + const auto traj_extracted_closest = motion_utils::findNearestIndex( *traj_extracted, current_pose_ptr_->pose, std::numeric_limits::max(), node_param_.delta_yaw_threshold); if (!traj_extracted_closest) { @@ -502,7 +502,7 @@ bool MotionVelocitySmootherNode::smoothVelocity( } // Resample trajectory with ego-velocity based interval distance - const auto traj_pre_resampled_closest = tier4_autoware_utils::findNearestIndex( + const auto traj_pre_resampled_closest = motion_utils::findNearestIndex( *traj_lateral_acc_filtered, current_pose_ptr_->pose, std::numeric_limits::max(), node_param_.delta_yaw_threshold); auto traj_resampled = smoother_->resampleTrajectory( @@ -525,7 +525,7 @@ bool MotionVelocitySmootherNode::smoothVelocity( double initial_vel{}; double initial_acc{}; InitializeType type{}; - const auto traj_resampled_closest = tier4_autoware_utils::findNearestIndex( + const auto traj_resampled_closest = motion_utils::findNearestIndex( *traj_resampled, current_pose_ptr_->pose, std::numeric_limits::max(), node_param_.delta_yaw_threshold); if (!traj_resampled_closest) { @@ -617,7 +617,7 @@ void MotionVelocitySmootherNode::publishStopDistance( // stop distance calculation const double stop_dist_lim{50.0}; double stop_dist{stop_dist_lim}; - const auto stop_idx{tier4_autoware_utils::searchZeroVelocityIndex(trajectory)}; + const auto stop_idx{motion_utils::searchZeroVelocityIndex(trajectory)}; if (stop_idx) { stop_dist = trajectory_utils::calcArcLength(trajectory, closest, *stop_idx); stop_dist = closest > *stop_idx ? stop_dist : -stop_dist; @@ -673,7 +673,7 @@ MotionVelocitySmootherNode::calcInitialMotion( const double engage_vel_thr = node_param_.engage_velocity * node_param_.engage_exit_ratio; if (vehicle_speed < engage_vel_thr) { if (target_vel >= node_param_.engage_velocity) { - const auto idx = tier4_autoware_utils::searchZeroVelocityIndex(input_traj); + const auto idx = motion_utils::searchZeroVelocityIndex(input_traj); const double stop_dist = idx ? tier4_autoware_utils::calcDistance2d( input_traj.at(*idx), input_traj.at(input_closest)) : 0.0; @@ -714,13 +714,13 @@ MotionVelocitySmootherNode::calcInitialMotion( void MotionVelocitySmootherNode::overwriteStopPoint( const TrajectoryPoints & input, TrajectoryPoints & output) const { - const auto stop_idx = tier4_autoware_utils::searchZeroVelocityIndex(input); + const auto stop_idx = motion_utils::searchZeroVelocityIndex(input); if (!stop_idx) { return; } // Get Closest Point from Output - const auto nearest_output_point_idx = tier4_autoware_utils::findNearestIndex( + const auto nearest_output_point_idx = motion_utils::findNearestIndex( output, input.at(*stop_idx).pose, std::numeric_limits::max(), node_param_.delta_yaw_threshold); @@ -766,7 +766,7 @@ void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & t trajectory_utils::applyMaximumVelocityLimit( 0, traj.size(), max_velocity_with_deceleration_, traj); - const auto closest_idx = tier4_autoware_utils::findNearestIndex( + const auto closest_idx = motion_utils::findNearestIndex( traj, current_pose_ptr_->pose, std::numeric_limits::max(), node_param_.delta_yaw_threshold); if (!closest_idx) { @@ -789,7 +789,7 @@ void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & t void MotionVelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) const { - const auto stop_idx = tier4_autoware_utils::searchZeroVelocityIndex(traj); + const auto stop_idx = motion_utils::searchZeroVelocityIndex(traj); if (!stop_idx) { return; // no stop point. } @@ -913,7 +913,7 @@ bool MotionVelocitySmootherNode::isEngageStatus(const double target_vel) const Trajectory MotionVelocitySmootherNode::toTrajectoryMsg( const TrajectoryPoints & points, const std_msgs::msg::Header & header) const { - auto trajectory = tier4_autoware_utils::convertToTrajectory(points); + auto trajectory = motion_utils::convertToTrajectory(points); trajectory.header = header; return trajectory; } diff --git a/planning/motion_velocity_smoother/src/resample.cpp b/planning/motion_velocity_smoother/src/resample.cpp index d0768086ffa4b..23245f12d44f2 100644 --- a/planning/motion_velocity_smoother/src/resample.cpp +++ b/planning/motion_velocity_smoother/src/resample.cpp @@ -29,13 +29,12 @@ boost::optional resampleTrajectory( const double front_arclength_value = trajectory_utils::calcArcLength(input, 0, closest_id); // Get the nearest point where velocity is zero - auto zero_vel_id = tier4_autoware_utils::searchZeroVelocityIndex(input, closest_id, input.size()); + auto zero_vel_id = motion_utils::searchZeroVelocityIndex(input, closest_id, input.size()); // Arc length from the closest point to the point where velocity is zero double zero_vel_arclength_value = param.max_trajectory_length; if (zero_vel_id) { zero_vel_arclength_value = std::min( - zero_vel_arclength_value, - tier4_autoware_utils::calcSignedArcLength(input, closest_id, *zero_vel_id)); + zero_vel_arclength_value, motion_utils::calcSignedArcLength(input, closest_id, *zero_vel_id)); } // Get the resample size from the closest point @@ -151,12 +150,12 @@ boost::optional resampleTrajectory( // Get the nearest point where velocity is zero // to avoid getting closest_id as a stop point, search zero velocity index from closest_id + 1. - auto stop_id = tier4_autoware_utils::searchZeroVelocityIndex(input, closest_id + 1, input.size()); + auto stop_id = motion_utils::searchZeroVelocityIndex(input, closest_id + 1, input.size()); // Arc length from the closest point to the point where velocity is zero double stop_arclength_value = param.max_trajectory_length; if (stop_id) { stop_arclength_value = std::min( - stop_arclength_value, tier4_autoware_utils::calcSignedArcLength(input, closest_id, *stop_id)); + stop_arclength_value, motion_utils::calcSignedArcLength(input, closest_id, *stop_id)); } // Do dense resampling before the stop line(3[m] ahead of the stop line) diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index b7607b1da732c..9332c971aa9d0 100644 --- a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -123,7 +123,7 @@ bool JerkFilteredSmoother::apply( // to avoid getting 0 as a stop point, search zero velocity index from 1. // the size of the resampled trajectory must not be less than 2. - const auto zero_vel_id = tier4_autoware_utils::searchZeroVelocityIndex( + const auto zero_vel_id = motion_utils::searchZeroVelocityIndex( *opt_resampled_trajectory, 1, opt_resampled_trajectory->size()); if (!zero_vel_id) { diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp index de3250da63d3e..c2b907eec29cb 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp @@ -76,7 +76,7 @@ TrajectoryPoint calcInterpolatedTrajectoryPoint( } const size_t segment_idx = - tier4_autoware_utils::findNearestSegmentIndex(trajectory, target_pose.position); + motion_utils::findNearestSegmentIndex(trajectory, target_pose.position); auto v1 = getTransVector3(trajectory.at(segment_idx).pose, trajectory.at(segment_idx + 1).pose); auto v2 = getTransVector3(trajectory.at(segment_idx).pose, target_pose); diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index 035563e4dfd10..d781b15cd6297 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -14,6 +14,7 @@ #ifndef OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/common_structs.hpp" #include "obstacle_avoidance_planner/costmap_generator.hpp" #include "obstacle_avoidance_planner/eb_path_optimizer.hpp" @@ -56,9 +57,9 @@ boost::optional lerpPose( constexpr double epsilon = 1e-6; const double closest_to_target_dist = - tier4_autoware_utils::calcSignedArcLength(points, closest_seg_idx, target_pos); + motion_utils::calcSignedArcLength(points, closest_seg_idx, target_pos); const double seg_dist = - tier4_autoware_utils::calcSignedArcLength(points, closest_seg_idx, closest_seg_idx + 1); + motion_utils::calcSignedArcLength(points, closest_seg_idx, closest_seg_idx + 1); const auto & closest_pose = points[closest_seg_idx].pose; const auto & next_pose = points[closest_seg_idx + 1].pose; @@ -101,9 +102,9 @@ double lerpTwistX( constexpr double epsilon = 1e-6; const double closest_to_target_dist = - tier4_autoware_utils::calcSignedArcLength(points, closest_seg_idx, target_pos); + motion_utils::calcSignedArcLength(points, closest_seg_idx, target_pos); const double seg_dist = - tier4_autoware_utils::calcSignedArcLength(points, closest_seg_idx, closest_seg_idx + 1); + motion_utils::calcSignedArcLength(points, closest_seg_idx, closest_seg_idx + 1); const double closest_vel = points[closest_seg_idx].longitudinal_velocity_mps; const double next_vel = points[closest_seg_idx + 1].longitudinal_velocity_mps; @@ -127,9 +128,9 @@ double lerpPoseZ( constexpr double epsilon = 1e-6; const double closest_to_target_dist = - tier4_autoware_utils::calcSignedArcLength(points, closest_seg_idx, target_pos); + motion_utils::calcSignedArcLength(points, closest_seg_idx, target_pos); const double seg_dist = - tier4_autoware_utils::calcSignedArcLength(points, closest_seg_idx, closest_seg_idx + 1); + motion_utils::calcSignedArcLength(points, closest_seg_idx, closest_seg_idx + 1); const double closest_z = points[closest_seg_idx].pose.position.z; const double next_z = points[closest_seg_idx + 1].pose.position.z; diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp index 3072a418b47e9..a3b229ba55276 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp @@ -18,8 +18,8 @@ #include "eigen3/Eigen/Core" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/common_structs.hpp" -#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/path_point.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" @@ -197,12 +197,12 @@ T clipBackwardPoints( return T{}; } - const auto target_idx_optional = tier4_autoware_utils::findNearestIndex( - points, pose, std::numeric_limits::max(), delta_yaw); + const auto target_idx_optional = + motion_utils::findNearestIndex(points, pose, std::numeric_limits::max(), delta_yaw); const size_t target_idx = target_idx_optional ? *target_idx_optional - : tier4_autoware_utils::findNearestIndex(points, pose.position); + : motion_utils::findNearestIndex(points, pose.position); const int begin_idx = std::max(0, static_cast(target_idx) - static_cast(backward_length / delta_length)); diff --git a/planning/obstacle_avoidance_planner/package.xml b/planning/obstacle_avoidance_planner/package.xml index 57f485d108ecc..e1ac7eba874ed 100644 --- a/planning/obstacle_avoidance_planner/package.xml +++ b/planning/obstacle_avoidance_planner/package.xml @@ -15,6 +15,7 @@ autoware_auto_planning_msgs geometry_msgs interpolation + motion_utils nav_msgs osqp_interface rclcpp diff --git a/planning/obstacle_avoidance_planner/src/costmap_generator.cpp b/planning/obstacle_avoidance_planner/src/costmap_generator.cpp index dafc2d5881b3e..86322802e941c 100644 --- a/planning/obstacle_avoidance_planner/src/costmap_generator.cpp +++ b/planning/obstacle_avoidance_planner/src/costmap_generator.cpp @@ -85,7 +85,7 @@ bool isAvoidingObject( return false; } - const int nearest_idx = tier4_autoware_utils::findNearestIndex( + const int nearest_idx = motion_utils::findNearestIndex( path_points, object.kinematics.initial_pose_with_covariance.pose.position); const auto nearest_path_point = path_points[nearest_idx]; const auto rel_p = geometry_utils::transformToRelativeCoordinate2D( @@ -115,7 +115,7 @@ bool isAvoidingObject( nearest_path_point_image.get().y))[static_cast(nearest_path_point_image.get().x)] * map_info.resolution; */ - const double lateral_offset_to_path = tier4_autoware_utils::calcLateralOffset( + const double lateral_offset_to_path = motion_utils::calcLateralOffset( path_points, object.kinematics.initial_pose_with_covariance.pose.position); if ( // nearest_path_point_clearance - traj_param.center_line_width * 0.5 < @@ -254,9 +254,9 @@ cv::Mat CostmapGenerator::drawObstaclesOnImage( const PolygonPoints polygon_points = cv_polygon_utils::getPolygonPoints(object, map_info); if (isAvoidingObject( polygon_points, object, clearance_map, map_info, path_points_inside_area, traj_param)) { - const double lon_dist_to_path = tier4_autoware_utils::calcSignedArcLength( + const double lon_dist_to_path = motion_utils::calcSignedArcLength( path_points, 0, object.kinematics.initial_pose_with_covariance.pose.position); - const double lat_dist_to_path = tier4_autoware_utils::calcLateralOffset( + const double lat_dist_to_path = motion_utils::calcLateralOffset( path_points, object.kinematics.initial_pose_with_covariance.pose.position); obj_cog_info.push_back({lon_dist_to_path, lat_dist_to_path}); obj_positions.push_back(object.kinematics.initial_pose_with_covariance.pose.position); @@ -273,7 +273,7 @@ cv::Mat CostmapGenerator::drawObstaclesOnImage( // fill between objects in the same side const auto get_closest_obj_point = [&](size_t idx) { const auto & path_point = - path_points.at(tier4_autoware_utils::findNearestIndex(path_points, obj_positions.at(idx))); + path_points.at(motion_utils::findNearestIndex(path_points, obj_positions.at(idx))); double min_dist = std::numeric_limits::min(); size_t min_idx = 0; for (size_t p_idx = 0; p_idx < cv_polygons.at(idx).size(); ++p_idx) { diff --git a/planning/obstacle_avoidance_planner/src/cv_utils.cpp b/planning/obstacle_avoidance_planner/src/cv_utils.cpp index 3e59bda315b6c..3a0275f1550d8 100644 --- a/planning/obstacle_avoidance_planner/src/cv_utils.cpp +++ b/planning/obstacle_avoidance_planner/src/cv_utils.cpp @@ -213,7 +213,7 @@ std::vector getCVPolygon( const std::vector & path_points, const cv::Mat & clearance_map, const nav_msgs::msg::MapMetaData & map_info) { - const int nearest_idx = tier4_autoware_utils::findNearestIndex( + const int nearest_idx = motion_utils::findNearestIndex( path_points, object.kinematics.initial_pose_with_covariance.pose.position); const auto nearest_path_point = path_points[nearest_idx]; if (path_points.empty()) { @@ -249,8 +249,7 @@ std::vector getExtendedCVPolygon( } const Edges edges = optional_edges.get(); - const int nearest_polygon_idx = - tier4_autoware_utils::findNearestIndex(points_in_image, edges.origin); + const int nearest_polygon_idx = motion_utils::findNearestIndex(points_in_image, edges.origin); std::vector cv_polygon; if (edges.back_idx == nearest_polygon_idx || edges.front_idx == nearest_polygon_idx) { // make polygon only with edges and extended edges diff --git a/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp b/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp index 732ea4dbc43cc..70a9c1cec08e0 100644 --- a/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp @@ -236,7 +236,7 @@ std::vector EBPathOptimizer::getFixedPoints( std::vector empty_points; return empty_points; } - const auto opt_begin_idx = tier4_autoware_utils::findNearestIndex( + const auto opt_begin_idx = motion_utils::findNearestIndex( prev_trajs->smoothed_trajectory, ego_pose, std::numeric_limits::max(), traj_param_.delta_yaw_threshold_for_closest_point); const int begin_idx = opt_begin_idx ? *opt_begin_idx : 0; @@ -280,7 +280,7 @@ EBPathOptimizer::CandidatePoints EBPathOptimizer::getCandidatePoints( } // try to find non-fix points - const auto opt_begin_idx = tier4_autoware_utils::findNearestIndex( + const auto opt_begin_idx = motion_utils::findNearestIndex( path_points, fixed_points.back(), std::numeric_limits::max(), traj_param_.delta_yaw_threshold_for_closest_point); if (!opt_begin_idx) { @@ -463,7 +463,7 @@ EBPathOptimizer::Anchor EBPathOptimizer::getAnchor( pose.orientation = geometry_utils::getQuaternionFromPoints( interpolated_points[interpolated_idx + 1], interpolated_points[interpolated_idx]); } - const auto opt_nearest_idx = tier4_autoware_utils::findNearestIndex( + const auto opt_nearest_idx = motion_utils::findNearestIndex( path_points, pose, std::numeric_limits::max(), traj_param_.delta_yaw_threshold_for_closest_point); const int nearest_idx = opt_nearest_idx ? *opt_nearest_idx : 0; diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 925bef756fbe6..f06fbe2120b90 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -329,7 +329,7 @@ std::vector MPTOptimizer::getReferencePoints( // calc non fixed traj points const auto fixed_ref_points_with_yaw = points_utils::convertToPosesWithYawEstimation( points_utils::convertToPoints(fixed_ref_points)); - const auto seg_idx_optional = tier4_autoware_utils::findNearestSegmentIndex( + const auto seg_idx_optional = motion_utils::findNearestSegmentIndex( smoothed_points, fixed_ref_points_with_yaw.back(), std::numeric_limits::max(), traj_param_.delta_yaw_threshold_for_closest_point); @@ -344,7 +344,7 @@ std::vector MPTOptimizer::getReferencePoints( std::vector{ smoothed_points.begin() + seg_idx, smoothed_points.end()}; - const double offset = tier4_autoware_utils::calcLongitudinalOffsetToSegment( + const double offset = motion_utils::calcLongitudinalOffsetToSegment( non_fixed_traj_points, 0, fixed_ref_points.back().p) + mpt_param_.delta_arc_length_for_mpt_points; const auto non_fixed_interpolated_traj_points = interpolation_utils::getInterpolatedPoints( @@ -411,7 +411,7 @@ void MPTOptimizer::calcPlanningFromEgo(std::vector & ref_points) { // if plan from ego constexpr double epsilon = 1e-04; - const double trajectory_length = tier4_autoware_utils::calcArcLength(ref_points); + const double trajectory_length = motion_utils::calcArcLength(ref_points); const bool plan_from_ego = mpt_param_.plan_from_ego && std::abs(current_ego_vel_) < epsilon && ref_points.size() > 1 && @@ -504,7 +504,7 @@ std::vector MPTOptimizer::getFixedReferencePoints( const auto & prev_ref_point = prev_ref_points.at(i); if (!points_reaching_prev_points_flag) { - if (tier4_autoware_utils::calcSignedArcLength(points, 0, prev_ref_point.p) < 0) { + if (motion_utils::calcSignedArcLength(points, 0, prev_ref_point.p) < 0) { continue; } points_reaching_prev_points_flag = true; @@ -723,9 +723,9 @@ boost::optional MPTOptimizer::executeOptimization( const size_t D_x = vehicle_model_ptr_->getDimX(); if (prev_trajs && prev_trajs->mpt_ref_points.size() > 1) { - const size_t seg_idx = tier4_autoware_utils::findNearestSegmentIndex( - prev_trajs->mpt_ref_points, ref_points.front().p); - double offset = tier4_autoware_utils::calcLongitudinalOffsetToSegment( + const size_t seg_idx = + motion_utils::findNearestSegmentIndex(prev_trajs->mpt_ref_points, ref_points.front().p); + double offset = motion_utils::calcLongitudinalOffsetToSegment( prev_trajs->mpt_ref_points, seg_idx, ref_points.front().p); u0(0) = prev_trajs->mpt_ref_points.at(seg_idx).optimized_kinematic_state(0); @@ -1230,10 +1230,9 @@ size_t MPTOptimizer::findNearestIndexWithSoftYawConstraints( const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points); const auto nearest_idx_optional = - tier4_autoware_utils::findNearestIndex(points_with_yaw, pose, dist_threshold, yaw_threshold); - return nearest_idx_optional - ? *nearest_idx_optional - : tier4_autoware_utils::findNearestIndex(points_with_yaw, pose.position); + motion_utils::findNearestIndex(points_with_yaw, pose, dist_threshold, yaw_threshold); + return nearest_idx_optional ? *nearest_idx_optional + : motion_utils::findNearestIndex(points_with_yaw, pose.position); } void MPTOptimizer::calcOrientation(std::vector & ref_points) const diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 80a88aae55a1c..a1b91bc85bc23 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -15,13 +15,13 @@ #include "obstacle_avoidance_planner/node.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" #include "obstacle_avoidance_planner/cv_utils.hpp" #include "obstacle_avoidance_planner/debug_visualization.hpp" #include "obstacle_avoidance_planner/utils.hpp" #include "rclcpp/time.hpp" #include "tf2/utils.h" #include "tier4_autoware_utils/ros/update_param.hpp" -#include "tier4_autoware_utils/trajectory/tmp_conversion.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -42,10 +42,9 @@ template size_t searchExtendedZeroVelocityIndex( const std::vector & fine_points, const std::vector & vel_points) { - const auto opt_zero_vel_idx = tier4_autoware_utils::searchZeroVelocityIndex(vel_points); + const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(vel_points); const size_t zero_vel_idx = opt_zero_vel_idx ? opt_zero_vel_idx.get() : vel_points.size() - 1; - return tier4_autoware_utils::findNearestIndex( - fine_points, vel_points.at(zero_vel_idx).pose.position); + return motion_utils::findNearestIndex(fine_points, vel_points.at(zero_vel_idx).pose.position); } bool isPathShapeChanged( @@ -61,14 +60,14 @@ bool isPathShapeChanged( } // truncate prev points from ego pose to fixed end points - const auto opt_prev_begin_idx = tier4_autoware_utils::findNearestIndex( + const auto opt_prev_begin_idx = motion_utils::findNearestIndex( *prev_path_points, ego_pose, std::numeric_limits::max(), delta_yaw_threshold); const size_t prev_begin_idx = opt_prev_begin_idx ? *opt_prev_begin_idx : 0; const auto truncated_prev_points = points_utils::clipForwardPoints(*prev_path_points, prev_begin_idx, max_mpt_length); // truncate points from ego pose to fixed end points - const auto opt_begin_idx = tier4_autoware_utils::findNearestIndex( + const auto opt_begin_idx = motion_utils::findNearestIndex( path_points, ego_pose, std::numeric_limits::max(), delta_yaw_threshold); const size_t begin_idx = opt_begin_idx ? *opt_begin_idx : 0; const auto truncated_points = @@ -82,7 +81,7 @@ bool isPathShapeChanged( // calculate lateral deviations between truncated path_points and prev_path_points for (const auto & prev_point : truncated_prev_points) { const double dist = - std::abs(tier4_autoware_utils::calcLateralOffset(truncated_points, prev_point.pose.position)); + std::abs(motion_utils::calcLateralOffset(truncated_points, prev_point.pose.position)); if (dist > max_path_shape_change_dist) { return true; } @@ -128,7 +127,7 @@ bool hasValidNearestPointFromEgo( const auto interpolated_poses_with_yaw = points_utils::convertToPosesWithYawEstimation(interpolated_points); - const auto opt_nearest_idx = tier4_autoware_utils::findNearestIndex( + const auto opt_nearest_idx = motion_utils::findNearestIndex( interpolated_poses_with_yaw, ego_pose, traj_param.delta_dist_threshold_for_closest_point, traj_param.delta_yaw_threshold_for_closest_point); @@ -208,10 +207,9 @@ std::tuple, std::vector> calcVehicleCirclesInfo( const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points); const auto nearest_idx_optional = - tier4_autoware_utils::findNearestIndex(points_with_yaw, pose, dist_threshold, yaw_threshold); - return nearest_idx_optional - ? *nearest_idx_optional - : tier4_autoware_utils::findNearestIndex(points_with_yaw, pose.position); + motion_utils::findNearestIndex(points_with_yaw, pose, dist_threshold, yaw_threshold); + return nearest_idx_optional ? *nearest_idx_optional + : motion_utils::findNearestIndex(points_with_yaw, pose.position); } } // namespace @@ -901,7 +899,7 @@ autoware_auto_planning_msgs::msg::Trajectory ObstacleAvoidancePlanner::generateT "[ObstacleAvoidancePlanner] Negative velocity is NOT supported. Just converting path to " "trajectory"); const auto traj_points = points_utils::convertToTrajectoryPoints(path.points); - output_traj_msg = tier4_autoware_utils::convertToTrajectory(traj_points); + output_traj_msg = motion_utils::convertToTrajectory(traj_points); output_traj_msg.header = path.header; return output_traj_msg; @@ -914,7 +912,7 @@ autoware_auto_planning_msgs::msg::Trajectory ObstacleAvoidancePlanner::generateT generatePostProcessedTrajectory(path.points, optimized_traj_points); // convert to output msg type - output_traj_msg = tier4_autoware_utils::convertToTrajectory(post_processed_traj_points); + output_traj_msg = motion_utils::convertToTrajectory(post_processed_traj_points); output_traj_msg.header = path.header; return output_traj_msg; @@ -1125,14 +1123,13 @@ void ObstacleAvoidancePlanner::calcVelocity( { for (size_t i = 0; i < traj_points.size(); i++) { const size_t nearest_seg_idx = [&]() { - const auto opt_seg_idx = tier4_autoware_utils::findNearestSegmentIndex( + const auto opt_seg_idx = motion_utils::findNearestSegmentIndex( path_points, traj_points.at(i).pose, traj_param_.delta_dist_threshold_for_closest_point, traj_param_.delta_yaw_threshold_for_closest_point); if (opt_seg_idx) { return opt_seg_idx.get(); } - return tier4_autoware_utils::findNearestSegmentIndex( - path_points, traj_points.at(i).pose.position); + return motion_utils::findNearestSegmentIndex(path_points, traj_points.at(i).pose.position); }(); // add this line not to exceed max index size @@ -1160,7 +1157,7 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( const auto & road_clearance_map = cv_maps.clearance_map; const size_t nearest_idx = - tier4_autoware_utils::findNearestIndex(traj_points, current_ego_pose_.position); + motion_utils::findNearestIndex(traj_points, current_ego_pose_.position); // NOTE: Some end trajectory points will be ignored to check if outside the drivable area // since these points might be outside drivable area if only end reference points have high @@ -1201,21 +1198,19 @@ void ObstacleAvoidancePlanner::publishDebugDataInOptimization( stop_watch_.tic(__func__); { // publish trajectories - auto debug_eb_traj = tier4_autoware_utils::convertToTrajectory(debug_data_ptr_->eb_traj); + auto debug_eb_traj = motion_utils::convertToTrajectory(debug_data_ptr_->eb_traj); debug_eb_traj.header = path.header; debug_eb_traj_pub_->publish(debug_eb_traj); - auto debug_mpt_fixed_traj = - tier4_autoware_utils::convertToTrajectory(debug_data_ptr_->mpt_fixed_traj); + auto debug_mpt_fixed_traj = motion_utils::convertToTrajectory(debug_data_ptr_->mpt_fixed_traj); debug_mpt_fixed_traj.header = path.header; debug_mpt_fixed_traj_pub_->publish(debug_mpt_fixed_traj); - auto debug_mpt_ref_traj = - tier4_autoware_utils::convertToTrajectory(debug_data_ptr_->mpt_ref_traj); + auto debug_mpt_ref_traj = motion_utils::convertToTrajectory(debug_data_ptr_->mpt_ref_traj); debug_mpt_ref_traj.header = path.header; debug_mpt_ref_traj_pub_->publish(debug_mpt_ref_traj); - auto debug_mpt_traj = tier4_autoware_utils::convertToTrajectory(debug_data_ptr_->mpt_traj); + auto debug_mpt_traj = motion_utils::convertToTrajectory(debug_data_ptr_->mpt_traj); debug_mpt_traj.header = path.header; debug_mpt_traj_pub_->publish(debug_mpt_traj); } @@ -1321,7 +1316,7 @@ ObstacleAvoidancePlanner::getExtendedTrajectory( assert(!path_points.empty()); - const double accum_arc_length = tier4_autoware_utils::calcArcLength(optimized_points); + const double accum_arc_length = motion_utils::calcArcLength(optimized_points); if (accum_arc_length > traj_param_.trajectory_length) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(10000).count(), @@ -1330,7 +1325,7 @@ ObstacleAvoidancePlanner::getExtendedTrajectory( } // calculate end idx of optimized points on path points - const auto opt_end_path_idx = tier4_autoware_utils::findNearestIndex( + const auto opt_end_path_idx = motion_utils::findNearestIndex( path_points, optimized_points.back().pose, std::numeric_limits::max(), traj_param_.delta_yaw_threshold_for_closest_point); if (!opt_end_path_idx) { @@ -1443,10 +1438,10 @@ ObstacleAvoidancePlanner::alignVelocity( const auto path_zero_vel_info = [&]() -> std::pair< std::vector, boost::optional> { - const auto opt_path_zero_vel_idx = tier4_autoware_utils::searchZeroVelocityIndex(path_points); + const auto opt_path_zero_vel_idx = motion_utils::searchZeroVelocityIndex(path_points); if (opt_path_zero_vel_idx) { const auto & zero_vel_path_point = path_points.at(opt_path_zero_vel_idx.get()); - const auto opt_traj_seg_idx = tier4_autoware_utils::findNearestSegmentIndex( + const auto opt_traj_seg_idx = motion_utils::findNearestSegmentIndex( fine_traj_points, zero_vel_path_point.pose, std::numeric_limits::max(), traj_param_.delta_yaw_threshold_for_closest_point); if (opt_traj_seg_idx) { @@ -1503,14 +1498,14 @@ ObstacleAvoidancePlanner::alignVelocity( const auto truncated_points = points_utils::clipForwardPoints(path_points, prev_begin_idx, 5.0); const auto & target_pose = fine_traj_points_with_vel[i].pose; - const auto closest_seg_idx_optional = tier4_autoware_utils::findNearestSegmentIndex( + const auto closest_seg_idx_optional = motion_utils::findNearestSegmentIndex( truncated_points, target_pose, traj_param_.delta_dist_threshold_for_closest_point, traj_param_.delta_yaw_threshold_for_closest_point); const auto closest_seg_idx = closest_seg_idx_optional ? *closest_seg_idx_optional - : tier4_autoware_utils::findNearestSegmentIndex(truncated_points, target_pose.position); + : motion_utils::findNearestSegmentIndex(truncated_points, target_pose.position); // lerp z fine_traj_points_with_vel[i].pose.position.z = @@ -1541,12 +1536,12 @@ void ObstacleAvoidancePlanner::publishDebugDataInMain( { // publish trajectories auto debug_extended_fixed_traj = - tier4_autoware_utils::convertToTrajectory(debug_data_ptr_->extended_fixed_traj); + motion_utils::convertToTrajectory(debug_data_ptr_->extended_fixed_traj); debug_extended_fixed_traj.header = path.header; debug_extended_fixed_traj_pub_->publish(debug_extended_fixed_traj); auto debug_extended_non_fixed_traj = - tier4_autoware_utils::convertToTrajectory(debug_data_ptr_->extended_non_fixed_traj); + motion_utils::convertToTrajectory(debug_data_ptr_->extended_non_fixed_traj); debug_extended_non_fixed_traj.header = path.header; debug_extended_non_fixed_traj_pub_->publish(debug_extended_non_fixed_traj); } diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp index 58e38c3174b45..7ee55401564c2 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp @@ -81,23 +81,23 @@ class PIDBasedPlanner : public PlannerInterface size_t findExtendedNearestIndex( const Trajectory traj, const geometry_msgs::msg::Pose & pose) const { - const auto nearest_idx = tier4_autoware_utils::findNearestIndex( + const auto nearest_idx = motion_utils::findNearestIndex( traj.points, pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); if (nearest_idx) { return nearest_idx.get(); } - return tier4_autoware_utils::findNearestIndex(traj.points, pose.position); + return motion_utils::findNearestIndex(traj.points, pose.position); } size_t findExtendedNearestSegmentIndex( const Trajectory traj, const geometry_msgs::msg::Pose & pose) const { - const auto nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex( + const auto nearest_segment_idx = motion_utils::findNearestSegmentIndex( traj.points, pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); if (nearest_segment_idx) { return nearest_segment_idx.get(); } - return tier4_autoware_utils::findNearestSegmentIndex(traj.points, pose.position); + return motion_utils::findNearestSegmentIndex(traj.points, pose.position); } // ROS parameters diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp index 8c391df154bd8..1a3809662589d 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp @@ -16,6 +16,7 @@ #define OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ #include "common_structs.hpp" +#include "motion_utils/motion_utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index c1a3d46e06795..f8264cc814ddb 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -14,10 +14,10 @@ #include "obstacle_cruise_planner/node.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" #include "obstacle_cruise_planner/polygon_utils.hpp" #include "obstacle_cruise_planner/utils.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" -#include "tier4_autoware_utils/trajectory/tmp_conversion.hpp" #include @@ -40,12 +40,11 @@ size_t findExtendedNearestIndex( const Trajectory traj, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw) { - const auto nearest_idx = - tier4_autoware_utils::findNearestIndex(traj.points, pose, max_dist, max_yaw); + const auto nearest_idx = motion_utils::findNearestIndex(traj.points, pose, max_dist, max_yaw); if (nearest_idx) { return nearest_idx.get(); } - return tier4_autoware_utils::findNearestIndex(traj.points, pose.position); + return motion_utils::findNearestIndex(traj.points, pose.position); } Trajectory trimTrajectoryFrom(const Trajectory & input, const double nearest_idx) @@ -62,10 +61,10 @@ Trajectory trimTrajectoryFrom(const Trajectory & input, const double nearest_idx bool isFrontObstacle( const Trajectory & traj, const size_t ego_idx, const geometry_msgs::msg::Point & obj_pos) { - size_t obj_idx = tier4_autoware_utils::findNearestSegmentIndex(traj.points, obj_pos); + size_t obj_idx = motion_utils::findNearestSegmentIndex(traj.points, obj_pos); const double ego_to_obj_distance = - tier4_autoware_utils::calcSignedArcLength(traj.points, ego_idx, obj_idx); + motion_utils::calcSignedArcLength(traj.points, ego_idx, obj_idx); if (ego_to_obj_distance < 0) { return false; @@ -145,7 +144,7 @@ bool isAngleAlignedWithTrajectory( const double obj_yaw = tf2::getYaw(pose.orientation); - const size_t nearest_idx = tier4_autoware_utils::findNearestIndex(traj.points, pose.position); + const size_t nearest_idx = motion_utils::findNearestIndex(traj.points, pose.position); const double traj_yaw = tf2::getYaw(traj.points.at(nearest_idx).pose.orientation); const double diff_yaw = tier4_autoware_utils::normalizeRadian(obj_yaw - traj_yaw); @@ -164,7 +163,7 @@ double calcAlignedAdaptiveCruise( const auto & object_vel = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x; - const size_t object_idx = tier4_autoware_utils::findNearestIndex(trajectory.points, object_pos); + const size_t object_idx = motion_utils::findNearestIndex(trajectory.points, object_pos); const double object_yaw = tf2::getYaw(predicted_object.kinematics.initial_pose_with_covariance.pose.orientation); @@ -671,7 +670,7 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( // rough detection area filtering without polygons const double dist_from_obstacle_to_traj = [&]() { - return tier4_autoware_utils::calcLateralOffset(decimated_traj.points, object_pose.position); + return motion_utils::calcLateralOffset(decimated_traj.points, object_pose.position); }(); if ( std::fabs(dist_from_obstacle_to_traj) > @@ -914,7 +913,7 @@ geometry_msgs::msg::Point ObstacleCruisePlannerNode::calcNearestCollisionPoint( for (size_t cp_idx = 0; cp_idx < collision_points.size(); ++cp_idx) { const auto & collision_point = collision_points.at(cp_idx); const double dist = - tier4_autoware_utils::calcLongitudinalOffsetToSegment(segment_points, 0, collision_point); + motion_utils::calcLongitudinalOffsetToSegment(segment_points, 0, collision_point); if (dist < min_dist) { min_dist = dist; min_idx = cp_idx; @@ -939,7 +938,7 @@ double ObstacleCruisePlannerNode::calcCollisionTimeMargin( const double time_to_collision = [&]() { const double dist_from_ego_to_obstacle = - tier4_autoware_utils::calcSignedArcLength( + motion_utils::calcSignedArcLength( decimated_traj.points, current_pose.position, nearest_collision_point) - vehicle_info_.max_longitudinal_offset_m; return dist_from_ego_to_obstacle / std::max(1e-6, current_vel); @@ -953,7 +952,7 @@ double ObstacleCruisePlannerNode::calcCollisionTimeMargin( return std::numeric_limits::max(); } - const double dist_to_obstacle_getting_out = tier4_autoware_utils::calcSignedArcLength( + const double dist_to_obstacle_getting_out = motion_utils::calcSignedArcLength( decimated_traj.points, object_pose.position, obstacle_getting_out_idx.get()); return dist_to_obstacle_getting_out / object_velocity; diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index 6fdc296e95f4e..98b40f978cfcd 100644 --- a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -128,7 +128,7 @@ Trajectory OptimizationBasedPlanner::generateCruiseTrajectory( } // Get the nearest point on the trajectory - const auto closest_idx = tier4_autoware_utils::findNearestIndex( + const auto closest_idx = motion_utils::findNearestIndex( planner_data.traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); if (!closest_idx) { // Check validity of the closest index @@ -273,7 +273,7 @@ Trajectory OptimizationBasedPlanner::generateCruiseTrajectory( break; } } - const auto traj_stop_dist = tier4_autoware_utils::calcDistanceToForwardStopPoint( + const auto traj_stop_dist = motion_utils::calcDistanceToForwardStopPoint( planner_data.traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); if (traj_stop_dist) { @@ -432,7 +432,7 @@ std::tuple OptimizationBasedPlanner::calcInitialMotion( const double engage_vel_thr = engage_velocity_ * engage_exit_ratio_; if (vehicle_speed < engage_vel_thr) { if (target_vel >= engage_velocity_) { - const auto stop_dist = tier4_autoware_utils::calcDistanceToForwardStopPoint( + const auto stop_dist = motion_utils::calcDistanceToForwardStopPoint( input_traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); if ((stop_dist && *stop_dist > stop_dist_to_prohibit_engage_) || !stop_dist) { @@ -483,7 +483,7 @@ TrajectoryPoint OptimizationBasedPlanner::calcInterpolatedTrajectoryPoint( } const size_t segment_idx = - tier4_autoware_utils::findNearestSegmentIndex(trajectory.points, target_pose.position); + motion_utils::findNearestSegmentIndex(trajectory.points, target_pose.position); const auto v1 = getTransVector3( trajectory.points.at(segment_idx).pose, trajectory.points.at(segment_idx + 1).pose); @@ -508,7 +508,7 @@ TrajectoryPoint OptimizationBasedPlanner::calcInterpolatedTrajectoryPoint( bool OptimizationBasedPlanner::checkHasReachedGoal(const ObstacleCruisePlannerData & planner_data) { // If goal is close and current velocity is low, we don't optimize trajectory - const auto closest_stop_dist = tier4_autoware_utils::calcDistanceToForwardStopPoint( + const auto closest_stop_dist = motion_utils::calcDistanceToForwardStopPoint( planner_data.traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); if (closest_stop_dist && *closest_stop_dist < 0.5 && planner_data.current_vel < 0.6) { @@ -523,7 +523,7 @@ OptimizationBasedPlanner::TrajectoryData OptimizationBasedPlanner::getTrajectory { TrajectoryData base_traj; const auto nearest_segment_idx = - tier4_autoware_utils::findNearestSegmentIndex(traj.points, current_pose.position); + motion_utils::findNearestSegmentIndex(traj.points, current_pose.position); const auto interpolated_point = calcInterpolatedTrajectoryPoint(traj, current_pose); const auto dist = tier4_autoware_utils::calcDistance2d( interpolated_point.pose.position, traj.points.at(nearest_segment_idx).pose.position); @@ -559,8 +559,7 @@ OptimizationBasedPlanner::TrajectoryData OptimizationBasedPlanner::resampleTraje } // Obtain trajectory length until the velocity is zero or stop dist - const auto closest_stop_id = - tier4_autoware_utils::searchZeroVelocityIndex(base_traj_data.traj.points); + const auto closest_stop_id = motion_utils::searchZeroVelocityIndex(base_traj_data.traj.points); const double closest_stop_dist = closest_stop_id ? base_s.at(*closest_stop_id) : base_s.back(); const double traj_length = std::min(closest_stop_dist, std::min(base_s.back(), max_traj_length)); @@ -693,7 +692,7 @@ boost::optional OptimizationBasedPlanner::getSBoundaries( const double rss_dist = calcRSSDistance(planner_data.current_vel, obj_vel); const auto & safe_distance_margin = longitudinal_info_.safe_distance_margin; - const double ego_obj_length = tier4_autoware_utils::calcSignedArcLength( + const double ego_obj_length = motion_utils::calcSignedArcLength( ego_traj_data.traj.points, planner_data.current_pose.position, current_object_pose.get().position); const double slow_down_point_length = @@ -886,8 +885,7 @@ boost::optional OptimizationBasedPlanner::getDistanceToCollisionPoint( size_t min_nearest_idx = ego_traj_data.s.size(); size_t max_nearest_idx = 0; for (const auto & obj_p : object_points) { - size_t nearest_idx = - tier4_autoware_utils::findNearestSegmentIndex(ego_traj_data.traj.points, obj_p); + size_t nearest_idx = motion_utils::findNearestSegmentIndex(ego_traj_data.traj.points, obj_p); min_nearest_idx = std::min(nearest_idx, min_nearest_idx); max_nearest_idx = std::max(nearest_idx, max_nearest_idx); } @@ -917,7 +915,7 @@ boost::optional OptimizationBasedPlanner::getDistanceToCollisionPoint( if (collision_idx) { // TODO(shimizu) Consider the time difference between ego vehicle and objects - return tier4_autoware_utils::calcSignedArcLength( + return motion_utils::calcSignedArcLength( ego_traj_data.traj.points, ego_traj_data.traj.points.front().pose.position, obj_pose.position); } @@ -1081,7 +1079,7 @@ void OptimizationBasedPlanner::publishDebugTrajectory( } boundary_pub_->publish(boundary_traj); - const double s_before = tier4_autoware_utils::calcSignedArcLength(traj.points, 0, closest_idx); + const double s_before = motion_utils::calcSignedArcLength(traj.points, 0, closest_idx); Trajectory optimized_sv_traj; optimized_sv_traj.header.stamp = current_time; optimized_sv_traj.points.resize(opt_result.s.size()); diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index fe3253eabd31b..82e4fde53f430 100644 --- a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -158,11 +158,11 @@ double PIDBasedPlanner::calcDistanceToObstacle( const size_t ego_segment_idx = findExtendedNearestSegmentIndex(planner_data.traj, planner_data.current_pose); const double segment_offset = std::max( - 0.0, tier4_autoware_utils::calcLongitudinalOffsetToSegment( + 0.0, motion_utils::calcLongitudinalOffsetToSegment( planner_data.traj.points, ego_segment_idx, planner_data.current_pose.position)); const double offset = vehicle_info_.max_longitudinal_offset_m + segment_offset; - return tier4_autoware_utils::calcSignedArcLength( + return motion_utils::calcSignedArcLength( planner_data.traj.points, ego_segment_idx, obstacle.collision_point) - offset; } diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 4f131adf2343f..1839752e6a3be 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -72,10 +72,10 @@ Trajectory PlannerInterface::generateStopTrajectory( } // Get Closest Obstacle Stop Distance - const double closest_obstacle_dist = tier4_autoware_utils::calcSignedArcLength( + const double closest_obstacle_dist = motion_utils::calcSignedArcLength( planner_data.traj.points, 0, closest_stop_obstacle->collision_point); - const auto negative_dist_to_ego = tier4_autoware_utils::calcSignedArcLength( + const auto negative_dist_to_ego = motion_utils::calcSignedArcLength( planner_data.traj.points, planner_data.current_pose, 0, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); if (!negative_dist_to_ego) { @@ -86,10 +86,9 @@ Trajectory PlannerInterface::generateStopTrajectory( // If behavior stop point is ahead of the closest_obstacle_stop point within a certain margin // we set closest_obstacle_stop_distance to closest_behavior_stop_distance const double margin_from_obstacle = [&]() { - const auto closest_behavior_stop_dist_from_ego = - tier4_autoware_utils::calcDistanceToForwardStopPoint( - planner_data.traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + const auto closest_behavior_stop_dist_from_ego = motion_utils::calcDistanceToForwardStopPoint( + planner_data.traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_, + nearest_yaw_deviation_threshold_); if (closest_behavior_stop_dist_from_ego) { const double closest_obstacle_stop_dist_from_ego = closest_obstacle_dist - dist_to_ego - @@ -121,7 +120,7 @@ Trajectory PlannerInterface::generateStopTrajectory( will_collide_with_obstacle = true; } - const size_t collision_idx = tier4_autoware_utils::findNearestIndex( + const size_t collision_idx = motion_utils::findNearestIndex( planner_data.traj.points, closest_stop_obstacle->collision_point); const size_t zero_vel_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset( planner_data.traj.points, diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp index 170943484033d..d048b265aa382 100644 --- a/planning/obstacle_cruise_planner/src/utils.cpp +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -49,8 +49,7 @@ boost::optional calcForwardPose( size_t search_idx = start_idx; double length_to_search_idx = 0.0; for (; search_idx < traj.points.size(); ++search_idx) { - length_to_search_idx = - tier4_autoware_utils::calcSignedArcLength(traj.points, start_idx, search_idx); + length_to_search_idx = motion_utils::calcSignedArcLength(traj.points, start_idx, search_idx); if (length_to_search_idx > target_length) { break; } else if (search_idx == traj.points.size() - 1) { @@ -161,7 +160,7 @@ autoware_auto_planning_msgs::msg::Trajectory insertStopPoint( return trajectory; } - const double traj_length = tier4_autoware_utils::calcArcLength(trajectory.points); + const double traj_length = motion_utils::calcArcLength(trajectory.points); if (traj_length < distance_to_stop_point) { return trajectory; } @@ -233,7 +232,7 @@ boost::optional getClosestStopObstacle( } const double dist_to_stop_obstacle = - tier4_autoware_utils::calcSignedArcLength(traj.points, 0, obstacle.collision_point); + motion_utils::calcSignedArcLength(traj.points, 0, obstacle.collision_point); if (dist_to_stop_obstacle < dist_to_closest_stop_obstacle) { dist_to_closest_stop_obstacle = dist_to_stop_obstacle; closest_stop_obstacle = obstacle; diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index ec7429cb203d7..e56f2a8604ed5 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -18,6 +18,8 @@ #include "obstacle_stop_planner/adaptive_cruise_control.hpp" #include "obstacle_stop_planner/debug_marker.hpp" +#include +#include #include #include #include @@ -26,8 +28,6 @@ #include #include #include -#include -#include #include #include diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 064adbc3fd1f7..5091df4bce7d4 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -42,11 +42,11 @@ namespace motion_planning { +using motion_utils::calcSignedArcLength; +using motion_utils::findNearestIndex; using tier4_autoware_utils::calcAzimuthAngle; using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcSignedArcLength; using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::findNearestIndex; using tier4_autoware_utils::getRPY; namespace @@ -617,11 +617,11 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu Trajectory output_trajectory = *input_msg; TrajectoryPoints output_trajectory_points = - tier4_autoware_utils::convertToTrajectoryPointArray(*input_msg); + motion_utils::convertToTrajectoryPointArray(*input_msg); // trim trajectory from self pose const auto base_trajectory = trimTrajectoryWithIndexFromSelfPose( - tier4_autoware_utils::convertToTrajectoryPointArray(*input_msg), planner_data.current_pose, + motion_utils::convertToTrajectoryPointArray(*input_msg), planner_data.current_pose, planner_data.trajectory_trim_index); // extend trajectory to consider obstacles after the goal const auto extend_trajectory = extendTrajectory(base_trajectory, stop_param.extend_distance); @@ -645,7 +645,7 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu resetExternalVelocityLimit(current_acc); } - auto trajectory = tier4_autoware_utils::convertToTrajectory(output_trajectory_points); + auto trajectory = motion_utils::convertToTrajectory(output_trajectory_points); publishDebugData(planner_data, current_acc); trajectory.header = input_msg->header; @@ -1252,10 +1252,10 @@ TrajectoryPoints ObstacleStopPlannerNode::trimTrajectoryWithIndexFromSelfPose( TrajectoryPoints output{}; size_t min_distance_index = 0; - const auto nearest_index = tier4_autoware_utils::findNearestIndex( - input, self_pose, 10.0, node_param_.max_yaw_deviation_rad); + const auto nearest_index = + motion_utils::findNearestIndex(input, self_pose, 10.0, node_param_.max_yaw_deviation_rad); if (!nearest_index) { - min_distance_index = tier4_autoware_utils::findNearestIndex(input, self_pose.position); + min_distance_index = motion_utils::findNearestIndex(input, self_pose.position); } else { min_distance_index = nearest_index.value(); } diff --git a/planning/planning_evaluator/package.xml b/planning/planning_evaluator/package.xml index 8681543485c21..2f427e79e4d39 100644 --- a/planning/planning_evaluator/package.xml +++ b/planning/planning_evaluator/package.xml @@ -16,6 +16,7 @@ diagnostic_msgs eigen geometry_msgs + motion_utils nav_msgs rclcpp rclcpp_components diff --git a/planning/planning_evaluator/src/metrics/deviation_metrics.cpp b/planning/planning_evaluator/src/metrics/deviation_metrics.cpp index 669008ff86c9e..d6d8aa661e5ea 100644 --- a/planning/planning_evaluator/src/metrics/deviation_metrics.cpp +++ b/planning/planning_evaluator/src/metrics/deviation_metrics.cpp @@ -14,6 +14,7 @@ #include "planning_evaluator/metrics/deviation_metrics.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" namespace planning_diagnostics @@ -35,8 +36,7 @@ Stat calcLateralDeviation(const Trajectory & ref, const Trajectory & tra * need more precise calculation, e.g., lateral distance from spline of the reference traj */ for (TrajectoryPoint p : traj.points) { - const size_t nearest_index = - tier4_autoware_utils::findNearestIndex(ref.points, p.pose.position); + const size_t nearest_index = motion_utils::findNearestIndex(ref.points, p.pose.position); stat.add( tier4_autoware_utils::calcLateralDeviation(ref.points[nearest_index].pose, p.pose.position)); } @@ -55,8 +55,7 @@ Stat calcYawDeviation(const Trajectory & ref, const Trajectory & traj) * need more precise calculation, e.g., yaw distance from spline of the reference traj */ for (TrajectoryPoint p : traj.points) { - const size_t nearest_index = - tier4_autoware_utils::findNearestIndex(ref.points, p.pose.position); + const size_t nearest_index = motion_utils::findNearestIndex(ref.points, p.pose.position); stat.add(tier4_autoware_utils::calcYawDeviation(ref.points[nearest_index].pose, p.pose)); } return stat; @@ -72,8 +71,7 @@ Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & tr // TODO(Maxime CLEMENT) need more precise calculation for (TrajectoryPoint p : traj.points) { - const size_t nearest_index = - tier4_autoware_utils::findNearestIndex(ref.points, p.pose.position); + const size_t nearest_index = motion_utils::findNearestIndex(ref.points, p.pose.position); stat.add(p.longitudinal_velocity_mps - ref.points[nearest_index].longitudinal_velocity_mps); } return stat; diff --git a/planning/planning_evaluator/src/metrics/stability_metrics.cpp b/planning/planning_evaluator/src/metrics/stability_metrics.cpp index 2fdd04d2456ea..931764f5bb669 100644 --- a/planning/planning_evaluator/src/metrics/stability_metrics.cpp +++ b/planning/planning_evaluator/src/metrics/stability_metrics.cpp @@ -15,6 +15,7 @@ #include "planning_evaluator/metrics/stability_metrics.hpp" #include "eigen3/Eigen/Core" +#include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" @@ -64,20 +65,19 @@ Stat calcLateralDistance(const Trajectory & traj1, const Trajectory & tr for (const auto & point : traj2.points) { const auto p0 = tier4_autoware_utils::getPoint(point); // find nearest segment - const size_t nearest_segment_idx = - tier4_autoware_utils::findNearestSegmentIndex(traj1.points, p0); + const size_t nearest_segment_idx = motion_utils::findNearestSegmentIndex(traj1.points, p0); double dist; // distance to segment if ( nearest_segment_idx == traj1.points.size() - 2 && - tier4_autoware_utils::calcLongitudinalOffsetToSegment(traj1.points, nearest_segment_idx, p0) > + motion_utils::calcLongitudinalOffsetToSegment(traj1.points, nearest_segment_idx, p0) > tier4_autoware_utils::calcDistance2d( traj1.points[nearest_segment_idx], traj1.points[nearest_segment_idx + 1])) { // distance to last point dist = tier4_autoware_utils::calcDistance2d(traj1.points.back(), p0); } else if ( // NOLINT - nearest_segment_idx == 0 && tier4_autoware_utils::calcLongitudinalOffsetToSegment( - traj1.points, nearest_segment_idx, p0) <= 0) { + nearest_segment_idx == 0 && + motion_utils::calcLongitudinalOffsetToSegment(traj1.points, nearest_segment_idx, p0) <= 0) { // distance to first point dist = tier4_autoware_utils::calcDistance2d(traj1.points.front(), p0); } else { diff --git a/planning/planning_evaluator/src/metrics_calculator.cpp b/planning/planning_evaluator/src/metrics_calculator.cpp index 045331d1ee778..802e1dde94fd1 100644 --- a/planning/planning_evaluator/src/metrics_calculator.cpp +++ b/planning/planning_evaluator/src/metrics_calculator.cpp @@ -14,6 +14,7 @@ #include "planning_evaluator/metrics_calculator.hpp" +#include "motion_utils/motion_utils.hpp" #include "planning_evaluator/metrics/deviation_metrics.hpp" #include "planning_evaluator/metrics/obstacle_metrics.hpp" #include "planning_evaluator/metrics/stability_metrics.hpp" @@ -99,8 +100,7 @@ Trajectory MetricsCalculator::getLookaheadTrajectory( return traj; } - const auto ego_index = - tier4_autoware_utils::findNearestSegmentIndex(traj.points, ego_pose_.position); + const auto ego_index = motion_utils::findNearestSegmentIndex(traj.points, ego_pose_.position); Trajectory lookahead_traj; lookahead_traj.header = traj.header; double dist = 0.0; diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index e6e3c890e196a..85aaf9c303ea3 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -16,6 +16,7 @@ #define ROUTE_HANDLER__ROUTE_HANDLER_HPP_ #include +#include #include #include diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml index d85cb5fee79b1..f83872e6eabcd 100644 --- a/planning/route_handler/package.xml +++ b/planning/route_handler/package.xml @@ -18,6 +18,7 @@ autoware_auto_planning_msgs geometry_msgs lanelet2_extension + motion_utils rclcpp rclcpp_components tf2_ros diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 453361e94e399..607e3c7b590aa 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -406,9 +406,9 @@ void RouteHandler::setPullOverGoalPose( lanelet::utils::getArcCoordinates({target_lane}, route_msg_.goal_pose); Path centerline_path = convertToPathFromPathWithLaneId( getCenterLinePath({target_lane}, 0.0, arc_position_goal.length + 10)); - const auto seg_idx = tier4_autoware_utils::findNearestSegmentIndex( - centerline_path.points, route_msg_.goal_pose.position); - const double d_lat = tier4_autoware_utils::calcLongitudinalOffsetToSegment( + const auto seg_idx = + motion_utils::findNearestSegmentIndex(centerline_path.points, route_msg_.goal_pose.position); + const double d_lat = motion_utils::calcLongitudinalOffsetToSegment( centerline_path.points, seg_idx, route_msg_.goal_pose.position); const auto shoulder_point = tier4_autoware_utils::calcOffsetPose(centerline_path.points.at(seg_idx).pose, d_lat, 0.0, 0.0); diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp index e437787c234ac..663907788e835 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp @@ -17,6 +17,7 @@ #include "surround_obstacle_checker/debug_marker.hpp" +#include #include #include #include