diff --git a/common/autoware_motion_utils/README.md b/common/autoware_motion_utils/README.md index ca63591267..8d80ae9218 100644 --- a/common/autoware_motion_utils/README.md +++ b/common/autoware_motion_utils/README.md @@ -47,7 +47,7 @@ The second function finds the nearest index in the lane whose id is `lane_id`. ```cpp size_t findNearestIndexFromLaneId( - const autoware_planning_msgs::msg::PathWithLaneId & path, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, const int64_t lane_id); ``` diff --git a/common/autoware_motion_utils/docs/vehicle/vehicle.md b/common/autoware_motion_utils/docs/vehicle/vehicle.md index 32cbf6be28..734139aa63 100644 --- a/common/autoware_motion_utils/docs/vehicle/vehicle.md +++ b/common/autoware_motion_utils/docs/vehicle/vehicle.md @@ -36,7 +36,7 @@ bool isVehicleStopped(const double stop_duration) Necessary includes: ```c++ -#include +#include ``` 1.Create a checker instance. @@ -116,7 +116,7 @@ bool isVehicleStoppedAtStopPoint(const double stop_duration) Necessary includes: ```c++ -#include +#include ``` 1.Create a checker instance. diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/factor/planning_factor_interface.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/factor/planning_factor_interface.hpp index 9187f815eb..00a557ee46 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/factor/planning_factor_interface.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/factor/planning_factor_interface.hpp @@ -18,12 +18,12 @@ #include #include -#include +#include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -33,10 +33,10 @@ namespace autoware::motion_utils { -using autoware_planning_msgs::msg::ControlPoint; -using autoware_planning_msgs::msg::PlanningFactor; -using autoware_planning_msgs::msg::PlanningFactorArray; -using autoware_planning_msgs::msg::SafetyFactorArray; +using autoware_internal_planning_msgs::msg::ControlPoint; +using autoware_internal_planning_msgs::msg::PlanningFactor; +using autoware_internal_planning_msgs::msg::PlanningFactorArray; +using autoware_internal_planning_msgs::msg::SafetyFactorArray; using geometry_msgs::msg::Pose; class PlanningFactorInterface @@ -124,13 +124,13 @@ class PlanningFactorInterface const SafetyFactorArray & safety_factors, const bool is_driving_forward = true, const double velocity = 0.0, const double shift_length = 0.0, const std::string & detail = "") { - const auto control_point = autoware_planning_msgs::build() + const auto control_point = autoware_internal_planning_msgs::build() .pose(control_point_pose) .velocity(velocity) .shift_length(shift_length) .distance(distance); - const auto factor = autoware_planning_msgs::build() + const auto factor = autoware_internal_planning_msgs::build() .module(name_) .is_driving_forward(is_driving_forward) .control_points({control_point}) @@ -161,19 +161,19 @@ class PlanningFactorInterface const bool is_driving_forward = true, const double velocity = 0.0, const double shift_length = 0.0, const std::string & detail = "") { - const auto control_start_point = autoware_planning_msgs::build() + const auto control_start_point = autoware_internal_planning_msgs::build() .pose(start_pose) .velocity(velocity) .shift_length(shift_length) .distance(start_distance); - const auto control_end_point = autoware_planning_msgs::build() + const auto control_end_point = autoware_internal_planning_msgs::build() .pose(end_pose) .velocity(velocity) .shift_length(shift_length) .distance(end_distance); - const auto factor = autoware_planning_msgs::build() + const auto factor = autoware_internal_planning_msgs::build() .module(name_) .is_driving_forward(is_driving_forward) .control_points({control_start_point, control_end_point}) @@ -209,8 +209,8 @@ class PlanningFactorInterface std::vector factors_; }; -extern template void PlanningFactorInterface::add( - const std::vector &, const Pose &, const Pose &, +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, const std::string &); extern template void PlanningFactorInterface::add( @@ -222,8 +222,8 @@ extern template void PlanningFactorInterface::add( - const std::vector &, const Pose &, const Pose &, +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, const std::string &); extern template void PlanningFactorInterface::add( diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp index d6af4a755a..1aec77557b 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ #include "autoware_planning_msgs/msg/path.hpp" -#include "autoware_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include @@ -113,8 +113,8 @@ std::vector resamplePoseVector( * longitudinal and lateral velocity. Otherwise, it uses linear interpolation * @return resampled path */ -autoware_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_planning_msgs::msg::PathWithLaneId & input_path, +autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); @@ -136,8 +136,8 @@ autoware_planning_msgs::msg::PathWithLaneId resamplePath( * @param resample_input_path_stop_point If true, resample closest stop point in input path * @return resampled path */ -autoware_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval, +autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true, const bool resample_input_path_stop_point = true); diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp index 5d622c54da..e15375d73b 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp @@ -15,11 +15,11 @@ #ifndef AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ #define AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ -#include "autoware/universe_utils/system/backtrace.hpp" +#include "autoware_utils/system/backtrace.hpp" #include #include -#include +#include #include @@ -50,9 +50,9 @@ template bool validate_points_duplication(const T & points) { for (size_t i = 0; i < points.size() - 1; ++i) { - const auto & curr_pt = autoware::universe_utils::getPoint(points.at(i)); - const auto & next_pt = autoware::universe_utils::getPoint(points.at(i + 1)); - const double ds = autoware::universe_utils::calcDistance2d(curr_pt, next_pt); + const auto & curr_pt = autoware_utils::get_point(points.at(i)); + const auto & next_pt = autoware_utils::get_point(points.at(i + 1)); + const double ds = autoware_utils::calc_distance2d(curr_pt, next_pt); if (ds < close_s_threshold) { return false; } @@ -67,27 +67,27 @@ bool validate_arguments(const T & input_points, const std::vector & resa // Check size of the arguments if (!validate_size(input_points)) { RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2"); - autoware::universe_utils::print_backtrace(); + autoware_utils::print_backtrace(); return false; } if (!validate_size(resampling_intervals)) { RCLCPP_DEBUG( get_logger(), "invalid argument: The number of resampling intervals is less than 2"); - autoware::universe_utils::print_backtrace(); + autoware_utils::print_backtrace(); return false; } // Check resampling range if (!validate_resampling_range(input_points, resampling_intervals)) { RCLCPP_DEBUG(get_logger(), "invalid argument: resampling interval is longer than input points"); - autoware::universe_utils::print_backtrace(); + autoware_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points"); - autoware::universe_utils::print_backtrace(); + autoware_utils::print_backtrace(); return false; } @@ -100,7 +100,7 @@ bool validate_arguments(const T & input_points, const double resampling_interval // Check size of the arguments if (!validate_size(input_points)) { RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2"); - autoware::universe_utils::print_backtrace(); + autoware_utils::print_backtrace(); return false; } @@ -109,14 +109,14 @@ bool validate_arguments(const T & input_points, const double resampling_interval RCLCPP_DEBUG( get_logger(), "invalid argument: resampling interval is less than %f", autoware::motion_utils::overlap_threshold); - autoware::universe_utils::print_backtrace(); + autoware_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points"); - autoware::universe_utils::print_backtrace(); + autoware_utils::print_backtrace(); return false; } diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp index ffb635efc8..babefdbc33 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ #include "autoware_planning_msgs/msg/detail/path__struct.hpp" -#include "autoware_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" +#include "autoware_internal_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" #include "autoware_planning_msgs/msg/detail/trajectory__struct.hpp" #include "autoware_planning_msgs/msg/detail/trajectory_point__struct.hpp" #include "std_msgs/msg/header.hpp" @@ -58,7 +58,7 @@ autoware_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input template <> inline autoware_planning_msgs::msg::Path convertToPath( - const autoware_planning_msgs::msg::PathWithLaneId & input) + const autoware_internal_planning_msgs::msg::PathWithLaneId & input) { autoware_planning_msgs::msg::Path output{}; output.header = input.header; @@ -80,7 +80,7 @@ TrajectoryPoints convertToTrajectoryPoints([[maybe_unused]] const T & input) template <> inline TrajectoryPoints convertToTrajectoryPoints( - const autoware_planning_msgs::msg::PathWithLaneId & input) + const autoware_internal_planning_msgs::msg::PathWithLaneId & input) { TrajectoryPoints output{}; for (const auto & p : input.points) { @@ -95,7 +95,7 @@ inline TrajectoryPoints convertToTrajectoryPoints( } template -autoware_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( +autoware_internal_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( [[maybe_unused]] const T & input) { static_assert(sizeof(T) == 0, "Only specializations of convertToPathWithLaneId can be used."); @@ -103,12 +103,12 @@ autoware_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( } template <> -inline autoware_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( +inline autoware_internal_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( const TrajectoryPoints & input) { - autoware_planning_msgs::msg::PathWithLaneId output{}; + autoware_internal_planning_msgs::msg::PathWithLaneId output{}; for (const auto & p : input) { - autoware_planning_msgs::msg::PathPointWithLaneId pp; + autoware_internal_planning_msgs::msg::PathPointWithLaneId pp; pp.point.pose = p.pose; pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; output.points.emplace_back(pp); diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp index eca2630a0f..920235d5ce 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp @@ -15,9 +15,9 @@ #ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ #define AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" -#include "autoware_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include @@ -51,8 +51,8 @@ autoware_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint( * twist information * @return resampled path(poses) */ -autoware_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint( - const autoware_planning_msgs::msg::PathWithLaneId & path, +autoware_internal_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); @@ -73,22 +73,22 @@ geometry_msgs::msg::Pose calcInterpolatedPose(const T & points, const double tar } if (points.size() < 2 || target_length < 0.0) { - return autoware::universe_utils::getPose(points.front()); + return autoware_utils::get_pose(points.front()); } double accumulated_length = 0; for (size_t i = 0; i < points.size() - 1; ++i) { - const auto & curr_pose = autoware::universe_utils::getPose(points.at(i)); - const auto & next_pose = autoware::universe_utils::getPose(points.at(i + 1)); - const double length = autoware::universe_utils::calcDistance3d(curr_pose, next_pose); + const auto & curr_pose = autoware_utils::get_pose(points.at(i)); + const auto & next_pose = autoware_utils::get_pose(points.at(i + 1)); + const double length = autoware_utils::calc_distance3d(curr_pose, next_pose); if (accumulated_length + length > target_length) { const double ratio = (target_length - accumulated_length) / std::max(length, 1e-6); - return autoware::universe_utils::calcInterpolatedPose(curr_pose, next_pose, ratio); + return autoware_utils::calc_interpolated_pose(curr_pose, next_pose, ratio); } accumulated_length += length; } - return autoware::universe_utils::getPose(points.back()); + return autoware_utils::get_pose(points.back()); } } // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp index 0c181dec46..8b25f00fb7 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ #define AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ -#include "autoware_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp" #include #include @@ -23,14 +23,14 @@ namespace autoware::motion_utils { std::optional> getPathIndexRangeWithLaneId( - const autoware_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); size_t findNearestIndexFromLaneId( - const autoware_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, const int64_t lane_id); size_t findNearestSegmentIndexFromLaneId( - const autoware_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, const int64_t lane_id); // @brief Calculates the path to be followed by the rear wheel center in order to make the vehicle @@ -38,8 +38,8 @@ size_t findNearestSegmentIndexFromLaneId( // @param [in] path with position to be followed by the center of the vehicle // @param [out] PathWithLaneId to be followed by the rear wheel center follow to make the vehicle // center follow the input path NOTE: rear_to_cog is supposed to be positive -autoware_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( - const autoware_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, +autoware_internal_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, const bool enable_last_point_compensation = true); } // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp index c886f3c3bc..8f012cab68 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp @@ -15,16 +15,16 @@ #ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ #define AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/geometry/pose_deviation.hpp" -#include "autoware/universe_utils/math/constants.hpp" -#include "autoware/universe_utils/system/backtrace.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/pose_deviation.hpp" +#include "autoware_utils/math/constants.hpp" +#include "autoware_utils/system/backtrace.hpp" #include #include #include -#include +#include #include #include @@ -51,7 +51,7 @@ template void validateNonEmpty(const T & points) { if (points.empty()) { - autoware::universe_utils::print_backtrace(); + autoware_utils::print_backtrace(); throw std::invalid_argument("[autoware_motion_utils] validateNonEmpty(): Points is empty."); } } @@ -59,8 +59,8 @@ void validateNonEmpty(const T & points) extern template void validateNonEmpty>( const std::vector &); extern template void -validateNonEmpty>( - const std::vector &); +validateNonEmpty>( + const std::vector &); extern template void validateNonEmpty>( const std::vector &); @@ -73,22 +73,22 @@ extern template void validateNonEmpty void validateNonSharpAngle( const T & point1, const T & point2, const T & point3, - const double angle_threshold = autoware::universe_utils::pi / 4) + const double angle_threshold = autoware_utils::pi / 4) { - const auto p1 = autoware::universe_utils::getPoint(point1); - const auto p2 = autoware::universe_utils::getPoint(point2); - const auto p3 = autoware::universe_utils::getPoint(point3); + const auto p1 = autoware_utils::get_point(point1); + const auto p2 = autoware_utils::get_point(point2); + const auto p3 = autoware_utils::get_point(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 = autoware::universe_utils::calcDistance3d(p1, p2); - const auto dist_3to2 = autoware::universe_utils::calcDistance3d(p3, p2); + const auto dist_1to2 = autoware_utils::calc_distance3d(p1, p2); + const auto dist_3to2 = autoware_utils::calc_distance3d(p3, p2); constexpr double epsilon = 1e-3; if (std::cos(angle_threshold) < product / dist_1to2 / dist_3to2 + epsilon) { - autoware::universe_utils::print_backtrace(); + autoware_utils::print_backtrace(); throw std::invalid_argument( "[autoware_motion_utils] validateNonSharpAngle(): Too sharp angle."); } @@ -107,18 +107,18 @@ std::optional isDrivingForward(const T & points) } // check the first point direction - const auto & first_pose = autoware::universe_utils::getPose(points.at(0)); - const auto & second_pose = autoware::universe_utils::getPose(points.at(1)); + const auto & first_pose = autoware_utils::get_pose(points.at(0)); + const auto & second_pose = autoware_utils::get_pose(points.at(1)); - return autoware::universe_utils::isDrivingForward(first_pose, second_pose); + return autoware_utils::is_driving_forward(first_pose, second_pose); } extern template std::optional isDrivingForward>( const std::vector &); extern template std::optional -isDrivingForward>( - const std::vector &); +isDrivingForward>( + const std::vector &); extern template std::optional isDrivingForward>( const std::vector &); @@ -136,10 +136,10 @@ std::optional isDrivingForwardWithTwist(const T & points_with_twist) return std::nullopt; } if (points_with_twist.size() == 1) { - if (0.0 < autoware::universe_utils::getLongitudinalVelocity(points_with_twist.front())) { + if (0.0 < autoware_utils::get_longitudinal_velocity(points_with_twist.front())) { return true; } - if (0.0 > autoware::universe_utils::getLongitudinalVelocity(points_with_twist.front())) { + if (0.0 > autoware_utils::get_longitudinal_velocity(points_with_twist.front())) { return false; } return std::nullopt; @@ -152,8 +152,8 @@ extern template std::optional isDrivingForwardWithTwist>( const std::vector &); extern template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); extern template std::optional isDrivingForwardWithTwist>( const std::vector &); @@ -183,8 +183,8 @@ T removeOverlapPoints(const T & points, const size_t start_idx = 0) constexpr double eps = 1.0E-08; for (size_t i = start_idx + 1; i < points.size(); ++i) { - const auto prev_p = autoware::universe_utils::getPoint(dst.back()); - const auto curr_p = autoware::universe_utils::getPoint(points.at(i)); + const auto prev_p = autoware_utils::get_point(dst.back()); + const auto curr_p = autoware_utils::get_point(points.at(i)); if (std::abs(prev_p.x - curr_p.x) < eps && std::abs(prev_p.y - curr_p.y) < eps) { continue; } @@ -197,9 +197,9 @@ T removeOverlapPoints(const T & points, const size_t start_idx = 0) extern template std::vector removeOverlapPoints>( const std::vector & points, const size_t start_idx = 0); -extern template std::vector -removeOverlapPoints>( - const std::vector & points, +extern template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx = 0); extern template std::vector removeOverlapPoints>( @@ -300,7 +300,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 = autoware::universe_utils::calcSquaredDistance2d(points.at(i), point); + const auto dist = autoware_utils::calc_squared_distance2d(points.at(i), point); if (dist < min_dist) { min_dist = dist; min_idx = i; @@ -313,8 +313,8 @@ extern template size_t findNearestIndex & points, const geometry_msgs::msg::Point & point); extern template size_t -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); extern template size_t findNearestIndex>( const std::vector & points, @@ -353,13 +353,13 @@ std::optional findNearestIndex( size_t min_idx = 0; for (size_t i = 0; i < points.size(); ++i) { - const auto squared_dist = autoware::universe_utils::calcSquaredDistance2d(points.at(i), pose); + const auto squared_dist = autoware_utils::calc_squared_distance2d(points.at(i), pose); if (squared_dist > max_squared_dist || squared_dist >= min_squared_dist) { continue; } - const auto yaw = autoware::universe_utils::calcYawDeviation( - autoware::universe_utils::getPose(points.at(i)), pose); + const auto yaw = autoware_utils::calc_yaw_deviation( + autoware_utils::get_pose(points.at(i)), pose); if (std::fabs(yaw) > max_yaw) { continue; } @@ -381,8 +381,8 @@ findNearestIndex>( const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional @@ -411,7 +411,7 @@ double calcLongitudinalOffsetToSegment( "[autoware_motion_utils] " + std::string(__func__) + ": Failed to calculate longitudinal offset because the given segment index is out of the " "points size."); - autoware::universe_utils::print_backtrace(); + autoware_utils::print_backtrace(); if (throw_exception) { throw std::out_of_range(error_message); } @@ -439,7 +439,7 @@ double calcLongitudinalOffsetToSegment( const std::string error_message( "[autoware_motion_utils] " + std::string(__func__) + ": Longitudinal offset calculation is not supported for the same points."); - autoware::universe_utils::print_backtrace(); + autoware_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } @@ -450,8 +450,8 @@ double calcLongitudinalOffsetToSegment( return std::nan(""); } - const auto p_front = autoware::universe_utils::getPoint(overlap_removed_points.at(seg_idx)); - const auto p_back = autoware::universe_utils::getPoint(overlap_removed_points.at(seg_idx + 1)); + const auto p_front = autoware_utils::get_point(overlap_removed_points.at(seg_idx)); + const auto p_back = autoware_utils::get_point(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}; @@ -464,8 +464,8 @@ calcLongitudinalOffsetToSegment & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); extern template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); extern template double @@ -506,8 +506,8 @@ extern template size_t findNearestSegmentIndex & points, const geometry_msgs::msg::Point & point); extern template size_t -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); extern template size_t findNearestSegmentIndex>( @@ -558,8 +558,8 @@ findNearestSegmentIndex>( const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional @@ -602,7 +602,7 @@ double calcLateralOffset( const std::string error_message( "[autoware_motion_utils] " + std::string(__func__) + ": Lateral offset calculation is not supported for the same points."); - autoware::universe_utils::print_backtrace(); + autoware_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } @@ -617,8 +617,8 @@ double calcLateralOffset( const auto p_front_idx = (p_indices > seg_idx) ? seg_idx : p_indices; const auto p_back_idx = p_front_idx + 1; - const auto p_front = autoware::universe_utils::getPoint(overlap_removed_points.at(p_front_idx)); - const auto p_back = autoware::universe_utils::getPoint(overlap_removed_points.at(p_back_idx)); + const auto p_front = autoware_utils::get_point(overlap_removed_points.at(p_front_idx)); + const auto p_back = autoware_utils::get_point(overlap_removed_points.at(p_back_idx)); 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}; @@ -632,8 +632,8 @@ extern template double calcLateralOffset>( - const std::vector & points, +calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception = false); extern template double calcLateralOffset>( @@ -675,7 +675,7 @@ double calcLateralOffset( const std::string error_message( "[autoware_motion_utils] " + std::string(__func__) + ": Lateral offset calculation is not supported for the same points."); - autoware::universe_utils::print_backtrace(); + autoware_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } @@ -694,8 +694,8 @@ extern template double calcLateralOffset & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); extern template double -calcLateralOffset>( - const std::vector & points, +calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); extern template double calcLateralOffset>( const std::vector & points, @@ -727,7 +727,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 += autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); + dist_sum += autoware_utils::calc_distance2d(points.at(i), points.at(i + 1)); } return dist_sum; } @@ -736,8 +736,8 @@ extern template double calcSignedArcLength & points, const size_t src_idx, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const size_t dst_idx); extern template double calcSignedArcLength>( @@ -775,7 +775,7 @@ std::vector calcSignedArcLengthPartialSum( double dist_sum = 0.0; partial_dist.push_back(dist_sum); for (size_t i = src_idx; i < dst_idx - 1; ++i) { - dist_sum += autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); + dist_sum += autoware_utils::calc_distance2d(points.at(i), points.at(i + 1)); partial_dist.push_back(dist_sum); } return partial_dist; @@ -786,8 +786,8 @@ calcSignedArcLengthPartialSum & points, const size_t src_idx, const size_t dst_idx); extern template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, const size_t dst_idx); extern template std::vector calcSignedArcLengthPartialSum>( @@ -828,8 +828,8 @@ extern template double calcSignedArcLength & points, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector &, +calcSignedArcLength>( + const std::vector &, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); extern template double calcSignedArcLength>( @@ -864,8 +864,8 @@ extern template double calcSignedArcLength & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point); extern template double calcSignedArcLength>( @@ -911,8 +911,8 @@ extern template double calcSignedArcLength & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); extern template double calcSignedArcLength>( @@ -939,8 +939,8 @@ double calcArcLength(const T & points) extern template double calcArcLength>( const std::vector & points); -extern template double calcArcLength>( - const std::vector & points); +extern template double calcArcLength>( + const std::vector & points); extern template double calcArcLength>( const std::vector & points); @@ -954,7 +954,7 @@ extern template double calcArcLength -std::vector calcCurvature(const T & points) +std::vector calc_curvature(const T & points) { std::vector curvature_vec(points.size(), 0.0); if (points.size() < 3) { @@ -962,10 +962,10 @@ std::vector calcCurvature(const T & points) } for (size_t i = 1; i < points.size() - 1; ++i) { - const auto p1 = autoware::universe_utils::getPoint(points.at(i - 1)); - const auto p2 = autoware::universe_utils::getPoint(points.at(i)); - const auto p3 = autoware::universe_utils::getPoint(points.at(i + 1)); - curvature_vec.at(i) = (autoware::universe_utils::calcCurvature(p1, p2, p3)); + const auto p1 = autoware_utils::get_point(points.at(i - 1)); + const auto p2 = autoware_utils::get_point(points.at(i)); + const auto p3 = autoware_utils::get_point(points.at(i + 1)); + curvature_vec.at(i) = (autoware_utils::calc_curvature(p1, p2, p3)); } curvature_vec.at(0) = curvature_vec.at(1); curvature_vec.at(curvature_vec.size() - 1) = curvature_vec.at(curvature_vec.size() - 2); @@ -974,13 +974,13 @@ std::vector calcCurvature(const T & points) } extern template std::vector -calcCurvature>( +calc_curvature>( const std::vector & points); extern template std::vector -calcCurvature>( - const std::vector & points); +calc_curvature>( + const std::vector & points); extern template std::vector -calcCurvature>( +calc_curvature>( const std::vector & points); /** @@ -994,7 +994,7 @@ calcCurvature>( * curvature calculation */ template -std::vector>> calcCurvatureAndSegmentLength( +std::vector>> calc_curvatureAndSegmentLength( const T & points) { // segment length is pair of segment length between {p1, p2} and {p2, p3} @@ -1002,24 +1002,24 @@ std::vector>> calcCurvatureAndSegmen curvature_and_segment_length_vec.reserve(points.size()); curvature_and_segment_length_vec.emplace_back(0.0, std::make_pair(0.0, 0.0)); for (size_t i = 1; i < points.size() - 1; ++i) { - const auto p1 = autoware::universe_utils::getPoint(points.at(i - 1)); - const auto p2 = autoware::universe_utils::getPoint(points.at(i)); - const auto p3 = autoware::universe_utils::getPoint(points.at(i + 1)); - const double curvature = autoware::universe_utils::calcCurvature(p1, p2, p3); + const auto p1 = autoware_utils::get_point(points.at(i - 1)); + const auto p2 = autoware_utils::get_point(points.at(i)); + const auto p3 = autoware_utils::get_point(points.at(i + 1)); + const double curvature = autoware_utils::calc_curvature(p1, p2, p3); // The first point has only the next point, so put the distance to that point. // In other words, assign the first segment length at the second point to the // second_segment_length at the first point. if (i == 1) { curvature_and_segment_length_vec.at(0).second.second = - autoware::universe_utils::calcDistance2d(p1, p2); + autoware_utils::calc_distance2d(p1, p2); } // The second_segment_length of the previous point and the first segment length of the current // point are equal. const std::pair arc_length{ curvature_and_segment_length_vec.back().second.second, - autoware::universe_utils::calcDistance2d(p2, p3)}; + autoware_utils::calc_distance2d(p2, p3)}; curvature_and_segment_length_vec.emplace_back(curvature, arc_length); } @@ -1032,13 +1032,13 @@ std::vector>> calcCurvatureAndSegmen } extern template std::vector>> -calcCurvatureAndSegmentLength>( +calc_curvatureAndSegmentLength>( const std::vector & points); extern template std::vector>> -calcCurvatureAndSegmentLength>( - const std::vector & points); +calc_curvatureAndSegmentLength>( + const std::vector & points); extern template std::vector>> -calcCurvatureAndSegmentLength>( +calc_curvatureAndSegmentLength>( const std::vector & points); /** @@ -1098,7 +1098,7 @@ std::optional calcLongitudinalOffsetPoint( "[autoware_motion_utils] " + std::string(__func__) + " error: The given source index is out of the points size. Failed to calculate longitudinal " "offset."); - autoware::universe_utils::print_backtrace(); + autoware_utils::print_backtrace(); if (throw_exception) { throw std::out_of_range(error_message); } @@ -1114,7 +1114,7 @@ std::optional calcLongitudinalOffsetPoint( } if (src_idx + 1 == points.size() && offset == 0.0) { - return autoware::universe_utils::getPoint(points.at(src_idx)); + return autoware_utils::get_point(points.at(src_idx)); } if (offset < 0.0) { @@ -1130,12 +1130,12 @@ std::optional calcLongitudinalOffsetPoint( const auto & p_front = points.at(i); const auto & p_back = points.at(i + 1); - const auto dist_segment = autoware::universe_utils::calcDistance2d(p_front, p_back); + const auto dist_segment = autoware_utils::calc_distance2d(p_front, p_back); dist_sum += dist_segment; const auto dist_res = offset - dist_sum; if (dist_res <= 0.0) { - return autoware::universe_utils::calcInterpolatedPoint( + return autoware_utils::calc_interpolated_point( p_back, p_front, std::abs(dist_res / dist_segment)); } } @@ -1149,8 +1149,8 @@ calcLongitudinalOffsetPoint> const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception = false); extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception = false); extern template std::optional calcLongitudinalOffsetPoint>( @@ -1194,8 +1194,8 @@ calcLongitudinalOffsetPoint> const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); extern template std::optional calcLongitudinalOffsetPoint>( @@ -1229,7 +1229,7 @@ std::optional calcLongitudinalOffsetPose( "[autoware_motion_utils] " + std::string(__func__) + " error: The given source index is out of the points size. Failed to calculate longitudinal " "offset."); - autoware::universe_utils::print_backtrace(); + autoware_utils::print_backtrace(); if (throw_exception) { throw std::out_of_range(error_message); } @@ -1243,7 +1243,7 @@ std::optional calcLongitudinalOffsetPose( } if (src_idx + 1 == points.size() && offset == 0.0) { - return autoware::universe_utils::getPose(points.at(src_idx)); + return autoware_utils::get_pose(points.at(src_idx)); } if (offset < 0.0) { @@ -1256,12 +1256,12 @@ std::optional calcLongitudinalOffsetPose( const auto & p_front = reverse_points.at(i); const auto & p_back = reverse_points.at(i + 1); - const auto dist_segment = autoware::universe_utils::calcDistance2d(p_front, p_back); + const auto dist_segment = autoware_utils::calc_distance2d(p_front, p_back); dist_sum += dist_segment; const auto dist_res = -offset - dist_sum; if (dist_res <= 0.0) { - return autoware::universe_utils::calcInterpolatedPose( + return autoware_utils::calc_interpolated_pose( p_back, p_front, std::abs(dist_res / dist_segment), set_orientation_from_position_direction); } @@ -1273,12 +1273,12 @@ std::optional calcLongitudinalOffsetPose( const auto & p_front = points.at(i); const auto & p_back = points.at(i + 1); - const auto dist_segment = autoware::universe_utils::calcDistance2d(p_front, p_back); + const auto dist_segment = autoware_utils::calc_distance2d(p_front, p_back); dist_sum += dist_segment; const auto dist_res = offset - dist_sum; if (dist_res <= 0.0) { - return autoware::universe_utils::calcInterpolatedPose( + return autoware_utils::calc_interpolated_pose( p_front, p_back, 1.0 - std::abs(dist_res / dist_segment), set_orientation_from_position_direction); } @@ -1295,8 +1295,8 @@ calcLongitudinalOffsetPose>( const double offset, const bool set_orientation_from_position_direction = true, const bool throw_exception = false); extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, const double offset, const bool set_orientation_from_position_direction = true, const bool throw_exception = false); extern template std::optional @@ -1342,8 +1342,8 @@ calcLongitudinalOffsetPose>( const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction = true); extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction = true); extern template std::optional @@ -1378,8 +1378,8 @@ std::optional insertTargetPoint( return {}; } - const auto p_front = autoware::universe_utils::getPoint(points.at(seg_idx)); - const auto p_back = autoware::universe_utils::getPoint(points.at(seg_idx + 1)); + const auto p_front = autoware_utils::get_point(points.at(seg_idx)); + const auto p_back = autoware_utils::get_point(points.at(seg_idx + 1)); try { validateNonSharpAngle(p_front, p_target, p_back); @@ -1389,9 +1389,9 @@ std::optional insertTargetPoint( } const auto overlap_with_front = - autoware::universe_utils::calcDistance2d(p_target, p_front) < overlap_threshold; + autoware_utils::calc_distance2d(p_target, p_front) < overlap_threshold; const auto overlap_with_back = - autoware::universe_utils::calcDistance2d(p_target, p_back) < overlap_threshold; + autoware_utils::calc_distance2d(p_target, p_back) < overlap_threshold; const auto is_driving_forward = isDrivingForward(points); if (!is_driving_forward) { @@ -1401,31 +1401,31 @@ std::optional insertTargetPoint( geometry_msgs::msg::Pose target_pose; { const auto p_base = is_driving_forward.value() ? p_back : p_front; - const auto pitch = autoware::universe_utils::calcElevationAngle(p_target, p_base); - const auto yaw = autoware::universe_utils::calcAzimuthAngle(p_target, p_base); + const auto pitch = autoware_utils::calc_elevation_angle(p_target, p_base); + const auto yaw = autoware_utils::calc_azimuth_angle(p_target, p_base); target_pose.position = p_target; - target_pose.orientation = autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, yaw); + target_pose.orientation = autoware_utils::create_quaternion_from_rpy(0.0, pitch, yaw); } auto p_insert = points.at(seg_idx); - autoware::universe_utils::setPose(target_pose, p_insert); + autoware_utils::set_pose(target_pose, p_insert); geometry_msgs::msg::Pose base_pose; { const auto p_base = is_driving_forward.value() ? p_front : p_back; - const auto pitch = autoware::universe_utils::calcElevationAngle(p_base, p_target); - const auto yaw = autoware::universe_utils::calcAzimuthAngle(p_base, p_target); + const auto pitch = autoware_utils::calc_elevation_angle(p_base, p_target); + const auto yaw = autoware_utils::calc_azimuth_angle(p_base, p_target); - base_pose.position = autoware::universe_utils::getPoint(p_base); - base_pose.orientation = autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, yaw); + base_pose.position = autoware_utils::get_point(p_base); + base_pose.orientation = autoware_utils::create_quaternion_from_rpy(0.0, pitch, yaw); } if (!overlap_with_front && !overlap_with_back) { if (is_driving_forward.value()) { - autoware::universe_utils::setPose(base_pose, points.at(seg_idx)); + autoware_utils::set_pose(base_pose, points.at(seg_idx)); } else { - autoware::universe_utils::setPose(base_pose, points.at(seg_idx + 1)); + autoware_utils::set_pose(base_pose, points.at(seg_idx + 1)); } points.insert(points.begin() + seg_idx + 1, p_insert); return seg_idx + 1; @@ -1444,9 +1444,9 @@ insertTargetPoint>( std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional insertTargetPoint>( @@ -1499,9 +1499,9 @@ insertTargetPoint>( std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional insertTargetPoint>( @@ -1559,9 +1559,9 @@ std::optional insertTargetPoint( const double target_length = insert_point_length - calcSignedArcLength(points, src_segment_idx, *segment_idx); const double ratio = std::clamp(target_length / segment_length, 0.0, 1.0); - const auto p_target = autoware::universe_utils::calcInterpolatedPoint( - autoware::universe_utils::getPoint(points.at(*segment_idx)), - autoware::universe_utils::getPoint(points.at(*segment_idx + 1)), ratio); + const auto p_target = autoware_utils::calc_interpolated_point( + autoware_utils::get_point(points.at(*segment_idx)), + autoware_utils::get_point(points.at(*segment_idx + 1)), ratio); return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); } @@ -1572,9 +1572,9 @@ insertTargetPoint>( std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional insertTargetPoint>( @@ -1626,9 +1626,9 @@ insertTargetPoint>( const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, + std::vector & points, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional @@ -1665,7 +1665,7 @@ std::optional insertStopPoint( } for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { - autoware::universe_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); + autoware_utils::set_longitudinal_velocity(0.0, points_with_twist.at(i)); } return stop_idx; @@ -1677,9 +1677,9 @@ insertStopPoint>( std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional insertStopPoint>( @@ -1707,9 +1707,9 @@ std::optional insertStopPoint( double accumulated_length = 0; for (size_t i = 0; i < points_with_twist.size() - 1; ++i) { - const auto curr_pose = autoware::universe_utils::getPose(points_with_twist.at(i)); - const auto next_pose = autoware::universe_utils::getPose(points_with_twist.at(i + 1)); - const double length = autoware::universe_utils::calcDistance2d(curr_pose, next_pose); + const auto curr_pose = autoware_utils::get_pose(points_with_twist.at(i)); + const auto next_pose = autoware_utils::get_pose(points_with_twist.at(i + 1)); + const double length = autoware_utils::calc_distance2d(curr_pose, next_pose); if (accumulated_length + length + overlap_threshold > distance_to_stop_point) { const double insert_length = distance_to_stop_point - accumulated_length; return insertStopPoint(i, insert_length, points_with_twist, overlap_threshold); @@ -1726,9 +1726,9 @@ insertStopPoint>( std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional insertStopPoint>( @@ -1769,7 +1769,7 @@ std::optional insertStopPoint( } for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { - autoware::universe_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); + autoware_utils::set_longitudinal_velocity(0.0, points_with_twist.at(i)); } return stop_idx; @@ -1782,9 +1782,9 @@ insertStopPoint>( const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional @@ -1816,7 +1816,7 @@ std::optional insertStopPoint( } for (size_t i = insert_idx.value(); i < points_with_twist.size(); ++i) { - autoware::universe_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); + autoware_utils::set_longitudinal_velocity(0.0, points_with_twist.at(i)); } return insert_idx; @@ -1856,8 +1856,8 @@ std::optional insertDecelPoint( for (size_t i = insert_idx.value(); i < points_with_twist.size(); ++i) { const auto & original_velocity = - autoware::universe_utils::getLongitudinalVelocity(points_with_twist.at(i)); - autoware::universe_utils::setLongitudinalVelocity( + autoware_utils::get_longitudinal_velocity(points_with_twist.at(i)); + autoware_utils::set_longitudinal_velocity( std::min(original_velocity, velocity), points_with_twist.at(i)); } @@ -1880,38 +1880,38 @@ void insertOrientation(T & points, const bool is_driving_forward) { if (is_driving_forward) { for (size_t i = 0; i < points.size() - 1; ++i) { - const auto & src_point = autoware::universe_utils::getPoint(points.at(i)); - const auto & dst_point = autoware::universe_utils::getPoint(points.at(i + 1)); - const double pitch = autoware::universe_utils::calcElevationAngle(src_point, dst_point); - const double yaw = autoware::universe_utils::calcAzimuthAngle(src_point, dst_point); - autoware::universe_utils::setOrientation( - autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, yaw), points.at(i)); + const auto & src_point = autoware_utils::get_point(points.at(i)); + const auto & dst_point = autoware_utils::get_point(points.at(i + 1)); + const double pitch = autoware_utils::calc_elevation_angle(src_point, dst_point); + const double yaw = autoware_utils::calc_azimuth_angle(src_point, dst_point); + autoware_utils::set_orientation( + autoware_utils::create_quaternion_from_rpy(0.0, pitch, yaw), points.at(i)); if (i == points.size() - 2) { // Terminal orientation is same as the point before it - autoware::universe_utils::setOrientation( - autoware::universe_utils::getPose(points.at(i)).orientation, points.at(i + 1)); + autoware_utils::set_orientation( + autoware_utils::get_pose(points.at(i)).orientation, points.at(i + 1)); } } } else { for (size_t i = points.size() - 1; i >= 1; --i) { - const auto & src_point = autoware::universe_utils::getPoint(points.at(i)); - const auto & dst_point = autoware::universe_utils::getPoint(points.at(i - 1)); - const double pitch = autoware::universe_utils::calcElevationAngle(src_point, dst_point); - const double yaw = autoware::universe_utils::calcAzimuthAngle(src_point, dst_point); - autoware::universe_utils::setOrientation( - autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, yaw), points.at(i)); + const auto & src_point = autoware_utils::get_point(points.at(i)); + const auto & dst_point = autoware_utils::get_point(points.at(i - 1)); + const double pitch = autoware_utils::calc_elevation_angle(src_point, dst_point); + const double yaw = autoware_utils::calc_azimuth_angle(src_point, dst_point); + autoware_utils::set_orientation( + autoware_utils::create_quaternion_from_rpy(0.0, pitch, yaw), points.at(i)); } // Initial orientation is same as the point after it - autoware::universe_utils::setOrientation( - autoware::universe_utils::getPose(points.at(1)).orientation, points.at(0)); + autoware_utils::set_orientation( + autoware_utils::get_pose(points.at(1)).orientation, points.at(0)); } } extern template void insertOrientation>( std::vector & points, const bool is_driving_forward); extern template void -insertOrientation>( - std::vector & points, +insertOrientation>( + std::vector & points, const bool is_driving_forward); extern template void insertOrientation>( std::vector & points, @@ -1929,14 +1929,14 @@ template void removeFirstInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2) { for (auto itr = points.begin(); std::next(itr) != points.end();) { - const auto p1 = autoware::universe_utils::getPose(*itr); - const auto p2 = autoware::universe_utils::getPose(*std::next(itr)); + const auto p1 = autoware_utils::get_pose(*itr); + const auto p2 = autoware_utils::get_pose(*std::next(itr)); const double yaw1 = tf2::getYaw(p1.orientation); const double yaw2 = tf2::getYaw(p2.orientation); if ( - max_yaw_diff < std::abs(autoware::universe_utils::normalizeRadian(yaw1 - yaw2)) || - !autoware::universe_utils::isDrivingForward(p1, p2)) { + max_yaw_diff < std::abs(autoware_utils::normalize_radian(yaw1 - yaw2)) || + !autoware_utils::is_driving_forward(p1, p2)) { points.erase(std::next(itr)); return; } else { @@ -1978,8 +1978,8 @@ extern template double calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); extern template double @@ -2017,8 +2017,8 @@ extern template double calcSignedArcLength & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); extern template double calcSignedArcLength>( @@ -2054,8 +2054,8 @@ extern template double calcSignedArcLength & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); extern template double calcSignedArcLength>( @@ -2089,9 +2089,9 @@ size_t findFirstNearestIndexWithSoftConstraints( bool is_within_constraints = false; for (size_t i = 0; i < points.size(); ++i) { const auto squared_dist = - autoware::universe_utils::calcSquaredDistance2d(points.at(i), pose.position); - const auto yaw = autoware::universe_utils::calcYawDeviation( - autoware::universe_utils::getPose(points.at(i)), pose); + autoware_utils::calc_squared_distance2d(points.at(i), pose.position); + const auto yaw = autoware_utils::calc_yaw_deviation( + autoware_utils::get_pose(points.at(i)), pose); if (squared_dist_threshold < squared_dist || yaw_threshold < std::abs(yaw)) { if (is_within_constraints) { @@ -2122,7 +2122,7 @@ size_t findFirstNearestIndexWithSoftConstraints( bool is_within_constraints = false; for (size_t i = 0; i < points.size(); ++i) { const auto squared_dist = - autoware::universe_utils::calcSquaredDistance2d(points.at(i), pose.position); + autoware_utils::calc_squared_distance2d(points.at(i), pose.position); if (squared_dist_threshold < squared_dist) { if (is_within_constraints) { @@ -2157,8 +2157,8 @@ findFirstNearestIndexWithSoftConstraints::max(), const double yaw_threshold = std::numeric_limits::max()); extern template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); @@ -2213,8 +2213,8 @@ extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); @@ -2277,8 +2277,8 @@ calcDistanceToForwardStopPoint::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional @@ -2301,7 +2301,7 @@ T cropForwardPoints( double sum_length = -autoware::motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { - sum_length += autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length += autoware_utils::calc_distance2d(points.at(i), points.at(i - 1)); if (forward_length < sum_length) { const size_t end_idx = i; return T{points.begin(), points.begin() + end_idx}; @@ -2316,9 +2316,9 @@ cropForwardPoints>( const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); -extern template std::vector -cropForwardPoints>( - const std::vector & points, +extern template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); extern template std::vector @@ -2341,7 +2341,7 @@ T cropBackwardPoints( double sum_length = -autoware::motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); for (int i = target_seg_idx; 0 < i; --i) { - sum_length -= autoware::universe_utils::calcDistance2d(points.at(i), points.at(i - 1)); + sum_length -= autoware_utils::calc_distance2d(points.at(i), points.at(i - 1)); if (sum_length < -backward_length) { const size_t begin_idx = i; return T{points.begin() + begin_idx, points.end()}; @@ -2356,9 +2356,9 @@ cropBackwardPoints>( const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); -extern template std::vector -cropBackwardPoints>( - const std::vector & points, +extern template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); extern template std::vector @@ -2398,9 +2398,9 @@ cropPoints>( const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); -extern template std::vector -cropPoints>( - const std::vector & points, +extern template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); extern template std::vector @@ -2440,7 +2440,7 @@ double calcYawDeviation( const std::string error_message( "[autoware_motion_utils] " + std::string(__func__) + " Given points size is less than 2. Failed to calculate yaw deviation."); - autoware::universe_utils::print_backtrace(); + autoware_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } @@ -2453,20 +2453,20 @@ double calcYawDeviation( const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, pose.position); - const double path_yaw = autoware::universe_utils::calcAzimuthAngle( - autoware::universe_utils::getPoint(overlap_removed_points.at(seg_idx)), - autoware::universe_utils::getPoint(overlap_removed_points.at(seg_idx + 1))); + const double path_yaw = autoware_utils::calc_azimuth_angle( + autoware_utils::get_point(overlap_removed_points.at(seg_idx)), + autoware_utils::get_point(overlap_removed_points.at(seg_idx + 1))); const double pose_yaw = tf2::getYaw(pose.orientation); - return autoware::universe_utils::normalizeRadian(pose_yaw - path_yaw); + return autoware_utils::normalize_radian(pose_yaw - path_yaw); } extern template double calcYawDeviation>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); extern template double -calcYawDeviation>( - const std::vector & points, +calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); extern template double calcYawDeviation>( const std::vector & points, @@ -2501,8 +2501,8 @@ extern template bool isTargetPointFront>( - const std::vector & points, +isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold = 0.0); extern template bool isTargetPointFront>( diff --git a/common/autoware_motion_utils/package.xml b/common/autoware_motion_utils/package.xml index 03df415c68..45992d815d 100644 --- a/common/autoware_motion_utils/package.xml +++ b/common/autoware_motion_utils/package.xml @@ -25,7 +25,8 @@ autoware_adapi_v1_msgs autoware_interpolation autoware_planning_msgs - autoware_universe_utils + autoware_internal_planning_msgs + autoware_utils autoware_vehicle_msgs builtin_interfaces geometry_msgs diff --git a/common/autoware_motion_utils/src/factor/planing_factor_interface.cpp b/common/autoware_motion_utils/src/factor/planing_factor_interface.cpp index 0dc2d29ddf..fb9191a643 100644 --- a/common/autoware_motion_utils/src/factor/planing_factor_interface.cpp +++ b/common/autoware_motion_utils/src/factor/planing_factor_interface.cpp @@ -19,8 +19,8 @@ namespace autoware::motion_utils { -template void PlanningFactorInterface::add( - const std::vector &, const Pose &, const Pose &, +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, const std::string &); template void PlanningFactorInterface::add( @@ -32,8 +32,8 @@ template void PlanningFactorInterface::add( - const std::vector &, const Pose &, const Pose &, +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, const std::string &); template void PlanningFactorInterface::add( diff --git a/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp b/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp index 4594252729..2666a867d3 100644 --- a/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp +++ b/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include #include @@ -48,8 +48,8 @@ void VelocityFactorInterface::set( velocity_factor_.detail = detail; } -template void VelocityFactorInterface::set( - const std::vector &, const Pose &, const Pose &, +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, const Pose &, const VelocityFactorStatus, const std::string &); template void VelocityFactorInterface::set( const std::vector &, const Pose &, const Pose &, diff --git a/common/autoware_motion_utils/src/marker/marker_helper.cpp b/common/autoware_motion_utils/src/marker/marker_helper.cpp index 44b621a53c..07dbcd8f2d 100644 --- a/common/autoware_motion_utils/src/marker/marker_helper.cpp +++ b/common/autoware_motion_utils/src/marker/marker_helper.cpp @@ -14,18 +14,18 @@ #include "autoware/motion_utils/marker/marker_helper.hpp" -#include "autoware/universe_utils/ros/marker_helper.hpp" +#include "autoware_utils/ros/marker_helper.hpp" -#include +#include #include #include -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createDeletedDefaultMarker; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerScale; +using autoware_utils::create_default_marker; +using autoware_utils::create_deleted_default_marker; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_scale; using visualization_msgs::msg::MarkerArray; namespace @@ -40,9 +40,9 @@ inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray( // Virtual Wall { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, ns_prefix + "virtual_wall", id, visualization_msgs::msg::Marker::CUBE, - createMarkerScale(0.1, 5.0, 2.0), color); + create_marker_scale(0.1, 5.0, 2.0), color); marker.pose = vehicle_front_pose; marker.pose.position.z += 1.0; @@ -52,9 +52,9 @@ inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray( // Factor Text { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0 /*font size*/), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + create_marker_scale(0.0, 0.0, 1.0 /*font size*/), create_marker_color(1.0, 1.0, 1.0, 1.0)); marker.pose = vehicle_front_pose; marker.pose.position.z += 2.0; @@ -73,13 +73,13 @@ inline visualization_msgs::msg::MarkerArray createDeletedVirtualWallMarkerArray( // Virtual Wall { - auto marker = createDeletedDefaultMarker(now, ns_prefix + "virtual_wall", id); + auto marker = create_deleted_default_marker(now, ns_prefix + "virtual_wall", id); marker_array.markers.push_back(marker); } // Factor Text { - auto marker = createDeletedDefaultMarker(now, ns_prefix + "factor_text", id); + auto marker = create_deleted_default_marker(now, ns_prefix + "factor_text", id); marker_array.markers.push_back(marker); } @@ -95,9 +95,9 @@ inline visualization_msgs::msg::MarkerArray createIntendedPassArrowMarkerArray( // Arrow { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, ns_prefix + "direction", id, visualization_msgs::msg::Marker::ARROW, - createMarkerScale(2.5 /*length*/, 0.15 /*width*/, 1.0 /*height*/), color); + create_marker_scale(2.5 /*length*/, 0.15 /*width*/, 1.0 /*height*/), color); marker.pose = vehicle_front_pose; @@ -106,9 +106,9 @@ inline visualization_msgs::msg::MarkerArray createIntendedPassArrowMarkerArray( // Factor Text { - auto marker = createDefaultMarker( + auto marker = create_default_marker( "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 0.4 /*font size*/), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + create_marker_scale(0.0, 0.0, 0.4 /*font size*/), create_marker_color(1.0, 1.0, 1.0, 1.0)); marker.pose = vehicle_front_pose; marker.pose.position.z += 2.0; @@ -129,11 +129,11 @@ visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, const bool is_driving_forward) { - const auto pose_with_offset = autoware::universe_utils::calcOffsetPose( + const auto pose_with_offset = autoware_utils::calc_offset_pose( pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "stop_", now, id, - createMarkerColor(1.0, 0.0, 0.0, 0.5)); + create_marker_color(1.0, 0.0, 0.0, 0.5)); } visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( @@ -141,11 +141,11 @@ visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, const bool is_driving_forward) { - const auto pose_with_offset = autoware::universe_utils::calcOffsetPose( + const auto pose_with_offset = autoware_utils::calc_offset_pose( pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "slow_down_", now, id, - createMarkerColor(1.0, 1.0, 0.0, 0.5)); + create_marker_color(1.0, 1.0, 0.0, 0.5)); } visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( @@ -153,11 +153,11 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, const bool is_driving_forward) { - const auto pose_with_offset = autoware::universe_utils::calcOffsetPose( + const auto pose_with_offset = autoware_utils::calc_offset_pose( pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "dead_line_", now, id, - createMarkerColor(0.0, 1.0, 0.0, 0.5)); + create_marker_color(0.0, 1.0, 0.0, 0.5)); } visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker( @@ -165,11 +165,11 @@ visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker( const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, const bool is_driving_forward) { - const auto pose_with_offset = autoware::universe_utils::calcOffsetPose( + const auto pose_with_offset = autoware_utils::calc_offset_pose( pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createIntendedPassArrowMarkerArray( pose_with_offset, module_name, ns_prefix + "intended_pass_", now, id, - createMarkerColor(0.77, 0.77, 0.77, 0.5)); + create_marker_color(0.77, 0.77, 0.77, 0.5)); } visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( diff --git a/common/autoware_motion_utils/src/resample/resample.cpp b/common/autoware_motion_utils/src/resample/resample.cpp index 52b80a3695..f691f9c4c2 100644 --- a/common/autoware_motion_utils/src/resample/resample.cpp +++ b/common/autoware_motion_utils/src/resample/resample.cpp @@ -19,7 +19,7 @@ #include "autoware/interpolation/zero_order_hold.hpp" #include "autoware/motion_utils/resample/resample_utils.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include #include @@ -55,7 +55,7 @@ std::vector resamplePointVector( for (size_t i = 1; i < points.size(); ++i) { const auto & prev_pt = points.at(i - 1); const auto & curr_pt = points.at(i); - const double ds = autoware::universe_utils::calcDistance2d(prev_pt, curr_pt); + const double ds = autoware_utils::calc_distance2d(prev_pt, curr_pt); input_arclength.push_back(ds + input_arclength.back()); x.push_back(curr_pt.x); y.push_back(curr_pt.y); @@ -149,7 +149,7 @@ std::vector resamplePoseVector( } const bool is_driving_forward = - autoware::universe_utils::isDrivingForward(points.at(0), points.at(1)); + autoware_utils::is_driving_forward(points.at(0), points.at(1)); autoware::motion_utils::insertOrientation(resampled_points, is_driving_forward); // Initial orientation is depend on the initial value of the resampled_arclength @@ -186,8 +186,8 @@ std::vector resamplePoseVector( return resamplePoseVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); } -autoware_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_planning_msgs::msg::PathWithLaneId & input_path, +autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) { @@ -265,7 +265,7 @@ autoware_planning_msgs::msg::PathWithLaneId resamplePath( const auto & prev_pt = input_path.points.at(i - 1).point; const auto & curr_pt = input_path.points.at(i).point; const double ds = - autoware::universe_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); + autoware_utils::calc_distance2d(prev_pt.pose.position, curr_pt.pose.position); input_arclength.push_back(ds + input_arclength.back()); input_pose.push_back(curr_pt.pose); v_lon.push_back(curr_pt.longitudinal_velocity_mps); @@ -340,7 +340,7 @@ autoware_planning_msgs::msg::PathWithLaneId resamplePath( return input_path; } - autoware_planning_msgs::msg::PathWithLaneId resampled_path; + autoware_internal_planning_msgs::msg::PathWithLaneId resampled_path; resampled_path.header = input_path.header; resampled_path.left_bound = input_path.left_bound; resampled_path.right_bound = input_path.right_bound; @@ -359,8 +359,8 @@ autoware_planning_msgs::msg::PathWithLaneId resamplePath( return resampled_path; } -autoware_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval, +autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v, const bool resample_input_path_stop_point) { @@ -456,7 +456,7 @@ autoware_planning_msgs::msg::Path resamplePath( const auto & prev_pt = input_path.points.at(i - 1); const auto & curr_pt = input_path.points.at(i); const double ds = - autoware::universe_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); + autoware_utils::calc_distance2d(prev_pt.pose.position, curr_pt.pose.position); input_arclength.push_back(ds + input_arclength.back()); input_pose.push_back(curr_pt.pose); v_lon.push_back(curr_pt.longitudinal_velocity_mps); @@ -613,7 +613,7 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( const auto & prev_pt = input_trajectory.points.at(i - 1); const auto & curr_pt = input_trajectory.points.at(i); const double ds = - autoware::universe_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); + autoware_utils::calc_distance2d(prev_pt.pose.position, curr_pt.pose.position); input_arclength.push_back(ds + input_arclength.back()); input_pose.push_back(curr_pt.pose); diff --git a/common/autoware_motion_utils/src/trajectory/interpolation.cpp b/common/autoware_motion_utils/src/trajectory/interpolation.cpp index b4fbfa4804..4b07aedb6f 100644 --- a/common/autoware_motion_utils/src/trajectory/interpolation.cpp +++ b/common/autoware_motion_utils/src/trajectory/interpolation.cpp @@ -17,8 +17,8 @@ #include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -using autoware_planning_msgs::msg::PathPointWithLaneId; -using autoware_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -44,8 +44,8 @@ TrajectoryPoint calcInterpolatedPoint( // Calculate interpolation ratio const auto & curr_pt = trajectory.points.at(segment_idx); const auto & next_pt = trajectory.points.at(segment_idx + 1); - const auto v1 = autoware::universe_utils::point2tfVector(curr_pt, next_pt); - const auto v2 = autoware::universe_utils::point2tfVector(curr_pt, target_pose); + const auto v1 = autoware_utils::point_2_tf_vector(curr_pt, next_pt); + const auto v2 = autoware_utils::point_2_tf_vector(curr_pt, target_pose); if (v1.length2() < 1e-3) { return curr_pt; } @@ -58,7 +58,7 @@ TrajectoryPoint calcInterpolatedPoint( // pose interpolation interpolated_point.pose = - autoware::universe_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); + autoware_utils::calc_interpolated_pose(curr_pt, next_pt, clamped_ratio); // twist interpolation if (use_zero_order_hold_for_twist) { @@ -113,8 +113,8 @@ PathPointWithLaneId calcInterpolatedPoint( // Calculate interpolation ratio const auto & curr_pt = path.points.at(segment_idx); const auto & next_pt = path.points.at(segment_idx + 1); - const auto v1 = autoware::universe_utils::point2tfVector(curr_pt.point, next_pt.point); - const auto v2 = autoware::universe_utils::point2tfVector(curr_pt.point, target_pose); + const auto v1 = autoware_utils::point_2_tf_vector(curr_pt.point, next_pt.point); + const auto v2 = autoware_utils::point_2_tf_vector(curr_pt.point, target_pose); if (v1.length2() < 1e-3) { return curr_pt; } @@ -127,7 +127,7 @@ PathPointWithLaneId calcInterpolatedPoint( // pose interpolation interpolated_point.point.pose = - autoware::universe_utils::calcInterpolatedPose(curr_pt.point, next_pt.point, clamped_ratio); + autoware_utils::calc_interpolated_pose(curr_pt.point, next_pt.point, clamped_ratio); // twist interpolation if (use_zero_order_hold_for_twist) { diff --git a/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp b/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp index 18ad117ef0..5c10440bc4 100644 --- a/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp +++ b/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp @@ -25,7 +25,7 @@ namespace autoware::motion_utils { std::optional> getPathIndexRangeWithLaneId( - const autoware_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) { size_t start_idx = 0; // NOTE: to prevent from maybe-uninitialized error size_t end_idx = 0; // NOTE: to prevent from maybe-uninitialized error @@ -56,7 +56,7 @@ std::optional> getPathIndexRangeWithLaneId( } size_t findNearestIndexFromLaneId( - const autoware_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, const int64_t lane_id) { const auto opt_range = getPathIndexRangeWithLaneId(path, lane_id); @@ -66,7 +66,7 @@ size_t findNearestIndexFromLaneId( validateNonEmpty(path.points); - const auto sub_points = std::vector{ + const auto sub_points = std::vector{ path.points.begin() + start_idx, path.points.begin() + end_idx + 1}; validateNonEmpty(sub_points); @@ -77,7 +77,7 @@ size_t findNearestIndexFromLaneId( } size_t findNearestSegmentIndexFromLaneId( - const autoware_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, const int64_t lane_id) { const size_t nearest_idx = findNearestIndexFromLaneId(path, pos, lane_id); @@ -99,8 +99,8 @@ size_t findNearestSegmentIndexFromLaneId( } // NOTE: rear_to_cog is supposed to be positive -autoware_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( - const autoware_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, +autoware_internal_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, const bool enable_last_point_compensation) { auto cog_path = path; @@ -116,15 +116,15 @@ autoware_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( // apply beta to CoG pose geometry_msgs::msg::Pose cog_pose_with_beta; - cog_pose_with_beta.position = autoware::universe_utils::getPoint(path.points.at(i)); + cog_pose_with_beta.position = autoware_utils::get_point(path.points.at(i)); cog_pose_with_beta.orientation = - autoware::universe_utils::createQuaternionFromYaw(yaw_vec.at(i) - beta); + autoware_utils::create_quaternion_from_yaw(yaw_vec.at(i) - beta); const auto rear_pose = - autoware::universe_utils::calcOffsetPose(cog_pose_with_beta, -rear_to_cog, 0.0, 0.0); + autoware_utils::calc_offset_pose(cog_pose_with_beta, -rear_to_cog, 0.0, 0.0); // update pose - autoware::universe_utils::setPose(rear_pose, cog_path.points.at(i)); + autoware_utils::set_pose(rear_pose, cog_path.points.at(i)); } // compensate for the last pose diff --git a/common/autoware_motion_utils/src/trajectory/trajectory.cpp b/common/autoware_motion_utils/src/trajectory/trajectory.cpp index 33821fa372..7f915014ed 100644 --- a/common/autoware_motion_utils/src/trajectory/trajectory.cpp +++ b/common/autoware_motion_utils/src/trajectory/trajectory.cpp @@ -24,8 +24,8 @@ namespace autoware::motion_utils // template void validateNonEmpty>( const std::vector &); -template void validateNonEmpty>( - const std::vector &); +template void validateNonEmpty>( + const std::vector &); template void validateNonEmpty>( const std::vector &); @@ -33,8 +33,8 @@ template void validateNonEmpty isDrivingForward>( const std::vector &); template std::optional -isDrivingForward>( - const std::vector &); +isDrivingForward>( + const std::vector &); template std::optional isDrivingForward>( const std::vector &); @@ -44,8 +44,8 @@ template std::optional isDrivingForwardWithTwist>( const std::vector &); template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); template std::optional isDrivingForwardWithTwist>( const std::vector &); @@ -54,9 +54,9 @@ isDrivingForwardWithTwist removeOverlapPoints>( const std::vector & points, const size_t start_idx); -template std::vector -removeOverlapPoints>( - const std::vector & points, +template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx); template std::vector removeOverlapPoints>( @@ -83,8 +83,8 @@ searchZeroVelocityIndex>( const std::vector & points, const geometry_msgs::msg::Point & point); -template size_t findNearestIndex>( - const std::vector & points, +template size_t findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); template size_t findNearestIndex>( const std::vector & points, @@ -96,8 +96,8 @@ findNearestIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional findNearestIndex>( @@ -110,8 +110,8 @@ calcLongitudinalOffsetToSegment & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); template double calcLongitudinalOffsetToSegment>( @@ -123,8 +123,8 @@ template size_t findNearestSegmentIndex & points, const geometry_msgs::msg::Point & point); template size_t -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); template size_t findNearestSegmentIndex>( const std::vector & points, @@ -136,8 +136,8 @@ findNearestSegmentIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional findNearestSegmentIndex>( @@ -148,8 +148,8 @@ findNearestSegmentIndex>( const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); -template double calcLateralOffset>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); template double calcLateralOffset>( const std::vector & points, @@ -159,8 +159,8 @@ template double calcLateralOffset>( const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception); -template double calcLateralOffset>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception); template double calcLateralOffset>( const std::vector & points, @@ -170,8 +170,8 @@ template double calcLateralOffset>( const std::vector & points, const size_t src_idx, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const size_t dst_idx); template double calcSignedArcLength>( const std::vector & points, const size_t src_idx, @@ -183,8 +183,8 @@ calcSignedArcLengthPartialSum & points, const size_t src_idx, const size_t dst_idx); template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, const size_t dst_idx); template std::vector calcSignedArcLengthPartialSum>( @@ -195,8 +195,8 @@ calcSignedArcLengthPartialSum>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector &, +template double calcSignedArcLength>( + const std::vector &, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); template double calcSignedArcLength>( const std::vector &, @@ -206,8 +206,8 @@ template double calcSignedArcLength>( const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point); -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point); template double calcSignedArcLength>( const std::vector & points, const size_t src_idx, @@ -217,8 +217,8 @@ template double calcSignedArcLength>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); template double calcSignedArcLength>( const std::vector & points, @@ -227,30 +227,30 @@ template double calcSignedArcLength>( const std::vector & points); -template double calcArcLength>( - const std::vector & points); +template double calcArcLength>( + const std::vector & points); template double calcArcLength>( const std::vector & points); // -template std::vector calcCurvature>( +template std::vector calc_curvature>( const std::vector & points); template std::vector -calcCurvature>( - const std::vector & points); +calc_curvature>( + const std::vector & points); template std::vector -calcCurvature>( +calc_curvature>( const std::vector & points); // template std::vector>> -calcCurvatureAndSegmentLength>( +calc_curvatureAndSegmentLength>( const std::vector & points); template std::vector>> -calcCurvatureAndSegmentLength>( - const std::vector & points); +calc_curvatureAndSegmentLength>( + const std::vector & points); template std::vector>> -calcCurvatureAndSegmentLength>( +calc_curvatureAndSegmentLength>( const std::vector & points); // @@ -265,8 +265,8 @@ calcLongitudinalOffsetPoint> const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception); template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception); template std::optional calcLongitudinalOffsetPoint>( @@ -279,8 +279,8 @@ calcLongitudinalOffsetPoint> const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); template std::optional calcLongitudinalOffsetPoint>( @@ -294,8 +294,8 @@ calcLongitudinalOffsetPose>( const double offset, const bool set_orientation_from_position_direction, const bool throw_exception); template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, const bool throw_exception); template std::optional @@ -311,8 +311,8 @@ calcLongitudinalOffsetPose>( const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction); template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction); template std::optional @@ -327,9 +327,9 @@ insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold); template std::optional insertTargetPoint>( @@ -343,9 +343,9 @@ insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold); template std::optional insertTargetPoint>( @@ -359,9 +359,9 @@ insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, + std::vector & points, const double overlap_threshold); template std::optional insertTargetPoint>( @@ -376,9 +376,9 @@ insertTargetPoint>( std::vector & points, const double max_dist, const double max_yaw, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, const double max_dist, + std::vector & points, const double max_dist, const double max_yaw, const double overlap_threshold); template std::optional insertTargetPoint>( @@ -392,9 +392,9 @@ template std::optional insertStopPoint & points_with_twist, const double overlap_threshold = 1e-3); template std::optional -insertStopPoint>( +insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); template std::optional insertStopPoint>( @@ -408,9 +408,9 @@ template std::optional insertStopPoint & points_with_twist, const double overlap_threshold); template std::optional -insertStopPoint>( +insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold); template std::optional insertStopPoint>( @@ -424,9 +424,9 @@ template std::optional insertStopPoint & points_with_twist, const double max_dist, const double max_yaw, const double overlap_threshold); template std::optional -insertStopPoint>( +insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double max_dist, const double max_yaw, const double overlap_threshold); template std::optional insertStopPoint>( @@ -451,8 +451,8 @@ insertDecelPoint>( // template void insertOrientation>( std::vector & points, const bool is_driving_forward); -template void insertOrientation>( - std::vector & points, +template void insertOrientation>( + std::vector & points, const bool is_driving_forward); template void insertOrientation>( std::vector & points, @@ -463,8 +463,8 @@ template double calcSignedArcLength & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); template double calcSignedArcLength>( @@ -476,8 +476,8 @@ template double calcSignedArcLength>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); template double calcSignedArcLength>( const std::vector & points, @@ -487,8 +487,8 @@ template double calcSignedArcLength>( const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); template double calcSignedArcLength>( const std::vector & points, const size_t src_idx, @@ -500,8 +500,8 @@ findFirstNearestIndexWithSoftConstraints & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); template size_t findFirstNearestIndexWithSoftConstraints>( @@ -514,8 +514,8 @@ template size_t findFirstNearestSegmentIndexWithSoftConstraints< const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); template size_t findFirstNearestSegmentIndexWithSoftConstraints< std::vector>( @@ -534,9 +534,9 @@ cropForwardPoints>( const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); -template std::vector -cropForwardPoints>( - const std::vector & points, +template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); template std::vector @@ -551,9 +551,9 @@ cropBackwardPoints>( const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); -template std::vector -cropBackwardPoints>( - const std::vector & points, +template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); template std::vector @@ -568,9 +568,9 @@ cropPoints>( const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); -template std::vector -cropPoints>( - const std::vector & points, +template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); template std::vector @@ -583,8 +583,8 @@ cropPoints>( template double calcYawDeviation>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception); -template double calcYawDeviation>( - const std::vector & points, +template double calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception); template double calcYawDeviation>( const std::vector & points, @@ -595,8 +595,8 @@ template bool isTargetPointFront & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold); -template bool isTargetPointFront>( - const std::vector & points, +template bool isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold); template bool isTargetPointFront>( @@ -622,7 +622,7 @@ void calculate_time_from_start( const auto velocity = std::max(min_velocity, from.longitudinal_velocity_mps); if (velocity != 0.0) { auto & to = trajectory[idx]; - const auto t = universe_utils::calcDistance2d(from, to) / velocity; + const auto t = autoware_utils::calc_distance2d(from, to) / velocity; to.time_from_start = rclcpp::Duration::from_seconds(t) + from.time_from_start; } } diff --git a/common/autoware_motion_utils/test/src/resample/test_resample.cpp b/common/autoware_motion_utils/test/src/resample/test_resample.cpp index 90a77ad4dd..c25b500a0a 100644 --- a/common/autoware_motion_utils/test/src/resample/test_resample.cpp +++ b/common/autoware_motion_utils/test/src/resample/test_resample.cpp @@ -14,10 +14,10 @@ #include "autoware/motion_utils/constants.hpp" #include "autoware/motion_utils/resample/resample.hpp" -#include "autoware/universe_utils/geometry/boost_geometry.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/constants.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware_utils/geometry/boost_geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/constants.hpp" +#include "autoware_utils/math/unit_conversion.hpp" #include #include @@ -28,13 +28,13 @@ namespace { -using autoware::universe_utils::createPoint; -using autoware::universe_utils::createQuaternionFromRPY; -using autoware::universe_utils::transformPoint; +using autoware_utils::create_point; +using autoware_utils::create_quaternion_from_rpy; +using autoware_utils::transform_point; using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::PathPoint; -using autoware_planning_msgs::msg::PathPointWithLaneId; -using autoware_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -44,8 +44,8 @@ geometry_msgs::msg::Pose createPose( double x, double y, double z, double roll, double pitch, double yaw) { geometry_msgs::msg::Pose p; - p.position = createPoint(x, y, z); - p.orientation = createQuaternionFromRPY(roll, pitch, yaw); + p.position = create_point(x, y, z); + p.orientation = create_quaternion_from_rpy(roll, pitch, yaw); return p; } @@ -270,7 +270,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Change the last point orientation path.points.back() = generateTestPathPointWithLaneId( - 9.0, 0.0, 0.0, autoware::universe_utils::pi / 3.0, 3.0, 1.0, 0.01, true, {9}); + 9.0, 0.0, 0.0, autoware_utils::pi / 3.0, 3.0, 1.0, 0.01, true, {9}); { const auto resampled_path = resamplePath(path, resampled_arclength); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -295,7 +295,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) const auto p = resampled_path.points.back(); const auto ans_p = path.points.back(); - const auto ans_quat = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + const auto ans_quat = autoware_utils::create_quaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon); EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon); EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon); @@ -316,7 +316,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Output key is not same as input { - autoware_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -422,7 +422,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Duplicated points in the original path { - autoware_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(11); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -456,7 +456,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) { // Input path size is not enough for interpolation { - autoware_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(1); for (size_t i = 0; i < 1; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -489,7 +489,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Resampled Arclength size is not enough for interpolation { - autoware_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -523,7 +523,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Resampled Arclength is longer than input path { - autoware_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -562,11 +562,11 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) using autoware::motion_utils::resamplePath; { - autoware_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, autoware::universe_utils::pi, i * 1.0, i * 0.5, i * 0.1, false, + i * 1.0, 0.0, 0.0, autoware_utils::pi, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); } path.points.back().point.is_final = true; @@ -658,7 +658,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) } } - const auto ans_quat = autoware::universe_utils::createQuaternionFromYaw(M_PI); + const auto ans_quat = autoware_utils::create_quaternion_from_yaw(M_PI); for (size_t i = 0; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i).point; EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -670,7 +670,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) // change initial orientation { - autoware_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -678,7 +678,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) } path.points.back().point.is_final = true; path.points.at(0).point.pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); + autoware_utils::create_quaternion_from_yaw(M_PI + M_PI / 3.0); std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; const auto resampled_path = resamplePath(path, resampled_arclength); @@ -769,7 +769,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) // Initial Orientation { - const auto ans_quat = autoware::universe_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); + const auto ans_quat = autoware_utils::create_quaternion_from_yaw(M_PI + M_PI / 3.0); const auto p = resampled_path.points.at(0).point; EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); @@ -777,7 +777,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); } - const auto ans_quat = autoware::universe_utils::createQuaternionFromYaw(M_PI); + const auto ans_quat = autoware_utils::create_quaternion_from_yaw(M_PI); for (size_t i = 1; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i).point; EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -794,7 +794,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) // Lerp x, y { - autoware_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -871,7 +871,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) // Slerp z { - autoware_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -938,7 +938,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) } const double pitch = std::atan(1.0); - const auto ans_quat = autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, 0.0); + const auto ans_quat = autoware_utils::create_quaternion_from_rpy(0.0, pitch, 0.0); for (size_t i = 0; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i).point; EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -950,7 +950,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) // Lerp v { - autoware_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -1064,7 +1064,7 @@ TEST(resample_path_with_lane_id, resample_path_by_same_interval) } // Change the last point orientation path.points.back() = generateTestPathPointWithLaneId( - 9.0, 0.0, 0.0, autoware::universe_utils::pi / 3.0, 3.0, 1.0, 0.01, true, {9}); + 9.0, 0.0, 0.0, autoware_utils::pi / 3.0, 3.0, 1.0, 0.01, true, {9}); { const auto resampled_path = resamplePath(path, 1.0); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -1089,7 +1089,7 @@ TEST(resample_path_with_lane_id, resample_path_by_same_interval) const auto p = resampled_path.points.back(); const auto ans_p = path.points.back(); - const auto ans_quat = autoware::universe_utils::createQuaternionFromYaw(0.0); + const auto ans_quat = autoware_utils::create_quaternion_from_yaw(0.0); EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon); EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon); EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon); @@ -1609,7 +1609,7 @@ TEST(resample_path, resample_path_by_vector) // Change the last point orientation path.points.back() = - generateTestPathPoint(9.0, 0.0, 0.0, autoware::universe_utils::pi / 3.0, 3.0, 1.0, 0.01); + generateTestPathPoint(9.0, 0.0, 0.0, autoware_utils::pi / 3.0, 3.0, 1.0, 0.01); { const auto resampled_path = resamplePath(path, resampled_arclength); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -1629,7 +1629,7 @@ TEST(resample_path, resample_path_by_vector) const auto p = resampled_path.points.back(); const auto ans_p = path.points.back(); - const auto ans_quat = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + const auto ans_quat = autoware_utils::create_quaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); @@ -1903,7 +1903,7 @@ TEST(resample_path, resample_path_by_vector_backward) EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); } - const auto ans_quat = autoware::universe_utils::createQuaternionFromYaw(M_PI); + const auto ans_quat = autoware_utils::create_quaternion_from_yaw(M_PI); for (size_t i = 0; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i); EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -1921,7 +1921,7 @@ TEST(resample_path, resample_path_by_vector_backward) path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1); } path.points.at(0).pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); + autoware_utils::create_quaternion_from_yaw(M_PI + M_PI / 3.0); std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; const auto resampled_path = resamplePath(path, resampled_arclength); @@ -1988,7 +1988,7 @@ TEST(resample_path, resample_path_by_vector_backward) // Initial Orientation { - const auto ans_quat = autoware::universe_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0); + const auto ans_quat = autoware_utils::create_quaternion_from_yaw(M_PI + M_PI / 3.0); const auto p = resampled_path.points.at(0); EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); @@ -1996,7 +1996,7 @@ TEST(resample_path, resample_path_by_vector_backward) EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); } - const auto ans_quat = autoware::universe_utils::createQuaternionFromYaw(M_PI); + const auto ans_quat = autoware_utils::create_quaternion_from_yaw(M_PI); for (size_t i = 1; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i); EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -2122,7 +2122,7 @@ TEST(resample_path, resample_path_by_vector_non_default) } const double pitch = std::atan(1.0); - const auto ans_quat = autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, 0.0); + const auto ans_quat = autoware_utils::create_quaternion_from_rpy(0.0, pitch, 0.0); for (size_t i = 0; i < resampled_path.points.size(); ++i) { const auto p = resampled_path.points.at(i); EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -2224,7 +2224,7 @@ TEST(resample_path, resample_path_by_same_interval) // Change the last point orientation path.points.back() = - generateTestPathPoint(9.0, 0.0, 0.0, autoware::universe_utils::pi / 3.0, 3.0, 1.0, 0.01); + generateTestPathPoint(9.0, 0.0, 0.0, autoware_utils::pi / 3.0, 3.0, 1.0, 0.01); { const auto resampled_path = resamplePath(path, 1.0); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -2244,7 +2244,7 @@ TEST(resample_path, resample_path_by_same_interval) const auto p = resampled_path.points.back(); const auto ans_p = path.points.back(); - const auto ans_quat = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + const auto ans_quat = autoware_utils::create_quaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); @@ -2679,7 +2679,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) // Change the last point orientation traj.points.back() = generateTestTrajectoryPoint( - 9.0, 0.0, 0.0, autoware::universe_utils::pi / 3.0, 3.0, 1.0, 0.01, 0.5); + 9.0, 0.0, 0.0, autoware_utils::pi / 3.0, 3.0, 1.0, 0.01, 0.5); { const auto resampled_path = resampleTrajectory(traj, resampled_arclength); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -2700,7 +2700,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) const auto p = resampled_path.points.back(); const auto ans_p = setZeroVelocityAfterStop(traj.points).back(); - const auto ans_quat = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + const auto ans_quat = autoware_utils::create_quaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); @@ -3032,7 +3032,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) } const double pitch = std::atan(1.0); - const auto ans_quat = autoware::universe_utils::createQuaternionFromRPY(0.0, pitch, 0.0); + const auto ans_quat = autoware_utils::create_quaternion_from_rpy(0.0, pitch, 0.0); for (size_t i = 0; i < resampled_traj.points.size(); ++i) { const auto p = resampled_traj.points.at(i); EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); @@ -3141,7 +3141,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Change the last point orientation traj.points.back() = generateTestTrajectoryPoint( - 9.0, 0.0, 0.0, autoware::universe_utils::pi / 3.0, 3.0, 1.0, 0.01, 0.5); + 9.0, 0.0, 0.0, autoware_utils::pi / 3.0, 3.0, 1.0, 0.01, 0.5); { const auto resampled_path = resampleTrajectory(traj, 1.0); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { @@ -3162,7 +3162,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) const auto p = resampled_path.points.back(); const auto ans_p = setZeroVelocityAfterStop(traj.points).back(); - const auto ans_quat = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); + const auto ans_quat = autoware_utils::create_quaternion(0.0, 0.0, 0.0, 1.0); EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); diff --git a/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp b/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp index d00053a7b4..eee2ac8f77 100644 --- a/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp @@ -22,16 +22,16 @@ namespace { -using autoware::universe_utils::createPoint; -using autoware::universe_utils::createQuaternionFromRPY; +using autoware_utils::create_point; +using autoware_utils::create_quaternion_from_rpy; using autoware_planning_msgs::msg::Trajectory; geometry_msgs::msg::Pose createPose( double x, double y, double z, double roll, double pitch, double yaw) { geometry_msgs::msg::Pose p; - p.position = createPoint(x, y, z); - p.orientation = createQuaternionFromRPY(roll, pitch, yaw); + p.position = create_point(x, y, z); + p.orientation = create_quaternion_from_rpy(roll, pitch, yaw); return p; } @@ -69,7 +69,7 @@ TEST(trajectory_benchmark, DISABLED_calcLateralOffset) const auto traj = generateTestTrajectory(1000, 1.0, 0.0, 0.0, 0.1); constexpr auto nb_iteration = 10000; for (auto i = 0; i < nb_iteration; ++i) { - const auto point = createPoint(uniform_dist(e1), uniform_dist(e1), 0.0); + const auto point = create_point(uniform_dist(e1), uniform_dist(e1), 0.0); calcLateralOffset(traj.points, point); } } diff --git a/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp b/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp index 72f225b4f3..9a7b751920 100644 --- a/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp @@ -15,8 +15,8 @@ #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/motion_utils/trajectory/interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/boost_geometry.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware_utils/geometry/boost_geometry.hpp" +#include "autoware_utils/math/unit_conversion.hpp" #include #include @@ -27,11 +27,11 @@ namespace { -using autoware::universe_utils::createPoint; -using autoware::universe_utils::createQuaternionFromRPY; -using autoware::universe_utils::transformPoint; -using autoware_planning_msgs::msg::PathPointWithLaneId; -using autoware_planning_msgs::msg::PathWithLaneId; +using autoware_utils::create_point; +using autoware_utils::create_quaternion_from_rpy; +using autoware_utils::transform_point; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -41,8 +41,8 @@ geometry_msgs::msg::Pose createPose( double x, double y, double z, double roll, double pitch, double yaw) { geometry_msgs::msg::Pose p; - p.position = createPoint(x, y, z); - p.orientation = createQuaternionFromRPY(roll, pitch, yaw); + p.position = create_point(x, y, z); + p.orientation = create_quaternion_from_rpy(roll, pitch, yaw); return p; } @@ -351,7 +351,7 @@ TEST(Interpolation, interpolate_path_for_path) using autoware::motion_utils::calcInterpolatedPoint; { - autoware_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -515,7 +515,7 @@ TEST(Interpolation, interpolate_path_for_path) // Duplicated Points { - autoware_planning_msgs::msg::PathWithLaneId path; + autoware_internal_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); diff --git a/common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp index 9e06202896..cf9a777c6b 100644 --- a/common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include @@ -22,16 +22,16 @@ namespace { -using autoware::universe_utils::createPoint; -using autoware_planning_msgs::msg::PathPointWithLaneId; -using autoware_planning_msgs::msg::PathWithLaneId; +using autoware_utils::create_point; +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; geometry_msgs::msg::Pose createPose( double x, double y, double z, double roll, double pitch, double yaw) { geometry_msgs::msg::Pose p; - p.position = createPoint(x, y, z); - p.orientation = autoware::universe_utils::createQuaternionFromRPY(roll, pitch, yaw); + p.position = create_point(x, y, z); + p.orientation = autoware_utils::create_quaternion_from_rpy(roll, pitch, yaw); return p; } @@ -55,7 +55,7 @@ PathWithLaneId generateTestPathWithLaneId(const size_t num_points, const double TEST(path_with_lane_id, getPathIndexRangeWithLaneId) { using autoware::motion_utils::getPathIndexRangeWithLaneId; - using autoware_planning_msgs::msg::PathWithLaneId; + using autoware_internal_planning_msgs::msg::PathWithLaneId; // Usual cases { @@ -110,9 +110,9 @@ TEST(path_with_lane_id, findNearestIndexFromLaneId) for (size_t i = 0; i < 10; ++i) { modified_path.points.at(i).lane_ids = {100}; } - EXPECT_EQ(findNearestIndexFromLaneId(modified_path, createPoint(2.4, 1.3, 0.0), 100), 2U); + EXPECT_EQ(findNearestIndexFromLaneId(modified_path, create_point(2.4, 1.3, 0.0), 100), 2U); EXPECT_EQ( - findNearestSegmentIndexFromLaneId(modified_path, createPoint(2.4, 1.3, 0.0), 100), 2U); + findNearestSegmentIndexFromLaneId(modified_path, create_point(2.4, 1.3, 0.0), 100), 2U); } { @@ -120,9 +120,9 @@ TEST(path_with_lane_id, findNearestIndexFromLaneId) for (size_t i = 3; i < 6; ++i) { modified_path.points.at(i).lane_ids = {100}; } - EXPECT_EQ(findNearestIndexFromLaneId(modified_path, createPoint(4.1, 0.3, 0.0), 100), 4U); + EXPECT_EQ(findNearestIndexFromLaneId(modified_path, create_point(4.1, 0.3, 0.0), 100), 4U); EXPECT_EQ( - findNearestSegmentIndexFromLaneId(modified_path, createPoint(4.1, 0.3, 0.0), 100), 4U); + findNearestSegmentIndexFromLaneId(modified_path, create_point(4.1, 0.3, 0.0), 100), 4U); } { @@ -130,9 +130,9 @@ TEST(path_with_lane_id, findNearestIndexFromLaneId) for (size_t i = 8; i < 9; ++i) { modified_path.points.at(i).lane_ids = {100}; } - EXPECT_EQ(findNearestIndexFromLaneId(modified_path, createPoint(8.5, -0.5, 0.0), 100), 8U); + EXPECT_EQ(findNearestIndexFromLaneId(modified_path, create_point(8.5, -0.5, 0.0), 100), 8U); EXPECT_EQ( - findNearestSegmentIndexFromLaneId(modified_path, createPoint(8.5, -0.5, 0.0), 100), 8U); + findNearestSegmentIndexFromLaneId(modified_path, create_point(8.5, -0.5, 0.0), 100), 8U); } // Nearest is not within range @@ -141,23 +141,23 @@ TEST(path_with_lane_id, findNearestIndexFromLaneId) for (size_t i = 3; i < 9; ++i) { modified_path.points.at(i).lane_ids = {100}; } - EXPECT_EQ(findNearestIndexFromLaneId(modified_path, createPoint(2.4, 1.3, 0.0), 100), 2U); + EXPECT_EQ(findNearestIndexFromLaneId(modified_path, create_point(2.4, 1.3, 0.0), 100), 2U); EXPECT_EQ( - findNearestSegmentIndexFromLaneId(modified_path, createPoint(2.4, 1.3, 0.0), 100), 2U); + findNearestSegmentIndexFromLaneId(modified_path, create_point(2.4, 1.3, 0.0), 100), 2U); } // Path does not contain lane_id. { - EXPECT_EQ(findNearestIndexFromLaneId(path, createPoint(2.4, 1.3, 0.0), 100), 2U); - EXPECT_EQ(findNearestSegmentIndexFromLaneId(path, createPoint(2.4, 1.3, 0.0), 100), 2U); + EXPECT_EQ(findNearestIndexFromLaneId(path, create_point(2.4, 1.3, 0.0), 100), 2U); + EXPECT_EQ(findNearestSegmentIndexFromLaneId(path, create_point(2.4, 1.3, 0.0), 100), 2U); } // Empty points EXPECT_THROW( - findNearestIndexFromLaneId(PathWithLaneId{}, createPoint(2.4, 1.3, 0.0), 100), + findNearestIndexFromLaneId(PathWithLaneId{}, create_point(2.4, 1.3, 0.0), 100), std::invalid_argument); EXPECT_THROW( - findNearestSegmentIndexFromLaneId(PathWithLaneId{}, createPoint(2.4, 1.3, 0.0), 100), + findNearestSegmentIndexFromLaneId(PathWithLaneId{}, create_point(2.4, 1.3, 0.0), 100), std::invalid_argument); } diff --git a/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp b/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp index f34365b08a..169b1c0bd0 100644 --- a/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp @@ -14,8 +14,8 @@ #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/boost_geometry.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware_utils/geometry/boost_geometry.hpp" +#include "autoware_utils/math/unit_conversion.hpp" #include #include @@ -29,9 +29,9 @@ namespace { using autoware_planning_msgs::msg::Trajectory; using TrajectoryPointArray = std::vector; -using autoware::universe_utils::createPoint; -using autoware::universe_utils::createQuaternionFromRPY; -using autoware::universe_utils::transformPoint; +using autoware_utils::create_point; +using autoware_utils::create_quaternion_from_rpy; +using autoware_utils::transform_point; constexpr double epsilon = 1e-6; @@ -39,8 +39,8 @@ geometry_msgs::msg::Pose createPose( double x, double y, double z, double roll, double pitch, double yaw) { geometry_msgs::msg::Pose p; - p.position = createPoint(x, y, z); - p.orientation = createQuaternionFromRPY(roll, pitch, yaw); + p.position = create_point(x, y, z); + p.orientation = create_quaternion_from_rpy(roll, pitch, yaw); return p; } @@ -137,7 +137,7 @@ TEST(trajectory, validateNonSharpAngle_DefaultThreshold) TEST(trajectory, validateNonSharpAngle_SetThreshold) { using autoware::motion_utils::validateNonSharpAngle; - using autoware::universe_utils::pi; + using autoware_utils::pi; using autoware_planning_msgs::msg::TrajectoryPoint; TrajectoryPoint p1; @@ -317,24 +317,24 @@ TEST(trajectory, findNearestIndex_Pos_StraightTrajectory) findNearestIndex(Trajectory{}.points, geometry_msgs::msg::Point{}), std::invalid_argument); // Start point - EXPECT_EQ(findNearestIndex(traj.points, createPoint(0.0, 0.0, 0.0)), 0U); + EXPECT_EQ(findNearestIndex(traj.points, create_point(0.0, 0.0, 0.0)), 0U); // End point - EXPECT_EQ(findNearestIndex(traj.points, createPoint(9.0, 0.0, 0.0)), 9U); + EXPECT_EQ(findNearestIndex(traj.points, create_point(9.0, 0.0, 0.0)), 9U); // Boundary conditions - EXPECT_EQ(findNearestIndex(traj.points, createPoint(0.5, 0.0, 0.0)), 0U); - EXPECT_EQ(findNearestIndex(traj.points, createPoint(0.51, 0.0, 0.0)), 1U); + EXPECT_EQ(findNearestIndex(traj.points, create_point(0.5, 0.0, 0.0)), 0U); + EXPECT_EQ(findNearestIndex(traj.points, create_point(0.51, 0.0, 0.0)), 1U); // Point before start point - EXPECT_EQ(findNearestIndex(traj.points, createPoint(-4.0, 5.0, 0.0)), 0U); + EXPECT_EQ(findNearestIndex(traj.points, create_point(-4.0, 5.0, 0.0)), 0U); // Point after end point - EXPECT_EQ(findNearestIndex(traj.points, createPoint(100.0, -3.0, 0.0)), 9U); + EXPECT_EQ(findNearestIndex(traj.points, create_point(100.0, -3.0, 0.0)), 9U); // Random cases - EXPECT_EQ(findNearestIndex(traj.points, createPoint(2.4, 1.3, 0.0)), 2U); - EXPECT_EQ(findNearestIndex(traj.points, createPoint(4.0, 0.0, 0.0)), 4U); + EXPECT_EQ(findNearestIndex(traj.points, create_point(2.4, 1.3, 0.0)), 2U); + EXPECT_EQ(findNearestIndex(traj.points, create_point(4.0, 0.0, 0.0)), 4U); } TEST(trajectory, findNearestIndex_Pos_CurvedTrajectory) @@ -344,7 +344,7 @@ TEST(trajectory, findNearestIndex_Pos_CurvedTrajectory) const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); // Random cases - EXPECT_EQ(findNearestIndex(traj.points, createPoint(5.1, 3.4, 0.0)), 6U); + EXPECT_EQ(findNearestIndex(traj.points, create_point(5.1, 3.4, 0.0)), 6U); } TEST(trajectory, findNearestIndex_Pose_NoThreshold) @@ -434,32 +434,32 @@ TEST(trajectory, findNearestSegmentIndex) std::invalid_argument); // Start point - EXPECT_EQ(findNearestSegmentIndex(traj.points, createPoint(0.0, 0.0, 0.0)), 0U); + EXPECT_EQ(findNearestSegmentIndex(traj.points, create_point(0.0, 0.0, 0.0)), 0U); // End point - EXPECT_EQ(findNearestSegmentIndex(traj.points, createPoint(9.0, 0.0, 0.0)), 8U); + EXPECT_EQ(findNearestSegmentIndex(traj.points, create_point(9.0, 0.0, 0.0)), 8U); // Boundary conditions - EXPECT_EQ(findNearestSegmentIndex(traj.points, createPoint(1.0, 0.0, 0.0)), 0U); - EXPECT_EQ(findNearestSegmentIndex(traj.points, createPoint(1.1, 0.0, 0.0)), 1U); + EXPECT_EQ(findNearestSegmentIndex(traj.points, create_point(1.0, 0.0, 0.0)), 0U); + EXPECT_EQ(findNearestSegmentIndex(traj.points, create_point(1.1, 0.0, 0.0)), 1U); // Point before start point - EXPECT_EQ(findNearestSegmentIndex(traj.points, createPoint(-4.0, 5.0, 0.0)), 0U); + EXPECT_EQ(findNearestSegmentIndex(traj.points, create_point(-4.0, 5.0, 0.0)), 0U); // Point after end point - EXPECT_EQ(findNearestSegmentIndex(traj.points, createPoint(100.0, -3.0, 0.0)), 8U); + EXPECT_EQ(findNearestSegmentIndex(traj.points, create_point(100.0, -3.0, 0.0)), 8U); // Random cases - EXPECT_EQ(findNearestSegmentIndex(traj.points, createPoint(2.4, 1.0, 0.0)), 2U); - EXPECT_EQ(findNearestSegmentIndex(traj.points, createPoint(4.0, 0.0, 0.0)), 3U); + EXPECT_EQ(findNearestSegmentIndex(traj.points, create_point(2.4, 1.0, 0.0)), 2U); + EXPECT_EQ(findNearestSegmentIndex(traj.points, create_point(4.0, 0.0, 0.0)), 3U); // Two nearest trajectory points are not the edges of the nearest segment. std::vector sparse_points{ - createPoint(0.0, 0.0, 0.0), - createPoint(10.0, 0.0, 0.0), - createPoint(11.0, 0.0, 0.0), + create_point(0.0, 0.0, 0.0), + create_point(10.0, 0.0, 0.0), + create_point(11.0, 0.0, 0.0), }; - EXPECT_EQ(findNearestSegmentIndex(sparse_points, createPoint(9.0, 1.0, 0.0)), 0U); + EXPECT_EQ(findNearestSegmentIndex(sparse_points, create_point(9.0, 1.0, 0.0)), 0U); } TEST(trajectory, calcLongitudinalOffsetToSegment_StraightTrajectory) @@ -487,7 +487,7 @@ TEST(trajectory, calcLongitudinalOffsetToSegment_StraightTrajectory) // Same close points in trajectory { const auto invalid_traj = generateTestTrajectory(10, 0.0); - const auto p = createPoint(3.0, 0.0, 0.0); + const auto p = create_point(3.0, 0.0, 0.0); EXPECT_THROW( calcLongitudinalOffsetToSegment(invalid_traj.points, 3, p, throw_exception), std::runtime_error); @@ -495,21 +495,21 @@ TEST(trajectory, calcLongitudinalOffsetToSegment_StraightTrajectory) // Same point EXPECT_NEAR( - calcLongitudinalOffsetToSegment(traj.points, 3, createPoint(3.0, 0.0, 0.0)), 0.0, epsilon); + calcLongitudinalOffsetToSegment(traj.points, 3, create_point(3.0, 0.0, 0.0)), 0.0, epsilon); // Point before start point EXPECT_NEAR( - calcLongitudinalOffsetToSegment(traj.points, 6, createPoint(-3.9, 3.0, 0.0)), -9.9, epsilon); + calcLongitudinalOffsetToSegment(traj.points, 6, create_point(-3.9, 3.0, 0.0)), -9.9, epsilon); // Point after start point EXPECT_NEAR( - calcLongitudinalOffsetToSegment(traj.points, 7, createPoint(13.3, -10.0, 0.0)), 6.3, epsilon); + calcLongitudinalOffsetToSegment(traj.points, 7, create_point(13.3, -10.0, 0.0)), 6.3, epsilon); // Random cases EXPECT_NEAR( - calcLongitudinalOffsetToSegment(traj.points, 2, createPoint(4.3, 7.0, 0.0)), 2.3, epsilon); + calcLongitudinalOffsetToSegment(traj.points, 2, create_point(4.3, 7.0, 0.0)), 2.3, epsilon); EXPECT_NEAR( - calcLongitudinalOffsetToSegment(traj.points, 8, createPoint(1.0, 3.0, 0.0)), -7, epsilon); + calcLongitudinalOffsetToSegment(traj.points, 8, create_point(1.0, 3.0, 0.0)), -7, epsilon); } TEST(trajectory, calcLongitudinalOffsetToSegment_CurveTrajectory) @@ -520,7 +520,7 @@ TEST(trajectory, calcLongitudinalOffsetToSegment_CurveTrajectory) // Random cases EXPECT_NEAR( - calcLongitudinalOffsetToSegment(traj.points, 2, createPoint(2.0, 0.5, 0.0)), 0.083861449, + calcLongitudinalOffsetToSegment(traj.points, 2, create_point(2.0, 0.5, 0.0)), 0.083861449, epsilon); } @@ -547,22 +547,22 @@ TEST(trajectory, calcLateralOffset) // Same close points in trajectory { const auto invalid_traj = generateTestTrajectory(10, 0.0); - const auto p = createPoint(3.0, 0.0, 0.0); + const auto p = create_point(3.0, 0.0, 0.0); EXPECT_THROW(calcLateralOffset(invalid_traj.points, p, throw_exception), std::runtime_error); } // Point on trajectory - EXPECT_NEAR(calcLateralOffset(traj.points, createPoint(3.1, 0.0, 0.0)), 0.0, epsilon); + EXPECT_NEAR(calcLateralOffset(traj.points, create_point(3.1, 0.0, 0.0)), 0.0, epsilon); // Point before start point - EXPECT_NEAR(calcLateralOffset(traj.points, createPoint(-3.9, 3.0, 0.0)), 3.0, epsilon); + EXPECT_NEAR(calcLateralOffset(traj.points, create_point(-3.9, 3.0, 0.0)), 3.0, epsilon); // Point after start point - EXPECT_NEAR(calcLateralOffset(traj.points, createPoint(13.3, -10.0, 0.0)), -10.0, epsilon); + EXPECT_NEAR(calcLateralOffset(traj.points, create_point(13.3, -10.0, 0.0)), -10.0, epsilon); // Random cases - EXPECT_NEAR(calcLateralOffset(traj.points, createPoint(4.3, 7.0, 0.0)), 7.0, epsilon); - EXPECT_NEAR(calcLateralOffset(traj.points, createPoint(1.0, -3.0, 0.0)), -3.0, epsilon); + EXPECT_NEAR(calcLateralOffset(traj.points, create_point(4.3, 7.0, 0.0)), 7.0, epsilon); + EXPECT_NEAR(calcLateralOffset(traj.points, create_point(1.0, -3.0, 0.0)), -3.0, epsilon); } TEST(trajectory, calcLateralOffset_without_segment_idx) @@ -590,7 +590,7 @@ TEST(trajectory, calcLateralOffset_without_segment_idx) // Same close points in trajectory { const auto invalid_traj = generateTestTrajectory(10, 0.0); - const auto p = createPoint(3.0, 0.0, 0.0); + const auto p = create_point(3.0, 0.0, 0.0); EXPECT_THROW( calcLateralOffset(invalid_traj.points, p, static_cast(2), throw_exception), std::runtime_error); @@ -601,28 +601,28 @@ TEST(trajectory, calcLateralOffset_without_segment_idx) // Point on trajectory EXPECT_NEAR( - calcLateralOffset(traj.points, createPoint(3.1, 0.0, 0.0), static_cast(3)), 0.0, + calcLateralOffset(traj.points, create_point(3.1, 0.0, 0.0), static_cast(3)), 0.0, epsilon); // Point before start point EXPECT_NEAR( - calcLateralOffset(traj.points, createPoint(-3.9, 3.0, 0.0), static_cast(0)), 3.0, + calcLateralOffset(traj.points, create_point(-3.9, 3.0, 0.0), static_cast(0)), 3.0, epsilon); // Point after start point EXPECT_NEAR( - calcLateralOffset(traj.points, createPoint(13.3, -10.0, 0.0), static_cast(8)), -10.0, + calcLateralOffset(traj.points, create_point(13.3, -10.0, 0.0), static_cast(8)), -10.0, epsilon); // Random cases EXPECT_NEAR( - calcLateralOffset(traj.points, createPoint(4.3, 7.0, 0.0), static_cast(4)), 7.0, + calcLateralOffset(traj.points, create_point(4.3, 7.0, 0.0), static_cast(4)), 7.0, epsilon); EXPECT_NEAR( - calcLateralOffset(traj.points, createPoint(1.0, -3.0, 0.0), static_cast(0)), -3.0, + calcLateralOffset(traj.points, create_point(1.0, -3.0, 0.0), static_cast(0)), -3.0, epsilon); EXPECT_NEAR( - calcLateralOffset(traj.points, createPoint(1.0, -3.0, 0.0), static_cast(1)), -3.0, + calcLateralOffset(traj.points, create_point(1.0, -3.0, 0.0), static_cast(1)), -3.0, epsilon); } @@ -633,8 +633,8 @@ TEST(trajectory, calcLateralOffset_CurveTrajectory) const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); // Random cases - EXPECT_NEAR(calcLateralOffset(traj.points, createPoint(2.0, 0.5, 0.0)), 0.071386083, epsilon); - EXPECT_NEAR(calcLateralOffset(traj.points, createPoint(5.0, 1.0, 0.0)), -1.366602819, epsilon); + EXPECT_NEAR(calcLateralOffset(traj.points, create_point(2.0, 0.5, 0.0)), 0.071386083, epsilon); + EXPECT_NEAR(calcLateralOffset(traj.points, create_point(5.0, 1.0, 0.0)), -1.366602819, epsilon); } TEST(trajectory, calcSignedArcLengthFromIndexToIndex) @@ -670,23 +670,23 @@ TEST(trajectory, calcSignedArcLengthFromPointToIndex) EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); // Same point - EXPECT_NEAR(calcSignedArcLength(traj.points, createPoint(3.0, 0.0, 0.0), 3), 0, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, create_point(3.0, 0.0, 0.0), 3), 0, epsilon); // Forward - EXPECT_NEAR(calcSignedArcLength(traj.points, createPoint(0.0, 0.0, 0.0), 3), 3, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, create_point(0.0, 0.0, 0.0), 3), 3, epsilon); // Backward - EXPECT_NEAR(calcSignedArcLength(traj.points, createPoint(9.0, 0.0, 0.0), 5), -4, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, create_point(9.0, 0.0, 0.0), 5), -4, epsilon); // Point before start point - EXPECT_NEAR(calcSignedArcLength(traj.points, createPoint(-3.9, 3.0, 0.0), 6), 9.9, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, create_point(-3.9, 3.0, 0.0), 6), 9.9, epsilon); // Point after end point - EXPECT_NEAR(calcSignedArcLength(traj.points, createPoint(13.3, -10.0, 0.0), 7), -6.3, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, create_point(13.3, -10.0, 0.0), 7), -6.3, epsilon); // Random cases - EXPECT_NEAR(calcSignedArcLength(traj.points, createPoint(1.0, 3.0, 0.0), 9), 8, epsilon); - EXPECT_NEAR(calcSignedArcLength(traj.points, createPoint(4.3, 7.0, 0.0), 2), -2.3, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, create_point(1.0, 3.0, 0.0), 9), 8, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, create_point(4.3, 7.0, 0.0), 2), -2.3, epsilon); } TEST(trajectory, calcSignedArcLengthFromIndexToPoint) @@ -699,23 +699,23 @@ TEST(trajectory, calcSignedArcLengthFromIndexToPoint) EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); // Same point - EXPECT_NEAR(calcSignedArcLength(traj.points, 3, createPoint(3.0, 0.0, 0.0)), 0, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, 3, create_point(3.0, 0.0, 0.0)), 0, epsilon); // Forward - EXPECT_NEAR(calcSignedArcLength(traj.points, 0, createPoint(3.0, 0.0, 0.0)), 3, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, 0, create_point(3.0, 0.0, 0.0)), 3, epsilon); // Backward - EXPECT_NEAR(calcSignedArcLength(traj.points, 9, createPoint(5.0, 0.0, 0.0)), -4, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, 9, create_point(5.0, 0.0, 0.0)), -4, epsilon); // Point before start point - EXPECT_NEAR(calcSignedArcLength(traj.points, 6, createPoint(-3.9, 3.0, 0.0)), -9.9, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, 6, create_point(-3.9, 3.0, 0.0)), -9.9, epsilon); // Point after end point - EXPECT_NEAR(calcSignedArcLength(traj.points, 7, createPoint(13.3, -10.0, 0.0)), 6.3, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, 7, create_point(13.3, -10.0, 0.0)), 6.3, epsilon); // Random cases - EXPECT_NEAR(calcSignedArcLength(traj.points, 1, createPoint(9.0, 3.0, 0.0)), 8, epsilon); - EXPECT_NEAR(calcSignedArcLength(traj.points, 4, createPoint(1.7, 7.0, 0.0)), -2.3, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, 1, create_point(9.0, 3.0, 0.0)), 8, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, 4, create_point(1.7, 7.0, 0.0)), -2.3, epsilon); } TEST(trajectory, calcSignedArcLengthFromPointToPoint) @@ -729,54 +729,54 @@ TEST(trajectory, calcSignedArcLengthFromPointToPoint) // Same point { - const auto p = createPoint(3.0, 0.0, 0.0); + const auto p = create_point(3.0, 0.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p, p), 0, epsilon); } // Forward { - const auto p1 = createPoint(0.0, 0.0, 0.0); - const auto p2 = createPoint(3.0, 1.0, 0.0); + const auto p1 = create_point(0.0, 0.0, 0.0); + const auto p2 = create_point(3.0, 1.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, p2), 3, epsilon); } // Backward { - const auto p1 = createPoint(8.0, 0.0, 0.0); - const auto p2 = createPoint(9.0, 0.0, 0.0); + const auto p1 = create_point(8.0, 0.0, 0.0); + const auto p2 = create_point(9.0, 0.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, p2), 1, epsilon); } // Point before start point { - const auto p1 = createPoint(-3.9, 3.0, 0.0); - const auto p2 = createPoint(6.0, -10.0, 0.0); + const auto p1 = create_point(-3.9, 3.0, 0.0); + const auto p2 = create_point(6.0, -10.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, p2), 9.9, epsilon); } // Point after end point { - const auto p1 = createPoint(7.0, -5.0, 0.0); - const auto p2 = createPoint(13.3, -10.0, 0.0); + const auto p1 = create_point(7.0, -5.0, 0.0); + const auto p2 = create_point(13.3, -10.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, p2), 6.3, epsilon); } // Point before start point and after end point { - const auto p1 = createPoint(-4.3, 10.0, 0.0); - const auto p2 = createPoint(13.8, -1.0, 0.0); + const auto p1 = create_point(-4.3, 10.0, 0.0); + const auto p2 = create_point(13.8, -1.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, p2), 18.1, epsilon); } // Random cases { - const auto p1 = createPoint(1.0, 3.0, 0.0); - const auto p2 = createPoint(9.0, -1.0, 0.0); + const auto p1 = create_point(1.0, 3.0, 0.0); + const auto p2 = create_point(9.0, -1.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, p2), 8, epsilon); } { - const auto p1 = createPoint(4.3, 7.0, 0.0); - const auto p2 = createPoint(2.0, 3.0, 0.0); + const auto p1 = create_point(4.3, 7.0, 0.0); + const auto p2 = create_point(2.0, 3.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, p2), -2.3, epsilon); } } @@ -1011,7 +1011,7 @@ TEST(trajectory, calcDistanceToForwardStopPoint_DistThreshold) TEST(trajectory, calcDistanceToForwardStopPoint_YawThreshold) { using autoware::motion_utils::calcDistanceToForwardStopPoint; - using autoware::universe_utils::deg2rad; + using autoware_utils::deg2rad; const auto max_d = std::numeric_limits::max(); auto traj_input = generateTestTrajectory(100, 1.0, 3.0); @@ -1065,7 +1065,7 @@ TEST(trajectory, calcLongitudinalOffsetPointFromIndex) using autoware::motion_utils::calcArcLength; using autoware::motion_utils::calcLongitudinalOffsetPoint; using autoware::motion_utils::calcSignedArcLength; - using autoware::universe_utils::getPoint; + using autoware_utils::get_point; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -1079,7 +1079,7 @@ TEST(trajectory, calcLongitudinalOffsetPointFromIndex) // Found Pose(forward) for (size_t i = 0; i < traj.points.size(); ++i) { - double x_ans = getPoint(traj.points.at(i)).x; + double x_ans = get_point(traj.points.at(i)).x; const auto d_back = calcSignedArcLength(traj.points, i, traj.points.size() - 1); @@ -1097,7 +1097,7 @@ TEST(trajectory, calcLongitudinalOffsetPointFromIndex) // Found Pose(backward) for (size_t i = 0; i < traj.points.size(); ++i) { - double x_ans = getPoint(traj.points.at(i)).x; + double x_ans = get_point(traj.points.at(i)).x; const auto d_front = calcSignedArcLength(traj.points, i, 0); @@ -1141,8 +1141,8 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint) using autoware::motion_utils::calcArcLength; using autoware::motion_utils::calcLongitudinalOffsetPoint; using autoware::motion_utils::calcSignedArcLength; - using autoware::universe_utils::createPoint; - using autoware::universe_utils::getPoint; + using autoware_utils::create_point; + using autoware_utils::get_point; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -1155,7 +1155,7 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint) constexpr double lateral_deviation = 0.5; double x_ans = x_start; - const auto p_src = createPoint(x_start, lateral_deviation, 0.0); + const auto p_src = create_point(x_start, lateral_deviation, 0.0); const auto d_back = calcSignedArcLength(traj.points, p_src, traj.points.size() - 1); for (double len = 0.0; len < d_back + epsilon; len += 0.1) { @@ -1175,7 +1175,7 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint) constexpr double lateral_deviation = 0.5; double x_ans = x_start; - const auto p_src = createPoint(x_start, lateral_deviation, 0.0); + const auto p_src = create_point(x_start, lateral_deviation, 0.0); const auto d_front = calcSignedArcLength(traj.points, p_src, 0); for (double len = 0.0; d_front - epsilon < len; len -= 0.1) { @@ -1192,7 +1192,7 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint) // No found { - const auto p_src = createPoint(0.0, 0.0, 0.0); + const auto p_src = create_point(0.0, 0.0, 0.0); const auto p_out = calcLongitudinalOffsetPoint(traj.points, p_src, total_length + 1.0); EXPECT_EQ(p_out, std::nullopt); @@ -1200,7 +1200,7 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint) // No found { - const auto p_src = createPoint(9.0, 0.0, 0.0); + const auto p_src = create_point(9.0, 0.0, 0.0); const auto p_out = calcLongitudinalOffsetPoint(traj.points, p_src, -total_length - 1.0); EXPECT_EQ(p_out, std::nullopt); @@ -1219,7 +1219,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) using autoware::motion_utils::calcArcLength; using autoware::motion_utils::calcLongitudinalOffsetPose; using autoware::motion_utils::calcSignedArcLength; - using autoware::universe_utils::getPoint; + using autoware_utils::get_point; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -1233,7 +1233,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) // Found Pose(forward) for (size_t i = 0; i < traj.points.size(); ++i) { - double x_ans = getPoint(traj.points.at(i)).x; + double x_ans = get_point(traj.points.at(i)).x; const auto d_back = calcSignedArcLength(traj.points, i, traj.points.size() - 1); @@ -1255,7 +1255,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) // Found Pose(backward) for (size_t i = 0; i < traj.points.size(); ++i) { - double x_ans = getPoint(traj.points.at(i)).x; + double x_ans = get_point(traj.points.at(i)).x; const auto d_front = calcSignedArcLength(traj.points, i, 0); @@ -1302,7 +1302,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) { using autoware::motion_utils::calcArcLength; using autoware::motion_utils::calcLongitudinalOffsetPose; - using autoware::universe_utils::deg2rad; + using autoware_utils::deg2rad; using autoware_planning_msgs::msg::TrajectoryPoint; Trajectory traj{}; @@ -1326,7 +1326,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) // Found pose(forward) for (double len = 0.0; len < total_length; len += 0.1) { const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, len); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); EXPECT_NE(p_out, std::nullopt); EXPECT_NEAR(p_out.value().position.x, len * std::cos(deg2rad(45.0)), epsilon); @@ -1341,7 +1341,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) // Found pose(backward) for (double len = total_length; 0.0 < len; len -= 0.1) { const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, -len); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); EXPECT_NE(p_out, std::nullopt); EXPECT_NEAR(p_out.value().position.x, 1.0 - len * std::cos(deg2rad(45.0)), epsilon); @@ -1356,7 +1356,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) // Boundary condition { const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, total_length); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NE(p_out, std::nullopt); EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); @@ -1371,7 +1371,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) // Boundary condition { const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, 0.0); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NE(p_out, std::nullopt); EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); @@ -1388,7 +1388,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) { using autoware::motion_utils::calcArcLength; using autoware::motion_utils::calcLongitudinalOffsetPose; - using autoware::universe_utils::deg2rad; + using autoware_utils::deg2rad; using autoware_planning_msgs::msg::TrajectoryPoint; Trajectory traj{}; @@ -1415,7 +1415,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) // ratio between two points const auto ratio = len / total_length; const auto ans_quat = - createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 - 45.0 * ratio)); + create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 - 45.0 * ratio)); EXPECT_NE(p_out, std::nullopt); EXPECT_NEAR(p_out.value().position.x, len * std::cos(deg2rad(45.0)), epsilon); @@ -1433,7 +1433,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) // ratio between two points const auto ratio = len / total_length; const auto ans_quat = - createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 * ratio)); + create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 * ratio)); EXPECT_NE(p_out, std::nullopt); EXPECT_NEAR(p_out.value().position.x, 1.0 - len * std::cos(deg2rad(45.0)), epsilon); @@ -1448,7 +1448,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) // Boundary condition { const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, total_length, false); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NE(p_out, std::nullopt); EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); @@ -1463,7 +1463,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) // Boundary condition { const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, 0.0, false); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NE(p_out, std::nullopt); EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); @@ -1481,8 +1481,8 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) using autoware::motion_utils::calcArcLength; using autoware::motion_utils::calcLongitudinalOffsetPose; using autoware::motion_utils::calcSignedArcLength; - using autoware::universe_utils::createPoint; - using autoware::universe_utils::getPoint; + using autoware_utils::create_point; + using autoware_utils::get_point; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -1495,7 +1495,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) constexpr double lateral_deviation = 0.5; double x_ans = x_start; - const auto p_src = createPoint(x_start, lateral_deviation, 0.0); + const auto p_src = create_point(x_start, lateral_deviation, 0.0); const auto d_back = calcSignedArcLength(traj.points, p_src, traj.points.size() - 1); for (double len = 0.0; len < d_back + epsilon; len += 0.1) { @@ -1519,7 +1519,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) constexpr double lateral_deviation = 0.5; double x_ans = x_start; - const auto p_src = createPoint(x_start, lateral_deviation, 0.0); + const auto p_src = create_point(x_start, lateral_deviation, 0.0); const auto d_front = calcSignedArcLength(traj.points, p_src, 0); for (double len = 0.0; d_front - epsilon < len; len -= 0.1) { @@ -1540,7 +1540,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) // No found { - const auto p_src = createPoint(0.0, 0.0, 0.0); + const auto p_src = create_point(0.0, 0.0, 0.0); const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, total_length + 1.0); EXPECT_EQ(p_out, std::nullopt); @@ -1548,7 +1548,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) // No found { - const auto p_src = createPoint(9.0, 0.0, 0.0); + const auto p_src = create_point(9.0, 0.0, 0.0); const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, -total_length - 1.0); EXPECT_EQ(p_out, std::nullopt); @@ -1567,8 +1567,8 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) using autoware::motion_utils::calcArcLength; using autoware::motion_utils::calcLongitudinalOffsetPose; using autoware::motion_utils::calcLongitudinalOffsetToSegment; - using autoware::universe_utils::createPoint; - using autoware::universe_utils::deg2rad; + using autoware_utils::create_point; + using autoware_utils::deg2rad; using autoware_planning_msgs::msg::TrajectoryPoint; Trajectory traj{}; @@ -1593,14 +1593,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) for (double len_start = 0.0; len_start < total_length; len_start += 0.1) { constexpr double deviation = 0.1; - const auto p_src = createPoint( + const auto p_src = create_point( len_start * std::cos(deg2rad(45.0)) + deviation, len_start * std::sin(deg2rad(45.0)) - deviation, 0.0); const auto src_offset = calcLongitudinalOffsetToSegment(traj.points, 0, p_src); for (double len = -src_offset; len < total_length - src_offset; len += 0.1) { const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, len); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); EXPECT_NE(p_out, std::nullopt); EXPECT_NEAR( @@ -1619,11 +1619,11 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) { constexpr double deviation = 0.1; - const auto p_src = createPoint(1.0 + deviation, 1.0 - deviation, 0.0); + const auto p_src = create_point(1.0 + deviation, 1.0 - deviation, 0.0); const auto src_offset = calcLongitudinalOffsetToSegment(traj.points, 0, p_src); const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, total_length - src_offset); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NE(p_out, std::nullopt); EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); @@ -1641,8 +1641,8 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) using autoware::motion_utils::calcArcLength; using autoware::motion_utils::calcLongitudinalOffsetPose; using autoware::motion_utils::calcLongitudinalOffsetToSegment; - using autoware::universe_utils::createPoint; - using autoware::universe_utils::deg2rad; + using autoware_utils::create_point; + using autoware_utils::deg2rad; using autoware_planning_msgs::msg::TrajectoryPoint; Trajectory traj{}; @@ -1667,7 +1667,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) for (double len_start = 0.0; len_start < total_length; len_start += 0.1) { constexpr double deviation = 0.1; - const auto p_src = createPoint( + const auto p_src = create_point( len_start * std::cos(deg2rad(45.0)) + deviation, len_start * std::sin(deg2rad(45.0)) - deviation, 0.0); const auto src_offset = calcLongitudinalOffsetToSegment(traj.points, 0, p_src); @@ -1677,7 +1677,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) // ratio between two points const auto ratio = (src_offset + len) / total_length; const auto ans_quat = - createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 - 45.0 * ratio)); + create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 - 45.0 * ratio)); EXPECT_NE(p_out, std::nullopt); EXPECT_NEAR( @@ -1696,12 +1696,12 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) { constexpr double deviation = 0.1; - const auto p_src = createPoint(1.0 + deviation, 1.0 - deviation, 0.0); + const auto p_src = create_point(1.0 + deviation, 1.0 - deviation, 0.0); const auto src_offset = calcLongitudinalOffsetToSegment(traj.points, 0, p_src); const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, total_length - src_offset, false); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NE(p_out, std::nullopt); EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); @@ -1719,10 +1719,10 @@ TEST(trajectory, insertTargetPoint) using autoware::motion_utils::calcArcLength; using autoware::motion_utils::findNearestSegmentIndex; using autoware::motion_utils::insertTargetPoint; - using autoware::universe_utils::calcDistance2d; - using autoware::universe_utils::createPoint; - using autoware::universe_utils::deg2rad; - using autoware::universe_utils::getPose; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -1731,7 +1731,7 @@ TEST(trajectory, insertTargetPoint) for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start, 0.0, 0.0); + const auto p_target = create_point(x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); @@ -1740,12 +1740,12 @@ TEST(trajectory, insertTargetPoint) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -1756,8 +1756,8 @@ TEST(trajectory, insertTargetPoint) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -1769,7 +1769,7 @@ TEST(trajectory, insertTargetPoint) for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start + 1.1e-3, 0.0, 0.0); + const auto p_target = create_point(x_start + 1.1e-3, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); @@ -1778,12 +1778,12 @@ TEST(trajectory, insertTargetPoint) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -1794,8 +1794,8 @@ TEST(trajectory, insertTargetPoint) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -1807,7 +1807,7 @@ TEST(trajectory, insertTargetPoint) for (double x_start = 0.25; x_start < total_length; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start, 0.25 * std::tan(deg2rad(60.0)), 0.0); + const auto p_target = create_point(x_start, 0.25 * std::tan(deg2rad(60.0)), 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); @@ -1816,12 +1816,12 @@ TEST(trajectory, insertTargetPoint) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(-30.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(-30.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -1832,8 +1832,8 @@ TEST(trajectory, insertTargetPoint) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(60.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(60.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -1845,7 +1845,7 @@ TEST(trajectory, insertTargetPoint) for (double x_start = 0.0; x_start < total_length - 1.0 + epsilon; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start + 1e-4, 0.0, 0.0); + const auto p_target = create_point(x_start + 1e-4, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); @@ -1854,7 +1854,7 @@ TEST(trajectory, insertTargetPoint) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } } @@ -1862,7 +1862,7 @@ TEST(trajectory, insertTargetPoint) for (double x_start = 1.0; x_start < total_length + epsilon; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start - 1e-4, 0.0, 0.0); + const auto p_target = create_point(x_start - 1e-4, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); @@ -1871,7 +1871,7 @@ TEST(trajectory, insertTargetPoint) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } } @@ -1879,7 +1879,7 @@ TEST(trajectory, insertTargetPoint) { auto traj_out = traj; - const auto p_target = createPoint(-1.0, 0.0, 0.0); + const auto p_target = create_point(-1.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); @@ -1890,7 +1890,7 @@ TEST(trajectory, insertTargetPoint) { auto traj_out = traj; - const auto p_target = createPoint(10.0, 0.0, 0.0); + const auto p_target = create_point(10.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); @@ -1901,7 +1901,7 @@ TEST(trajectory, insertTargetPoint) { auto traj_out = traj; - const auto p_target = createPoint(4.0, 10.0, 0.0); + const auto p_target = create_point(4.0, 10.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); @@ -1913,7 +1913,7 @@ TEST(trajectory, insertTargetPoint) auto traj_out = traj; const size_t segment_idx = 9U; - const auto p_target = createPoint(10.0, 0.0, 0.0); + const auto p_target = create_point(10.0, 0.0, 0.0); const auto insert_idx = insertTargetPoint(segment_idx, p_target, traj_out.points); EXPECT_EQ(insert_idx, std::nullopt); @@ -1932,23 +1932,23 @@ TEST(trajectory, insertTargetPoint_Reverse) using autoware::motion_utils::calcArcLength; using autoware::motion_utils::findNearestSegmentIndex; using autoware::motion_utils::insertTargetPoint; - using autoware::universe_utils::calcDistance2d; - using autoware::universe_utils::createPoint; - using autoware::universe_utils::createQuaternionFromYaw; - using autoware::universe_utils::deg2rad; - using autoware::universe_utils::getPose; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::create_quaternion_from_yaw; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; constexpr double overlap_threshold = 1e-4; auto traj = generateTestTrajectory(10, 1.0); for (size_t i = 0; i < traj.points.size(); ++i) { - traj.points.at(i).pose.orientation = createQuaternionFromYaw(autoware::universe_utils::pi); + traj.points.at(i).pose.orientation = create_quaternion_from_yaw(autoware_utils::pi); } const auto total_length = calcArcLength(traj.points); for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start + 1.1e-4, 0.0, 0.0); + const auto p_target = create_point(x_start + 1.1e-4, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold); @@ -1959,12 +1959,12 @@ TEST(trajectory, insertTargetPoint_Reverse) for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE( - calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold); + calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(180)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(180)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -1975,8 +1975,8 @@ TEST(trajectory, insertTargetPoint_Reverse) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(180)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(180)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -1990,10 +1990,10 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold) using autoware::motion_utils::calcArcLength; using autoware::motion_utils::findNearestSegmentIndex; using autoware::motion_utils::insertTargetPoint; - using autoware::universe_utils::calcDistance2d; - using autoware::universe_utils::createPoint; - using autoware::universe_utils::deg2rad; - using autoware::universe_utils::getPose; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; constexpr double overlap_threshold = 1e-4; const auto traj = generateTestTrajectory(10, 1.0); @@ -2003,7 +2003,7 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold) for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start + 1.1e-4, 0.0, 0.0); + const auto p_target = create_point(x_start + 1.1e-4, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold); @@ -2014,12 +2014,12 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold) for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE( - calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold); + calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -2030,8 +2030,8 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -2043,7 +2043,7 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold) for (double x_start = 0.0; x_start < total_length - 1.0 + epsilon; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start + 1e-5, 0.0, 0.0); + const auto p_target = create_point(x_start + 1e-5, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold); @@ -2054,7 +2054,7 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold) for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE( - calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold); + calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold); } } @@ -2062,7 +2062,7 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold) for (double x_start = 1.0; x_start < total_length + epsilon; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start - 1e-5, 0.0, 0.0); + const auto p_target = create_point(x_start - 1e-5, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold); @@ -2073,7 +2073,7 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold) for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE( - calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold); + calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold); } } } @@ -2083,10 +2083,10 @@ TEST(trajectory, insertTargetPoint_Length) using autoware::motion_utils::calcArcLength; using autoware::motion_utils::findNearestSegmentIndex; using autoware::motion_utils::insertTargetPoint; - using autoware::universe_utils::calcDistance2d; - using autoware::universe_utils::createPoint; - using autoware::universe_utils::deg2rad; - using autoware::universe_utils::getPose; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -2095,7 +2095,7 @@ TEST(trajectory, insertTargetPoint_Length) for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start, 0.0, 0.0); + const auto p_target = create_point(x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(x_start, p_target, traj_out.points); @@ -2104,12 +2104,12 @@ TEST(trajectory, insertTargetPoint_Length) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -2120,8 +2120,8 @@ TEST(trajectory, insertTargetPoint_Length) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -2133,7 +2133,7 @@ TEST(trajectory, insertTargetPoint_Length) for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start + 1.1e-3, 0.0, 0.0); + const auto p_target = create_point(x_start + 1.1e-3, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(x_start + 1.1e-3, p_target, traj_out.points); @@ -2142,12 +2142,12 @@ TEST(trajectory, insertTargetPoint_Length) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -2158,8 +2158,8 @@ TEST(trajectory, insertTargetPoint_Length) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -2171,7 +2171,7 @@ TEST(trajectory, insertTargetPoint_Length) { auto traj_out = traj; - const auto p_target = createPoint(9.0, 0.0, 0.0); + const auto p_target = create_point(9.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(9.0, p_target, traj_out.points); @@ -2180,12 +2180,12 @@ TEST(trajectory, insertTargetPoint_Length) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -2196,8 +2196,8 @@ TEST(trajectory, insertTargetPoint_Length) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -2209,7 +2209,7 @@ TEST(trajectory, insertTargetPoint_Length) for (double x_start = 0.25; x_start < total_length; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start, 0.25 * std::tan(deg2rad(60.0)), 0.0); + const auto p_target = create_point(x_start, 0.25 * std::tan(deg2rad(60.0)), 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(x_start, p_target, traj_out.points); @@ -2218,12 +2218,12 @@ TEST(trajectory, insertTargetPoint_Length) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(-30.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(-30.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -2234,8 +2234,8 @@ TEST(trajectory, insertTargetPoint_Length) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(60.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(60.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -2247,7 +2247,7 @@ TEST(trajectory, insertTargetPoint_Length) for (double x_start = 0.0; x_start < total_length - 1.0 + epsilon; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start + 1e-4, 0.0, 0.0); + const auto p_target = create_point(x_start + 1e-4, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(x_start + 1e-4, p_target, traj_out.points); @@ -2256,7 +2256,7 @@ TEST(trajectory, insertTargetPoint_Length) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } } @@ -2264,7 +2264,7 @@ TEST(trajectory, insertTargetPoint_Length) for (double x_start = 1.0; x_start < total_length + epsilon; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start - 1e-4, 0.0, 0.0); + const auto p_target = create_point(x_start - 1e-4, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(x_start - 1e-4, p_target, traj_out.points); @@ -2273,7 +2273,7 @@ TEST(trajectory, insertTargetPoint_Length) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } } @@ -2281,7 +2281,7 @@ TEST(trajectory, insertTargetPoint_Length) { auto traj_out = traj; - const auto p_target = createPoint(-1.0, 0.0, 0.0); + const auto p_target = create_point(-1.0, 0.0, 0.0); const auto insert_idx = insertTargetPoint(-1.0, p_target, traj_out.points); EXPECT_EQ(insert_idx, std::nullopt); @@ -2291,7 +2291,7 @@ TEST(trajectory, insertTargetPoint_Length) { auto traj_out = traj; - const auto p_target = createPoint(10.0, 0.0, 0.0); + const auto p_target = create_point(10.0, 0.0, 0.0); const auto insert_idx = insertTargetPoint(10.0, p_target, traj_out.points); EXPECT_EQ(insert_idx, std::nullopt); @@ -2301,7 +2301,7 @@ TEST(trajectory, insertTargetPoint_Length) { auto traj_out = traj; - const auto p_target = createPoint(4.0, 10.0, 0.0); + const auto p_target = create_point(4.0, 10.0, 0.0); const auto insert_idx = insertTargetPoint(4.0, p_target, traj_out.points); EXPECT_EQ(insert_idx, std::nullopt); @@ -2321,10 +2321,10 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) using autoware::motion_utils::calcArcLength; using autoware::motion_utils::findNearestSegmentIndex; using autoware::motion_utils::insertTargetPoint; - using autoware::universe_utils::calcDistance2d; - using autoware::universe_utils::createPoint; - using autoware::universe_utils::deg2rad; - using autoware::universe_utils::getPose; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -2333,7 +2333,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start, 0.0, 0.0); + const auto p_target = create_point(x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(0, x_start, traj_out.points); @@ -2342,12 +2342,12 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -2358,8 +2358,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -2371,7 +2371,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start + 1.1e-3, 0.0, 0.0); + const auto p_target = create_point(x_start + 1.1e-3, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(0, x_start + 1.1e-3, traj_out.points); @@ -2380,12 +2380,12 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -2396,8 +2396,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -2409,7 +2409,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) { auto traj_out = traj; - const auto p_target = createPoint(9.0, 0.0, 0.0); + const auto p_target = create_point(9.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(0, 9.0, traj_out.points); @@ -2418,12 +2418,12 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -2434,8 +2434,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -2447,7 +2447,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) for (double x_start = 0.0; x_start < total_length - 1.0 + epsilon; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start + 1e-4, 0.0, 0.0); + const auto p_target = create_point(x_start + 1e-4, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(0, x_start + 1e-4, traj_out.points); @@ -2456,7 +2456,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } } @@ -2464,7 +2464,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) for (double x_start = 1.0; x_start < total_length + epsilon; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start - 1e-4, 0.0, 0.0); + const auto p_target = create_point(x_start - 1e-4, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(0, x_start - 1e-4, traj_out.points); @@ -2473,7 +2473,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } } @@ -2507,10 +2507,10 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id using autoware::motion_utils::calcArcLength; using autoware::motion_utils::findNearestSegmentIndex; using autoware::motion_utils::insertTargetPoint; - using autoware::universe_utils::calcDistance2d; - using autoware::universe_utils::createPoint; - using autoware::universe_utils::deg2rad; - using autoware::universe_utils::getPose; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; const auto traj = generateTestTrajectory(10, 1.0); @@ -2519,7 +2519,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id auto traj_out = traj; const size_t start_idx = 2; - const auto p_target = createPoint(x_start + 2.0, 0.0, 0.0); + const auto p_target = create_point(x_start + 2.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); @@ -2528,12 +2528,12 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -2544,8 +2544,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -2559,7 +2559,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id const double x_start = 1.0 - 1e-2; const size_t start_idx = 8; - const auto p_target = createPoint(9.0 - 1e-2, 0.0, 0.0); + const auto p_target = create_point(9.0 - 1e-2, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); @@ -2568,12 +2568,12 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -2584,8 +2584,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -2599,7 +2599,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id const double x_start = 1.0; const size_t start_idx = 8; - const auto p_target = createPoint(9.0, 0.0, 0.0); + const auto p_target = create_point(9.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); @@ -2608,12 +2608,12 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -2624,8 +2624,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -2639,7 +2639,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id const double x_start = 4.0; const size_t start_idx = 5; - const auto p_target = createPoint(9.0, 0.0, 0.0); + const auto p_target = create_point(9.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); @@ -2648,12 +2648,12 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -2664,8 +2664,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -2679,7 +2679,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id const double x_start = 0.0; const size_t start_idx = 0; - const auto p_target = createPoint(0.0, 0.0, 0.0); + const auto p_target = create_point(0.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); @@ -2688,12 +2688,12 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -2704,8 +2704,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -2737,10 +2737,10 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero using autoware::motion_utils::calcArcLength; using autoware::motion_utils::findNearestSegmentIndex; using autoware::motion_utils::insertTargetPoint; - using autoware::universe_utils::calcDistance2d; - using autoware::universe_utils::createPoint; - using autoware::universe_utils::deg2rad; - using autoware::universe_utils::getPose; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; const auto traj = generateTestTrajectory(10, 1.0); @@ -2749,7 +2749,7 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero auto traj_out = traj; const size_t start_idx = 7; - const auto p_target = createPoint(7.0 + x_start, 0.0, 0.0); + const auto p_target = create_point(7.0 + x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); @@ -2758,12 +2758,12 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -2774,8 +2774,8 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -2789,7 +2789,7 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero const double x_start = -1.0 - 1e-2; const size_t start_idx = 8; - const auto p_target = createPoint(7.0 - 1e-2, 0.0, 0.0); + const auto p_target = create_point(7.0 - 1e-2, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); @@ -2798,12 +2798,12 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -2814,8 +2814,8 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -2829,7 +2829,7 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero const double x_start = -1.0; const size_t start_idx = 8; - const auto p_target = createPoint(7.0, 0.0, 0.0); + const auto p_target = create_point(7.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); @@ -2838,12 +2838,12 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -2854,8 +2854,8 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -2869,7 +2869,7 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero const double x_start = -5.0; const size_t start_idx = 5; - const auto p_target = createPoint(0.0, 0.0, 0.0); + const auto p_target = create_point(0.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); @@ -2878,12 +2878,12 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -2894,8 +2894,8 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -2924,10 +2924,10 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) using autoware::motion_utils::calcArcLength; using autoware::motion_utils::findNearestSegmentIndex; using autoware::motion_utils::insertTargetPoint; - using autoware::universe_utils::calcDistance2d; - using autoware::universe_utils::createPoint; - using autoware::universe_utils::deg2rad; - using autoware::universe_utils::getPose; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; const auto traj = generateTestTrajectory(10, 1.0); const auto total_length = calcArcLength(traj.points); @@ -2937,7 +2937,7 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) auto traj_out = traj; const geometry_msgs::msg::Pose src_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - const auto p_target = createPoint(x_start, 0.0, 0.0); + const auto p_target = create_point(x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); @@ -2946,12 +2946,12 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -2962,8 +2962,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -2980,7 +2980,7 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - const auto p_target = createPoint(src_pose_x + x_start, 0.0, 0.0); + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); @@ -2989,12 +2989,12 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -3005,8 +3005,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -3024,7 +3024,7 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - const auto p_target = createPoint(src_pose_x + x_start, 0.0, 0.0); + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); @@ -3033,7 +3033,7 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } } } @@ -3047,7 +3047,7 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - const auto p_target = createPoint(src_pose_x + x_start, 0.0, 0.0); + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); @@ -3056,12 +3056,12 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -3072,8 +3072,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -3091,7 +3091,7 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - const auto p_target = createPoint(src_pose_x + x_start, 0.0, 0.0); + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); @@ -3100,12 +3100,12 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -3116,8 +3116,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -3157,7 +3157,7 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); const double x_start = -src_pose_x; - const auto p_target = createPoint(src_pose_x + x_start, 0.0, 0.0); + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); @@ -3166,12 +3166,12 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -3182,8 +3182,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -3199,7 +3199,7 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); const double x_start = 0.0; - const auto p_target = createPoint(src_pose_x + x_start, 0.0, 0.0); + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); @@ -3208,12 +3208,12 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -3224,8 +3224,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -3276,10 +3276,10 @@ TEST(trajectory, insertStopPoint_from_a_source_index) using autoware::motion_utils::calcArcLength; using autoware::motion_utils::findNearestSegmentIndex; using autoware::motion_utils::insertStopPoint; - using autoware::universe_utils::calcDistance2d; - using autoware::universe_utils::createPoint; - using autoware::universe_utils::deg2rad; - using autoware::universe_utils::getPose; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; const auto traj = generateTestTrajectory(10, 1.0, 10.0); @@ -3288,7 +3288,7 @@ TEST(trajectory, insertStopPoint_from_a_source_index) auto traj_out = traj; const size_t start_idx = 2; - const auto p_target = createPoint(x_start + 2.0, 0.0, 0.0); + const auto p_target = create_point(x_start + 2.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(start_idx, x_start, traj_out.points); @@ -3297,7 +3297,7 @@ TEST(trajectory, insertStopPoint_from_a_source_index) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { @@ -3306,8 +3306,8 @@ TEST(trajectory, insertStopPoint_from_a_source_index) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -3318,8 +3318,8 @@ TEST(trajectory, insertStopPoint_from_a_source_index) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -3333,7 +3333,7 @@ TEST(trajectory, insertStopPoint_from_a_source_index) const double x_start = 1.0 - 1e-2; const size_t start_idx = 8; - const auto p_target = createPoint(9.0 - 1e-2, 0.0, 0.0); + const auto p_target = create_point(9.0 - 1e-2, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(start_idx, x_start, traj_out.points); @@ -3342,7 +3342,7 @@ TEST(trajectory, insertStopPoint_from_a_source_index) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { @@ -3351,8 +3351,8 @@ TEST(trajectory, insertStopPoint_from_a_source_index) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -3363,8 +3363,8 @@ TEST(trajectory, insertStopPoint_from_a_source_index) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -3378,7 +3378,7 @@ TEST(trajectory, insertStopPoint_from_a_source_index) const double x_start = 1.0; const size_t start_idx = 8; - const auto p_target = createPoint(9.0, 0.0, 0.0); + const auto p_target = create_point(9.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(start_idx, x_start, traj_out.points); @@ -3387,7 +3387,7 @@ TEST(trajectory, insertStopPoint_from_a_source_index) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { @@ -3396,8 +3396,8 @@ TEST(trajectory, insertStopPoint_from_a_source_index) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -3408,8 +3408,8 @@ TEST(trajectory, insertStopPoint_from_a_source_index) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -3423,7 +3423,7 @@ TEST(trajectory, insertStopPoint_from_a_source_index) const double x_start = 4.0; const size_t start_idx = 5; - const auto p_target = createPoint(9.0, 0.0, 0.0); + const auto p_target = create_point(9.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(start_idx, x_start, traj_out.points); @@ -3432,7 +3432,7 @@ TEST(trajectory, insertStopPoint_from_a_source_index) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { @@ -3441,8 +3441,8 @@ TEST(trajectory, insertStopPoint_from_a_source_index) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -3453,8 +3453,8 @@ TEST(trajectory, insertStopPoint_from_a_source_index) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -3468,7 +3468,7 @@ TEST(trajectory, insertStopPoint_from_a_source_index) const double x_start = 0.0; const size_t start_idx = 0; - const auto p_target = createPoint(0.0, 0.0, 0.0); + const auto p_target = create_point(0.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(start_idx, x_start, traj_out.points); @@ -3477,7 +3477,7 @@ TEST(trajectory, insertStopPoint_from_a_source_index) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { @@ -3486,8 +3486,8 @@ TEST(trajectory, insertStopPoint_from_a_source_index) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -3498,8 +3498,8 @@ TEST(trajectory, insertStopPoint_from_a_source_index) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -3531,10 +3531,10 @@ TEST(trajectory, insertStopPoint_from_front_point) using autoware::motion_utils::calcArcLength; using autoware::motion_utils::findNearestSegmentIndex; using autoware::motion_utils::insertStopPoint; - using autoware::universe_utils::calcDistance2d; - using autoware::universe_utils::createPoint; - using autoware::universe_utils::deg2rad; - using autoware::universe_utils::getPose; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; const auto traj = generateTestTrajectory(10, 1.0, 10.0); @@ -3542,7 +3542,7 @@ TEST(trajectory, insertStopPoint_from_front_point) for (double x_start = 0.5; x_start < 5.0; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start + 2.0, 0.0, 0.0); + const auto p_target = create_point(x_start + 2.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(x_start + 2.0, traj_out.points); @@ -3551,7 +3551,7 @@ TEST(trajectory, insertStopPoint_from_front_point) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { @@ -3560,8 +3560,8 @@ TEST(trajectory, insertStopPoint_from_front_point) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -3572,8 +3572,8 @@ TEST(trajectory, insertStopPoint_from_front_point) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -3586,7 +3586,7 @@ TEST(trajectory, insertStopPoint_from_front_point) auto traj_out = traj; const double x_start = 1.0 - 1e-2; - const auto p_target = createPoint(9.0 - 1e-2, 0.0, 0.0); + const auto p_target = create_point(9.0 - 1e-2, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(8.0 + x_start, traj_out.points); @@ -3595,7 +3595,7 @@ TEST(trajectory, insertStopPoint_from_front_point) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { @@ -3604,8 +3604,8 @@ TEST(trajectory, insertStopPoint_from_front_point) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -3616,8 +3616,8 @@ TEST(trajectory, insertStopPoint_from_front_point) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -3629,7 +3629,7 @@ TEST(trajectory, insertStopPoint_from_front_point) { auto traj_out = traj; - const auto p_target = createPoint(9.0, 0.0, 0.0); + const auto p_target = create_point(9.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(9.0, traj_out.points); @@ -3638,7 +3638,7 @@ TEST(trajectory, insertStopPoint_from_front_point) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { @@ -3647,8 +3647,8 @@ TEST(trajectory, insertStopPoint_from_front_point) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -3659,8 +3659,8 @@ TEST(trajectory, insertStopPoint_from_front_point) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -3673,7 +3673,7 @@ TEST(trajectory, insertStopPoint_from_front_point) auto traj_out = traj; const double x_start = 0.0; - const auto p_target = createPoint(0.0, 0.0, 0.0); + const auto p_target = create_point(0.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(x_start, traj_out.points); @@ -3682,7 +3682,7 @@ TEST(trajectory, insertStopPoint_from_front_point) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { @@ -3691,8 +3691,8 @@ TEST(trajectory, insertStopPoint_from_front_point) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -3703,8 +3703,8 @@ TEST(trajectory, insertStopPoint_from_front_point) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -3728,10 +3728,10 @@ TEST(trajectory, insertStopPoint_from_a_pose) using autoware::motion_utils::calcArcLength; using autoware::motion_utils::findNearestSegmentIndex; using autoware::motion_utils::insertStopPoint; - using autoware::universe_utils::calcDistance2d; - using autoware::universe_utils::createPoint; - using autoware::universe_utils::deg2rad; - using autoware::universe_utils::getPose; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; const auto traj = generateTestTrajectory(10, 1.0, 10.0); const auto total_length = calcArcLength(traj.points); @@ -3741,7 +3741,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) auto traj_out = traj; const geometry_msgs::msg::Pose src_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - const auto p_target = createPoint(x_start, 0.0, 0.0); + const auto p_target = create_point(x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); @@ -3750,7 +3750,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { @@ -3759,8 +3759,8 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -3771,8 +3771,8 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -3789,7 +3789,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - const auto p_target = createPoint(src_pose_x + x_start, 0.0, 0.0); + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); @@ -3798,7 +3798,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { @@ -3807,8 +3807,8 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -3819,8 +3819,8 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -3838,7 +3838,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - const auto p_target = createPoint(src_pose_x + x_start, 0.0, 0.0); + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); @@ -3847,7 +3847,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { @@ -3866,7 +3866,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - const auto p_target = createPoint(src_pose_x + x_start, 0.0, 0.0); + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); @@ -3875,7 +3875,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { @@ -3884,8 +3884,8 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -3896,8 +3896,8 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -3915,7 +3915,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - const auto p_target = createPoint(src_pose_x + x_start, 0.0, 0.0); + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); @@ -3924,7 +3924,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { @@ -3933,8 +3933,8 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -3945,8 +3945,8 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -3986,7 +3986,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); const double x_start = -src_pose_x; - const auto p_target = createPoint(src_pose_x + x_start, 0.0, 0.0); + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); @@ -3995,7 +3995,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { @@ -4004,8 +4004,8 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -4016,8 +4016,8 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -4033,7 +4033,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); const double x_start = 0.0; - const auto p_target = createPoint(src_pose_x + x_start, 0.0, 0.0); + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); @@ -4042,7 +4042,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { @@ -4051,8 +4051,8 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -4063,8 +4063,8 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -4114,10 +4114,10 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) using autoware::motion_utils::calcArcLength; using autoware::motion_utils::findNearestSegmentIndex; using autoware::motion_utils::insertStopPoint; - using autoware::universe_utils::calcDistance2d; - using autoware::universe_utils::createPoint; - using autoware::universe_utils::deg2rad; - using autoware::universe_utils::getPose; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; const auto traj = generateTestTrajectory(10, 1.0, 3.0); const auto total_length = calcArcLength(traj.points); @@ -4126,7 +4126,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start, 0.0, 0.0); + const auto p_target = create_point(x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); @@ -4135,7 +4135,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } for (size_t i = 0; i < insert_idx.value(); ++i) { @@ -4146,8 +4146,8 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -4158,8 +4158,8 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -4171,7 +4171,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start + 1.1e-3, 0.0, 0.0); + const auto p_target = create_point(x_start + 1.1e-3, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); @@ -4180,7 +4180,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } for (size_t i = 0; i < insert_idx.value(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 3.0, epsilon); @@ -4190,8 +4190,8 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -4202,8 +4202,8 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -4215,7 +4215,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) for (double x_start = 0.25; x_start < total_length; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start, 0.25 * std::tan(deg2rad(60.0)), 0.0); + const auto p_target = create_point(x_start, 0.25 * std::tan(deg2rad(60.0)), 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); @@ -4224,7 +4224,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } for (size_t i = 0; i < insert_idx.value(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 3.0, epsilon); @@ -4234,8 +4234,8 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(-30.0)); + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(-30.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); EXPECT_EQ(p_insert.position.z, p_target.z); @@ -4246,8 +4246,8 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) } { - const auto p_base = getPose(traj_out.points.at(base_idx)); - const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(60.0)); + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(60.0)); EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); @@ -4259,7 +4259,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) for (double x_start = 0.0; x_start < total_length - 1.0 + epsilon; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start + 1e-4, 0.0, 0.0); + const auto p_target = create_point(x_start + 1e-4, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); @@ -4268,7 +4268,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } for (size_t i = 0; i < insert_idx.value(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 3.0, epsilon); @@ -4282,7 +4282,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) for (double x_start = 1.0; x_start < total_length + epsilon; x_start += 1.0) { auto traj_out = traj; - const auto p_target = createPoint(x_start - 1e-4, 0.0, 0.0); + const auto p_target = create_point(x_start - 1e-4, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); @@ -4291,7 +4291,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } for (size_t i = 0; i < insert_idx.value(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 3.0, epsilon); @@ -4305,7 +4305,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) { auto traj_out = traj; - const auto p_target = createPoint(-1.0, 0.0, 0.0); + const auto p_target = create_point(-1.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); @@ -4316,7 +4316,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) { auto traj_out = traj; - const auto p_target = createPoint(10.0, 0.0, 0.0); + const auto p_target = create_point(10.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); @@ -4327,7 +4327,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) { auto traj_out = traj; - const auto p_target = createPoint(4.0, 10.0, 0.0); + const auto p_target = create_point(4.0, 10.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); @@ -4339,7 +4339,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) auto traj_out = traj; const size_t segment_idx = 9U; - const auto p_target = createPoint(10.0, 0.0, 0.0); + const auto p_target = create_point(10.0, 0.0, 0.0); const auto insert_idx = insertStopPoint(segment_idx, p_target, traj_out.points); EXPECT_EQ(insert_idx, std::nullopt); @@ -4358,9 +4358,9 @@ TEST(trajectory, insertDecelPoint_from_a_point) using autoware::motion_utils::calcArcLength; using autoware::motion_utils::findNearestSegmentIndex; using autoware::motion_utils::insertDecelPoint; - using autoware::universe_utils::calcDistance2d; - using autoware::universe_utils::createPoint; - using autoware::universe_utils::getLongitudinalVelocity; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::get_longitudinal_velocity; const auto traj = generateTestTrajectory(10, 1.0, 10.0); const auto total_length = calcArcLength(traj.points); @@ -4370,8 +4370,8 @@ TEST(trajectory, insertDecelPoint_from_a_point) { for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { auto traj_out = traj; - const geometry_msgs::msg::Point src_point = createPoint(0.0, 0.0, 0.0); - const auto p_target = createPoint(x_start, 0.0, 0.0); + const geometry_msgs::msg::Point src_point = create_point(0.0, 0.0, 0.0); + const auto p_target = create_point(x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertDecelPoint(src_point, x_start, decel_velocity, traj_out.points); @@ -4380,11 +4380,11 @@ TEST(trajectory, insertDecelPoint_from_a_point) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); if (i < insert_idx.value()) { - EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), 10.0, epsilon); + EXPECT_NEAR(get_longitudinal_velocity(traj_out.points.at(i)), 10.0, epsilon); } else { - EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), decel_velocity, epsilon); + EXPECT_NEAR(get_longitudinal_velocity(traj_out.points.at(i)), decel_velocity, epsilon); } } } @@ -4396,8 +4396,8 @@ TEST(trajectory, insertDecelPoint_from_a_point) const double src_point_y = 3.0; for (double x_start = 0.5; x_start < total_length - src_point_x; x_start += 1.0) { auto traj_out = traj; - const geometry_msgs::msg::Point src_point = createPoint(src_point_x, src_point_y, 0.0); - const auto p_target = createPoint(src_point_x + x_start, 0.0, 0.0); + const geometry_msgs::msg::Point src_point = create_point(src_point_x, src_point_y, 0.0); + const auto p_target = create_point(src_point_x + x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertDecelPoint(src_point, x_start, decel_velocity, traj_out.points); @@ -4406,11 +4406,11 @@ TEST(trajectory, insertDecelPoint_from_a_point) EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); if (i < insert_idx.value()) { - EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), 10.0, epsilon); + EXPECT_NEAR(get_longitudinal_velocity(traj_out.points.at(i)), 10.0, epsilon); } else { - EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), decel_velocity, epsilon); + EXPECT_NEAR(get_longitudinal_velocity(traj_out.points.at(i)), decel_velocity, epsilon); } } } @@ -4422,8 +4422,8 @@ TEST(trajectory, insertDecelPoint_from_a_point) const double src_point_y = 3.0; for (double x_start = 1e-3; x_start < total_length - src_point_x; x_start += 1.0) { auto traj_out = traj; - const geometry_msgs::msg::Point src_point = createPoint(src_point_x, src_point_y, 0.0); - const auto p_target = createPoint(src_point_x + x_start, 0.0, 0.0); + const geometry_msgs::msg::Point src_point = create_point(src_point_x, src_point_y, 0.0); + const auto p_target = create_point(src_point_x + x_start, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertDecelPoint(src_point, x_start, decel_velocity, traj_out.points); @@ -4432,11 +4432,11 @@ TEST(trajectory, insertDecelPoint_from_a_point) EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); if (i < insert_idx.value()) { - EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), 10.0, epsilon); + EXPECT_NEAR(get_longitudinal_velocity(traj_out.points.at(i)), 10.0, epsilon); } else { - EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), decel_velocity, epsilon); + EXPECT_NEAR(get_longitudinal_velocity(traj_out.points.at(i)), decel_velocity, epsilon); } } } @@ -4447,7 +4447,7 @@ TEST(trajectory, findFirstNearestIndexWithSoftConstraints) { using autoware::motion_utils::findFirstNearestIndexWithSoftConstraints; using autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; - using autoware::universe_utils::pi; + using autoware_utils::pi; const auto traj = generateTestTrajectory(10, 1.0); @@ -4980,60 +4980,60 @@ TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointAndSegmentInd // Same point { - const auto p = createPoint(3.0, 0.0, 0.0); + const auto p = create_point(3.0, 0.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p, 2, p, 2), 0, epsilon); EXPECT_NEAR(calcSignedArcLength(traj.points, p, 3, p, 3), 0, epsilon); } // Forward { - const auto p1 = createPoint(0.0, 0.0, 0.0); - const auto p2 = createPoint(3.0, 1.0, 0.0); + const auto p1 = create_point(0.0, 0.0, 0.0); + const auto p2 = create_point(3.0, 1.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 2), 3, epsilon); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 3), 3, epsilon); } // Backward { - const auto p1 = createPoint(9.0, 0.0, 0.0); - const auto p2 = createPoint(8.0, 0.0, 0.0); + const auto p1 = create_point(9.0, 0.0, 0.0); + const auto p2 = create_point(8.0, 0.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 8, p2, 7), -1, epsilon); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 8, p2, 8), -1, epsilon); } // Point before start point { - const auto p1 = createPoint(-3.9, 3.0, 0.0); - const auto p2 = createPoint(6.0, -10.0, 0.0); + const auto p1 = create_point(-3.9, 3.0, 0.0); + const auto p2 = create_point(6.0, -10.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 5), 9.9, epsilon); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 6), 9.9, epsilon); } // Point after end point { - const auto p1 = createPoint(7.0, -5.0, 0.0); - const auto p2 = createPoint(13.3, -10.0, 0.0); + const auto p1 = create_point(7.0, -5.0, 0.0); + const auto p2 = create_point(13.3, -10.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 6, p2, 8), 6.3, epsilon); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 7, p2, 8), 6.3, epsilon); } // Point before start point and after end point { - const auto p1 = createPoint(-4.3, 10.0, 0.0); - const auto p2 = createPoint(13.8, -1.0, 0.0); + const auto p1 = create_point(-4.3, 10.0, 0.0); + const auto p2 = create_point(13.8, -1.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 8), 18.1, epsilon); } // Random cases { - const auto p1 = createPoint(1.0, 3.0, 0.0); - const auto p2 = createPoint(9.0, -1.0, 0.0); + const auto p1 = create_point(1.0, 3.0, 0.0); + const auto p2 = create_point(9.0, -1.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 8), 8, epsilon); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 1, p2, 8), 8, epsilon); } { - const auto p1 = createPoint(4.3, 7.0, 0.0); - const auto p2 = createPoint(2.0, 3.0, 0.0); + const auto p1 = create_point(4.3, 7.0, 0.0); + const auto p2 = create_point(2.0, 3.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 4, p2, 2), -2.3, epsilon); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 4, p2, 1), -2.3, epsilon); } @@ -5050,15 +5050,15 @@ TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointIndex) // Same point { - const auto p = createPoint(3.0, 0.0, 0.0); + const auto p = create_point(3.0, 0.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p, 2, 3), 0, epsilon); EXPECT_NEAR(calcSignedArcLength(traj.points, 3, p, 3), 0, epsilon); } // Forward { - const auto p1 = createPoint(0.0, 0.0, 0.0); - const auto p2 = createPoint(3.0, 1.0, 0.0); + const auto p1 = create_point(0.0, 0.0, 0.0); + const auto p2 = create_point(3.0, 1.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, 3), 3, epsilon); EXPECT_NEAR(calcSignedArcLength(traj.points, 0, p2, 2), 3, epsilon); EXPECT_NEAR(calcSignedArcLength(traj.points, 0, p2, 3), 3, epsilon); @@ -5066,8 +5066,8 @@ TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointIndex) // Backward { - const auto p1 = createPoint(9.0, 0.0, 0.0); - const auto p2 = createPoint(8.0, 0.0, 0.0); + const auto p1 = create_point(9.0, 0.0, 0.0); + const auto p2 = create_point(8.0, 0.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 8, 8), -1, epsilon); EXPECT_NEAR(calcSignedArcLength(traj.points, 8, p2, 7), 0, epsilon); EXPECT_NEAR(calcSignedArcLength(traj.points, 8, p2, 8), 0, epsilon); @@ -5075,43 +5075,43 @@ TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointIndex) // Point before start point { - const auto p1 = createPoint(-3.9, 3.0, 0.0); + const auto p1 = create_point(-3.9, 3.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, 6), 9.9, epsilon); } // Point after end point { - const auto p2 = createPoint(13.3, -10.0, 0.0); + const auto p2 = create_point(13.3, -10.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, 7, p2, 8), 6.3, epsilon); } // Start point { - const auto p1 = createPoint(0.0, 3.0, 0.0); - const auto p2 = createPoint(5.3, -10.0, 0.0); + const auto p1 = create_point(0.0, 3.0, 0.0); + const auto p2 = create_point(5.3, -10.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, 5), 5, epsilon); EXPECT_NEAR(calcSignedArcLength(traj.points, 0, p2, 5), 5.3, epsilon); } // Point after end point { - const auto p1 = createPoint(7.3, -5.0, 0.0); - const auto p2 = createPoint(9.0, -10.0, 0.0); + const auto p1 = create_point(7.3, -5.0, 0.0); + const auto p2 = create_point(9.0, -10.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 7, 9), 1.7, epsilon); EXPECT_NEAR(calcSignedArcLength(traj.points, 7, p2, 8), 2.0, epsilon); } // Random cases { - const auto p1 = createPoint(1.0, 3.0, 0.0); - const auto p2 = createPoint(9.0, -1.0, 0.0); + const auto p1 = create_point(1.0, 3.0, 0.0); + const auto p2 = create_point(9.0, -1.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, 9), 8, epsilon); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 1, 9), 8, epsilon); EXPECT_NEAR(calcSignedArcLength(traj.points, 1, p2, 8), 8, epsilon); } { - const auto p1 = createPoint(4.3, 7.0, 0.0); - const auto p2 = createPoint(2.3, 3.0, 0.0); + const auto p1 = create_point(4.3, 7.0, 0.0); + const auto p2 = create_point(2.3, 3.0, 0.0); EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 4, 2), -2.3, epsilon); EXPECT_NEAR(calcSignedArcLength(traj.points, 4, p2, 2), -1.7, epsilon); EXPECT_NEAR(calcSignedArcLength(traj.points, 4, p2, 1), -1.7, epsilon); @@ -5253,25 +5253,25 @@ TEST(trajectory, cropForwardPoints) { // Normal case const auto cropped_traj_points = - cropForwardPoints(traj.points, autoware::universe_utils::createPoint(1.5, 1.5, 0.0), 1, 4.8); + cropForwardPoints(traj.points, autoware_utils::create_point(1.5, 1.5, 0.0), 1, 4.8); EXPECT_EQ(cropped_traj_points.size(), static_cast(7)); } { // Forward length is longer than points arc length. const auto cropped_traj_points = - cropForwardPoints(traj.points, autoware::universe_utils::createPoint(1.5, 1.5, 0.0), 1, 10.0); + cropForwardPoints(traj.points, autoware_utils::create_point(1.5, 1.5, 0.0), 1, 10.0); EXPECT_EQ(cropped_traj_points.size(), static_cast(10)); } { // Point is on the previous segment const auto cropped_traj_points = - cropForwardPoints(traj.points, autoware::universe_utils::createPoint(1.0, 1.0, 0.0), 0, 2.5); + cropForwardPoints(traj.points, autoware_utils::create_point(1.0, 1.0, 0.0), 0, 2.5); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } { // Point is on the next segment const auto cropped_traj_points = - cropForwardPoints(traj.points, autoware::universe_utils::createPoint(1.0, 1.0, 0.0), 1, 2.5); + cropForwardPoints(traj.points, autoware_utils::create_point(1.0, 1.0, 0.0), 1, 2.5); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } } @@ -5284,25 +5284,25 @@ TEST(trajectory, cropBackwardPoints) { // Normal case const auto cropped_traj_points = - cropBackwardPoints(traj.points, autoware::universe_utils::createPoint(8.5, 8.5, 0.0), 8, 4.8); + cropBackwardPoints(traj.points, autoware_utils::create_point(8.5, 8.5, 0.0), 8, 4.8); EXPECT_EQ(cropped_traj_points.size(), static_cast(6)); } { // Backward length is longer than points arc length. const auto cropped_traj_points = cropBackwardPoints( - traj.points, autoware::universe_utils::createPoint(8.5, 8.5, 0.0), 8, 10.0); + traj.points, autoware_utils::create_point(8.5, 8.5, 0.0), 8, 10.0); EXPECT_EQ(cropped_traj_points.size(), static_cast(10)); } { // Point is on the previous segment const auto cropped_traj_points = - cropBackwardPoints(traj.points, autoware::universe_utils::createPoint(8.0, 8.0, 0.0), 7, 2.5); + cropBackwardPoints(traj.points, autoware_utils::create_point(8.0, 8.0, 0.0), 7, 2.5); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } { // Point is on the next segment const auto cropped_traj_points = - cropBackwardPoints(traj.points, autoware::universe_utils::createPoint(8.0, 8.0, 0.0), 8, 2.5); + cropBackwardPoints(traj.points, autoware_utils::create_point(8.0, 8.0, 0.0), 8, 2.5); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } } @@ -5315,25 +5315,25 @@ TEST(trajectory, cropPoints) { // Normal case const auto cropped_traj_points = - cropPoints(traj.points, autoware::universe_utils::createPoint(3.5, 3.5, 0.0), 3, 2.3, 1.2); + cropPoints(traj.points, autoware_utils::create_point(3.5, 3.5, 0.0), 3, 2.3, 1.2); EXPECT_EQ(cropped_traj_points.size(), static_cast(3)); } { // Length is longer than points arc length. const auto cropped_traj_points = - cropPoints(traj.points, autoware::universe_utils::createPoint(3.5, 3.5, 0.0), 3, 10.0, 10.0); + cropPoints(traj.points, autoware_utils::create_point(3.5, 3.5, 0.0), 3, 10.0, 10.0); EXPECT_EQ(cropped_traj_points.size(), static_cast(10)); } { // Point is on the previous segment const auto cropped_traj_points = - cropPoints(traj.points, autoware::universe_utils::createPoint(3.0, 3.0, 0.0), 2, 2.2, 1.9); + cropPoints(traj.points, autoware_utils::create_point(3.0, 3.0, 0.0), 2, 2.2, 1.9); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } { // Point is on the next segment const auto cropped_traj_points = - cropPoints(traj.points, autoware::universe_utils::createPoint(3.0, 3.0, 0.0), 3, 2.2, 1.9); + cropPoints(traj.points, autoware_utils::create_point(3.0, 3.0, 0.0), 3, 2.2, 1.9); EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); } } @@ -5358,7 +5358,7 @@ TEST(Trajectory, removeFirstInvalidOrientationPoints) EXPECT_EQ(traj.points.at(i), modified_traj.points.at(i)); const double yaw1 = tf2::getYaw(modified_traj.points.at(i).pose.orientation); const double yaw2 = tf2::getYaw(modified_traj.points.at(i + 1).pose.orientation); - const double yaw_diff = std::abs(autoware::universe_utils::normalizeRadian(yaw1 - yaw2)); + const double yaw_diff = std::abs(autoware_utils::normalize_radian(yaw1 - yaw2)); EXPECT_LE(yaw_diff, max_yaw_diff); } }; @@ -5421,7 +5421,7 @@ TEST(trajectory, calcYawDeviation) TEST(trajectory, isTargetPointFront) { using autoware::motion_utils::isTargetPointFront; - using autoware::universe_utils::createPoint; + using autoware_utils::create_point; using autoware_planning_msgs::msg::TrajectoryPoint; // Generate test trajectory @@ -5429,54 +5429,54 @@ TEST(trajectory, isTargetPointFront) // Front point is base { - const auto base_point = createPoint(5.0, 0.0, 0.0); - const auto target_point = createPoint(1.0, 0.0, 0.0); + const auto base_point = create_point(5.0, 0.0, 0.0); + const auto target_point = create_point(1.0, 0.0, 0.0); EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point)); } // Front point is target { - const auto base_point = createPoint(1.0, 0.0, 0.0); - const auto target_point = createPoint(3.0, 0.0, 0.0); + const auto base_point = create_point(1.0, 0.0, 0.0); + const auto target_point = create_point(3.0, 0.0, 0.0); EXPECT_TRUE(isTargetPointFront(trajectory.points, base_point, target_point)); } // boundary condition { - const auto base_point = createPoint(1.0, 0.0, 0.0); - const auto target_point = createPoint(1.0, 0.0, 0.0); + const auto base_point = create_point(1.0, 0.0, 0.0); + const auto target_point = create_point(1.0, 0.0, 0.0); EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point)); } // before the start point { - const auto base_point = createPoint(1.0, 0.0, 0.0); - const auto target_point = createPoint(-3.0, 0.0, 0.0); + const auto base_point = create_point(1.0, 0.0, 0.0); + const auto target_point = create_point(-3.0, 0.0, 0.0); EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point)); } { - const auto base_point = createPoint(-5.0, 0.0, 0.0); - const auto target_point = createPoint(1.0, 0.0, 0.0); + const auto base_point = create_point(-5.0, 0.0, 0.0); + const auto target_point = create_point(1.0, 0.0, 0.0); EXPECT_TRUE(isTargetPointFront(trajectory.points, base_point, target_point)); } // after the end point { - const auto base_point = createPoint(10.0, 0.0, 0.0); - const auto target_point = createPoint(3.0, 0.0, 0.0); + const auto base_point = create_point(10.0, 0.0, 0.0); + const auto target_point = create_point(3.0, 0.0, 0.0); EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point)); } { - const auto base_point = createPoint(2.0, 0.0, 0.0); - const auto target_point = createPoint(14.0, 0.0, 0.0); + const auto base_point = create_point(2.0, 0.0, 0.0); + const auto target_point = create_point(14.0, 0.0, 0.0); EXPECT_TRUE(isTargetPointFront(trajectory.points, base_point, target_point)); } @@ -5484,8 +5484,8 @@ TEST(trajectory, isTargetPointFront) // empty point { const Trajectory traj; - const auto base_point = createPoint(2.0, 0.0, 0.0); - const auto target_point = createPoint(5.0, 0.0, 0.0); + const auto base_point = create_point(2.0, 0.0, 0.0); + const auto target_point = create_point(5.0, 0.0, 0.0); EXPECT_FALSE(isTargetPointFront(traj.points, base_point, target_point)); } @@ -5494,22 +5494,22 @@ TEST(trajectory, isTargetPointFront) const double threshold = 3.0; { - const auto base_point = createPoint(5.0, 0.0, 0.0); - const auto target_point = createPoint(3.0, 0.0, 0.0); + const auto base_point = create_point(5.0, 0.0, 0.0); + const auto target_point = create_point(3.0, 0.0, 0.0); EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point, threshold)); } { - const auto base_point = createPoint(1.0, 0.0, 0.0); - const auto target_point = createPoint(4.0, 0.0, 0.0); + const auto base_point = create_point(1.0, 0.0, 0.0); + const auto target_point = create_point(4.0, 0.0, 0.0); EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point, threshold)); } { - const auto base_point = createPoint(1.0, 0.0, 0.0); - const auto target_point = createPoint(4.1, 0.0, 0.0); + const auto base_point = create_point(1.0, 0.0, 0.0); + const auto target_point = create_point(4.1, 0.0, 0.0); EXPECT_TRUE(isTargetPointFront(trajectory.points, base_point, target_point, threshold)); } diff --git a/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp b/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp index b2d2f3a2e8..e2e884045e 100644 --- a/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp +++ b/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include "autoware/motion_utils/vehicle/vehicle_state_checker.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include "test_vehicle_state_checker_helper.hpp" #include @@ -34,9 +34,9 @@ constexpr double STOP_DURATION_THRESHOLD_1000_MS = 1.0; using autoware::motion_utils::VehicleArrivalChecker; using autoware::motion_utils::VehicleStopChecker; -using autoware::universe_utils::createPoint; -using autoware::universe_utils::createQuaternion; -using autoware::universe_utils::createTranslation; +using autoware_utils::create_point; +using autoware_utils::create_quaternion; +using autoware_utils::create_translation; using nav_msgs::msg::Odometry; class CheckerNode : public rclcpp::Node @@ -78,8 +78,8 @@ class PubManager : public rclcpp::Node Odometry odometry; odometry.header.stamp = now; odometry.pose.pose = pose; - odometry.twist.twist.linear = createTranslation(0.0, 0.0, 0.0); - odometry.twist.twist.angular = createTranslation(0.0, 0.0, 0.0); + odometry.twist.twist.linear = create_translation(0.0, 0.0, 0.0); + odometry.twist.twist.angular = create_translation(0.0, 0.0, 0.0); this->pub_odom_->publish(odometry); rclcpp::WallRate(10).sleep(); @@ -99,10 +99,10 @@ class PubManager : public rclcpp::Node Odometry odometry; odometry.header.stamp = now; - odometry.pose.pose.position = createPoint(0.0, 0.0, 0.0); - odometry.pose.pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); - odometry.twist.twist.linear = createTranslation(0.0, 0.0, 0.0); - odometry.twist.twist.angular = createTranslation(0.0, 0.0, 0.0); + odometry.pose.pose.position = create_point(0.0, 0.0, 0.0); + odometry.pose.pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0); + odometry.twist.twist.linear = create_translation(0.0, 0.0, 0.0); + odometry.twist.twist.angular = create_translation(0.0, 0.0, 0.0); this->pub_odom_->publish(odometry); rclcpp::WallRate(10).sleep(); @@ -122,12 +122,12 @@ class PubManager : public rclcpp::Node Odometry odometry; odometry.header.stamp = now; - odometry.pose.pose.position = createPoint(0.0, 0.0, 0.0); - odometry.pose.pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); + odometry.pose.pose.position = create_point(0.0, 0.0, 0.0); + odometry.pose.pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0); odometry.twist.twist.linear = time_diff.seconds() < publish_duration / 2.0 - ? createTranslation(1.0, 0.0, 0.0) - : createTranslation(0.0, 0.0, 0.0); - odometry.twist.twist.angular = createTranslation(0.0, 0.0, 0.0); + ? create_translation(1.0, 0.0, 0.0) + : create_translation(0.0, 0.0, 0.0); + odometry.twist.twist.angular = create_translation(0.0, 0.0, 0.0); this->pub_odom_->publish(odometry); rclcpp::WallRate(10).sleep(); @@ -147,10 +147,10 @@ class PubManager : public rclcpp::Node Odometry odometry; odometry.header.stamp = now - rclcpp::Duration(15, 0); // 15 seconds old data - odometry.pose.pose.position = createPoint(0.0, 0.0, 0.0); - odometry.pose.pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); - odometry.twist.twist.linear = createTranslation(0.0, 0.0, 0.0); - odometry.twist.twist.angular = createTranslation(0.0, 0.0, 0.0); + odometry.pose.pose.position = create_point(0.0, 0.0, 0.0); + odometry.pose.pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0); + odometry.twist.twist.linear = create_translation(0.0, 0.0, 0.0); + odometry.twist.twist.angular = create_translation(0.0, 0.0, 0.0); this->pub_odom_->publish(odometry); rclcpp::WallRate(10).sleep(); @@ -407,12 +407,12 @@ TEST(vehicle_arrival_checker, isVehicleStoppedAtStopPoint) std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); geometry_msgs::msg::Pose odom_pose; - odom_pose.position = createPoint(10.0, 0.0, 0.0); - odom_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); + odom_pose.position = create_point(10.0, 0.0, 0.0); + odom_pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0); geometry_msgs::msg::Pose goal_pose; - goal_pose.position = createPoint(10.0, 0.0, 0.0); - goal_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); + goal_pose.position = create_point(10.0, 0.0, 0.0); + goal_pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0); manager->pub_traj_->publish(generateTrajectoryWithStopPoint(goal_pose)); manager->publishStoppedOdometry(odom_pose, ODOMETRY_HISTORY_500_MS); @@ -452,12 +452,12 @@ TEST(vehicle_arrival_checker, isVehicleStoppedAtStopPoint) std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); geometry_msgs::msg::Pose odom_pose; - odom_pose.position = createPoint(0.0, 0.0, 0.0); - odom_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); + odom_pose.position = create_point(0.0, 0.0, 0.0); + odom_pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0); geometry_msgs::msg::Pose goal_pose; - goal_pose.position = createPoint(10.0, 0.0, 0.0); - goal_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); + goal_pose.position = create_point(10.0, 0.0, 0.0); + goal_pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0); manager->pub_traj_->publish(generateTrajectoryWithStopPoint(goal_pose)); manager->publishStoppedOdometry(odom_pose, ODOMETRY_HISTORY_500_MS); @@ -497,12 +497,12 @@ TEST(vehicle_arrival_checker, isVehicleStoppedAtStopPoint) std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); geometry_msgs::msg::Pose odom_pose; - odom_pose.position = createPoint(10.0, 0.0, 0.0); - odom_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); + odom_pose.position = create_point(10.0, 0.0, 0.0); + odom_pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0); geometry_msgs::msg::Pose goal_pose; - goal_pose.position = createPoint(10.0, 0.0, 0.0); - goal_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0); + goal_pose.position = create_point(10.0, 0.0, 0.0); + goal_pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0); manager->pub_traj_->publish(generateTrajectoryWithoutStopPoint(goal_pose)); manager->publishStoppedOdometry(odom_pose, ODOMETRY_HISTORY_500_MS);