From b9931c91ee383c474cc5d57f9439e5f3c6f4b9a7 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 12 Feb 2025 19:21:44 +0900 Subject: [PATCH 1/2] feat(goal_planner): ensure stop while path candidates are empty (#10101) Signed-off-by: Mamoru Sobue --- .../goal_searcher.hpp | 2 +- .../src/goal_planner_module.cpp | 26 +++++++++++++------ .../src/goal_searcher.cpp | 2 +- 3 files changed, 20 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp index 89b440c5bb795..8744fc1d12f43 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp @@ -49,7 +49,7 @@ class GoalSearcher // todo(kosuke55): Functions for this specific use should not be in the interface, // so it is better to consider interface design when we implement other goal searchers. - GoalCandidate getClosetGoalCandidateAlongLanes( + std::optional getClosestGoalCandidateAlongLanes( const GoalCandidates & goal_candidates, const std::shared_ptr & planner_data) const; bool isSafeGoalWithMarginScaleFactor( diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 5ff1a23aa4cea..e0805dbe9cefe 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1441,7 +1441,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate( // if pull over path candidates generation is not finished, use previous module output if (context_data.lane_parking_response.pull_over_path_candidates.empty()) { - return getPreviousModuleOutput(); + auto stop_path = getPreviousModuleOutput(); + stop_path.path = generateStopPath(context_data, detail); + return stop_path; } BehaviorModuleOutput output{}; @@ -1712,11 +1714,13 @@ PathWithLaneId GoalPlannerModule::generateStopPath( // calculate search start offset pose from the closest goal candidate pose with // approximate_pull_over_distance_ ego vehicle decelerates to this position. or if no feasible // stop point is found, stop at this position. - const auto closest_goal_candidate = - goal_searcher.getClosetGoalCandidateAlongLanes(goal_candidates_, planner_data_); + const auto closest_searched_goal_candidate = + goal_searcher.getClosestGoalCandidateAlongLanes(goal_candidates_, planner_data_); + const auto closest_goal_candidate = closest_searched_goal_candidate + ? closest_searched_goal_candidate.value().goal_pose + : route_handler->getOriginalGoalPose(); const auto decel_pose = calcLongitudinalOffsetPose( - extended_prev_path.points, closest_goal_candidate.goal_pose.position, - -approximate_pull_over_distance_); + extended_prev_path.points, closest_goal_candidate.position, -approximate_pull_over_distance_); // if not approved stop road lane. // stop point priority is @@ -2041,12 +2045,18 @@ double GoalPlannerModule::calcSignedArcLengthFromEgo( void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + assert(goal_searcher_); + const auto & goal_searcher = goal_searcher_.value(); // decelerate before the search area start - const auto closest_goal_candidate = - goal_searcher_->getClosetGoalCandidateAlongLanes(goal_candidates_, planner_data_); + const auto & route_handler = planner_data_->route_handler; + const auto closest_searched_goal_candidate = + goal_searcher.getClosestGoalCandidateAlongLanes(goal_candidates_, planner_data_); + const auto closest_goal_candidate = closest_searched_goal_candidate + ? closest_searched_goal_candidate.value().goal_pose + : route_handler->getOriginalGoalPose(); const auto decel_pose = calcLongitudinalOffsetPose( - pull_over_path.full_path().points, closest_goal_candidate.goal_pose.position, + pull_over_path.full_path().points, closest_goal_candidate.position, -approximate_pull_over_distance_); auto & first_path = pull_over_path.partial_paths().front(); if (decel_pose) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index e12e2c1a1dfcb..91cc0e0d01e55 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -548,7 +548,7 @@ void GoalSearcher::createAreaPolygons( } } -GoalCandidate GoalSearcher::getClosetGoalCandidateAlongLanes( +std::optional GoalSearcher::getClosestGoalCandidateAlongLanes( const GoalCandidates & goal_candidates, const std::shared_ptr & planner_data) const { From da60c4e7ba3a23560075ea7fae1a13fd5736ad25 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Wed, 12 Feb 2025 19:48:41 +0900 Subject: [PATCH 2/2] feat: remove dependency on autoware_universe_utils from autoware_interpolation and autoware_motion_utils (#10092) Signed-off-by: Ryohsuke Mitsudome Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- build_depends.repos | 2 +- .../interpolation/spline_interpolation.hpp | 2 +- .../spline_interpolation_points_2d.hpp | 2 +- common/autoware_interpolation/package.xml | 2 +- .../src/spline_interpolation_points_2d.cpp | 3 +- .../test/src/test_spline_interpolation.cpp | 2 +- .../test_spline_interpolation_points_2d.cpp | 58 +- .../docs/vehicle/vehicle.md | 4 +- .../motion_utils/resample/resample_utils.hpp | 24 +- .../motion_utils/trajectory/interpolation.hpp | 14 +- .../motion_utils/trajectory/trajectory.hpp | 218 ++-- common/autoware_motion_utils/package.xml | 2 +- .../src/marker/marker_helper.cpp | 48 +- .../src/resample/resample.cpp | 16 +- .../src/trajectory/interpolation.cpp | 13 +- .../src/trajectory/path_with_lane_id.cpp | 8 +- .../src/trajectory/trajectory.cpp | 2 +- .../test/src/resample/test_resample.cpp | 70 +- .../benchmark_calcLateralOffset.cpp | 10 +- .../src/trajectory/test_interpolation.cpp | 14 +- .../src/trajectory/test_path_with_lane_id.cpp | 32 +- .../test/src/trajectory/test_trajectory.cpp | 1124 ++++++++--------- .../vehicle/test_vehicle_state_checker.cpp | 62 +- .../path_optimizer/utils/geometry_utils.hpp | 12 +- .../path_optimizer/utils/trajectory_utils.hpp | 26 +- .../src/mpt_optimizer.cpp | 23 +- planning/autoware_path_optimizer/src/node.cpp | 3 +- .../src/replan_checker.cpp | 4 +- .../src/utils/geometry_utils.cpp | 2 +- .../src/utils/trajectory_utils.cpp | 16 +- .../src/scene_intersection_prepare_data.cpp | 7 +- 31 files changed, 908 insertions(+), 917 deletions(-) diff --git a/build_depends.repos b/build_depends.repos index 96bb9a7c14bed..b7c5666114bf9 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -12,7 +12,7 @@ repositories: core/autoware_utils: type: git url: https://github.com/autowarefoundation/autoware_utils.git - version: 1.0.0 + version: 1.1.0 core/autoware_lanelet2_extension: type: git url: https://github.com/autowarefoundation/autoware_lanelet2_extension.git diff --git a/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp index f7feada78ff4f..b6f5ae3e5a789 100644 --- a/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_ #include "autoware/interpolation/interpolation_utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include diff --git a/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp index c9668a4cad7eb..68d79cff0bc65 100644 --- a/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp @@ -49,7 +49,7 @@ class SplineInterpolationPoints2d { std::vector points_inner; for (const auto & p : points) { - points_inner.push_back(autoware::universe_utils::getPoint(p)); + points_inner.push_back(autoware_utils::get_point(p)); } calcSplineCoefficientsInner(points_inner); } diff --git a/common/autoware_interpolation/package.xml b/common/autoware_interpolation/package.xml index 0a8d103b31f09..9be75ad8dc1d0 100644 --- a/common/autoware_interpolation/package.xml +++ b/common/autoware_interpolation/package.xml @@ -11,7 +11,7 @@ ament_cmake_auto autoware_cmake - autoware_universe_utils + autoware_utils eigen ament_cmake_ros diff --git a/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp b/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp index e0f55cfb24ba8..67937a1a52c3e 100644 --- a/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp @@ -89,8 +89,7 @@ geometry_msgs::msg::Pose SplineInterpolationPoints2d::getSplineInterpolatedPose( { geometry_msgs::msg::Pose pose; pose.position = getSplineInterpolatedPoint(idx, s); - pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(getSplineInterpolatedYaw(idx, s)); + pose.orientation = autoware_utils::create_quaternion_from_yaw(getSplineInterpolatedYaw(idx, s)); return pose; } diff --git a/common/autoware_interpolation/test/src/test_spline_interpolation.cpp b/common/autoware_interpolation/test/src/test_spline_interpolation.cpp index 766e94a47b968..f80c1225932b4 100644 --- a/common/autoware_interpolation/test/src/test_spline_interpolation.cpp +++ b/common/autoware_interpolation/test/src/test_spline_interpolation.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include "autoware/interpolation/spline_interpolation.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include diff --git a/common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp b/common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp index 974ad606b978d..0af6be21a0d32 100644 --- a/common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp +++ b/common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp @@ -14,7 +14,7 @@ #include "autoware/interpolation/spline_interpolation.hpp" #include "autoware/interpolation/spline_interpolation_points_2d.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include @@ -27,15 +27,15 @@ using autoware::interpolation::SplineInterpolationPoints2d; TEST(spline_interpolation, splineYawFromPoints) { - using autoware::universe_utils::createPoint; + using autoware_utils::create_point; { // straight std::vector points; - points.push_back(createPoint(0.0, 0.0, 0.0)); - points.push_back(createPoint(1.0, 1.5, 0.0)); - points.push_back(createPoint(2.0, 3.0, 0.0)); - points.push_back(createPoint(3.0, 4.5, 0.0)); - points.push_back(createPoint(4.0, 6.0, 0.0)); + points.push_back(create_point(0.0, 0.0, 0.0)); + points.push_back(create_point(1.0, 1.5, 0.0)); + points.push_back(create_point(2.0, 3.0, 0.0)); + points.push_back(create_point(3.0, 4.5, 0.0)); + points.push_back(create_point(4.0, 6.0, 0.0)); const std::vector ans{0.9827937, 0.9827937, 0.9827937, 0.9827937, 0.9827937}; @@ -47,11 +47,11 @@ TEST(spline_interpolation, splineYawFromPoints) { // curve std::vector points; - points.push_back(createPoint(-2.0, -10.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - points.push_back(createPoint(3.0, 3.0, 0.0)); - points.push_back(createPoint(5.0, 10.0, 0.0)); - points.push_back(createPoint(10.0, 12.5, 0.0)); + points.push_back(create_point(-2.0, -10.0, 0.0)); + points.push_back(create_point(2.0, 1.5, 0.0)); + points.push_back(create_point(3.0, 3.0, 0.0)); + points.push_back(create_point(5.0, 10.0, 0.0)); + points.push_back(create_point(10.0, 12.5, 0.0)); const std::vector ans{1.368174, 0.961318, 1.086098, 0.938357, 0.278594}; const auto yaws = autoware::interpolation::splineYawFromPoints(points); @@ -62,15 +62,15 @@ TEST(spline_interpolation, splineYawFromPoints) { // size of base_keys is 1 (infeasible to interpolate) std::vector points; - points.push_back(createPoint(1.0, 0.0, 0.0)); + points.push_back(create_point(1.0, 0.0, 0.0)); EXPECT_THROW(autoware::interpolation::splineYawFromPoints(points), std::logic_error); } { // straight: size of base_keys is 2 (edge case in the implementation) std::vector points; - points.push_back(createPoint(1.0, 0.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); + points.push_back(create_point(1.0, 0.0, 0.0)); + points.push_back(create_point(2.0, 1.5, 0.0)); const std::vector ans{0.9827937, 0.9827937}; @@ -82,9 +82,9 @@ TEST(spline_interpolation, splineYawFromPoints) { // straight: size of base_keys is 3 (edge case in the implementation) std::vector points; - points.push_back(createPoint(1.0, 0.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - points.push_back(createPoint(3.0, 3.0, 0.0)); + points.push_back(create_point(1.0, 0.0, 0.0)); + points.push_back(create_point(2.0, 1.5, 0.0)); + points.push_back(create_point(3.0, 3.0, 0.0)); const std::vector ans{0.9827937, 0.9827937, 0.9827937}; @@ -97,15 +97,15 @@ TEST(spline_interpolation, splineYawFromPoints) TEST(spline_interpolation, SplineInterpolationPoints2d) { - using autoware::universe_utils::createPoint; + using autoware_utils::create_point; // curve std::vector points; - points.push_back(createPoint(-2.0, -10.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - points.push_back(createPoint(3.0, 3.0, 0.0)); - points.push_back(createPoint(5.0, 10.0, 0.0)); - points.push_back(createPoint(10.0, 12.5, 0.0)); + points.push_back(create_point(-2.0, -10.0, 0.0)); + points.push_back(create_point(2.0, 1.5, 0.0)); + points.push_back(create_point(3.0, 3.0, 0.0)); + points.push_back(create_point(5.0, 10.0, 0.0)); + points.push_back(create_point(10.0, 12.5, 0.0)); SplineInterpolationPoints2d s(points); @@ -194,19 +194,19 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) // size of base_keys is 1 (infeasible to interpolate) std::vector single_points; - single_points.push_back(createPoint(1.0, 0.0, 0.0)); + single_points.push_back(create_point(1.0, 0.0, 0.0)); EXPECT_THROW(SplineInterpolationPoints2d{single_points}, std::logic_error); } TEST(spline_interpolation, SplineInterpolationPoints2dPolymorphism) { - using autoware::universe_utils::createPoint; using autoware_planning_msgs::msg::TrajectoryPoint; + using autoware_utils::create_point; std::vector points; - points.push_back(createPoint(-2.0, -10.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - points.push_back(createPoint(3.0, 3.0, 0.0)); + points.push_back(create_point(-2.0, -10.0, 0.0)); + points.push_back(create_point(2.0, 1.5, 0.0)); + points.push_back(create_point(3.0, 3.0, 0.0)); std::vector trajectory_points; for (const auto & p : points) { diff --git a/common/autoware_motion_utils/docs/vehicle/vehicle.md b/common/autoware_motion_utils/docs/vehicle/vehicle.md index 32cbf6be28dcc..a6ce0f1591a0c 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/resample/resample_utils.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp index 5d622c54da452..e15375d73b80c 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/interpolation.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp index 74805dbfde455..920235d5ceafb 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,7 +15,7 @@ #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_internal_planning_msgs/msg/path_with_lane_id.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -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/trajectory.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp index 558a4a94336da..38b51e1cf1114 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,10 +15,10 @@ #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 @@ -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."); } } @@ -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,10 +107,10 @@ 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 @@ -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; @@ -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; } @@ -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; @@ -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; } @@ -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}; @@ -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}; @@ -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); } @@ -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; } @@ -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; @@ -963,10 +963,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); @@ -1003,24 +1003,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); } @@ -1100,7 +1100,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); } @@ -1116,7 +1116,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) { @@ -1132,12 +1132,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)); } } @@ -1231,7 +1231,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); } @@ -1245,7 +1245,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) { @@ -1258,12 +1258,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); } @@ -1275,12 +1275,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); } @@ -1380,8 +1380,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); @@ -1391,9 +1391,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) { @@ -1403,31 +1403,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; @@ -1561,9 +1561,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); } @@ -1667,7 +1667,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; @@ -1709,9 +1709,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); @@ -1771,7 +1771,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; @@ -1818,7 +1818,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; @@ -1858,8 +1858,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)); } @@ -1882,30 +1882,30 @@ 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)); } } @@ -1931,14 +1931,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 { @@ -2091,9 +2091,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) { @@ -2124,7 +2124,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) { @@ -2303,7 +2303,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}; @@ -2343,7 +2343,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()}; @@ -2442,7 +2442,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); } @@ -2455,12 +2455,12 @@ 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>( diff --git a/common/autoware_motion_utils/package.xml b/common/autoware_motion_utils/package.xml index 0a5eae0db4fbb..dba3ca98b4e44 100644 --- a/common/autoware_motion_utils/package.xml +++ b/common/autoware_motion_utils/package.xml @@ -25,7 +25,7 @@ autoware_internal_planning_msgs autoware_interpolation autoware_planning_msgs - autoware_universe_utils + autoware_utils autoware_vehicle_msgs builtin_interfaces geometry_msgs diff --git a/common/autoware_motion_utils/src/marker/marker_helper.cpp b/common/autoware_motion_utils/src/marker/marker_helper.cpp index 44b621a53c5f1..07dbcd8f2dc8b 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 333f5bd11a421..75126ba1e4d58 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); @@ -148,8 +148,7 @@ std::vector resamplePoseVector( resampled_points.at(i) = pose; } - const bool is_driving_forward = - autoware::universe_utils::isDrivingForward(points.at(0), points.at(1)); + const bool is_driving_forward = 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 @@ -264,8 +263,7 @@ autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath( for (size_t i = 1; i < input_path.points.size(); ++i) { 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); + const double ds = 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); @@ -455,8 +453,7 @@ autoware_planning_msgs::msg::Path resamplePath( for (size_t i = 1; i < input_path.points.size(); ++i) { 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); + const double ds = 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); @@ -612,8 +609,7 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( for (size_t i = 1; i < input_trajectory.points.size(); ++i) { 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); + const double ds = 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 2c0698232a16e..e94b9c240a6d6 100644 --- a/common/autoware_motion_utils/src/trajectory/interpolation.cpp +++ b/common/autoware_motion_utils/src/trajectory/interpolation.cpp @@ -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; } @@ -57,8 +57,7 @@ TrajectoryPoint calcInterpolatedPoint( TrajectoryPoint interpolated_point{}; // pose interpolation - interpolated_point.pose = - autoware::universe_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); + interpolated_point.pose = autoware_utils::calc_interpolated_pose(curr_pt, next_pt, clamped_ratio); // twist interpolation if (use_zero_order_hold_for_twist) { @@ -113,8 +112,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 +126,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 973df4758f96b..4f9c329960b54 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 @@ -116,15 +116,15 @@ autoware_internal_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 61014d95c7588..aa8d1c6b066cb 100644 --- a/common/autoware_motion_utils/src/trajectory/trajectory.cpp +++ b/common/autoware_motion_utils/src/trajectory/trajectory.cpp @@ -637,7 +637,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 72ddd043fbddf..a7ff8037b7311 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,15 +28,15 @@ namespace { -using autoware::universe_utils::createPoint; -using autoware::universe_utils::createQuaternionFromRPY; -using autoware::universe_utils::transformPoint; using autoware_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::PathPoint; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_utils::create_point; +using autoware_utils::create_quaternion_from_rpy; +using autoware_utils::transform_point; constexpr double epsilon = 1e-6; @@ -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); @@ -566,7 +566,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) 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); @@ -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); @@ -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); @@ -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); @@ -2678,8 +2678,8 @@ 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); + traj.points.back() = + generateTestTrajectoryPoint(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); @@ -3140,8 +3140,8 @@ 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); + traj.points.back() = + generateTestTrajectoryPoint(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 d00053a7b4ff6..f40159d7dd833 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_planning_msgs::msg::Trajectory; +using autoware_utils::create_point; +using autoware_utils::create_quaternion_from_rpy; 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 ec878cfe8cbc2..7d05d69de4e1a 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,13 +27,13 @@ namespace { -using autoware::universe_utils::createPoint; -using autoware::universe_utils::createQuaternionFromRPY; -using autoware::universe_utils::transformPoint; 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; +using autoware_utils::create_point; +using autoware_utils::create_quaternion_from_rpy; +using autoware_utils::transform_point; constexpr double epsilon = 1e-6; @@ -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; } 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 e607c12397fe0..1315a0c6b155a 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_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_utils::create_point; 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; } @@ -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 f34365b08a943..a2a931741a235 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,8 +137,8 @@ TEST(trajectory, validateNonSharpAngle_DefaultThreshold) TEST(trajectory, validateNonSharpAngle_SetThreshold) { using autoware::motion_utils::validateNonSharpAngle; - using autoware::universe_utils::pi; using autoware_planning_msgs::msg::TrajectoryPoint; + using autoware_utils::pi; TrajectoryPoint p1; p1.pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); @@ -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,8 +1302,8 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) { using autoware::motion_utils::calcArcLength; using autoware::motion_utils::calcLongitudinalOffsetPose; - using autoware::universe_utils::deg2rad; using autoware_planning_msgs::msg::TrajectoryPoint; + using autoware_utils::deg2rad; 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,8 +1388,8 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) { using autoware::motion_utils::calcArcLength; using autoware::motion_utils::calcLongitudinalOffsetPose; - using autoware::universe_utils::deg2rad; using autoware_planning_msgs::msg::TrajectoryPoint; + using autoware_utils::deg2rad; 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,9 +1567,9 @@ 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_planning_msgs::msg::TrajectoryPoint; + using autoware_utils::create_point; + using autoware_utils::deg2rad; 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,9 +1641,9 @@ 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_planning_msgs::msg::TrajectoryPoint; + using autoware_utils::create_point; + using autoware_utils::deg2rad; 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); + const auto cropped_traj_points = + cropBackwardPoints(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,62 +5421,62 @@ TEST(trajectory, calcYawDeviation) TEST(trajectory, isTargetPointFront) { using autoware::motion_utils::isTargetPointFront; - using autoware::universe_utils::createPoint; using autoware_planning_msgs::msg::TrajectoryPoint; + using autoware_utils::create_point; // Generate test trajectory const auto trajectory = generateTestTrajectory(10, 1.0); // 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 b2d2f3a2e8003..e2e884045ee1f 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); diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp index 48e7d68e70e18..51265e07bd3cd 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp @@ -36,14 +36,14 @@ #include #include -namespace autoware::universe_utils +namespace autoware_utils { template <> -geometry_msgs::msg::Point getPoint(const autoware::path_optimizer::ReferencePoint & p); +geometry_msgs::msg::Point get_point(const autoware::path_optimizer::ReferencePoint & p); template <> -geometry_msgs::msg::Pose getPose(const autoware::path_optimizer::ReferencePoint & p); -} // namespace autoware::universe_utils +geometry_msgs::msg::Pose get_pose(const autoware::path_optimizer::ReferencePoint & p); +} // namespace autoware_utils namespace autoware::path_optimizer { @@ -52,8 +52,8 @@ namespace geometry_utils template bool isSamePoint(const T1 & t1, const T2 & t2) { - const auto p1 = autoware::universe_utils::getPoint(t1); - const auto p2 = autoware::universe_utils::getPoint(t2); + const auto p1 = autoware_utils::get_point(t1); + const auto p2 = autoware_utils::get_point(t2); constexpr double epsilon = 1e-6; if (epsilon < std::abs(p1.x - p2.x) || epsilon < std::abs(p1.y - p2.y)) { diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp index ff53cfbc3c230..9b22c5c8e8e89 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp @@ -21,6 +21,7 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/type_alias.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include @@ -35,17 +36,17 @@ #include #include -namespace autoware::universe_utils +namespace autoware_utils { template <> -geometry_msgs::msg::Point getPoint(const autoware::path_optimizer::ReferencePoint & p); +geometry_msgs::msg::Point get_point(const autoware::path_optimizer::ReferencePoint & p); template <> -geometry_msgs::msg::Pose getPose(const autoware::path_optimizer::ReferencePoint & p); +geometry_msgs::msg::Pose get_pose(const autoware::path_optimizer::ReferencePoint & p); template <> -double getLongitudinalVelocity(const autoware::path_optimizer::ReferencePoint & p); -} // namespace autoware::universe_utils +double get_longitudinal_velocity(const autoware::path_optimizer::ReferencePoint & p); +} // namespace autoware_utils namespace autoware::path_optimizer { @@ -68,7 +69,7 @@ std::optional getPointIndexAfter( // search forward if (sum_length < min_offset) { 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 (min_offset < sum_length) { output_idx = i - 1; } @@ -82,7 +83,7 @@ std::optional getPointIndexAfter( // search backward for (size_t i = target_seg_idx; 0 < i; --i) { // NOTE: use size_t since i is always positive value - 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 < min_offset) { output_idx = i - 1; } @@ -98,7 +99,7 @@ template TrajectoryPoint convertToTrajectoryPoint(const T & point) { TrajectoryPoint traj_point; - traj_point.pose = autoware::universe_utils::getPose(point); + traj_point.pose = autoware_utils::get_pose(point); traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; traj_point.lateral_velocity_mps = point.lateral_velocity_mps; traj_point.heading_rate_rps = point.heading_rate_rps; @@ -109,9 +110,8 @@ template <> inline TrajectoryPoint convertToTrajectoryPoint(const ReferencePoint & ref_point) { TrajectoryPoint traj_point; - traj_point.pose = autoware::universe_utils::getPose(ref_point); - traj_point.longitudinal_velocity_mps = - autoware::universe_utils::getLongitudinalVelocity(ref_point); + traj_point.pose = autoware_utils::get_pose(ref_point); + traj_point.longitudinal_velocity_mps = autoware_utils::get_longitudinal_velocity(ref_point); return traj_point; } @@ -169,7 +169,7 @@ std::optional updateFrontPointForFix( { // calculate front point to insert in points as a fixed point const size_t front_seg_idx_for_fix = trajectory_utils::findEgoSegmentIndex( - points_for_fix, autoware::universe_utils::getPose(points.front()), ego_nearest_param); + points_for_fix, autoware_utils::get_pose(points.front()), ego_nearest_param); const size_t front_point_idx_for_fix = front_seg_idx_for_fix; const auto & front_fix_point = points_for_fix.at(front_point_idx_for_fix); @@ -183,7 +183,7 @@ std::optional updateFrontPointForFix( return std::nullopt; } - const double dist = autoware::universe_utils::calcDistance2d(points.front(), front_fix_point); + const double dist = autoware_utils::calc_distance2d(points.front(), front_fix_point); // check if deviation is not too large constexpr double max_lat_error = 3.0; diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index bff3b78b6ba7d..a73e8cc4ca69c 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -156,7 +156,7 @@ double calcLateralDistToBounds( if (intersect_point) { const bool is_point_left = isLeft(pose, *intersect_point); const double dist_to_bound = - autoware::universe_utils::calcDistance2d(pose.position, *intersect_point) * + autoware_utils::calc_distance2d(pose.position, *intersect_point) * (is_point_left ? 1.0 : -1.0); // the bound which is closest to the centerline will be chosen @@ -693,7 +693,7 @@ void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points ref_points.at(i).delta_arc_length = (i == ref_points.size() - 1) ? 0.0 - : autoware::universe_utils::calcDistance2d(ref_points.at(i + 1), ref_points.at(i)); + : autoware_utils::calc_distance2d(ref_points.at(i + 1), ref_points.at(i)); } } @@ -706,8 +706,8 @@ void MPTOptimizer::updateExtraPoints(std::vector & ref_points) c const auto front_wheel_pos = trajectory_utils::getNearestPosition(ref_points, i, vehicle_info_.wheel_base_m); - const bool are_too_close_points = autoware::universe_utils::calcDistance2d( - front_wheel_pos, ref_points.at(i).pose.position) < 1e-03; + const bool are_too_close_points = + autoware_utils::calc_distance2d(front_wheel_pos, ref_points.at(i).pose.position) < 1e-03; const auto front_wheel_yaw = are_too_close_points ? ref_points.at(i).getYaw() : autoware::universe_utils::calcAzimuthAngle( @@ -773,11 +773,10 @@ void MPTOptimizer::updateExtraPoints(std::vector & ref_points) c if (prev_ref_points_ptr_ && !prev_ref_points_ptr_->empty()) { for (int i = 0; i < static_cast(ref_points.size()); ++i) { const size_t prev_idx = trajectory_utils::findEgoIndex( - *prev_ref_points_ptr_, autoware::universe_utils::getPose(ref_points.at(i)), - ego_nearest_param_); + *prev_ref_points_ptr_, autoware_utils::get_pose(ref_points.at(i)), ego_nearest_param_); - const double dist_to_prev = autoware::universe_utils::calcDistance2d( - ref_points.at(i), prev_ref_points_ptr_->at(prev_idx)); + const double dist_to_prev = + autoware_utils::calc_distance2d(ref_points.at(i), prev_ref_points_ptr_->at(prev_idx)); if (max_dist_threshold < dist_to_prev) { continue; } @@ -1077,8 +1076,7 @@ void MPTOptimizer::avoidSuddenSteering( return; } const size_t prev_ego_idx = trajectory_utils::findEgoIndex( - *prev_ref_points_ptr_, autoware::universe_utils::getPose(ref_points.front()), - ego_nearest_param_); + *prev_ref_points_ptr_, autoware_utils::get_pose(ref_points.front()), ego_nearest_param_); const double max_bound_fixing_length = ego_vel * mpt_param_.max_bound_fixing_time; const int max_bound_fixing_idx = @@ -1124,9 +1122,8 @@ void MPTOptimizer::updateVehicleBounds( const double tmp_yaw = std::atan2( collision_check_pose.position.y - ref_point.pose.position.y, collision_check_pose.position.x - ref_point.pose.position.x); - const double offset_y = - -autoware::universe_utils::calcDistance2d(ref_point, collision_check_pose) * - std::sin(tmp_yaw - collision_check_yaw); + const double offset_y = -autoware_utils::calc_distance2d(ref_point, collision_check_pose) * + std::sin(tmp_yaw - collision_check_yaw); const auto vehicle_bounds_pose = autoware::universe_utils::calcOffsetPose(collision_check_pose, 0.0, offset_y, 0.0); diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index 9d0345e304235..0a58dd7041e5f 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -79,8 +79,7 @@ std::vector calcSegmentLengthVector(const std::vector & { std::vector segment_length_vector; for (size_t i = 0; i < points.size() - 1; ++i) { - const double segment_length = - autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); + const double segment_length = autoware_utils::calc_distance2d(points.at(i), points.at(i + 1)); segment_length_vector.push_back(segment_length); } return segment_length_vector; diff --git a/planning/autoware_path_optimizer/src/replan_checker.cpp b/planning/autoware_path_optimizer/src/replan_checker.cpp index a52fbcc78211f..d3aac49747910 100644 --- a/planning/autoware_path_optimizer/src/replan_checker.cpp +++ b/planning/autoware_path_optimizer/src/replan_checker.cpp @@ -79,7 +79,7 @@ bool ReplanChecker::isResetRequired(const PlannerData & planner_data) const // ego pose is lost or new ego pose is designated in simulation const double delta_dist = - autoware::universe_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); + autoware_utils::calc_distance2d(p.ego_pose, prev_ego_pose_ptr_->position); if (max_ego_moving_dist_ < delta_dist) { RCLCPP_DEBUG( logger_, @@ -206,7 +206,7 @@ bool ReplanChecker::isPathGoalChanged( } const double goal_moving_dist = - autoware::universe_utils::calcDistance2d(p.traj_points.back(), prev_traj_points.back()); + autoware_utils::calc_distance2d(p.traj_points.back(), prev_traj_points.back()); if (goal_moving_dist < max_goal_moving_dist_) { return false; } diff --git a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp index 7eca9b9c7ab70..96b8f1a5f383e 100644 --- a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp @@ -18,7 +18,7 @@ #include "autoware/path_optimizer/mpt_optimizer.hpp" #include "tf2/utils.h" -#include +#include #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp index f5ef2ddcd2451..8c7d352963288 100644 --- a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp @@ -33,26 +33,26 @@ #include #include -namespace autoware::universe_utils +namespace autoware_utils { template <> -geometry_msgs::msg::Point getPoint(const autoware::path_optimizer::ReferencePoint & p) +geometry_msgs::msg::Point get_point(const autoware::path_optimizer::ReferencePoint & p) { return p.pose.position; } template <> -geometry_msgs::msg::Pose getPose(const autoware::path_optimizer::ReferencePoint & p) +geometry_msgs::msg::Pose get_pose(const autoware::path_optimizer::ReferencePoint & p) { return p.pose; } template <> -double getLongitudinalVelocity(const autoware::path_optimizer::ReferencePoint & p) +double get_longitudinal_velocity(const autoware::path_optimizer::ReferencePoint & p) { return p.longitudinal_velocity_mps; } -} // namespace autoware::universe_utils +} // namespace autoware_utils namespace autoware::path_optimizer { @@ -152,7 +152,7 @@ std::vector resampleReferencePoints( base_keys.push_back(0.0); } else { const double delta_arc_length = - autoware::universe_utils::calcDistance2d(ref_points.at(i), ref_points.at(i - 1)); + autoware_utils::calc_distance2d(ref_points.at(i), ref_points.at(i - 1)); base_keys.push_back(base_keys.back() + delta_arc_length); } @@ -164,8 +164,8 @@ std::vector resampleReferencePoints( if (i == 0) { query_keys.push_back(0.0); } else { - const double delta_arc_length = autoware::universe_utils::calcDistance2d( - resampled_ref_points.at(i), resampled_ref_points.at(i - 1)); + const double delta_arc_length = + autoware_utils::calc_distance2d(resampled_ref_points.at(i), resampled_ref_points.at(i - 1)); const double key = query_keys.back() + delta_arc_length; if (base_keys.back() < key) { break; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 15960d2d08e94..97daac7dc924c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -22,6 +22,7 @@ #include // for lanelet::autoware::RoadMarking #include #include +#include #include #include @@ -38,11 +39,11 @@ #include #include -namespace autoware::universe_utils +namespace autoware_utils { template <> -inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p) +inline geometry_msgs::msg::Point get_point(const lanelet::ConstPoint3d & p) { geometry_msgs::msg::Point point; point.x = p.x(); @@ -51,7 +52,7 @@ inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p) return point; } -} // namespace autoware::universe_utils +} // namespace autoware_utils namespace {