From 02733e7b2932ad0d1c3c9c3a2818e2e4229f2e92 Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Thu, 13 Feb 2025 14:04:50 +0900 Subject: [PATCH] feat(autoware_motion_utils)!: port to `autoware.core/common` (#10105) porting(autoware_motion_utils): port to `autoware.core/common` * Related PR: https://github.com/autowarefoundation/autoware.core/pull/184 Signed-off-by: Junya Sasaki --- common/autoware_motion_utils/CHANGELOG.rst | 242 - common/autoware_motion_utils/CMakeLists.txt | 25 - common/autoware_motion_utils/README.md | 104 - .../docs/vehicle/vehicle.md | 169 - .../autoware/motion_utils/constants.hpp | 23 - .../motion_utils/distance/distance.hpp | 33 - .../motion_utils/marker/marker_helper.hpp | 55 - .../marker/virtual_wall_marker_creator.hpp | 82 - .../motion_utils/resample/resample.hpp | 240 - .../motion_utils/resample/resample_utils.hpp | 127 - .../motion_utils/trajectory/conversion.hpp | 121 - .../motion_utils/trajectory/interpolation.hpp | 96 - .../motion_utils/trajectory/path_shift.hpp | 71 - .../trajectory/path_with_lane_id.hpp | 46 - .../motion_utils/trajectory/trajectory.hpp | 2526 -------- .../vehicle/vehicle_state_checker.hpp | 82 - .../media/ego_nearest_search.svg | 280 - .../autoware_motion_utils/media/segment.svg | 344 - common/autoware_motion_utils/package.xml | 45 - .../src/distance/distance.cpp | 274 - .../src/marker/marker_helper.cpp | 186 - .../marker/virtual_wall_marker_creator.cpp | 94 - .../src/resample/resample.cpp | 752 --- .../src/trajectory/conversion.cpp | 55 - .../src/trajectory/interpolation.cpp | 149 - .../src/trajectory/path_shift.cpp | 108 - .../src/trajectory/path_with_lane_id.cpp | 139 - .../src/trajectory/trajectory.cpp | 645 -- .../src/vehicle/vehicle_state_checker.cpp | 135 - .../test/src/distance/test_distance.cpp | 116 - .../test_virtual_wall_marker_creator.cpp | 117 - .../test/src/resample/test_resample.cpp | 3622 ----------- .../test/src/test_motion_utils.cpp | 26 - .../benchmark_calcLateralOffset.cpp | 75 - .../src/trajectory/test_interpolation.cpp | 664 -- .../test/src/trajectory/test_path_shift.cpp | 163 - .../src/trajectory/test_path_with_lane_id.cpp | 190 - .../test/src/trajectory/test_trajectory.cpp | 5517 ----------------- .../vehicle/test_vehicle_state_checker.cpp | 529 -- .../test_vehicle_state_checker_helper.hpp | 59 - 40 files changed, 18326 deletions(-) delete mode 100644 common/autoware_motion_utils/CHANGELOG.rst delete mode 100644 common/autoware_motion_utils/CMakeLists.txt delete mode 100644 common/autoware_motion_utils/README.md delete mode 100644 common/autoware_motion_utils/docs/vehicle/vehicle.md delete mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/constants.hpp delete mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp delete mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp delete mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp delete mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp delete mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp delete mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp delete mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp delete mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_shift.hpp delete mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp delete mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp delete mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/vehicle/vehicle_state_checker.hpp delete mode 100644 common/autoware_motion_utils/media/ego_nearest_search.svg delete mode 100644 common/autoware_motion_utils/media/segment.svg delete mode 100644 common/autoware_motion_utils/package.xml delete mode 100644 common/autoware_motion_utils/src/distance/distance.cpp delete mode 100644 common/autoware_motion_utils/src/marker/marker_helper.cpp delete mode 100644 common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp delete mode 100644 common/autoware_motion_utils/src/resample/resample.cpp delete mode 100644 common/autoware_motion_utils/src/trajectory/conversion.cpp delete mode 100644 common/autoware_motion_utils/src/trajectory/interpolation.cpp delete mode 100644 common/autoware_motion_utils/src/trajectory/path_shift.cpp delete mode 100644 common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp delete mode 100644 common/autoware_motion_utils/src/trajectory/trajectory.cpp delete mode 100644 common/autoware_motion_utils/src/vehicle/vehicle_state_checker.cpp delete mode 100644 common/autoware_motion_utils/test/src/distance/test_distance.cpp delete mode 100644 common/autoware_motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp delete mode 100644 common/autoware_motion_utils/test/src/resample/test_resample.cpp delete mode 100644 common/autoware_motion_utils/test/src/test_motion_utils.cpp delete mode 100644 common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp delete mode 100644 common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp delete mode 100644 common/autoware_motion_utils/test/src/trajectory/test_path_shift.cpp delete mode 100644 common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp delete mode 100644 common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp delete mode 100644 common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp delete mode 100644 common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp diff --git a/common/autoware_motion_utils/CHANGELOG.rst b/common/autoware_motion_utils/CHANGELOG.rst deleted file mode 100644 index 46ebcdb1054e3..0000000000000 --- a/common/autoware_motion_utils/CHANGELOG.rst +++ /dev/null @@ -1,242 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_motion_utils -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.41.0 (2025-01-29) -------------------- -* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base -* chore(planning): move package directory for planning factor interface (`#9948 `_) - * chore: add new package for planning factor interface - * chore(surround_obstacle_checker): update include file - * chore(obstacle_stop_planner): update include file - * chore(obstacle_cruise_planner): update include file - * chore(motion_velocity_planner): update include file - * chore(bpp): update include file - * chore(bvp-common): update include file - * chore(blind_spot): update include file - * chore(crosswalk): update include file - * chore(detection_area): update include file - * chore(intersection): update include file - * chore(no_drivable_area): update include file - * chore(no_stopping_area): update include file - * chore(occlusion_spot): update include file - * chore(run_out): update include file - * chore(speed_bump): update include file - * chore(stop_line): update include file - * chore(template_module): update include file - * chore(traffic_light): update include file - * chore(vtl): update include file - * chore(walkway): update include file - * chore(motion_utils): remove factor interface - --------- -* feat(motion_utils): add detail and pass type to VirtualWall (`#9940 `_) -* fix(autoware_motion_utils): remove clang compiler error (`#9713 `_) -* feat(motion_utils): add planning factor interface (`#9676 `_) - * feat(motion_utils): add planning factor interface - * fix: use extern template - * fix: define function in header - --------- -* Contributors: Fumiya Watanabe, Mamoru Sobue, Ryuta Kambe, Satoshi OTA - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - common (`#9564 `_) -* 0.39.0 -* update changelog -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* refactor(factor): move steering factor interface to motion utils (`#9337 `_) -* feat(behavior_velocity_stop_line): replace autoware_motion_utils to autoware_trajectory (`#9299 `_) - * feat(behavior_velocity_stop_line): replace autoware_motion_utils to autoware_trajectory - * update - --------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* feat(autoware_trajectory): move trajectory_container from autoware_motion_utils to a new package (`#9253 `_) - * create trajectory container package - * update - * update - * style(pre-commit): autofix - * update codeowner - * update - * fix cmake - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* feat(autoware_motion_utils): add new trajectory class (`#8693 `_) - * feat(autoware_motion_utils): add interpolator - * use int32_t instead of int - * use int32_t instead of int - * use int32_t instead of int - * add const as much as possible and use `at()` in `vector` - * fix directory name - * refactor code and add example - * update - * remove unused include - * refactor code - * add clone function - * fix stairstep - * make constructor to public - * feat(autoware_motion_utils): add trajectory class - * Update CMakeLists.txt - * fix - * fix package.xml - * update crop - * revert crtp change - * update package.xml - * updating... - * update - * solve build problem - * style(pre-commit): autofix - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Satoshi OTA, Yukinari Hisaki, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* feat(autoware_trajectory): move trajectory_container from autoware_motion_utils to a new package (`#9253 `_) - * create trajectory container package - * update - * update - * style(pre-commit): autofix - * update codeowner - * update - * fix cmake - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* feat(autoware_motion_utils): add new trajectory class (`#8693 `_) - * feat(autoware_motion_utils): add interpolator - * use int32_t instead of int - * use int32_t instead of int - * use int32_t instead of int - * add const as much as possible and use `at()` in `vector` - * fix directory name - * refactor code and add example - * update - * remove unused include - * refactor code - * add clone function - * fix stairstep - * make constructor to public - * feat(autoware_motion_utils): add trajectory class - * Update CMakeLists.txt - * fix - * fix package.xml - * update crop - * revert crtp change - * update package.xml - * updating... - * update - * solve build problem - * style(pre-commit): autofix - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* Contributors: Esteve Fernandez, Yukinari Hisaki, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* feat(autoware_motion_utils): add spherical linear interpolator (`#9175 `_) -* test(motion_utils): add test for path shift (`#9083 `_) - * remove unused function - * mover path shifter utils function to autoware motion utils - * minor change in license header - * fix warning message - * remove header file - * add test file - * add unit test to all function - * fix spelling - --------- -* refactor(bpp_common, motion_utils): move path shifter util functions to autoware::motion_utils (`#9081 `_) - * remove unused function - * mover path shifter utils function to autoware motion utils - * minor change in license header - * fix warning message - * remove header file - --------- -* refactor(autoware_motion_utils): refactor interpolator (`#8931 `_) - * refactor interpolator - * update cmake - * update - * rename - * Update CMakeLists.txt - --------- -* refactor(autoware_interpolation): prefix package and namespace with autoware (`#8088 `_) - Co-authored-by: kosuke55 -* feat(autoware_motion_utils): set zero velocity after stop in resample trajectory (`#8768 `_) - * feat(autoware_motion_utils): set zero velocity after stop in resample trajectory - * fix unit test - * simplify implementation - * update comment and add test - --------- -* fix(autoware_motion_utils): fix unusedFunction (`#8733 `_) - refactor:remove Path/Trajectory length calculation between designated points -* feat(autoware_motion_utils): add clone function and make the constructor public (`#8688 `_) - * feat(autoware_motion_utils): add interpolator - * use int32_t instead of int - * use int32_t instead of int - * use int32_t instead of int - * add const as much as possible and use `at()` in `vector` - * fix directory name - * refactor code and add example - * update - * remove unused include - * refactor code - * add clone function - * fix stairstep - * make constructor to public - --------- -* feat(out_of_lane): redesign to improve accuracy and performance (`#8453 `_) -* feat(autoware_motion_utils): add interpolator (`#8517 `_) - * feat(autoware_motion_utils): add interpolator - * use int32_t instead of int - * use int32_t instead of int - * use int32_t instead of int - * add const as much as possible and use `at()` in `vector` - * fix directory name - * refactor code and add example - * update - * remove unused include - * refactor code - --------- -* fix(autoware_motion_utils): fix unusedFunction (`#8519 `_) - fix: unusedFunction -* fix(start/goal_planner): fix addition of duplicate segments in calcBeforeShiftedArcLength (`#7902 `_) - * fix(start/goal_planner): fix addition of duplicate segments in calcBeforeShiftedArcLength - * Update trajectory.hpp - Co-authored-by: Kyoichi Sugahara - * Update trajectory.hpp - Co-authored-by: Kyoichi Sugahara - --------- - Co-authored-by: Kyoichi Sugahara -* refactor(universe_utils/motion_utils)!: add autoware namespace (`#7594 `_) -* refactor(motion_utils)!: add autoware prefix and include dir (`#7539 `_) - refactor(motion_utils): add autoware prefix and include dir -* Contributors: Esteve Fernandez, Go Sakayori, Kosuke Takeuchi, Maxime CLEMENT, Nagi70, Yukinari Hisaki, Yutaka Kondo, kobayu858 - -0.26.0 (2024-04-03) -------------------- diff --git a/common/autoware_motion_utils/CMakeLists.txt b/common/autoware_motion_utils/CMakeLists.txt deleted file mode 100644 index 4c36ef2f4e70d..0000000000000 --- a/common/autoware_motion_utils/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_motion_utils) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Boost REQUIRED) - -ament_auto_add_library(autoware_motion_utils SHARED - DIRECTORY src -) - -if(BUILD_TESTING) - find_package(ament_cmake_ros REQUIRED) - - file(GLOB_RECURSE test_files test/**/*.cpp) - - ament_add_ros_isolated_gtest(test_autoware_motion_utils ${test_files}) - - target_link_libraries(test_autoware_motion_utils - autoware_motion_utils - ) -endif() - -ament_auto_package() diff --git a/common/autoware_motion_utils/README.md b/common/autoware_motion_utils/README.md deleted file mode 100644 index 8d80ae92188f3..0000000000000 --- a/common/autoware_motion_utils/README.md +++ /dev/null @@ -1,104 +0,0 @@ -# Motion Utils package - -## Definition of terms - -### Segment - -`Segment` in Autoware is the line segment between two successive points as follows. - -![segment](./media/segment.svg){: style="width:600px"} - -The nearest segment index and nearest point index to a certain position is not always th same. -Therefore, we prepare two different utility functions to calculate a nearest index for points and segments. - -## Nearest index search - -In this section, the nearest index and nearest segment index search is explained. - -We have the same functions for the nearest index search and nearest segment index search. -Taking for the example the nearest index search, we have two types of functions. - -The first function finds the nearest index with distance and yaw thresholds. - -```cpp -template -size_t findFirstNearestIndexWithSoftConstraints( - const T & points, const geometry_msgs::msg::Pose & pose, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()); -``` - -This function finds the first local solution within thresholds. -The reason to find the first local one is to deal with some edge cases explained in the next subsection. - -There are default parameters for thresholds arguments so that you can decide which thresholds to pass to the function. - -1. When both the distance and yaw thresholds are given. - - First, try to find the nearest index with both the distance and yaw thresholds. - - If not found, try to find again with only the distance threshold. - - If not found, find without any thresholds. -2. When only distance are given. - - First, try to find the nearest index the distance threshold. - - If not found, find without any thresholds. -3. When no thresholds are given. - - Find the nearest index. - -The second function finds the nearest index in the lane whose id is `lane_id`. - -```cpp -size_t findNearestIndexFromLaneId( - const autoware_internal_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id); -``` - -### Application to various object - -Many node packages often calculate the nearest index of objects. -We will explain the recommended method to calculate it. - -#### Nearest index for the ego - -Assuming that the path length before the ego is short enough, we expect to find the correct nearest index in the following edge cases by `findFirstNearestIndexWithSoftConstraints` with both distance and yaw thresholds. -Blue circles describes the distance threshold from the base link position and two blue lines describe the yaw threshold against the base link orientation. -Among points in these cases, the correct nearest point which is red can be found. - -![ego_nearest_search](./media/ego_nearest_search.svg) - -Therefore, the implementation is as follows. - -```cpp -const size_t ego_nearest_idx = findFirstNearestIndexWithSoftConstraints(points, ego_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); -const size_t ego_nearest_seg_idx = findFirstNearestIndexWithSoftConstraints(points, ego_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); -``` - -#### Nearest index for dynamic objects - -For the ego nearest index, the orientation is considered in addition to the position since the ego is supposed to follow the points. -However, for the dynamic objects (e.g., predicted object), sometimes its orientation may be different from the points order, e.g. the dynamic object driving backward although the ego is driving forward. - -Therefore, the yaw threshold should not be considered for the dynamic object. -The implementation is as follows. - -```cpp -const size_t dynamic_obj_nearest_idx = findFirstNearestIndexWithSoftConstraints(points, dynamic_obj_pose, dynamic_obj_nearest_dist_threshold); -const size_t dynamic_obj_nearest_seg_idx = findFirstNearestIndexWithSoftConstraints(points, dynamic_obj_pose, dynamic_obj_nearest_dist_threshold); -``` - -#### Nearest index for traffic objects - -In lanelet maps, traffic objects belong to the specific lane. -With this specific lane's id, the correct nearest index can be found. - -The implementation is as follows. - -```cpp -// first extract `lane_id` which the traffic object belong to. -const size_t traffic_obj_nearest_idx = findNearestIndexFromLaneId(path_with_lane_id, traffic_obj_pos, lane_id); -const size_t traffic_obj_nearest_seg_idx = findNearestSegmentIndexFromLaneId(path_with_lane_id, traffic_obj_pos, lane_id); -``` - -## For developers - -Some of the template functions in `trajectory.hpp` are mostly used for specific types (`autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::TrajectoryPoint`), so they are exported as `extern template` functions to speed-up compilation time. - -`autoware_motion_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. diff --git a/common/autoware_motion_utils/docs/vehicle/vehicle.md b/common/autoware_motion_utils/docs/vehicle/vehicle.md deleted file mode 100644 index a6ce0f1591a0c..0000000000000 --- a/common/autoware_motion_utils/docs/vehicle/vehicle.md +++ /dev/null @@ -1,169 +0,0 @@ -# vehicle utils - -Vehicle utils provides a convenient library used to check vehicle status. - -## Feature - -The library contains following classes. - -### vehicle_stop_checker - -This class check whether the vehicle is stopped or not based on localization result. - -#### Subscribed Topics - -| Name | Type | Description | -| ------------------------------- | ------------------------- | ---------------- | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | vehicle odometry | - -#### Parameters - -| Name | Type | Default Value | Explanation | -| -------------------------- | ------ | ------------- | --------------------------- | -| `velocity_buffer_time_sec` | double | 10.0 | odometry buffering time [s] | - -#### Member functions - -```c++ -bool isVehicleStopped(const double stop_duration) -``` - -- Check simply whether the vehicle is stopped based on the localization result. -- Returns `true` if the vehicle is stopped, even if system outputs a non-zero target velocity. - -#### Example Usage - -Necessary includes: - -```c++ -#include -``` - -1.Create a checker instance. - -```c++ -class SampleNode : public rclcpp::Node -{ -public: - SampleNode() : Node("sample_node") - { - vehicle_stop_checker_ = std::make_unique(this); - } - - std::unique_ptr vehicle_stop_checker_; - - bool sampleFunc(); - - ... -} -``` - -2.Check the vehicle state. - -```c++ - -bool SampleNode::sampleFunc() -{ - ... - - const auto result_1 = vehicle_stop_checker_->isVehicleStopped(); - - ... - - const auto result_2 = vehicle_stop_checker_->isVehicleStopped(3.0); - - ... -} - -``` - -### vehicle_arrival_checker - -This class check whether the vehicle arrive at stop point based on localization and planning result. - -#### Subscribed Topics - -| Name | Type | Description | -| ---------------------------------------- | ----------------------------------------- | ---------------- | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | vehicle odometry | -| `/planning/scenario_planning/trajectory` | `autoware_planning_msgs::msg::Trajectory` | trajectory | - -#### Parameters - -| Name | Type | Default Value | Explanation | -| -------------------------- | ------ | ------------- | ---------------------------------------------------------------------- | -| `velocity_buffer_time_sec` | double | 10.0 | odometry buffering time [s] | -| `th_arrived_distance_m` | double | 1.0 | threshold distance to check if vehicle has arrived at target point [m] | - -#### Member functions - -```c++ -bool isVehicleStopped(const double stop_duration) -``` - -- Check simply whether the vehicle is stopped based on the localization result. -- Returns `true` if the vehicle is stopped, even if system outputs a non-zero target velocity. - -```c++ -bool isVehicleStoppedAtStopPoint(const double stop_duration) -``` - -- Check whether the vehicle is stopped at stop point based on the localization and planning result. -- Returns `true` if the vehicle is not only stopped but also arrived at stop point. - -#### Example Usage - -Necessary includes: - -```c++ -#include -``` - -1.Create a checker instance. - -```c++ -class SampleNode : public rclcpp::Node -{ -public: - SampleNode() : Node("sample_node") - { - vehicle_arrival_checker_ = std::make_unique(this); - } - - std::unique_ptr vehicle_arrival_checker_; - - bool sampleFunc(); - - ... -} -``` - -2.Check the vehicle state. - -```c++ - -bool SampleNode::sampleFunc() -{ - ... - - const auto result_1 = vehicle_arrival_checker_->isVehicleStopped(); - - ... - - const auto result_2 = vehicle_arrival_checker_->isVehicleStopped(3.0); - - ... - - const auto result_3 = vehicle_arrival_checker_->isVehicleStoppedAtStopPoint(); - - ... - - const auto result_4 = vehicle_arrival_checker_->isVehicleStoppedAtStopPoint(3.0); - - ... -} -``` - -## Assumptions / Known limits - -`vehicle_stop_checker` and `vehicle_arrival_checker` cannot check whether the vehicle is stopped more than `velocity_buffer_time_sec` second. diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/constants.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/constants.hpp deleted file mode 100644 index 9c1f7c876e73e..0000000000000 --- a/common/autoware_motion_utils/include/autoware/motion_utils/constants.hpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_ -#define AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_ - -namespace autoware::motion_utils -{ -constexpr double overlap_threshold = 0.1; -} // namespace autoware::motion_utils - -#endif // AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp deleted file mode 100644 index 308528954eccc..0000000000000 --- a/common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_ -#define AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_ - -#include -#include -#include -#include -#include -#include - -namespace autoware::motion_utils -{ -std::optional calcDecelDistWithJerkAndAccConstraints( - const double current_vel, const double target_vel, const double current_acc, const double acc_min, - const double jerk_acc, const double jerk_dec); - -} // namespace autoware::motion_utils - -#endif // AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp deleted file mode 100644 index 2b89fc19d1878..0000000000000 --- a/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ -#define AUTOWARE__MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ - -#include - -#include - -#include - -namespace autoware::motion_utils -{ -using geometry_msgs::msg::Pose; - -visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( - const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", - const bool is_driving_forward = true); - -visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( - const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", - const bool is_driving_forward = true); - -visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( - const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", - const bool is_driving_forward = true); - -visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker( - const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, - const bool is_driving_forward); - -visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( - const rclcpp::Time & now, const int32_t id); - -visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker( - const rclcpp::Time & now, const int32_t id); -} // namespace autoware::motion_utils - -#endif // AUTOWARE__MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp deleted file mode 100644 index fd86c54d65be9..0000000000000 --- a/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp +++ /dev/null @@ -1,82 +0,0 @@ -// Copyright 2023 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ -#define AUTOWARE__MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ - -#include - -#include -#include - -#include -#include -#include -#include - -namespace autoware::motion_utils -{ - -/// @brief type of virtual wall associated with different marker styles and namespace -enum VirtualWallType { stop, slowdown, deadline, pass }; -/// @brief virtual wall to be visualized in rviz -struct VirtualWall -{ - geometry_msgs::msg::Pose pose{}; - std::string text{}; - std::string detail{}; - std::string ns{}; - VirtualWallType style = stop; - double longitudinal_offset{}; - bool is_driving_forward{true}; -}; -using VirtualWalls = std::vector; - -/// @brief class to manage the creation of virtual wall markers -/// @details creates both ADD and DELETE markers -class VirtualWallMarkerCreator -{ - struct MarkerCount - { - size_t previous = 0UL; - size_t current = 0UL; - }; - - using create_wall_function = std::function; - - VirtualWalls virtual_walls_; - std::unordered_map marker_count_per_namespace_; - - /// @brief internal cleanup: clear the stored markers and remove unused namespace from the map - void cleanup(); - -public: - /// @brief add a virtual wall - /// @param virtual_wall virtual wall to add - void add_virtual_wall(const VirtualWall & virtual_wall); - /// @brief add virtual walls - /// @param virtual_walls virtual walls to add - void add_virtual_walls(const VirtualWalls & walls); - - /// @brief create markers for the stored virtual walls - /// @details also create DELETE markers for the namespace+ids that are no longer used - /// @param now current time to be used for displaying the markers - visualization_msgs::msg::MarkerArray create_markers(const rclcpp::Time & now = rclcpp::Time()); -}; -} // namespace autoware::motion_utils - -#endif // AUTOWARE__MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp deleted file mode 100644 index 9f8e214e6f1ee..0000000000000 --- a/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp +++ /dev/null @@ -1,240 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ -#define AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ - -#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp" -#include "autoware_planning_msgs/msg/path.hpp" -#include "autoware_planning_msgs/msg/trajectory.hpp" - -#include - -namespace autoware::motion_utils -{ -/** - * @brief A resampling function for a path(points). Note that in a default setting, position xy are - * resampled by spline interpolation, position z are resampled by linear interpolation, and - * orientation of the resampled path are calculated by a forward difference method - * based on the interpolated position x and y. - * @param input_path input path(point) to resample - * @param resampled_arclength arclength that contains length of each resampling points from initial - * point - * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and - * y. Otherwise, it uses spline interpolation - * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. - * Otherwise, it uses spline interpolation - * @return resampled path(poses) - */ -std::vector resamplePointVector( - const std::vector & points, - const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, - const bool use_lerp_for_z = true); - -/** - * @brief A resampling function for a path(position). Note that in a default setting, position xy - * are resampled by spline interpolation, position z are resampled by linear interpolation, and - * orientation of the resampled path are calculated by a forward difference method - * based on the interpolated position x and y. - * @param input_path input path(position) to resample - * @param resample_interval resampling interval - * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and - * y. Otherwise, it uses spline interpolation - * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. - * Otherwise, it uses spline interpolation - * @return resampled path(poses) - */ -std::vector resamplePointVector( - const std::vector & points, const double resample_interval, - const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true); - -/** - * @brief A resampling function for a path(poses). Note that in a default setting, position xy are - * resampled by spline interpolation, position z are resampled by linear interpolation, and - * orientation of the resampled path are calculated by a forward difference method - * based on the interpolated position x and y. - * @param input_path input path(poses) to resample - * @param resampled_arclength arclength that contains length of each resampling points from initial - * point - * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and - * y. Otherwise, it uses spline interpolation - * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. - * Otherwise, it uses spline interpolation - * @return resampled path(poses) - */ -std::vector resamplePoseVector( - const std::vector & points, - const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, - const bool use_lerp_for_z = true); - -/** - * @brief A resampling function for a path(poses). Note that in a default setting, position xy are - * resampled by spline interpolation, position z are resampled by linear interpolation, and - * orientation of the resampled path are calculated by a forward difference method - * based on the interpolated position x and y. - * @param input_path input path(poses) to resample - * @param resample_interval resampling interval - * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and - * y. Otherwise, it uses spline interpolation - * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. - * Otherwise, it uses spline interpolation - * @return resampled path(poses) - */ -std::vector resamplePoseVector( - const std::vector & points, const double resample_interval, - const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true); - -/** - * @brief A resampling function for a path with lane id. Note that in a default setting, position xy - * are resampled by spline interpolation, position z are resampled by linear interpolation, - * longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is - * resampled by linear interpolation. Orientation of the resampled path are calculated by a - * forward difference method based on the interpolated position x and y. Moreover, lane_ids - * and is_final are also interpolated by zero order hold - * @param input_path input path to resample - * @param resampled_arclength arclength that contains length of each resampling points from initial - * point - * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and - * y. Otherwise, it uses spline interpolation - * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. - * Otherwise, it uses spline interpolation - * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample - * longitudinal and lateral velocity. Otherwise, it uses linear interpolation - * @return resampled path - */ -autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, - const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, - const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); - -/** - * @brief A resampling function for a path with lane id. Note that in a default setting, position xy - * are resampled by spline interpolation, position z are resampled by linear interpolation, - * longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is - * resampled by linear interpolation. Orientation of the resampled path are calculated by a - * forward difference method based on the interpolated position x and y. Moreover, lane_ids - * and is_final are also interpolated by zero order hold - * @param input_path input path to resample - * @param resampled_interval resampling interval point - * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and - * y. Otherwise, it uses spline interpolation - * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. - * Otherwise, it uses spline interpolation - * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample - * longitudinal and lateral velocity. Otherwise, it uses linear interpolation - * @param resample_input_path_stop_point If true, resample closest stop point in input path - * @return resampled path - */ -autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, - const double resample_interval, const bool use_akima_spline_for_xy = false, - const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true, - const bool resample_input_path_stop_point = true); - -/** - * @brief A resampling function for a path. Note that in a default setting, position xy are - * resampled by spline interpolation, position z are resampled by linear interpolation, - * longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is - * resampled by linear interpolation. Orientation of the resampled path are calculated by a - * forward difference method based on the interpolated position x and y. - * @param input_path input path to resample - * @param resampled_arclength arclength that contains length of each resampling points from initial - * point - * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and - * y. Otherwise, it uses spline interpolation - * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. - * Otherwise, it uses spline interpolation - * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample - * longitudinal and lateral velocity. Otherwise, it uses linear interpolation - * @return resampled path - */ -autoware_planning_msgs::msg::Path resamplePath( - const autoware_planning_msgs::msg::Path & input_path, - const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, - const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); - -/** - * @brief A resampling function for a path. Note that in a default setting, position xy - * are resampled by spline interpolation, position z are resampled by linear interpolation, - * longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is - * resampled by linear interpolation. Orientation of the resampled path are calculated by a - * forward difference method based on the interpolated position x and y. - * @param input_path input path to resample - * @param resampled_interval resampling interval point - * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and - * y. Otherwise, it uses spline interpolation - * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. - * Otherwise, it uses spline interpolation - * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample - * longitudinal and lateral velocity. Otherwise, it uses linear interpolation - * @param resample_input_path_stop_point If true, resample closest stop point in input path - * @return resampled path - */ -autoware_planning_msgs::msg::Path resamplePath( - const autoware_planning_msgs::msg::Path & input_path, const double resample_interval, - const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, - const bool use_zero_order_hold_for_twist = true, - const bool resample_input_path_stop_point = true); - -/** - * @brief A resampling function for a trajectory. Note that in a default setting, position xy are - * resampled by spline interpolation, position z are resampled by linear interpolation, twist - * informaiton(velocity and acceleration) are resampled by zero_order_hold, and heading rate - * is resampled by linear interpolation. The rest of the category is resampled by linear - * interpolation. Orientation of the resampled path are calculated by a forward difference - * method based on the interpolated position x and y. - * @param input_trajectory input trajectory to resample - * @param resampled_arclength arclength that contains length of each resampling points from initial - * point - * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and - * y. Otherwise, it uses spline interpolation - * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. - * Otherwise, it uses spline interpolation - * @param use_zero_order_hold_for_twist If true, it uses zero_order_hold to resample - * longitudinal, lateral velocity and acceleration. Otherwise, it uses linear interpolation - * @return resampled trajectory - */ -autoware_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, - const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true); - -/** - * @brief A resampling function for a trajectory. This function resamples closest stop point, - * terminal point and points by resample interval. Note that in a default setting, position - * xy are resampled by spline interpolation, position z are resampled by linear interpolation, twist - * informaiton(velocity and acceleration) are resampled by zero_order_hold, and heading rate - * is resampled by linear interpolation. The rest of the category is resampled by linear - * interpolation. Orientation of the resampled path are calculated by a forward difference - * method based on the interpolated position x and y. - * @param input_trajectory input trajectory to resample - * @param resampled_interval resampling interval - * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and - * y. Otherwise, it uses spline interpolation - * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. - * Otherwise, it uses spline interpolation - * @param use_zero_order_hold_for_twist If true, it uses zero_order_hold to resample - * longitudinal, lateral velocity and acceleration. Otherwise, it uses linear interpolation - * @param resample_input_trajectory_stop_point If true, resample closest stop point in input - * trajectory - * @return resampled trajectory - */ -autoware_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double resample_interval, - const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, - const bool use_zero_order_hold_for_twist = true, - const bool resample_input_trajectory_stop_point = true); -} // namespace autoware::motion_utils - -#endif // AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ 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 deleted file mode 100644 index e15375d73b80c..0000000000000 --- a/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp +++ /dev/null @@ -1,127 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ -#define AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ - -#include "autoware_utils/system/backtrace.hpp" - -#include -#include -#include - -#include - -namespace resample_utils -{ -constexpr double close_s_threshold = 1e-6; - -static inline rclcpp::Logger get_logger() -{ - constexpr const char * logger{"autoware_motion_utils.resample_utils"}; - return rclcpp::get_logger(logger); -} - -template -bool validate_size(const T & points) -{ - return points.size() >= 2; -} - -template -bool validate_resampling_range(const T & points, const std::vector & resampling_intervals) -{ - const double points_length = autoware::motion_utils::calcArcLength(points); - return points_length >= resampling_intervals.back(); -} - -template -bool validate_points_duplication(const T & points) -{ - for (size_t i = 0; i < points.size() - 1; ++i) { - 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; - } - } - - return true; -} - -template -bool validate_arguments(const T & input_points, const std::vector & resampling_intervals) -{ - // 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_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_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_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_utils::print_backtrace(); - return false; - } - - return true; -} - -template -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_utils::print_backtrace(); - return false; - } - - // check resampling interval - if (resampling_interval < autoware::motion_utils::overlap_threshold) { - RCLCPP_DEBUG( - get_logger(), "invalid argument: resampling interval is less than %f", - autoware::motion_utils::overlap_threshold); - 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_utils::print_backtrace(); - return false; - } - - return true; -} -} // namespace resample_utils - -#endif // AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp deleted file mode 100644 index a00b1d274c809..0000000000000 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp +++ /dev/null @@ -1,121 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ -#define AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ - -#include "autoware_internal_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" -#include "autoware_planning_msgs/msg/detail/path__struct.hpp" -#include "autoware_planning_msgs/msg/detail/trajectory__struct.hpp" -#include "autoware_planning_msgs/msg/detail/trajectory_point__struct.hpp" -#include "std_msgs/msg/header.hpp" - -#include - -namespace autoware::motion_utils -{ -using TrajectoryPoints = std::vector; - -/** - * @brief Convert std::vector to - * autoware_planning_msgs::msg::Trajectory. This function is temporarily added for porting to - * autoware_msgs. We should consider whether to remove this function after the porting is done. - * @attention This function just clips - * std::vector up to the capacity of Trajectory. - * Therefore, the error handling out of this function is necessary if the size of the input greater - * than the capacity. - * @todo Decide how to handle the situation that we need to use the trajectory with the size of - * points larger than the capacity. (Tier IV) - */ -autoware_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory, - const std_msgs::msg::Header & header = std_msgs::msg::Header{}); - -/** - * @brief Convert autoware_planning_msgs::msg::Trajectory to - * std::vector. - */ -std::vector convertToTrajectoryPointArray( - const autoware_planning_msgs::msg::Trajectory & trajectory); - -template -autoware_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input) -{ - static_assert(sizeof(T) == 0, "Only specializations of convertToPath can be used."); - throw std::logic_error("Only specializations of convertToPath can be used."); -} - -template <> -inline autoware_planning_msgs::msg::Path convertToPath( - const autoware_internal_planning_msgs::msg::PathWithLaneId & input) -{ - autoware_planning_msgs::msg::Path output{}; - output.header = input.header; - output.left_bound = input.left_bound; - output.right_bound = input.right_bound; - output.points.resize(input.points.size()); - for (size_t i = 0; i < input.points.size(); ++i) { - output.points.at(i) = input.points.at(i).point; - } - return output; -} - -template -TrajectoryPoints convertToTrajectoryPoints([[maybe_unused]] const T & input) -{ - static_assert(sizeof(T) == 0, "Only specializations of convertToTrajectoryPoints can be used."); - throw std::logic_error("Only specializations of convertToTrajectoryPoints can be used."); -} - -template <> -inline TrajectoryPoints convertToTrajectoryPoints( - const autoware_internal_planning_msgs::msg::PathWithLaneId & input) -{ - TrajectoryPoints output{}; - for (const auto & p : input.points) { - autoware_planning_msgs::msg::TrajectoryPoint tp; - tp.pose = p.point.pose; - tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps; - // since path point doesn't have acc for now - tp.acceleration_mps2 = 0; - output.emplace_back(tp); - } - return output; -} - -template -autoware_internal_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( - [[maybe_unused]] const T & input) -{ - static_assert(sizeof(T) == 0, "Only specializations of convertToPathWithLaneId can be used."); - throw std::logic_error("Only specializations of convertToPathWithLaneId can be used."); -} - -template <> -inline autoware_internal_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( - const TrajectoryPoints & input) -{ - autoware_internal_planning_msgs::msg::PathWithLaneId output{}; - for (const auto & p : input) { - autoware_internal_planning_msgs::msg::PathPointWithLaneId pp; - pp.point.pose = p.pose; - pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; - output.points.emplace_back(pp); - } - return output; -} - -} // namespace autoware::motion_utils - -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ 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 deleted file mode 100644 index 920235d5ceafb..0000000000000 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp +++ /dev/null @@ -1,96 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ -#define AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_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" - -#include - -#include -#include - -namespace autoware::motion_utils -{ -/** - * @brief An interpolation function that finds the closest interpolated point on the trajectory from - * the given pose - * @param trajectory input trajectory - * @param target_pose target_pose - * @param use_zero_order_for_twist flag to decide wether to use zero order hold interpolation for - * twist information - * @return resampled path(poses) - */ -autoware_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint( - const autoware_planning_msgs::msg::Trajectory & trajectory, - const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()); - -/** - * @brief An interpolation function that finds the closest interpolated point on the path from - * the given pose - * @param path input path - * @param target_pose target_pose - * @param use_zero_order_for_twist flag to decide wether to use zero order hold interpolation for - * twist information - * @return resampled path(poses) - */ -autoware_internal_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint( - const autoware_internal_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()); - -/** - * @brief An interpolation function that finds the closest interpolated point on the path that is a - * certain length away from the given pose - * @param points input path - * @param target_length length from the front point of the path - * @return resampled pose - */ -template -geometry_msgs::msg::Pose calcInterpolatedPose(const T & points, const double target_length) -{ - if (points.empty()) { - geometry_msgs::msg::Pose interpolated_pose; - return interpolated_pose; - } - - if (points.size() < 2 || target_length < 0.0) { - 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_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_utils::calc_interpolated_pose(curr_pose, next_pose, ratio); - } - accumulated_length += length; - } - - return autoware_utils::get_pose(points.back()); -} - -} // namespace autoware::motion_utils - -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_shift.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_shift.hpp deleted file mode 100644 index f4602ffff83e4..0000000000000 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_shift.hpp +++ /dev/null @@ -1,71 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_SHIFT_HPP_ -#define AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_SHIFT_HPP_ - -namespace autoware::motion_utils -{ -/** - * @brief Calculates the velocity required for shifting - * @param lateral lateral distance - * @param jerk lateral jerk - * @param longitudinal_distance longitudinal distance - * @return velocity - */ -double calc_feasible_velocity_from_jerk( - const double lateral, const double jerk, const double longitudinal_distance); - -/** - * @brief Calculates the lateral distance required for shifting - * @param longitudinal longitudinal distance - * @param jerk lateral jerk - * @param velocity velocity - * @return lateral distance - */ -double calc_lateral_dist_from_jerk( - const double longitudinal, const double jerk, const double velocity); - -/** - * @brief Calculates the lateral distance required for shifting - * @param lateral lateral distance - * @param jerk lateral jerk - * @param velocity velocity - * @return longitudinal distance - */ -double calc_longitudinal_dist_from_jerk( - const double lateral, const double jerk, const double velocity); - -/** - * @brief Calculates the total time required for shifting - * @param lateral lateral distance - * @param jerk lateral jerk - * @param acc lateral acceleration - * @return time - */ -double calc_shift_time_from_jerk(const double lateral, const double jerk, const double acc); - -/** - * @brief Calculates the required jerk from lateral/longitudinal distance - * @param lateral lateral distance - * @param longitudinal longitudinal distance - * @param velocity velocity - * @return jerk - */ -double calc_jerk_from_lat_lon_distance( - const double lateral, const double longitudinal, const double velocity); - -} // namespace autoware::motion_utils - -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_SHIFT_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp deleted file mode 100644 index d23c0851537e7..0000000000000 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ -#define AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ - -#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp" -#include - -#include -#include -namespace autoware::motion_utils -{ -std::optional> getPathIndexRangeWithLaneId( - const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); - -size_t findNearestIndexFromLaneId( - const autoware_internal_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id); - -size_t findNearestSegmentIndexFromLaneId( - const autoware_internal_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id); - -// @brief Calculates the path to be followed by the rear wheel center in order to make the vehicle -// center follow the input path -// @param [in] path with position to be followed by the center of the vehicle -// @param [out] PathWithLaneId to be followed by the rear wheel center follow to make the vehicle -// center follow the input path NOTE: rear_to_cog is supposed to be positive -autoware_internal_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( - const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, - const bool enable_last_point_compensation = true); -} // namespace autoware::motion_utils - -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ 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 deleted file mode 100644 index 38b51e1cf1114..0000000000000 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp +++ /dev/null @@ -1,2526 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ -#define AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ - -#include "autoware_utils/geometry/geometry.hpp" -#include "autoware_utils/geometry/pose_deviation.hpp" -#include "autoware_utils/math/constants.hpp" -#include "autoware_utils/system/backtrace.hpp" - -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware::motion_utils -{ -static inline rclcpp::Logger get_logger() -{ - constexpr const char * logger{"autoware_motion_utils.trajectory"}; - return rclcpp::get_logger(logger); -} - -/** - * @brief validate if points container is empty or not - * @param points points of trajectory, path, ... - */ -template -void validateNonEmpty(const T & points) -{ - if (points.empty()) { - autoware_utils::print_backtrace(); - throw std::invalid_argument("[autoware_motion_utils] validateNonEmpty(): Points is empty."); - } -} - -extern template void validateNonEmpty>( - const std::vector &); -extern template void -validateNonEmpty>( - const std::vector &); -extern template void validateNonEmpty>( - const std::vector &); - -/** - * @brief validate a point is in a non-sharp angle between two points or not - * @param point1 front point - * @param point2 point to be validated - * @param point3 back point - */ -template -void validateNonSharpAngle( - const T & point1, const T & point2, const T & point3, - const double angle_threshold = autoware_utils::pi / 4) -{ - 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_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_utils::print_backtrace(); - throw std::invalid_argument( - "[autoware_motion_utils] validateNonSharpAngle(): Too sharp angle."); - } -} - -/** - * @brief checks whether a path of trajectory has forward driving direction - * @param points points of trajectory, path, ... - * @return (forward / backward) driving (true / false) - */ -template -std::optional isDrivingForward(const T & points) -{ - if (points.size() < 2) { - return std::nullopt; - } - - // check the first point direction - const auto & first_pose = autoware_utils::get_pose(points.at(0)); - const auto & second_pose = autoware_utils::get_pose(points.at(1)); - - return autoware_utils::is_driving_forward(first_pose, second_pose); -} - -extern template std::optional -isDrivingForward>( - const std::vector &); -extern template std::optional -isDrivingForward>( - const std::vector &); -extern template std::optional -isDrivingForward>( - const std::vector &); - -/** - * @brief checks whether a path of trajectory has forward driving direction using its longitudinal - * velocity - * @param points_with_twist points of trajectory, path, ... (with velocity) - * @return (forward / backward) driving (true, false, none "if velocity is zero") - */ -template -std::optional isDrivingForwardWithTwist(const T & points_with_twist) -{ - if (points_with_twist.empty()) { - return std::nullopt; - } - if (points_with_twist.size() == 1) { - if (0.0 < autoware_utils::get_longitudinal_velocity(points_with_twist.front())) { - return true; - } - if (0.0 > autoware_utils::get_longitudinal_velocity(points_with_twist.front())) { - return false; - } - return std::nullopt; - } - - return isDrivingForward(points_with_twist); -} - -extern template std::optional -isDrivingForwardWithTwist>( - const std::vector &); -extern template std::optional -isDrivingForwardWithTwist>( - const std::vector &); -extern template std::optional -isDrivingForwardWithTwist>( - const std::vector &); - -/** - * @brief remove overlapping points through points container. - * Overlapping is determined by calculating the distance between 2 consecutive points. - * If the distance between them is less than a threshold, they will be considered overlapping. - * @param points points of trajectory, path, ... - * @param start_idx index to start the overlap remove calculation from through the points - * container. Indices before that index will be considered non-overlapping. Default = 0 - * @return points container without overlapping points - */ -template -T removeOverlapPoints(const T & points, const size_t start_idx = 0) -{ - if (points.size() < start_idx + 1) { - return points; - } - - T dst; - dst.reserve(points.size()); - - for (size_t i = 0; i <= start_idx; ++i) { - dst.push_back(points.at(i)); - } - - constexpr double eps = 1.0E-08; - for (size_t i = start_idx + 1; i < points.size(); ++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; - } - dst.push_back(points.at(i)); - } - - return dst; -} - -extern template std::vector -removeOverlapPoints>( - const std::vector & points, const size_t start_idx = 0); -extern template std::vector -removeOverlapPoints>( - const std::vector & points, - const size_t start_idx = 0); -extern template std::vector -removeOverlapPoints>( - const std::vector & points, - const size_t start_idx = 0); - -/** - * @brief search through points container from specified start and end indices about first matching - * index of a zero longitudinal velocity point. - * @param points_with_twist points of trajectory, path, ... (with velocity) - * @param src_idx start index of the search - * @param dst_idx end index of the search - * @return first matching index of a zero velocity point inside the points container. - */ -template -std::optional searchZeroVelocityIndex( - const T & points_with_twist, const size_t src_idx, const size_t dst_idx) -{ - try { - validateNonEmpty(points_with_twist); - } catch (const std::exception & e) { - RCLCPP_DEBUG(get_logger(), "%s", e.what()); - return {}; - } - - constexpr double epsilon = 1e-3; - for (size_t i = src_idx; i < dst_idx; ++i) { - if (std::fabs(points_with_twist.at(i).longitudinal_velocity_mps) < epsilon) { - return i; - } - } - - return {}; -} - -extern template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist, - const size_t src_idx, const size_t dst_idx); - -/** - * @brief search through points container from specified start index till end of points container - * about first matching index of a zero longitudinal velocity point. - * @param points_with_twist points of trajectory, path, ... (with velocity) - * @param src_idx start index of the search - * @return first matching index of a zero velocity point inside the points container. - */ -template -std::optional searchZeroVelocityIndex(const T & points_with_twist, const size_t src_idx) -{ - try { - validateNonEmpty(points_with_twist); - } catch (const std::exception & e) { - RCLCPP_DEBUG(get_logger(), "%s", e.what()); - return {}; - } - - return searchZeroVelocityIndex(points_with_twist, src_idx, points_with_twist.size()); -} - -extern template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist, - const size_t src_idx); - -/** - * @brief search through points container from its start to end about first matching index of a zero - * longitudinal velocity point. - * @param points_with_twist points of trajectory, path, ... (with velocity) - * @return first matching index of a zero velocity point inside the points container. - */ -template -std::optional searchZeroVelocityIndex(const T & points_with_twist) -{ - return searchZeroVelocityIndex(points_with_twist, 0, points_with_twist.size()); -} - -extern template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist); - -/** - * @brief find nearest point index through points container for a given point. - * Finding nearest point is determined by looping through the points container, - * and calculating the 2D squared distance between each point in the container and the given point. - * The index of the point with minimum distance and yaw deviation comparing to the given point will - * be returned. - * @param points points of trajectory, path, ... - * @param point given point - * @return index of nearest point - */ -template -size_t findNearestIndex(const T & points, const geometry_msgs::msg::Point & point) -{ - validateNonEmpty(points); - - double min_dist = std::numeric_limits::max(); - size_t min_idx = 0; - - for (size_t i = 0; i < points.size(); ++i) { - const auto dist = autoware_utils::calc_squared_distance2d(points.at(i), point); - if (dist < min_dist) { - min_dist = dist; - min_idx = i; - } - } - return min_idx; -} - -extern template size_t findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); -extern template size_t -findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); -extern template size_t findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); - -/** - * @brief find nearest point index through points container for a given pose. - * Finding nearest point is determined by looping through the points container, - * and finding the nearest point to the given pose in terms of squared 2D distance and yaw - * deviation. The index of the point with minimum distance and yaw deviation comparing to the given - * pose will be returned. - * @param points points of trajectory, path, ... - * @param pose given pose - * @param max_dist max distance used to get squared distance for finding the nearest point to given - * pose - * @param max_yaw max yaw used for finding nearest point to given pose - * @return index of nearest point (index or none if not found) - */ -template -std::optional findNearestIndex( - const T & points, const geometry_msgs::msg::Pose & pose, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - RCLCPP_DEBUG(get_logger(), "%s", e.what()); - return {}; - } - - const double max_squared_dist = max_dist * max_dist; - - double min_squared_dist = std::numeric_limits::max(); - bool is_nearest_found = false; - size_t min_idx = 0; - - for (size_t i = 0; i < points.size(); ++i) { - 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_utils::calc_yaw_deviation(autoware_utils::get_pose(points.at(i)), pose); - if (std::fabs(yaw) > max_yaw) { - continue; - } - - min_squared_dist = squared_dist; - min_idx = i; - is_nearest_found = true; - } - - if (is_nearest_found) { - return min_idx; - } - return std::nullopt; -} - -extern template std::optional -findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()); -extern template std::optional -findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()); -extern template std::optional -findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()); - -/** - * @brief calculate longitudinal offset (length along trajectory from seg_idx point to nearest point - * to p_target on trajectory). If seg_idx point is after that nearest point, length is negative. - * Segment is straight path between two continuous points of trajectory. - * @param points points of trajectory, path, ... - * @param seg_idx segment index of point at beginning of length - * @param p_target target point at end of length - * @param throw_exception flag to enable/disable exception throwing - * @return signed length - */ -template -double calcLongitudinalOffsetToSegment( - const T & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - const bool throw_exception = false) -{ - if (seg_idx >= points.size() - 1) { - const std::string error_message( - "[autoware_motion_utils] " + std::string(__func__) + - ": Failed to calculate longitudinal offset because the given segment index is out of the " - "points size."); - autoware_utils::print_backtrace(); - if (throw_exception) { - throw std::out_of_range(error_message); - } - RCLCPP_DEBUG( - get_logger(), - "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", - error_message.c_str()); - return std::nan(""); - } - - const auto overlap_removed_points = removeOverlapPoints(points, seg_idx); - - if (throw_exception) { - validateNonEmpty(overlap_removed_points); - } else { - try { - validateNonEmpty(overlap_removed_points); - } catch (const std::exception & e) { - RCLCPP_DEBUG(get_logger(), "%s", e.what()); - return std::nan(""); - } - } - - if (seg_idx >= overlap_removed_points.size() - 1) { - const std::string error_message( - "[autoware_motion_utils] " + std::string(__func__) + - ": Longitudinal offset calculation is not supported for the same points."); - autoware_utils::print_backtrace(); - if (throw_exception) { - throw std::runtime_error(error_message); - } - RCLCPP_DEBUG( - get_logger(), - "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", - error_message.c_str()); - return std::nan(""); - } - - 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}; - - return segment_vec.dot(target_vec) / segment_vec.norm(); -} - -extern template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, const size_t seg_idx, - const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); -extern template double calcLongitudinalOffsetToSegment< - std::vector>( - const std::vector & points, - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - const bool throw_exception = false); -extern template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, const size_t seg_idx, - const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); - -/** - * @brief find nearest segment index to point. - * Segment is straight path between two continuous points of trajectory. - * When point is on a trajectory point whose index is nearest_idx, return nearest_idx - 1 - * @param points points of trajectory, path, ... - * @param point point to which to find nearest segment index - * @return nearest index - */ -template -size_t findNearestSegmentIndex(const T & points, const geometry_msgs::msg::Point & point) -{ - const size_t nearest_idx = findNearestIndex(points, point); - - if (nearest_idx == 0) { - return 0; - } - if (nearest_idx == points.size() - 1) { - return points.size() - 2; - } - - const double signed_length = calcLongitudinalOffsetToSegment(points, nearest_idx, point); - - if (signed_length <= 0) { - return nearest_idx - 1; - } - - return nearest_idx; -} - -extern template size_t findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); -extern template size_t -findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); -extern template size_t -findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); - -/** - * @brief find nearest segment index to pose - * Segment is straight path between two continuous points of trajectory. - * When pose is on a trajectory point whose index is nearest_idx, return nearest_idx - 1 - * @param points points of trajectory, path, .. - * @param pose pose to which to find nearest segment index - * @param max_dist max distance used for finding the nearest index to given pose - * @param max_yaw max yaw used for finding nearest index to given pose - * @return nearest index - */ -template -std::optional findNearestSegmentIndex( - const T & points, const geometry_msgs::msg::Pose & pose, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()) -{ - const auto nearest_idx = findNearestIndex(points, pose, max_dist, max_yaw); - - if (!nearest_idx) { - return std::nullopt; - } - - if (*nearest_idx == 0) { - return 0; - } - if (*nearest_idx == points.size() - 1) { - return points.size() - 2; - } - - const double signed_length = calcLongitudinalOffsetToSegment(points, *nearest_idx, pose.position); - - if (signed_length <= 0) { - return *nearest_idx - 1; - } - - return *nearest_idx; -} - -extern template std::optional -findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()); -extern template std::optional -findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()); -extern template std::optional -findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()); - -/** - * @brief calculate lateral offset from p_target (length from p_target to trajectory) using given - * segment index. Segment is straight path between two continuous points of trajectory. - * @param points points of trajectory, path, ... - * @param p_target target point - * @param seg_idx segment index of point at beginning of length - * @param throw_exception flag to enable/disable exception throwing - * @return length (unsigned) - */ -template -double calcLateralOffset( - const T & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, - const bool throw_exception = false) -{ - const auto overlap_removed_points = removeOverlapPoints(points, 0); - - if (throw_exception) { - validateNonEmpty(overlap_removed_points); - } else { - try { - validateNonEmpty(overlap_removed_points); - } catch (const std::exception & e) { - RCLCPP_DEBUG( - get_logger(), - "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", - e.what()); - return std::nan(""); - } - } - - if (overlap_removed_points.size() == 1) { - const std::string error_message( - "[autoware_motion_utils] " + std::string(__func__) + - ": Lateral offset calculation is not supported for the same points."); - autoware_utils::print_backtrace(); - if (throw_exception) { - throw std::runtime_error(error_message); - } - RCLCPP_DEBUG( - get_logger(), - "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", - error_message.c_str()); - return std::nan(""); - } - - const auto p_indices = overlap_removed_points.size() - 2; - 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_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}; - - const Eigen::Vector3d cross_vec = segment_vec.cross(target_vec); - return cross_vec(2) / segment_vec.norm(); -} - -extern template double calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const size_t seg_idx, - const bool throw_exception = false); -extern template double -calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const size_t seg_idx, - const bool throw_exception = false); -extern template double calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const size_t seg_idx, - const bool throw_exception = false); - -/** - * @brief calculate lateral offset from p_target (length from p_target to trajectory). - * The function gets the nearest segment index between the points of trajectory and the given target - * point, then uses that segment index to calculate lateral offset. Segment is straight path between - * two continuous points of trajectory. - * @param points points of trajectory, path, ... - * @param p_target target point - * @param throw_exception flag to enable/disable exception throwing - * @return length (unsigned) - */ -template -double calcLateralOffset( - const T & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false) -{ - const auto overlap_removed_points = removeOverlapPoints(points, 0); - - if (throw_exception) { - validateNonEmpty(overlap_removed_points); - } else { - try { - validateNonEmpty(overlap_removed_points); - } catch (const std::exception & e) { - RCLCPP_DEBUG( - get_logger(), - "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", - e.what()); - return std::nan(""); - } - } - - if (overlap_removed_points.size() == 1) { - const std::string error_message( - "[autoware_motion_utils] " + std::string(__func__) + - ": Lateral offset calculation is not supported for the same points."); - autoware_utils::print_backtrace(); - if (throw_exception) { - throw std::runtime_error(error_message); - } - RCLCPP_DEBUG( - get_logger(), - "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", - error_message.c_str()); - return std::nan(""); - } - - const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, p_target); - return calcLateralOffset(points, p_target, seg_idx, throw_exception); -} - -extern template double calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); -extern template double -calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); -extern template double calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); - -/** - * @brief calculate length of 2D distance between two points, specified by start and end points - * indicies through points container. - * @param points points of trajectory, path, ... - * @param src_idx index of start point - * @param dst_idx index of end point - * @return length of distance between two points. - * Length is positive if dst_idx is greater that src_idx (i.e. after it in trajectory, path, ...) - * and negative otherwise. - */ -template -double calcSignedArcLength(const T & points, const size_t src_idx, const size_t dst_idx) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - RCLCPP_DEBUG(get_logger(), "%s", e.what()); - return 0.0; - } - - if (src_idx > dst_idx) { - return -calcSignedArcLength(points, dst_idx, src_idx); - } - - double dist_sum = 0.0; - for (size_t i = src_idx; i < dst_idx; ++i) { - dist_sum += autoware_utils::calc_distance2d(points.at(i), points.at(i + 1)); - } - return dist_sum; -} - -extern template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const size_t dst_idx); -extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); -extern template double -calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const size_t dst_idx); - -/** - * @brief Computes the partial sums of the elements in the sub-ranges of the range [src_idx, - * dst_idx) and return these sum as vector - * @param points points of trajectory, path, ... - * @param src_idx index of start point - * @param dst_idx index of end point - * @return partial sums container - */ -template -std::vector calcSignedArcLengthPartialSum( - const T & points, const size_t src_idx, const size_t dst_idx) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - RCLCPP_DEBUG(get_logger(), "%s", e.what()); - return {}; - } - - if (src_idx + 1 > dst_idx) { - auto copied = points; - std::reverse(copied.begin(), copied.end()); - return calcSignedArcLengthPartialSum(points, dst_idx, src_idx); - } - - std::vector partial_dist; - partial_dist.reserve(dst_idx - src_idx); - - 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_utils::calc_distance2d(points.at(i), points.at(i + 1)); - partial_dist.push_back(dist_sum); - } - return partial_dist; -} - -extern template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, const size_t src_idx, - const size_t dst_idx); -extern template std::vector calcSignedArcLengthPartialSum< - std::vector>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); -extern template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, const size_t src_idx, - const size_t dst_idx); - -/** - * @brief calculate length of 2D distance between two points, specified by start point and end point - * index of points container. - * @param points points of trajectory, path, ... - * @param src_point start point - * @param dst_idx index of end point - * @return length of distance between two points. - * Length is positive if destination point associated to dst_idx is greater that src_idx (i.e. after - * it in trajectory, path, ...) and negative otherwise. - */ -template -double calcSignedArcLength( - const T & points, const geometry_msgs::msg::Point & src_point, const size_t dst_idx) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - RCLCPP_DEBUG(get_logger(), "%s", e.what()); - return 0.0; - } - - const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); - - const double signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_idx); - const double signed_length_src_offset = - calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); - - return signed_length_on_traj - signed_length_src_offset; -} - -extern template double calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t dst_idx); -extern template double -calcSignedArcLength>( - const std::vector &, - const geometry_msgs::msg::Point & src_point, const size_t dst_idx); -extern template double -calcSignedArcLength>( - const std::vector &, - const geometry_msgs::msg::Point & src_point, const size_t dst_idx); - -/** - * @brief calculate length of 2D distance between two points, specified by start index of points - * container and end point. - * @param points points of trajectory, path, ... - * @param src_idx index of start point - * @param dst_point end point - * @return length of distance between two points - * Length is positive if destination point is greater that source point associated to src_idx (i.e. - * after it in trajectory, path, ...) and negative otherwise. - */ -template -double calcSignedArcLength( - const T & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - RCLCPP_DEBUG(get_logger(), "%s", e.what()); - return 0.0; - } - - return -calcSignedArcLength(points, dst_point, src_idx); -} - -extern template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const geometry_msgs::msg::Point & dst_point); -extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point); -extern template double -calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const geometry_msgs::msg::Point & dst_point); - -/** - * @brief calculate length of 2D distance between two points, specified by start point and end - * point. - * @param points points of trajectory, path, ... - * @param src_point start point - * @param dst_point end point - * @return length of distance between two points. - * Length is positive if destination point is greater that source point (i.e. after it in - * trajectory, path, ...) and negative otherwise. - * - */ -template -double calcSignedArcLength( - const T & points, const geometry_msgs::msg::Point & src_point, - const geometry_msgs::msg::Point & dst_point) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - RCLCPP_DEBUG(get_logger(), "%s", e.what()); - return 0.0; - } - - const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); - const size_t dst_seg_idx = findNearestSegmentIndex(points, dst_point); - - const double signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_seg_idx); - const double signed_length_src_offset = - calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); - const double signed_length_dst_offset = - calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point); - - return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; -} - -extern template double calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); -extern template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); -extern template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); - -/** - * @brief calculate length of 2D distance for whole points container, from its start to its end. - * @param points points of trajectory, path, ... - * @return length of 2D distance for points container - */ -template -double calcArcLength(const T & points) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - RCLCPP_DEBUG(get_logger(), "%s", e.what()); - return 0.0; - } - - return calcSignedArcLength(points, 0, points.size() - 1); -} - -extern template double calcArcLength>( - const std::vector & points); -extern template double -calcArcLength>( - const std::vector & points); -extern template double calcArcLength>( - const std::vector & points); - -/** - * @brief calculate curvature through points container. - * The method used for calculating the curvature is using 3 consecutive points through the points - * container. Then the curvature is the reciprocal of the radius of the circle that passes through - * these three points. - * @details more details here : https://en.wikipedia.org/wiki/Menger_curvature - * @param points points of trajectory, path, ... - * @return calculated curvature container through points container - */ -template -std::vector calcCurvature(const T & points) -{ - std::vector curvature_vec(points.size(), 0.0); - if (points.size() < 3) { - return curvature_vec; - } - - for (size_t i = 1; i < points.size() - 1; ++i) { - 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); - - return curvature_vec; -} - -extern template std::vector -calcCurvature>( - const std::vector & points); -extern template std::vector -calcCurvature>( - const std::vector & points); -extern template std::vector -calcCurvature>( - const std::vector & points); - -/** - * @brief calculate curvature through points container and length of 2d distance for segment used - * for curvature calculation. The method used for calculating the curvature is using 3 consecutive - * points through the points container. Then the curvature is the reciprocal of the radius of the - * circle that passes through these three points. Then length of 2D distance of these points is - * calculated - * @param points points of trajectory, path, ... - * @return Container of pairs, calculated curvature and length of 2D distance for segment used for - * curvature calculation - */ -template -std::vector>> calcCurvatureAndSegmentLength( - const T & points) -{ - // segment length is pair of segment length between {p1, p2} and {p2, p3} - std::vector>> curvature_and_segment_length_vec; - 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_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_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_utils::calc_distance2d(p2, p3)}; - - curvature_and_segment_length_vec.emplace_back(curvature, arc_length); - } - - // set to the last point - curvature_and_segment_length_vec.emplace_back( - 0.0, std::make_pair(curvature_and_segment_length_vec.back().second.second, 0.0)); - - return curvature_and_segment_length_vec; -} - -extern template std::vector>> -calcCurvatureAndSegmentLength>( - const std::vector & points); -extern template std::vector>> -calcCurvatureAndSegmentLength< - std::vector>( - const std::vector & points); -extern template std::vector>> -calcCurvatureAndSegmentLength>( - const std::vector & points); - -/** - * @brief calculate length of 2D distance between given start point index in points container and - * first point in container with zero longitudinal velocity - * @param points_with_twist points of trajectory, path, ... (with velocity) - * @return Length of 2D distance between start point index in points container and first point in - * container with zero longitudinal velocity - */ -template -std::optional calcDistanceToForwardStopPoint( - const T & points_with_twist, const size_t src_idx = 0) -{ - try { - validateNonEmpty(points_with_twist); - } catch (const std::exception & e) { - RCLCPP_DEBUG(get_logger(), "%s", e.what()); - return {}; - } - - const auto closest_stop_idx = - searchZeroVelocityIndex(points_with_twist, src_idx, points_with_twist.size()); - if (!closest_stop_idx) { - return std::nullopt; - } - - return std::max(0.0, calcSignedArcLength(points_with_twist, src_idx, *closest_stop_idx)); -} - -extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, - const size_t src_idx = 0); - -/** - * @brief calculate the point offset from source point index along the trajectory (or path) (points - * container) - * @param points points of trajectory, path, ... - * @param src_idx index of source point - * @param offset length of offset from source point - * @param throw_exception flag to enable/disable exception throwing - * @return offset point - */ -template -std::optional calcLongitudinalOffsetPoint( - const T & points, const size_t src_idx, const double offset, const bool throw_exception = false) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - RCLCPP_DEBUG(get_logger(), "%s", e.what()); - return {}; - } - - if (points.size() - 1 < src_idx) { - const std::string error_message( - "[autoware_motion_utils] " + std::string(__func__) + - " error: The given source index is out of the points size. Failed to calculate longitudinal " - "offset."); - autoware_utils::print_backtrace(); - if (throw_exception) { - throw std::out_of_range(error_message); - } - RCLCPP_DEBUG( - get_logger(), - "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", - error_message.c_str()); - return {}; - } - - if (points.size() == 1) { - return {}; - } - - if (src_idx + 1 == points.size() && offset == 0.0) { - return autoware_utils::get_point(points.at(src_idx)); - } - - if (offset < 0.0) { - auto reverse_points = points; - std::reverse(reverse_points.begin(), reverse_points.end()); - return calcLongitudinalOffsetPoint( - reverse_points, reverse_points.size() - src_idx - 1, -offset); - } - - double dist_sum = 0.0; - - for (size_t i = src_idx; i < points.size() - 1; ++i) { - const auto & p_front = points.at(i); - const auto & p_back = points.at(i + 1); - - const auto dist_segment = 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_utils::calc_interpolated_point( - p_back, p_front, std::abs(dist_res / dist_segment)); - } - } - - // not found (out of range) - return {}; -} - -extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, const size_t src_idx, - const double offset, const bool throw_exception = false); -extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const size_t src_idx, const double offset, const bool throw_exception = false); -extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, const size_t src_idx, - const double offset, const bool throw_exception = false); - -/** - * @brief calculate the point offset from source point along the trajectory (or path) (points - * container) - * @param points points of trajectory, path, ... - * @param src_point source point - * @param offset length of offset from source point - * @return offset point - */ -template -std::optional calcLongitudinalOffsetPoint( - const T & points, const geometry_msgs::msg::Point & src_point, const double offset) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - RCLCPP_DEBUG(get_logger(), "Failed to calculate longitudinal offset: %s", e.what()); - return {}; - } - - if (offset < 0.0) { - auto reverse_points = points; - std::reverse(reverse_points.begin(), reverse_points.end()); - return calcLongitudinalOffsetPoint(reverse_points, src_point, -offset); - } - - const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); - const double signed_length_src_offset = - calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); - - return calcLongitudinalOffsetPoint(points, src_seg_idx, offset + signed_length_src_offset); -} - -extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset); -extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset); -extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset); - -/** - * @brief calculate the point offset from source point index along the trajectory (or path) (points - * container) - * @param points points of trajectory, path, ... - * @param src_idx index of source point - * @param offset length of offset from source point - * @param set_orientation_from_position_direction set orientation by spherical interpolation if - * false - * @return offset pose - */ -template -std::optional calcLongitudinalOffsetPose( - const T & points, const size_t src_idx, const double offset, - const bool set_orientation_from_position_direction = true, const bool throw_exception = false) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - RCLCPP_DEBUG(get_logger(), "Failed to calculate longitudinal offset: %s", e.what()); - return {}; - } - - if (points.size() - 1 < src_idx) { - const std::string error_message( - "[autoware_motion_utils] " + std::string(__func__) + - " error: The given source index is out of the points size. Failed to calculate longitudinal " - "offset."); - autoware_utils::print_backtrace(); - if (throw_exception) { - throw std::out_of_range(error_message); - } - RCLCPP_DEBUG(get_logger(), "%s", error_message.c_str()); - return {}; - } - - if (points.size() == 1) { - RCLCPP_DEBUG(get_logger(), "Failed to calculate longitudinal offset: points size is one."); - return {}; - } - - if (src_idx + 1 == points.size() && offset == 0.0) { - return autoware_utils::get_pose(points.at(src_idx)); - } - - if (offset < 0.0) { - auto reverse_points = points; - std::reverse(reverse_points.begin(), reverse_points.end()); - - double dist_sum = 0.0; - - for (size_t i = reverse_points.size() - src_idx - 1; i < reverse_points.size() - 1; ++i) { - const auto & p_front = reverse_points.at(i); - const auto & p_back = reverse_points.at(i + 1); - - const auto dist_segment = 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_utils::calc_interpolated_pose( - p_back, p_front, std::abs(dist_res / dist_segment), - set_orientation_from_position_direction); - } - } - } else { - double dist_sum = 0.0; - - for (size_t i = src_idx; i < points.size() - 1; ++i) { - const auto & p_front = points.at(i); - const auto & p_back = points.at(i + 1); - - const auto dist_segment = 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_utils::calc_interpolated_pose( - p_front, p_back, 1.0 - std::abs(dist_res / dist_segment), - set_orientation_from_position_direction); - } - } - } - - // not found (out of range) - return {}; -} - -extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, const size_t src_idx, - const double offset, const bool set_orientation_from_position_direction = true, - const bool throw_exception = false); -extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const size_t src_idx, const double offset, - const bool set_orientation_from_position_direction = true, const bool throw_exception = false); -extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, const size_t src_idx, - const double offset, const bool set_orientation_from_position_direction = true, - const bool throw_exception = false); - -/** - * @brief calculate the point offset from source point along the trajectory (or path) (points - * container) - * @param points points of trajectory, path, ... - * @param src_point source point - * @param offset length of offset from source point - * @param set_orientation_from_position_direction set orientation by spherical interpolation if - * false - * @return offset pose - */ -template -std::optional calcLongitudinalOffsetPose( - const T & points, const geometry_msgs::msg::Point & src_point, const double offset, - const bool set_orientation_from_position_direction = true) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - RCLCPP_DEBUG(get_logger(), "%s", e.what()); - return {}; - } - - const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); - const double signed_length_src_offset = - calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); - - return calcLongitudinalOffsetPose( - points, src_seg_idx, offset + signed_length_src_offset, - set_orientation_from_position_direction); -} - -extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset, - const bool set_orientation_from_position_direction = true); -extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset, - const bool set_orientation_from_position_direction = true); -extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset, - const bool set_orientation_from_position_direction = true); - -/** - * @brief insert a point in points container (trajectory, path, ...) using segment id - * @param seg_idx segment index of point at beginning of length - * @param p_target point to be inserted - * @param points output points of trajectory, path, ... - * @param overlap_threshold distance threshold, used to check if the inserted point is between start - * and end of nominated segment to be added in. - * @return index of segment id, where point is inserted - */ -template -std::optional insertTargetPoint( - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, T & points, - const double overlap_threshold = 1e-3) -{ - try { - validateNonEmpty(points); - } catch (const std::exception & e) { - RCLCPP_DEBUG(get_logger(), "%s", e.what()); - return {}; - } - - // invalid segment index - if (seg_idx + 1 >= points.size()) { - return {}; - } - - 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); - } catch (const std::exception & e) { - RCLCPP_DEBUG(get_logger(), "%s", e.what()); - return {}; - } - - const auto overlap_with_front = - autoware_utils::calc_distance2d(p_target, p_front) < overlap_threshold; - const auto overlap_with_back = - autoware_utils::calc_distance2d(p_target, p_back) < overlap_threshold; - - const auto is_driving_forward = isDrivingForward(points); - if (!is_driving_forward) { - return {}; - } - - geometry_msgs::msg::Pose target_pose; - { - const auto p_base = is_driving_forward.value() ? p_back : p_front; - 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_utils::create_quaternion_from_rpy(0.0, pitch, yaw); - } - - auto p_insert = points.at(seg_idx); - 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_utils::calc_elevation_angle(p_base, p_target); - const auto yaw = autoware_utils::calc_azimuth_angle(p_base, p_target); - - 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_utils::set_pose(base_pose, points.at(seg_idx)); - } else { - autoware_utils::set_pose(base_pose, points.at(seg_idx + 1)); - } - points.insert(points.begin() + seg_idx + 1, p_insert); - return seg_idx + 1; - } - - if (overlap_with_back) { - return seg_idx + 1; - } - - return seg_idx; -} - -extern template std::optional -insertTargetPoint>( - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold = 1e-3); -extern template std::optional -insertTargetPoint>( - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold = 1e-3); -extern template std::optional -insertTargetPoint>( - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold = 1e-3); - -/** - * @brief insert a point in points container (trajectory, path, ...) using length of point to be - * inserted - * @param insert_point_length length to insert point from the beginning of the points - * @param p_target point to be inserted - * @param points output points of trajectory, path, ... - * @param overlap_threshold distance threshold, used to check if the inserted point is between start - * and end of nominated segment to be added in. - * @return index of segment id, where point is inserted - */ -template -std::optional insertTargetPoint( - const double insert_point_length, const geometry_msgs::msg::Point & p_target, T & points, - const double overlap_threshold = 1e-3) -{ - validateNonEmpty(points); - - if (insert_point_length < 0.0) { - return std::nullopt; - } - - // Get Nearest segment index - std::optional segment_idx = std::nullopt; - for (size_t i = 1; i < points.size(); ++i) { - // TODO(Mamoru Sobue): find accumulated sum beforehand - const double length = calcSignedArcLength(points, 0, i); - if (insert_point_length <= length) { - segment_idx = i - 1; - break; - } - } - - if (!segment_idx) { - return std::nullopt; - } - - return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); -} - -extern template std::optional -insertTargetPoint>( - const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold = 1e-3); -extern template std::optional -insertTargetPoint>( - const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold = 1e-3); -extern template std::optional -insertTargetPoint>( - const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold = 1e-3); - -/** - * @brief insert a point in points container (trajectory, path, ...) using segment index and length - * of point to be inserted - * @param src_segment_idx source segment index on the trajectory - * @param insert_point_length length to insert point from the beginning of the points - * @param points output points of trajectory, path, ... - * @param overlap_threshold distance threshold, used to check if the inserted point is between start - * and end of nominated segment to be added in. - * @return index of insert point - */ -template -std::optional insertTargetPoint( - const size_t src_segment_idx, const double insert_point_length, T & points, - const double overlap_threshold = 1e-3) -{ - validateNonEmpty(points); - - if (src_segment_idx >= points.size() - 1) { - return std::nullopt; - } - - // Get Nearest segment index - std::optional segment_idx = std::nullopt; - if (0.0 <= insert_point_length) { - for (size_t i = src_segment_idx + 1; i < points.size(); ++i) { - const double length = calcSignedArcLength(points, src_segment_idx, i); - if (insert_point_length <= length) { - segment_idx = i - 1; - break; - } - } - } else { - for (int i = src_segment_idx - 1; 0 <= i; --i) { - const double length = calcSignedArcLength(points, src_segment_idx, i); - if (length <= insert_point_length) { - segment_idx = i; - break; - } - } - } - - if (!segment_idx) { - return std::nullopt; - } - - // Get Target Point - const double segment_length = calcSignedArcLength(points, *segment_idx, *segment_idx + 1); - const double target_length = - insert_point_length - calcSignedArcLength(points, src_segment_idx, *segment_idx); - const double ratio = std::clamp(target_length / segment_length, 0.0, 1.0); - const auto p_target = 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); -} - -extern template std::optional -insertTargetPoint>( - const size_t src_segment_idx, const double insert_point_length, - std::vector & points, - const double overlap_threshold = 1e-3); -extern template std::optional -insertTargetPoint>( - const size_t src_segment_idx, const double insert_point_length, - std::vector & points, - const double overlap_threshold = 1e-3); -extern template std::optional -insertTargetPoint>( - const size_t src_segment_idx, const double insert_point_length, - std::vector & points, - const double overlap_threshold = 1e-3); - -/** - * @brief Insert a target point from a source pose on the trajectory - * @param src_pose source pose on the trajectory - * @param insert_point_length length to insert point from the beginning of the points - * @param points output points of trajectory, path, ... - * @param max_dist max distance, used to search for nearest segment index in points container to the - * given source pose - * @param max_yaw max yaw, used to search for nearest segment index in points container to the given - * source pose - * @param overlap_threshold distance threshold, used to check if the inserted point is between start - * and end of nominated segment to be added in. - * @return index of insert point - */ -template -std::optional insertTargetPoint( - const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, T & points, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3) -{ - validateNonEmpty(points); - - if (insert_point_length < 0.0) { - return std::nullopt; - } - - const auto nearest_segment_idx = findNearestSegmentIndex(points, src_pose, max_dist, max_yaw); - if (!nearest_segment_idx) { - return std::nullopt; - } - - const double offset_length = - calcLongitudinalOffsetToSegment(points, *nearest_segment_idx, src_pose.position); - - return insertTargetPoint( - *nearest_segment_idx, insert_point_length + offset_length, points, overlap_threshold); -} - -extern template std::optional -insertTargetPoint>( - const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); -extern template std::optional -insertTargetPoint>( - const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); -extern template std::optional -insertTargetPoint>( - const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); - -/** - * @brief Insert stop point from the source segment index - * @param src_segment_idx start segment index on the trajectory - * @param distance_to_stop_point distance to stop point from the source index - * @param points_with_twist output points of trajectory, path, ... (with velocity) - * @param overlap_threshold distance threshold, used to check if the inserted point is between start - * and end of nominated segment to be added in. - * @return index of stop point - */ -template -std::optional insertStopPoint( - const size_t src_segment_idx, const double distance_to_stop_point, T & points_with_twist, - const double overlap_threshold = 1e-3) -{ - validateNonEmpty(points_with_twist); - - if (distance_to_stop_point < 0.0 || src_segment_idx >= points_with_twist.size() - 1) { - return std::nullopt; - } - - const auto stop_idx = insertTargetPoint( - src_segment_idx, distance_to_stop_point, points_with_twist, overlap_threshold); - if (!stop_idx) { - return std::nullopt; - } - - for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { - autoware_utils::set_longitudinal_velocity(0.0, points_with_twist.at(i)); - } - - return stop_idx; -} - -extern template std::optional -insertStopPoint>( - const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold = 1e-3); -extern template std::optional -insertStopPoint>( - const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold = 1e-3); -extern template std::optional -insertStopPoint>( - const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold = 1e-3); - -/** - * @brief Insert stop point from the front point of the path - * @param distance_to_stop_point distance to stop point from the front point of the path - * @param points_with_twist output points of trajectory, path, ... (with velocity) - * @param overlap_threshold distance threshold, used to check if the inserted point is between start - * and end of nominated segment to be added in. - * @return index of stop point - */ -template -std::optional insertStopPoint( - const double distance_to_stop_point, T & points_with_twist, const double overlap_threshold = 1e-3) -{ - validateNonEmpty(points_with_twist); - - if (distance_to_stop_point < 0.0) { - return std::nullopt; - } - - double accumulated_length = 0; - for (size_t i = 0; i < points_with_twist.size() - 1; ++i) { - 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); - } - accumulated_length += length; - } - - return std::nullopt; -} - -extern template std::optional -insertStopPoint>( - const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold = 1e-3); -extern template std::optional -insertStopPoint>( - const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold = 1e-3); -extern template std::optional -insertStopPoint>( - const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold = 1e-3); - -/** - * @brief Insert Stop point from the source pose - * @param src_pose source pose - * @param distance_to_stop_point distance to stop point from the src point - * @param points_with_twist output points of trajectory, path, ... (with velocity) - * @param max_dist max distance, used to search for nearest segment index in points container to the - * given source pose - * @param max_yaw max yaw, used to search for nearest segment index in points container to the given - * source pose - * @param overlap_threshold distance threshold, used to check if the inserted point is between start - * and end of nominated segment to be added in. - * @return index of stop point - */ -template -std::optional insertStopPoint( - const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - T & points_with_twist, const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3) -{ - validateNonEmpty(points_with_twist); - - if (distance_to_stop_point < 0.0) { - return std::nullopt; - } - - const auto stop_idx = insertTargetPoint( - src_pose, distance_to_stop_point, points_with_twist, max_dist, max_yaw, overlap_threshold); - - if (!stop_idx) { - return std::nullopt; - } - - for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { - autoware_utils::set_longitudinal_velocity(0.0, points_with_twist.at(i)); - } - - return stop_idx; -} - -extern template std::optional -insertStopPoint>( - const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); -extern template std::optional -insertStopPoint>( - const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); -extern template std::optional -insertStopPoint>( - const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); - -/** - * @brief Insert Stop point that is in the stop segment index - * @param stop_seg_idx segment index of the stop pose - * @param stop_point stop point - * @param points_with_twist output points of trajectory, path, ... (with velocity) - * @param overlap_threshold distance threshold, used to check if the inserted point is between start - * and end of nominated segment to be added in. - * @return index of stop point - */ -template -std::optional insertStopPoint( - const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, T & points_with_twist, - const double overlap_threshold = 1e-3) -{ - const auto insert_idx = autoware::motion_utils::insertTargetPoint( - stop_seg_idx, stop_point, points_with_twist, overlap_threshold); - - if (!insert_idx) { - return std::nullopt; - } - - for (size_t i = insert_idx.value(); i < points_with_twist.size(); ++i) { - autoware_utils::set_longitudinal_velocity(0.0, points_with_twist.at(i)); - } - - return insert_idx; -} - -extern template std::optional -insertStopPoint>( - const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, - std::vector & points_with_twist, - const double overlap_threshold = 1e-3); - -/** - * @brief Insert deceleration point from the source pose - * @param src_point source point - * @param distance_to_decel_point distance to deceleration point from the src point - * @param velocity velocity of stop point - * @param points_with_twist output points of trajectory, path, ... (with velocity) - */ -template -std::optional insertDecelPoint( - const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, - const double velocity, T & points_with_twist) -{ - const auto decel_point = - calcLongitudinalOffsetPoint(points_with_twist, src_point, distance_to_decel_point); - - if (!decel_point) { - return {}; - } - - const auto seg_idx = findNearestSegmentIndex(points_with_twist, decel_point.value()); - const auto insert_idx = insertTargetPoint(seg_idx, decel_point.value(), points_with_twist); - - if (!insert_idx) { - return {}; - } - - for (size_t i = insert_idx.value(); i < points_with_twist.size(); ++i) { - const auto & original_velocity = - 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)); - } - - return insert_idx; -} - -extern template std::optional -insertDecelPoint>( - const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, - const double velocity, - std::vector & points_with_twist); - -/** - * @brief Insert orientation to each point in points container (trajectory, path, ...) - * @param points points of trajectory, path, ... (input / output) - * @param is_driving_forward flag indicating the order of points is forward or backward - */ -template -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_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_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_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_utils::set_orientation( - autoware_utils::get_pose(points.at(1)).orientation, points.at(0)); - } -} - -extern template void insertOrientation>( - std::vector & points, const bool is_driving_forward); -extern template void -insertOrientation>( - std::vector & points, - const bool is_driving_forward); -extern template void insertOrientation>( - std::vector & points, - const bool is_driving_forward); - -/** - * @brief Remove points with invalid orientation differences from a given points container - * (trajectory, path, ...). Check the difference between the angles of two points and the difference - * between the azimuth angle between the two points and the angle of the next point. - * @param points Points of trajectory, path, or other point container (input / output) - * @param max_yaw_diff Maximum acceptable yaw angle difference between two consecutive points in - * radians (default: M_PI_2) - */ -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_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_utils::normalize_radian(yaw1 - yaw2)) || - !autoware_utils::is_driving_forward(p1, p2)) { - points.erase(std::next(itr)); - return; - } else { - itr++; - } - } -} - -/** - * @brief calculate length of 2D distance between two points, specified by start point and end - * point with their segment indices in points container - * @param points points of trajectory, path, ... - * @param src_point start point - * @param src_seg_idx index of start point segment - * @param dst_point end point - * @param dst_seg_idx index of end point segment - * @return length of distance between two points. - * Length is positive if destination point is greater that source point (i.e. after it in - * trajectory, path, ...) and negative otherwise. - */ -template -double calcSignedArcLength( - const T & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx) -{ - validateNonEmpty(points); - - const double signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_seg_idx); - const double signed_length_src_offset = - calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); - const double signed_length_dst_offset = - calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point); - - return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; -} - -extern template double calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -extern template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -extern template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); - -/** - * @brief calculate length of 2D distance between two points, specified by start point and its - * segment index in points container and end point index in points container - * @param points points of trajectory, path, ... - * @param src_point start point - * @param src_seg_idx index of start point segment - * @param dst_idx index of end point - * @return length of distance between two points - * Length is positive if destination point associated to dst_idx is greater that source point (i.e. - * after it in trajectory, path, ...) and negative otherwise. - */ -template -double calcSignedArcLength( - const T & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, - const size_t dst_idx) -{ - validateNonEmpty(points); - - const double signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_idx); - const double signed_length_src_offset = - calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); - - return signed_length_on_traj - signed_length_src_offset; -} - -extern template double calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); -extern template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); -extern template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); - -/** - * @brief calculate length of 2D distance between two points, specified by start point index in - * points container and end point and its segment index in points container - * @param points points of trajectory, path, ... - * @param src_idx index of start point start point - * @param dst_point end point - * @param dst_seg_idx index of end point segment - * @return length of distance between two points - * Length is positive if destination point is greater that source point associated to src_idx (i.e. - * after it in trajectory, path, ...) and negative otherwise. - */ -template -double calcSignedArcLength( - const T & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point, - const size_t dst_seg_idx) -{ - validateNonEmpty(points); - - const double signed_length_on_traj = calcSignedArcLength(points, src_idx, dst_seg_idx); - const double signed_length_dst_offset = - calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point); - - return signed_length_on_traj + signed_length_dst_offset; -} - -extern template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -extern template double -calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); - -/** - * @brief find first nearest point index through points container for a given pose with soft - * distance and yaw constraints. Finding nearest point is determined by looping through the points - * container, and finding the nearest point to the given pose in terms of squared 2D distance and - * yaw deviation. The index of the point with minimum distance and yaw deviation comparing to the - * given pose will be returned. - * @param points points of trajectory, path, ... - * @param pose given pose - * @param dist_threshold distance threshold used for searching for first nearest index to given pose - * @param yaw_threshold yaw threshold used for searching for first nearest index to given pose - * @return index of nearest point (index or none if not found) - */ -template -size_t findFirstNearestIndexWithSoftConstraints( - const T & points, const geometry_msgs::msg::Pose & pose, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()) -{ - validateNonEmpty(points); - - { // with dist and yaw thresholds - const double squared_dist_threshold = dist_threshold * dist_threshold; - double min_squared_dist = std::numeric_limits::max(); - size_t min_idx = 0; - bool is_within_constraints = false; - for (size_t i = 0; i < points.size(); ++i) { - const auto squared_dist = - 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) { - break; - } - continue; - } - - if (min_squared_dist <= squared_dist) { - continue; - } - - min_squared_dist = squared_dist; - min_idx = i; - is_within_constraints = true; - } - - // nearest index is found - if (is_within_constraints) { - return min_idx; - } - } - - { // with dist threshold - const double squared_dist_threshold = dist_threshold * dist_threshold; - double min_squared_dist = std::numeric_limits::max(); - size_t min_idx = 0; - bool is_within_constraints = false; - for (size_t i = 0; i < points.size(); ++i) { - const auto squared_dist = - autoware_utils::calc_squared_distance2d(points.at(i), pose.position); - - if (squared_dist_threshold < squared_dist) { - if (is_within_constraints) { - break; - } - continue; - } - - if (min_squared_dist <= squared_dist) { - continue; - } - - min_squared_dist = squared_dist; - min_idx = i; - is_within_constraints = true; - } - - // nearest index is found - if (is_within_constraints) { - return min_idx; - } - } - - // without any threshold - return findNearestIndex(points, pose.position); -} - -extern template size_t -findFirstNearestIndexWithSoftConstraints>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()); -extern template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()); -extern template size_t -findFirstNearestIndexWithSoftConstraints>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()); - -/** - * @brief find nearest segment index to pose with soft constraints - * Segment is straight path between two continuous points of trajectory - * When pose is on a trajectory point whose index is nearest_idx, return nearest_idx - 1 - * @param points points of trajectory, path, .. - * @param pose pose to which to find nearest segment index - * @param dist_threshold distance threshold used for searching for first nearest index to given pose - * @param yaw_threshold yaw threshold used for searching for first nearest index to given pose - * @return nearest index - */ -template -size_t findFirstNearestSegmentIndexWithSoftConstraints( - const T & points, const geometry_msgs::msg::Pose & pose, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()) -{ - // find first nearest index with soft constraints (not segment index) - const size_t nearest_idx = - findFirstNearestIndexWithSoftConstraints(points, pose, dist_threshold, yaw_threshold); - - // calculate segment index - if (nearest_idx == 0) { - return 0; - } - if (nearest_idx == points.size() - 1) { - return points.size() - 2; - } - - const double signed_length = calcLongitudinalOffsetToSegment(points, nearest_idx, pose.position); - - if (signed_length <= 0) { - return nearest_idx - 1; - } - - return nearest_idx; -} - -extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()); -extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()); -extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, - const double dist_threshold = std::numeric_limits::max(), - const double yaw_threshold = std::numeric_limits::max()); - -/** - * @brief calculate the point offset from source point along the trajectory (or path) - * @brief calculate length of 2D distance between given pose and first point in container with zero - * longitudinal velocity - * @param points_with_twist points of trajectory, path, ... (with velocity) - * @param pose given pose to start the distance calculation from - * @param max_dist max distance, used to search for nearest segment index in points container to the - * given pose - * @param max_yaw max yaw, used to search for nearest segment index in points container to the given - * pose - * @return Length of 2D distance between given pose and first point in container with zero - * longitudinal velocity - */ -template -std::optional calcDistanceToForwardStopPoint( - const T & points_with_twist, const geometry_msgs::msg::Pose & pose, - const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()) -{ - try { - validateNonEmpty(points_with_twist); - } catch (const std::exception & e) { - RCLCPP_DEBUG(get_logger(), "Failed to calculate stop distance %s", e.what()); - return {}; - } - - const auto nearest_segment_idx = - autoware::motion_utils::findNearestSegmentIndex(points_with_twist, pose, max_dist, max_yaw); - - if (!nearest_segment_idx) { - return std::nullopt; - } - - const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex( - points_with_twist, *nearest_segment_idx + 1, points_with_twist.size()); - - if (!stop_idx) { - return std::nullopt; - } - - const auto closest_stop_dist = - calcSignedArcLength(points_with_twist, pose.position, *nearest_segment_idx, *stop_idx); - - return std::max(0.0, closest_stop_dist); -} - -extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, - const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()); -extern template std::optional calcDistanceToForwardStopPoint< - std::vector>( - const std::vector & points_with_twist, - const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()); -extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, - const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), - const double max_yaw = std::numeric_limits::max()); - -// NOTE: Points after forward length from the point will be cropped -// forward_length is assumed to be positive. -template -T cropForwardPoints( - const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length) -{ - if (points.empty()) { - return T{}; - } - - 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_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}; - } - } - - return points; -} - -extern template std::vector -cropForwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length); -extern template std::vector -cropForwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length); -extern template std::vector -cropForwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length); - -// NOTE: Points before backward length from the point will be cropped -// backward_length is assumed to be positive. -template -T cropBackwardPoints( - const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double backward_length) -{ - if (points.empty()) { - return T{}; - } - - 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_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()}; - } - } - - return points; -} - -extern template std::vector -cropBackwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double backward_length); -extern template std::vector -cropBackwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double backward_length); -extern template std::vector -cropBackwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double backward_length); - -template -T cropPoints( - const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length, const double backward_length) -{ - if (points.empty()) { - return T{}; - } - - // NOTE: Cropping forward must be done first in order to keep target_seg_idx. - const auto cropped_forward_points = - cropForwardPoints(points, target_pos, target_seg_idx, forward_length); - - const size_t modified_target_seg_idx = - std::min(target_seg_idx, cropped_forward_points.size() - 2); - const auto cropped_points = cropBackwardPoints( - cropped_forward_points, target_pos, modified_target_seg_idx, backward_length); - - if (cropped_points.size() < 2) { - RCLCPP_DEBUG(get_logger(), "Return original points since cropped_points size is less than 2."); - return points; - } - - return cropped_points; -} - -extern template std::vector -cropPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length, const double backward_length); -extern template std::vector -cropPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length, const double backward_length); -extern template std::vector -cropPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length, const double backward_length); - -/** - * @brief Calculate the angle of the input pose with respect to the nearest trajectory segment. - * The function gets the nearest segment index between the points of the trajectory and the given - * pose's position, then calculates the azimuth angle of that segment and compares it to the yaw of - * the input pose. The segment is a straight path between two continuous points of the trajectory. - * @param points Points of the trajectory, path, ... - * @param pose Input pose with position and orientation (yaw) - * @param throw_exception Flag to enable/disable exception throwing - * @return Angle with respect to the trajectory segment (signed) in radians - */ -template -double calcYawDeviation( - const T & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception = false) -{ - const auto overlap_removed_points = removeOverlapPoints(points, 0); - - if (throw_exception) { - validateNonEmpty(overlap_removed_points); - } else { - try { - validateNonEmpty(overlap_removed_points); - } catch (const std::exception & e) { - RCLCPP_DEBUG(get_logger(), "%s", e.what()); - return 0.0; - } - } - - if (overlap_removed_points.size() <= 1) { - const std::string error_message( - "[autoware_motion_utils] " + std::string(__func__) + - " Given points size is less than 2. Failed to calculate yaw deviation."); - autoware_utils::print_backtrace(); - if (throw_exception) { - throw std::runtime_error(error_message); - } - RCLCPP_DEBUG( - get_logger(), - "%s Return 0 since no_throw option is enabled. The maintainer must check the code.", - error_message.c_str()); - return 0.0; - } - - const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, pose.position); - - 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_utils::normalize_radian(pose_yaw - path_yaw); -} - -extern template double calcYawDeviation>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); -extern template double -calcYawDeviation>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); -extern template double calcYawDeviation>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); - -/** - * @brief Check if the given target point is in front of the based pose from the trajectory. - * if the points is empty, the function returns false - * @param points Points of the trajectory, path, ... - * @param base_point Base point - * @param target_point Target point - * @param threshold threshold for judging front point - * @return true if the target pose is in front of the base pose - */ -template -bool isTargetPointFront( - const T & points, const geometry_msgs::msg::Point & base_point, - const geometry_msgs::msg::Point & target_point, const double threshold = 0.0) -{ - if (points.empty()) { - return false; - } - - const double s_base = calcSignedArcLength(points, 0, base_point); - const double s_target = calcSignedArcLength(points, 0, target_point); - - return s_target - s_base > threshold; -} - -extern template bool isTargetPointFront>( - const std::vector & points, - const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, - const double threshold = 0.0); -extern template bool -isTargetPointFront>( - const std::vector & points, - const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, - const double threshold = 0.0); -extern template bool isTargetPointFront>( - const std::vector & points, - const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, - const double threshold = 0.0); - -/// @brief calculate the time_from_start fields of the given trajectory points -/// @details this function assumes constant longitudinal velocity between points -/// @param trajectory trajectory for which to calculate the time_from_start -/// @param current_ego_point current ego position -/// @param min_velocity minimum velocity used for a trajectory point -void calculate_time_from_start( - std::vector & trajectory, - const geometry_msgs::msg::Point & current_ego_point, const float min_velocity = 1.0f); - -} // namespace autoware::motion_utils - -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/vehicle/vehicle_state_checker.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/vehicle/vehicle_state_checker.hpp deleted file mode 100644 index f1d5677d61f70..0000000000000 --- a/common/autoware_motion_utils/include/autoware/motion_utils/vehicle/vehicle_state_checker.hpp +++ /dev/null @@ -1,82 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__MOTION_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_ -#define AUTOWARE__MOTION_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_ - -#include - -#include -#include -#include - -#include - -namespace autoware::motion_utils -{ - -using autoware_planning_msgs::msg::Trajectory; -using geometry_msgs::msg::TwistStamped; -using nav_msgs::msg::Odometry; - -class VehicleStopCheckerBase -{ -public: - VehicleStopCheckerBase(rclcpp::Node * node, double buffer_duration); - rclcpp::Logger getLogger() { return logger_; } - void addTwist(const TwistStamped & twist); - bool isVehicleStopped(const double stop_duration = 0.0) const; - -protected: - rclcpp::Clock::SharedPtr clock_; - rclcpp::Logger logger_; - -private: - double buffer_duration_; - std::deque twist_buffer_; -}; - -class VehicleStopChecker : public VehicleStopCheckerBase -{ -public: - explicit VehicleStopChecker(rclcpp::Node * node); - -protected: - rclcpp::Subscription::SharedPtr sub_odom_; - Odometry::ConstSharedPtr odometry_ptr_; - -private: - static constexpr double velocity_buffer_time_sec = 10.0; - void onOdom(const Odometry::ConstSharedPtr msg); -}; - -class VehicleArrivalChecker : public VehicleStopChecker -{ -public: - explicit VehicleArrivalChecker(rclcpp::Node * node); - - bool isVehicleStoppedAtStopPoint(const double stop_duration = 0.0) const; - -private: - static constexpr double th_arrived_distance_m = 1.0; - - rclcpp::Subscription::SharedPtr sub_trajectory_; - - Trajectory::ConstSharedPtr trajectory_ptr_; - - void onTrajectory(const Trajectory::ConstSharedPtr msg); -}; -} // namespace autoware::motion_utils - -#endif // AUTOWARE__MOTION_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_ diff --git a/common/autoware_motion_utils/media/ego_nearest_search.svg b/common/autoware_motion_utils/media/ego_nearest_search.svg deleted file mode 100644 index fb7d94ec85248..0000000000000 --- a/common/autoware_motion_utils/media/ego_nearest_search.svg +++ /dev/null @@ -1,280 +0,0 @@ - - - - - - - - - - - - - -
-
-
nearest
-
-
- - nearest - - - - - - - - - -
-
-
- 2 * yaw -
- threshold -
-
-
- - 2 * yaw... - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
nearest
-
-
- - nearest - - - - - - - - - - - - - - - - - - - - - -
-
-
nearest
-
-
- - nearest - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
nearest
-
-
- - nearest - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- distance -
- threshold -
-
-
- - distance... - - - - - - - -
-
-
1.
-
-
- - 1. - - - - - - -
-
-
2.
-
-
- - 2. - - - - - - -
-
-
3.
-
-
- - 3. - - - - - - -
-
-
4.
-
-
- - 4. - - - - - - - Viewer does not support full SVG 1.1 - - - diff --git a/common/autoware_motion_utils/media/segment.svg b/common/autoware_motion_utils/media/segment.svg deleted file mode 100644 index eae72d6c9ab5c..0000000000000 --- a/common/autoware_motion_utils/media/segment.svg +++ /dev/null @@ -1,344 +0,0 @@ - - - - - - - - - - - - - - - - - - - - -
-
-
- point index -
-
-
- - point index - - - - - - - - -
-
-
1
-
-
- - 1 - - - - - - -
-
-
2
-
-
- - 2 - - - - - - -
-
-
- segment index -
-
-
- - segment index - - - - - - -
-
-
1
-
-
- - 1 - - - - - - -
-
-
7
-
-
- - 7 - - - - - - -
-
-
8
-
-
- - 8 - - - - - - -
-
-
7
-
-
- - 7 - - - - - - -
-
-
6
-
-
- - 6 - - - - - - -
-
-
6
-
-
- - 6 - - - - - - -
-
-
- nearest -
- index -
-
-
- - nearest... - - - - - - -
-
-
- nearest -
- segment index -
-
-
- - nearest... - - - - - - -
-
-
0
-
-
- - 0 - - - - - - -
-
-
0
-
-
- - 0 - - - - - - -
-
-
3
-
-
- - 3 - - - - - - -
-
-
2
-
-
- - 2 - - - - - - -
-
-
5
-
-
- - 5 - - - - - - -
-
-
3
-
-
- - 3 - - - - - - -
-
-
- 4 -
-
-
- - 4 - - - - - - -
-
-
5
-
-
- - 5 - - - - - - -
-
-
- 4 -
-
-
- - 4 - - - - - - - Viewer does not support full SVG 1.1 - - - diff --git a/common/autoware_motion_utils/package.xml b/common/autoware_motion_utils/package.xml deleted file mode 100644 index dba3ca98b4e44..0000000000000 --- a/common/autoware_motion_utils/package.xml +++ /dev/null @@ -1,45 +0,0 @@ - - - - autoware_motion_utils - 0.41.0 - The autoware_motion_utils package - Satoshi Ota - Takayuki Murooka - - Fumiya Watanabe - Kosuke Takeuchi - Taiki Tanaka - Takamasa Horibe - Tomoya Kimura - Mamoru Sobue - Apache License 2.0 - - Takayuki Murooka - Satoshi Ota - - ament_cmake_auto - autoware_cmake - - autoware_adapi_v1_msgs - autoware_internal_planning_msgs - autoware_interpolation - autoware_planning_msgs - autoware_utils - autoware_vehicle_msgs - builtin_interfaces - geometry_msgs - libboost-dev - rclcpp - tf2 - tf2_geometry_msgs - visualization_msgs - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/autoware_motion_utils/src/distance/distance.cpp b/common/autoware_motion_utils/src/distance/distance.cpp deleted file mode 100644 index c218561beacec..0000000000000 --- a/common/autoware_motion_utils/src/distance/distance.cpp +++ /dev/null @@ -1,274 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/motion_utils/distance/distance.hpp" - -#include - -namespace autoware::motion_utils -{ -namespace -{ -bool validCheckDecelPlan( - const double v_end, const double a_end, const double v_target, const double a_target, - const double v_margin, const double a_margin) -{ - const double v_min = v_target - std::abs(v_margin); - const double v_max = v_target + std::abs(v_margin); - const double a_min = a_target - std::abs(a_margin); - const double a_max = a_target + std::abs(a_margin); - - if (v_end < v_min || v_max < v_end) { - return false; - } - if (a_end < a_min || a_max < a_end) { - return false; - } - - return true; -} - -/** - * @brief update traveling distance, velocity and acceleration under constant jerk. - * @param (x) current traveling distance [m/s] - * @param (v) current velocity [m/s] - * @param (a) current acceleration [m/ss] - * @param (j) target jerk [m/sss] - * @param (t) time [s] - * @return updated traveling distance, velocity and acceleration - */ -std::tuple update( - const double x, const double v, const double a, const double j, const double t) -{ - const double a_new = a + j * t; - const double v_new = v + a * t + 0.5 * j * t * t; - const double x_new = x + v * t + 0.5 * a * t * t + (1.0 / 6.0) * j * t * t * t; - - return {x_new, v_new, a_new}; -} - -/** - * @brief calculate distance until velocity is reached target velocity (TYPE: TRAPEZOID - * ACCELERATION PROFILE). this type of profile has ZERO JERK time. - * - * [ACCELERATION PROFILE] - * a ^ - * | - * a0 * - * |* - * ----+-*-------------------*------> t - * | * * - * | * * - * | a1 *************** - * | - * - * [JERK PROFILE] - * j ^ - * | - * | ja **** - * | * - * ----+----***************---------> t - * | * - * | * - * jd ****** - * | - * - * @param (v0) current velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (vt) target velocity [m/s] - * @param (am) minimum deceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @param (jd) minimum jerk [m/sss] - * @param (t_during_min_acc) duration of constant deceleration [s] - * @return moving distance until velocity is reached vt [m] - */ -std::optional calcDecelDistPlanType1( - const double v0, const double vt, const double a0, const double am, const double ja, - const double jd, const double t_during_min_acc) -{ - constexpr double epsilon = 1e-3; - - // negative jerk time - const double j1 = am < a0 ? jd : ja; - const double t1 = epsilon < (am - a0) / j1 ? (am - a0) / j1 : 0.0; - const auto [x1, v1, a1] = update(0.0, v0, a0, j1, t1); - - // zero jerk time - const double t2 = epsilon < t_during_min_acc ? t_during_min_acc : 0.0; - const auto [x2, v2, a2] = update(x1, v1, a1, 0.0, t2); - - // positive jerk time - const double t3 = epsilon < (0.0 - am) / ja ? (0.0 - am) / ja : 0.0; - const auto [x3, v3, a3] = update(x2, v2, a2, ja, t3); - - const double a_target = 0.0; - const double v_margin = 0.3; // [m/s] - const double a_margin = 0.1; // [m/s^2] - - if (!validCheckDecelPlan(v3, a3, vt, a_target, v_margin, a_margin)) { - return {}; - } - - return x3; -} - -/** - * @brief calculate distance until velocity is reached target velocity (TYPE: TRIANGLE - * ACCELERATION PROFILE), This type of profile do NOT have ZERO JERK time. - * - * [ACCELERATION PROFILE] - * a ^ - * | - * a0 * - * |* - * ----+-*-----*--------------------> t - * | * * - * | * * - * | a1 * - * | - * - * [JERK PROFILE] - * j ^ - * | - * | ja **** - * | * - * ----+----*-----------------------> t - * | * - * | * - * jd ****** - * | - * - * @param (v0) current velocity [m/s] - * @param (vt) target velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (am) minimum deceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @param (jd) minimum jerk [m/sss] - * @return moving distance until velocity is reached vt [m] - */ -std::optional calcDecelDistPlanType2( - const double v0, const double vt, const double a0, const double ja, const double jd) -{ - constexpr double epsilon = 1e-3; - - const double a1_square = (vt - v0 - 0.5 * (0.0 - a0) / jd * a0) * (2.0 * ja * jd / (ja - jd)); - const double a1 = -std::sqrt(a1_square); - - // negative jerk time - const double t1 = epsilon < (a1 - a0) / jd ? (a1 - a0) / jd : 0.0; - const auto [x1, v1, no_use_a1] = update(0.0, v0, a0, jd, t1); - - // positive jerk time - const double t2 = epsilon < (0.0 - a1) / ja ? (0.0 - a1) / ja : 0.0; - const auto [x2, v2, a2] = update(x1, v1, a1, ja, t2); - - const double a_target = 0.0; - const double v_margin = 0.3; - const double a_margin = 0.1; - - if (!validCheckDecelPlan(v2, a2, vt, a_target, v_margin, a_margin)) { - return {}; - } - - return x2; -} - -/** - * @brief calculate distance until velocity is reached target velocity (TYPE: LINEAR ACCELERATION - * PROFILE). This type of profile has only positive jerk time. - * - * [ACCELERATION PROFILE] - * a ^ - * | - * ----+----*-----------------------> t - * | * - * | * - * | * - * |* - * a0 * - * | - * - * [JERK PROFILE] - * j ^ - * | - * ja ****** - * | * - * | * - * ----+----*-----------------------> t - * | - * - * @param (v0) current velocity [m/s] - * @param (vt) target velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @return moving distance until velocity is reached vt [m] - */ -std::optional calcDecelDistPlanType3( - const double v0, const double vt, const double a0, const double ja) -{ - constexpr double epsilon = 1e-3; - - // positive jerk time - const double t_acc = (0.0 - a0) / ja; - const double t1 = epsilon < t_acc ? t_acc : 0.0; - const auto [x1, v1, a1] = update(0.0, v0, a0, ja, t1); - - const double a_target = 0.0; - const double v_margin = 0.3; - const double a_margin = 0.1; - - if (!validCheckDecelPlan(v1, a1, vt, a_target, v_margin, a_margin)) { - return {}; - } - - return x1; -} -} // namespace - -std::optional calcDecelDistWithJerkAndAccConstraints( - const double current_vel, const double target_vel, const double current_acc, const double acc_min, - const double jerk_acc, const double jerk_dec) -{ - if (current_vel < target_vel) { - return {}; - } - - constexpr double epsilon = 1e-3; - const double jerk_before_min_acc = acc_min < current_acc ? jerk_dec : jerk_acc; - const double t_before_min_acc = (acc_min - current_acc) / jerk_before_min_acc; - const double jerk_after_min_acc = jerk_acc; - const double t_after_min_acc = (0.0 - acc_min) / jerk_after_min_acc; - - const double t_during_min_acc = - (target_vel - current_vel - current_acc * t_before_min_acc - - 0.5 * jerk_before_min_acc * std::pow(t_before_min_acc, 2) - acc_min * t_after_min_acc - - 0.5 * jerk_after_min_acc * std::pow(t_after_min_acc, 2)) / - acc_min; - - // check if it is possible to decelerate to the target velocity - // by simply bringing the current acceleration to zero. - const auto is_decel_needed = - 0.5 * (0.0 - current_acc) / jerk_acc * current_acc > target_vel - current_vel; - - if (t_during_min_acc > epsilon) { - return calcDecelDistPlanType1( - current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec, t_during_min_acc); - } - if (is_decel_needed || current_acc > epsilon) { - return calcDecelDistPlanType2(current_vel, target_vel, current_acc, jerk_acc, jerk_dec); - } - - return calcDecelDistPlanType3(current_vel, target_vel, current_acc, jerk_acc); -} -} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/marker/marker_helper.cpp b/common/autoware_motion_utils/src/marker/marker_helper.cpp deleted file mode 100644 index 07dbcd8f2dc8b..0000000000000 --- a/common/autoware_motion_utils/src/marker/marker_helper.cpp +++ /dev/null @@ -1,186 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/motion_utils/marker/marker_helper.hpp" - -#include "autoware_utils/ros/marker_helper.hpp" - -#include - -#include - -#include - -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 -{ - -inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray( - const geometry_msgs::msg::Pose & vehicle_front_pose, const std::string & module_name, - const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id, - const std_msgs::msg::ColorRGBA & color) -{ - visualization_msgs::msg::MarkerArray marker_array; - - // Virtual Wall - { - auto marker = create_default_marker( - "map", now, ns_prefix + "virtual_wall", id, visualization_msgs::msg::Marker::CUBE, - create_marker_scale(0.1, 5.0, 2.0), color); - - marker.pose = vehicle_front_pose; - marker.pose.position.z += 1.0; - - marker_array.markers.push_back(marker); - } - - // Factor Text - { - auto marker = create_default_marker( - "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - 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; - marker.text = module_name; - - marker_array.markers.push_back(marker); - } - - return marker_array; -} - -inline visualization_msgs::msg::MarkerArray createDeletedVirtualWallMarkerArray( - const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id) -{ - visualization_msgs::msg::MarkerArray marker_array; - - // Virtual Wall - { - auto marker = create_deleted_default_marker(now, ns_prefix + "virtual_wall", id); - marker_array.markers.push_back(marker); - } - - // Factor Text - { - auto marker = create_deleted_default_marker(now, ns_prefix + "factor_text", id); - marker_array.markers.push_back(marker); - } - - return marker_array; -} - -inline visualization_msgs::msg::MarkerArray createIntendedPassArrowMarkerArray( - const geometry_msgs::msg::Pose & vehicle_front_pose, const std::string & module_name, - const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id, - const std_msgs::msg::ColorRGBA & color) -{ - visualization_msgs::msg::MarkerArray marker_array; - - // Arrow - { - auto marker = create_default_marker( - "map", now, ns_prefix + "direction", id, visualization_msgs::msg::Marker::ARROW, - create_marker_scale(2.5 /*length*/, 0.15 /*width*/, 1.0 /*height*/), color); - - marker.pose = vehicle_front_pose; - - marker_array.markers.push_back(marker); - } - - // Factor Text - { - auto marker = create_default_marker( - "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - 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; - marker.text = module_name; - - marker_array.markers.push_back(marker); - } - - return marker_array; -} - -} // namespace - -namespace autoware::motion_utils -{ -visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( - const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, - const bool is_driving_forward) -{ - 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, - create_marker_color(1.0, 0.0, 0.0, 0.5)); -} - -visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( - const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, - const bool is_driving_forward) -{ - 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, - create_marker_color(1.0, 1.0, 0.0, 0.5)); -} - -visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( - const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, - const bool is_driving_forward) -{ - 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, - create_marker_color(0.0, 1.0, 0.0, 0.5)); -} - -visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker( - const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, - const bool is_driving_forward) -{ - 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, - create_marker_color(0.77, 0.77, 0.77, 0.5)); -} - -visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( - const rclcpp::Time & now, const int32_t id) -{ - return createDeletedVirtualWallMarkerArray("stop_", now, id); -} - -visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker( - const rclcpp::Time & now, const int32_t id) -{ - return createDeletedVirtualWallMarkerArray("slow_down_", now, id); -} -} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp b/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp deleted file mode 100644 index fb8708b5b7baa..0000000000000 --- a/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp +++ /dev/null @@ -1,94 +0,0 @@ -// Copyright 2023 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/motion_utils/marker/virtual_wall_marker_creator.hpp" - -#include "autoware/motion_utils/marker/marker_helper.hpp" - -namespace autoware::motion_utils -{ - -void VirtualWallMarkerCreator::cleanup() -{ - for (auto it = marker_count_per_namespace_.begin(); it != marker_count_per_namespace_.end();) { - const auto & marker_count = it->second; - const auto is_unused_namespace = marker_count.previous == 0 && marker_count.current == 0; - if (is_unused_namespace) - it = marker_count_per_namespace_.erase(it); - else - ++it; - } - virtual_walls_.clear(); -} - -void VirtualWallMarkerCreator::add_virtual_wall(const VirtualWall & virtual_wall) -{ - virtual_walls_.push_back(virtual_wall); -} -void VirtualWallMarkerCreator::add_virtual_walls(const VirtualWalls & walls) -{ - virtual_walls_.insert(virtual_walls_.end(), walls.begin(), walls.end()); -} - -visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( - const rclcpp::Time & now) -{ - visualization_msgs::msg::MarkerArray marker_array; - // update marker counts - for (auto & [ns, count] : marker_count_per_namespace_) { - count.previous = count.current; - count.current = 0UL; - } - // convert to markers - create_wall_function create_fn; - for (const auto & virtual_wall : virtual_walls_) { - switch (virtual_wall.style) { - case stop: - create_fn = autoware::motion_utils::createStopVirtualWallMarker; - break; - case slowdown: - create_fn = autoware::motion_utils::createSlowDownVirtualWallMarker; - break; - case deadline: - create_fn = autoware::motion_utils::createDeadLineVirtualWallMarker; - break; - case pass: - create_fn = autoware::motion_utils::createIntendedPassVirtualMarker; - break; - } - const auto wall_text = virtual_wall.detail.empty() - ? virtual_wall.text - : virtual_wall.text + "(" + virtual_wall.detail + ")"; - auto markers = create_fn( - virtual_wall.pose, wall_text, now, 0, virtual_wall.longitudinal_offset, virtual_wall.ns, - virtual_wall.is_driving_forward); - for (auto & marker : markers.markers) { - marker.id = static_cast(marker_count_per_namespace_[marker.ns].current++); - marker_array.markers.push_back(marker); - } - } - // create delete markers - visualization_msgs::msg::Marker marker; - marker.action = visualization_msgs::msg::Marker::DELETE; - for (const auto & [ns, count] : marker_count_per_namespace_) { - for (marker.id = static_cast(count.current); marker.id < static_cast(count.previous); - ++marker.id) { - marker.ns = ns; - marker_array.markers.push_back(marker); - } - } - cleanup(); - return marker_array; -} -} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/resample/resample.cpp b/common/autoware_motion_utils/src/resample/resample.cpp deleted file mode 100644 index 75126ba1e4d58..0000000000000 --- a/common/autoware_motion_utils/src/resample/resample.cpp +++ /dev/null @@ -1,752 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/motion_utils/resample/resample.hpp" - -#include "autoware/interpolation/linear_interpolation.hpp" -#include "autoware/interpolation/spline_interpolation.hpp" -#include "autoware/interpolation/zero_order_hold.hpp" -#include "autoware/motion_utils/resample/resample_utils.hpp" -#include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware_utils/geometry/geometry.hpp" - -#include -#include -#include -#include - -namespace autoware::motion_utils -{ -std::vector resamplePointVector( - const std::vector & points, - const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, - const bool use_lerp_for_z) -{ - // validate arguments - if (!resample_utils::validate_arguments(points, resampled_arclength)) { - return points; - } - - // Input Path Information - std::vector input_arclength; - std::vector x; - std::vector y; - std::vector z; - input_arclength.reserve(points.size()); - x.reserve(points.size()); - y.reserve(points.size()); - z.reserve(points.size()); - - input_arclength.push_back(0.0); - x.push_back(points.front().x); - y.push_back(points.front().y); - z.push_back(points.front().z); - 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_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); - z.push_back(curr_pt.z); - } - - // Interpolate - const auto lerp = [&](const auto & input) { - return autoware::interpolation::lerp(input_arclength, input, resampled_arclength); - }; - const auto spline = [&](const auto & input) { - return autoware::interpolation::spline(input_arclength, input, resampled_arclength); - }; - const auto spline_by_akima = [&](const auto & input) { - return autoware::interpolation::splineByAkima(input_arclength, input, resampled_arclength); - }; - - const auto interpolated_x = use_akima_spline_for_xy ? lerp(x) : spline_by_akima(x); - const auto interpolated_y = use_akima_spline_for_xy ? lerp(y) : spline_by_akima(y); - const auto interpolated_z = use_lerp_for_z ? lerp(z) : spline(z); - - std::vector resampled_points; - resampled_points.resize(interpolated_x.size()); - - // Insert Position - for (size_t i = 0; i < resampled_points.size(); ++i) { - geometry_msgs::msg::Point point; - point.x = interpolated_x.at(i); - point.y = interpolated_y.at(i); - point.z = interpolated_z.at(i); - resampled_points.at(i) = point; - } - - return resampled_points; -} - -std::vector resamplePointVector( - const std::vector & points, const double resample_interval, - const bool use_akima_spline_for_xy, const bool use_lerp_for_z) -{ - const double input_length = autoware::motion_utils::calcArcLength(points); - - std::vector resampling_arclength; - for (double s = 0.0; s < input_length; s += resample_interval) { - resampling_arclength.push_back(s); - } - if (resampling_arclength.empty()) { - std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; - return points; - } - - // Insert terminal point - if (input_length - resampling_arclength.back() < autoware::motion_utils::overlap_threshold) { - resampling_arclength.back() = input_length; - } else { - resampling_arclength.push_back(input_length); - } - - return resamplePointVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); -} - -std::vector resamplePoseVector( - const std::vector & points_raw, - const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, - const bool use_lerp_for_z) -{ - // Remove overlap points for resampling - const auto points = autoware::motion_utils::removeOverlapPoints(points_raw); - - // validate arguments - if (!resample_utils::validate_arguments(points, resampled_arclength)) { - return points_raw; - } - - std::vector position(points.size()); - for (size_t i = 0; i < points.size(); ++i) { - position.at(i) = points.at(i).position; - } - const auto resampled_position = - resamplePointVector(position, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); - - std::vector resampled_points(resampled_position.size()); - - // Insert Position - for (size_t i = 0; i < resampled_position.size(); ++i) { - geometry_msgs::msg::Pose pose; - pose.position.x = resampled_position.at(i).x; - pose.position.y = resampled_position.at(i).y; - pose.position.z = resampled_position.at(i).z; - resampled_points.at(i) = pose; - } - - 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 - // when backward driving - if (!is_driving_forward && resampled_arclength.front() < 1e-3) { - resampled_points.at(0).orientation = points.at(0).orientation; - } - - return resampled_points; -} - -std::vector resamplePoseVector( - const std::vector & points, const double resample_interval, - const bool use_akima_spline_for_xy, const bool use_lerp_for_z) -{ - const double input_length = autoware::motion_utils::calcArcLength(points); - - std::vector resampling_arclength; - for (double s = 0.0; s < input_length; s += resample_interval) { - resampling_arclength.push_back(s); - } - if (resampling_arclength.empty()) { - std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; - return points; - } - - // Insert terminal point - if (input_length - resampling_arclength.back() < autoware::motion_utils::overlap_threshold) { - resampling_arclength.back() = input_length; - } else { - resampling_arclength.push_back(input_length); - } - - return resamplePoseVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); -} - -autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, - const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, - const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) -{ - auto resampling_arclength = resampled_arclength; - - // Add resampling_arclength to insert input points which have multiple lane_ids - for (size_t i = 0; i < input_path.points.size(); ++i) { - if (input_path.points.at(i).lane_ids.size() < 2) { - continue; - } - - const double distance_to_resampling_point = calcSignedArcLength(input_path.points, 0, i); - for (size_t j = 1; j < resampling_arclength.size(); ++j) { - if ( - resampling_arclength.at(j - 1) <= distance_to_resampling_point && - distance_to_resampling_point < resampling_arclength.at(j)) { - const double dist_to_prev_point = - std::fabs(distance_to_resampling_point - resampling_arclength.at(j - 1)); - const double dist_to_following_point = - std::fabs(resampling_arclength.at(j) - distance_to_resampling_point); - if (dist_to_prev_point < autoware::motion_utils::overlap_threshold) { - resampling_arclength.at(j - 1) = distance_to_resampling_point; - } else if (dist_to_following_point < autoware::motion_utils::overlap_threshold) { - resampling_arclength.at(j) = distance_to_resampling_point; - } else { - resampling_arclength.insert( - resampling_arclength.begin() + j, distance_to_resampling_point); - } - break; - } - } - } - - // validate arguments - if (!resample_utils::validate_arguments(input_path.points, resampling_arclength)) { - return input_path; - } - - // For LaneIds, is_final - // - // ------|----|----|----|----|----|----|-------> resampled - // [0] [1] [2] [3] [4] [5] [6] - // - // ------|----------------|----------|---------> base - // [0] [1] [2] - // - // resampled[0~3] = base[0] - // resampled[4~5] = base[1] - // resampled[6] = base[2] - - // Input Path Information - std::vector input_arclength; - std::vector input_pose; - std::vector v_lon; - std::vector v_lat; - std::vector heading_rate; - std::vector is_final; - std::vector> lane_ids; - input_arclength.reserve(input_path.points.size()); - input_pose.reserve(input_path.points.size()); - v_lon.reserve(input_path.points.size()); - v_lat.reserve(input_path.points.size()); - heading_rate.reserve(input_path.points.size()); - is_final.reserve(input_path.points.size()); - lane_ids.reserve(input_path.points.size()); - - input_arclength.push_back(0.0); - input_pose.push_back(input_path.points.front().point.pose); - v_lon.push_back(input_path.points.front().point.longitudinal_velocity_mps); - v_lat.push_back(input_path.points.front().point.lateral_velocity_mps); - heading_rate.push_back(input_path.points.front().point.heading_rate_rps); - is_final.push_back(input_path.points.front().point.is_final); - lane_ids.push_back(input_path.points.front().lane_ids); - 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_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); - v_lat.push_back(curr_pt.lateral_velocity_mps); - heading_rate.push_back(curr_pt.heading_rate_rps); - is_final.push_back(curr_pt.is_final); - lane_ids.push_back(input_path.points.at(i).lane_ids); - } - - if (input_arclength.back() < resampling_arclength.back()) { - std::cerr << "[autoware_motion_utils]: resampled path length is longer than input path length" - << std::endl; - return input_path; - } - - // Interpolate - const auto lerp = [&](const auto & input) { - return autoware::interpolation::lerp(input_arclength, input, resampling_arclength); - }; - - auto closest_segment_indices = - autoware::interpolation::calc_closest_segment_indices(input_arclength, resampling_arclength); - - const auto zoh = [&](const auto & input) { - return autoware::interpolation::zero_order_hold( - input_arclength, input, closest_segment_indices); - }; - - const auto interpolated_pose = - resamplePoseVector(input_pose, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); - const auto interpolated_v_lon = use_zero_order_hold_for_v ? zoh(v_lon) : lerp(v_lon); - const auto interpolated_v_lat = use_zero_order_hold_for_v ? zoh(v_lat) : lerp(v_lat); - const auto interpolated_heading_rate = lerp(heading_rate); - const auto interpolated_is_final = zoh(is_final); - - // interpolate lane_ids - std::vector> interpolated_lane_ids; - interpolated_lane_ids.resize(resampling_arclength.size()); - constexpr double epsilon = 1e-6; - for (size_t i = 0; i < resampling_arclength.size(); ++i) { - const size_t seg_idx = std::min(closest_segment_indices.at(i), input_path.points.size() - 2); - const auto & prev_lane_ids = input_path.points.at(seg_idx).lane_ids; - const auto & next_lane_ids = input_path.points.at(seg_idx + 1).lane_ids; - - if (std::abs(input_arclength.at(seg_idx) - resampling_arclength.at(i)) <= epsilon) { - interpolated_lane_ids.at(i).insert( - interpolated_lane_ids.at(i).end(), prev_lane_ids.begin(), prev_lane_ids.end()); - } else if (std::abs(input_arclength.at(seg_idx + 1) - resampling_arclength.at(i)) <= epsilon) { - interpolated_lane_ids.at(i).insert( - interpolated_lane_ids.at(i).end(), next_lane_ids.begin(), next_lane_ids.end()); - } else { - // extract lane_ids those prev_lane_ids and next_lane_ids have in common - for (const auto target_lane_id : prev_lane_ids) { - if ( - std::find(next_lane_ids.begin(), next_lane_ids.end(), target_lane_id) != - next_lane_ids.end()) { - interpolated_lane_ids.at(i).push_back(target_lane_id); - } - } - // If there are no common lane_ids, the prev_lane_ids is assigned. - if (interpolated_lane_ids.at(i).empty()) { - interpolated_lane_ids.at(i).insert( - interpolated_lane_ids.at(i).end(), prev_lane_ids.begin(), prev_lane_ids.end()); - } - } - } - - if (interpolated_pose.size() != resampling_arclength.size()) { - std::cerr - << "[autoware_motion_utils]: Resampled pose size is different from resampled arclength" - << std::endl; - return input_path; - } - - autoware_internal_planning_msgs::msg::PathWithLaneId resampled_path; - resampled_path.header = input_path.header; - resampled_path.left_bound = input_path.left_bound; - resampled_path.right_bound = input_path.right_bound; - resampled_path.points.resize(interpolated_pose.size()); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - autoware_planning_msgs::msg::PathPoint path_point; - path_point.pose = interpolated_pose.at(i); - path_point.longitudinal_velocity_mps = interpolated_v_lon.at(i); - path_point.lateral_velocity_mps = interpolated_v_lat.at(i); - path_point.heading_rate_rps = interpolated_heading_rate.at(i); - path_point.is_final = interpolated_is_final.at(i); - resampled_path.points.at(i).point = path_point; - resampled_path.points.at(i).lane_ids = interpolated_lane_ids.at(i); - } - - return resampled_path; -} - -autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, - const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, - const bool use_zero_order_hold_for_v, const bool resample_input_path_stop_point) -{ - // validate arguments - if (!resample_utils::validate_arguments(input_path.points, resample_interval)) { - return input_path; - } - - // transform input_path - std::vector transformed_input_path( - input_path.points.size()); - for (size_t i = 0; i < input_path.points.size(); ++i) { - transformed_input_path.at(i) = input_path.points.at(i).point; - } - // compute path length - const double input_path_len = autoware::motion_utils::calcArcLength(transformed_input_path); - - std::vector resampling_arclength; - for (double s = 0.0; s < input_path_len; s += resample_interval) { - resampling_arclength.push_back(s); - } - if (resampling_arclength.empty()) { - std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; - return input_path; - } - - // Insert terminal point - if (input_path_len - resampling_arclength.back() < autoware::motion_utils::overlap_threshold) { - resampling_arclength.back() = input_path_len; - } else { - resampling_arclength.push_back(input_path_len); - } - - // Insert stop point - if (resample_input_path_stop_point) { - const auto distance_to_stop_point = - autoware::motion_utils::calcDistanceToForwardStopPoint(transformed_input_path, 0); - if (distance_to_stop_point && !resampling_arclength.empty()) { - for (size_t i = 1; i < resampling_arclength.size(); ++i) { - if ( - resampling_arclength.at(i - 1) <= *distance_to_stop_point && - *distance_to_stop_point < resampling_arclength.at(i)) { - const double dist_to_prev_point = - std::fabs(*distance_to_stop_point - resampling_arclength.at(i - 1)); - const double dist_to_following_point = - std::fabs(resampling_arclength.at(i) - *distance_to_stop_point); - if (dist_to_prev_point < autoware::motion_utils::overlap_threshold) { - resampling_arclength.at(i - 1) = *distance_to_stop_point; - } else if (dist_to_following_point < autoware::motion_utils::overlap_threshold) { - resampling_arclength.at(i) = *distance_to_stop_point; - } else { - resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point); - } - break; - } - } - } - } - - return resamplePath( - input_path, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z, - use_zero_order_hold_for_v); -} - -autoware_planning_msgs::msg::Path resamplePath( - const autoware_planning_msgs::msg::Path & input_path, - const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, - const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) -{ - // validate arguments - if (!resample_utils::validate_arguments(input_path.points, resampled_arclength)) { - return input_path; - } - - // Input Path Information - std::vector input_arclength; - std::vector input_pose; - std::vector v_lon; - std::vector v_lat; - std::vector heading_rate; - input_arclength.reserve(input_path.points.size()); - input_pose.reserve(input_path.points.size()); - v_lon.reserve(input_path.points.size()); - v_lat.reserve(input_path.points.size()); - heading_rate.reserve(input_path.points.size()); - - input_arclength.push_back(0.0); - input_pose.push_back(input_path.points.front().pose); - v_lon.push_back(input_path.points.front().longitudinal_velocity_mps); - v_lat.push_back(input_path.points.front().lateral_velocity_mps); - heading_rate.push_back(input_path.points.front().heading_rate_rps); - 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_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); - v_lat.push_back(curr_pt.lateral_velocity_mps); - heading_rate.push_back(curr_pt.heading_rate_rps); - } - - // Interpolate - const auto lerp = [&](const auto & input) { - return autoware::interpolation::lerp(input_arclength, input, resampled_arclength); - }; - - std::vector closest_segment_indices; - if (use_zero_order_hold_for_v) { - closest_segment_indices = - autoware::interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); - } - const auto zoh = [&](const auto & input) { - return autoware::interpolation::zero_order_hold( - input_arclength, input, closest_segment_indices); - }; - - const auto interpolated_pose = - resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); - const auto interpolated_v_lon = use_zero_order_hold_for_v ? zoh(v_lon) : lerp(v_lon); - const auto interpolated_v_lat = use_zero_order_hold_for_v ? zoh(v_lat) : lerp(v_lat); - const auto interpolated_heading_rate = lerp(heading_rate); - - if (interpolated_pose.size() != resampled_arclength.size()) { - std::cerr - << "[autoware_motion_utils]: Resampled pose size is different from resampled arclength" - << std::endl; - return input_path; - } - - autoware_planning_msgs::msg::Path resampled_path; - resampled_path.header = input_path.header; - resampled_path.left_bound = input_path.left_bound; - resampled_path.right_bound = input_path.right_bound; - resampled_path.points.resize(interpolated_pose.size()); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - autoware_planning_msgs::msg::PathPoint path_point; - path_point.pose = interpolated_pose.at(i); - path_point.longitudinal_velocity_mps = interpolated_v_lon.at(i); - path_point.lateral_velocity_mps = interpolated_v_lat.at(i); - path_point.heading_rate_rps = interpolated_heading_rate.at(i); - resampled_path.points.at(i) = path_point; - } - - return resampled_path; -} - -autoware_planning_msgs::msg::Path resamplePath( - const autoware_planning_msgs::msg::Path & input_path, const double resample_interval, - const bool use_akima_spline_for_xy, const bool use_lerp_for_z, - const bool use_zero_order_hold_for_twist, const bool resample_input_path_stop_point) -{ - // validate arguments - if (!resample_utils::validate_arguments(input_path.points, resample_interval)) { - return input_path; - } - - const double input_path_len = autoware::motion_utils::calcArcLength(input_path.points); - - std::vector resampling_arclength; - for (double s = 0.0; s < input_path_len; s += resample_interval) { - resampling_arclength.push_back(s); - } - if (resampling_arclength.empty()) { - std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; - return input_path; - } - - // Insert terminal point - if (input_path_len - resampling_arclength.back() < autoware::motion_utils::overlap_threshold) { - resampling_arclength.back() = input_path_len; - } else { - resampling_arclength.push_back(input_path_len); - } - - // Insert stop point - if (resample_input_path_stop_point) { - const auto distance_to_stop_point = - autoware::motion_utils::calcDistanceToForwardStopPoint(input_path.points, 0); - if (distance_to_stop_point && !resampling_arclength.empty()) { - for (size_t i = 1; i < resampling_arclength.size(); ++i) { - if ( - resampling_arclength.at(i - 1) <= *distance_to_stop_point && - *distance_to_stop_point < resampling_arclength.at(i)) { - const double dist_to_prev_point = - std::fabs(*distance_to_stop_point - resampling_arclength.at(i - 1)); - const double dist_to_following_point = - std::fabs(resampling_arclength.at(i) - *distance_to_stop_point); - if (dist_to_prev_point < autoware::motion_utils::overlap_threshold) { - resampling_arclength.at(i - 1) = *distance_to_stop_point; - } else if (dist_to_following_point < autoware::motion_utils::overlap_threshold) { - resampling_arclength.at(i) = *distance_to_stop_point; - } else { - resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point); - } - break; - } - } - } - } - - return resamplePath( - input_path, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z, - use_zero_order_hold_for_twist); -} - -autoware_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, - const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist) -{ - // validate arguments - if (!resample_utils::validate_arguments(input_trajectory.points, resampled_arclength)) { - return input_trajectory; - } - - // Input Trajectory Information - std::vector input_arclength; - std::vector input_pose; - std::vector v_lon; - std::vector v_lat; - std::vector heading_rate; - std::vector acceleration; - std::vector front_wheel_angle; - std::vector rear_wheel_angle; - std::vector time_from_start; - input_arclength.reserve(input_trajectory.points.size()); - input_pose.reserve(input_trajectory.points.size()); - v_lon.reserve(input_trajectory.points.size()); - v_lat.reserve(input_trajectory.points.size()); - heading_rate.reserve(input_trajectory.points.size()); - acceleration.reserve(input_trajectory.points.size()); - front_wheel_angle.reserve(input_trajectory.points.size()); - rear_wheel_angle.reserve(input_trajectory.points.size()); - time_from_start.reserve(input_trajectory.points.size()); - - input_arclength.push_back(0.0); - input_pose.push_back(input_trajectory.points.front().pose); - v_lon.push_back(input_trajectory.points.front().longitudinal_velocity_mps); - v_lat.push_back(input_trajectory.points.front().lateral_velocity_mps); - heading_rate.push_back(input_trajectory.points.front().heading_rate_rps); - acceleration.push_back(input_trajectory.points.front().acceleration_mps2); - front_wheel_angle.push_back(input_trajectory.points.front().front_wheel_angle_rad); - rear_wheel_angle.push_back(input_trajectory.points.front().rear_wheel_angle_rad); - time_from_start.push_back( - rclcpp::Duration(input_trajectory.points.front().time_from_start).seconds()); - - 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_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); - v_lat.push_back(curr_pt.lateral_velocity_mps); - heading_rate.push_back(curr_pt.heading_rate_rps); - acceleration.push_back(curr_pt.acceleration_mps2); - front_wheel_angle.push_back(curr_pt.front_wheel_angle_rad); - rear_wheel_angle.push_back(curr_pt.rear_wheel_angle_rad); - time_from_start.push_back(rclcpp::Duration(curr_pt.time_from_start).seconds()); - } - - // Set Zero Velocity After Stop Point - // If the longitudinal velocity is zero, set the velocity to zero after that point. - bool stop_point_found_in_v_lon = false; - constexpr double epsilon = 1e-4; - for (size_t i = 0; i < v_lon.size(); ++i) { - if (std::abs(v_lon.at(i)) < epsilon) { - stop_point_found_in_v_lon = true; - } - if (stop_point_found_in_v_lon) { - v_lon.at(i) = 0.0; - } - } - - // Interpolate - const auto lerp = [&](const auto & input) { - return autoware::interpolation::lerp(input_arclength, input, resampled_arclength); - }; - - std::vector closest_segment_indices; - if (use_zero_order_hold_for_twist) { - closest_segment_indices = - autoware::interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); - } - const auto zoh = [&](const auto & input) { - return autoware::interpolation::zero_order_hold( - input_arclength, input, closest_segment_indices); - }; - - const auto interpolated_pose = - resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); - const auto interpolated_v_lon = use_zero_order_hold_for_twist ? zoh(v_lon) : lerp(v_lon); - const auto interpolated_v_lat = use_zero_order_hold_for_twist ? zoh(v_lat) : lerp(v_lat); - const auto interpolated_heading_rate = lerp(heading_rate); - const auto interpolated_acceleration = - use_zero_order_hold_for_twist ? zoh(acceleration) : lerp(acceleration); - const auto interpolated_front_wheel_angle = lerp(front_wheel_angle); - const auto interpolated_rear_wheel_angle = lerp(rear_wheel_angle); - const auto interpolated_time_from_start = lerp(time_from_start); - - if (interpolated_pose.size() != resampled_arclength.size()) { - std::cerr - << "[autoware_motion_utils]: Resampled pose size is different from resampled arclength" - << std::endl; - return input_trajectory; - } - - autoware_planning_msgs::msg::Trajectory resampled_trajectory; - resampled_trajectory.header = input_trajectory.header; - resampled_trajectory.points.resize(interpolated_pose.size()); - for (size_t i = 0; i < resampled_trajectory.points.size(); ++i) { - autoware_planning_msgs::msg::TrajectoryPoint traj_point; - traj_point.pose = interpolated_pose.at(i); - traj_point.longitudinal_velocity_mps = interpolated_v_lon.at(i); - traj_point.lateral_velocity_mps = interpolated_v_lat.at(i); - traj_point.heading_rate_rps = interpolated_heading_rate.at(i); - traj_point.acceleration_mps2 = interpolated_acceleration.at(i); - traj_point.front_wheel_angle_rad = interpolated_front_wheel_angle.at(i); - traj_point.rear_wheel_angle_rad = interpolated_rear_wheel_angle.at(i); - traj_point.time_from_start = rclcpp::Duration::from_seconds(interpolated_time_from_start.at(i)); - resampled_trajectory.points.at(i) = traj_point; - } - - return resampled_trajectory; -} - -autoware_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double resample_interval, - const bool use_akima_spline_for_xy, const bool use_lerp_for_z, - const bool use_zero_order_hold_for_twist, const bool resample_input_trajectory_stop_point) -{ - // validate arguments - if (!resample_utils::validate_arguments(input_trajectory.points, resample_interval)) { - return input_trajectory; - } - - const double input_trajectory_len = - autoware::motion_utils::calcArcLength(input_trajectory.points); - - std::vector resampling_arclength; - for (double s = 0.0; s < input_trajectory_len; s += resample_interval) { - resampling_arclength.push_back(s); - } - if (resampling_arclength.empty()) { - std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; - return input_trajectory; - } - - // Insert terminal point - if ( - input_trajectory_len - resampling_arclength.back() < - autoware::motion_utils::overlap_threshold) { - resampling_arclength.back() = input_trajectory_len; - } else { - resampling_arclength.push_back(input_trajectory_len); - } - - // Insert stop point - if (resample_input_trajectory_stop_point) { - const auto distance_to_stop_point = - autoware::motion_utils::calcDistanceToForwardStopPoint(input_trajectory.points, 0); - if (distance_to_stop_point && !resampling_arclength.empty()) { - for (size_t i = 1; i < resampling_arclength.size(); ++i) { - if ( - resampling_arclength.at(i - 1) <= *distance_to_stop_point && - *distance_to_stop_point < resampling_arclength.at(i)) { - const double dist_to_prev_point = - std::fabs(*distance_to_stop_point - resampling_arclength.at(i - 1)); - const double dist_to_following_point = - std::fabs(resampling_arclength.at(i) - *distance_to_stop_point); - if (dist_to_prev_point < autoware::motion_utils::overlap_threshold) { - resampling_arclength.at(i - 1) = *distance_to_stop_point; - } else if (dist_to_following_point < autoware::motion_utils::overlap_threshold) { - resampling_arclength.at(i) = *distance_to_stop_point; - } else { - resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point); - } - break; - } - } - } - } - - return resampleTrajectory( - input_trajectory, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z, - use_zero_order_hold_for_twist); -} - -} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/trajectory/conversion.cpp b/common/autoware_motion_utils/src/trajectory/conversion.cpp deleted file mode 100644 index 706dd6dae79af..0000000000000 --- a/common/autoware_motion_utils/src/trajectory/conversion.cpp +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright 2023 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/motion_utils/trajectory/conversion.hpp" - -#include -#include - -namespace autoware::motion_utils -{ -/** - * @brief Convert std::vector to - * autoware_planning_msgs::msg::Trajectory. This function is temporarily added for porting to - * autoware_msgs. We should consider whether to remove this function after the porting is done. - * @attention This function just clips - * std::vector up to the capacity of Trajectory. - * Therefore, the error handling out of this function is necessary if the size of the input greater - * than the capacity. - * @todo Decide how to handle the situation that we need to use the trajectory with the size of - * points larger than the capacity. (Tier IV) - */ -autoware_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory, - const std_msgs::msg::Header & header) -{ - autoware_planning_msgs::msg::Trajectory output{}; - output.header = header; - for (const auto & pt : trajectory) output.points.push_back(pt); - return output; -} - -/** - * @brief Convert autoware_planning_msgs::msg::Trajectory to - * std::vector. - */ -std::vector convertToTrajectoryPointArray( - const autoware_planning_msgs::msg::Trajectory & trajectory) -{ - std::vector output(trajectory.points.size()); - std::copy(trajectory.points.begin(), trajectory.points.end(), output.begin()); - return output; -} - -} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/trajectory/interpolation.cpp b/common/autoware_motion_utils/src/trajectory/interpolation.cpp deleted file mode 100644 index e94b9c240a6d6..0000000000000 --- a/common/autoware_motion_utils/src/trajectory/interpolation.cpp +++ /dev/null @@ -1,149 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/motion_utils/trajectory/interpolation.hpp" - -#include "autoware/interpolation/linear_interpolation.hpp" -#include "autoware/motion_utils/trajectory/trajectory.hpp" - -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; - -namespace autoware::motion_utils -{ -TrajectoryPoint calcInterpolatedPoint( - const Trajectory & trajectory, const geometry_msgs::msg::Pose & target_pose, - const bool use_zero_order_hold_for_twist, const double dist_threshold, const double yaw_threshold) -{ - if (trajectory.points.empty()) { - TrajectoryPoint interpolated_point{}; - interpolated_point.pose = target_pose; - return interpolated_point; - } - if (trajectory.points.size() == 1) { - return trajectory.points.front(); - } - - const size_t segment_idx = - autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - trajectory.points, target_pose, dist_threshold, yaw_threshold); - - // 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_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; - } - - const double ratio = v1.dot(v2) / v1.length2(); - const double clamped_ratio = std::clamp(ratio, 0.0, 1.0); - - // Interpolate - TrajectoryPoint interpolated_point{}; - - // pose interpolation - interpolated_point.pose = autoware_utils::calc_interpolated_pose(curr_pt, next_pt, clamped_ratio); - - // twist interpolation - if (use_zero_order_hold_for_twist) { - interpolated_point.longitudinal_velocity_mps = curr_pt.longitudinal_velocity_mps; - interpolated_point.lateral_velocity_mps = curr_pt.lateral_velocity_mps; - interpolated_point.acceleration_mps2 = curr_pt.acceleration_mps2; - } else { - interpolated_point.longitudinal_velocity_mps = autoware::interpolation::lerp( - curr_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, clamped_ratio); - interpolated_point.lateral_velocity_mps = autoware::interpolation::lerp( - curr_pt.lateral_velocity_mps, next_pt.lateral_velocity_mps, clamped_ratio); - interpolated_point.acceleration_mps2 = autoware::interpolation::lerp( - curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); - } - - // heading rate interpolation - interpolated_point.heading_rate_rps = autoware::interpolation::lerp( - curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); - - // wheel interpolation - interpolated_point.front_wheel_angle_rad = autoware::interpolation::lerp( - curr_pt.front_wheel_angle_rad, next_pt.front_wheel_angle_rad, clamped_ratio); - interpolated_point.rear_wheel_angle_rad = autoware::interpolation::lerp( - curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); - - // time interpolation - const double interpolated_time = autoware::interpolation::lerp( - rclcpp::Duration(curr_pt.time_from_start).seconds(), - rclcpp::Duration(next_pt.time_from_start).seconds(), clamped_ratio); - interpolated_point.time_from_start = rclcpp::Duration::from_seconds(interpolated_time); - - return interpolated_point; -} - -PathPointWithLaneId calcInterpolatedPoint( - const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, - const bool use_zero_order_hold_for_twist, const double dist_threshold, const double yaw_threshold) -{ - if (path.points.empty()) { - PathPointWithLaneId interpolated_point{}; - interpolated_point.point.pose = target_pose; - return interpolated_point; - } - if (path.points.size() == 1) { - return path.points.front(); - } - - const size_t segment_idx = - autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, target_pose, dist_threshold, yaw_threshold); - - // 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_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; - } - - const double ratio = v1.dot(v2) / v1.length2(); - const double clamped_ratio = std::clamp(ratio, 0.0, 1.0); - - // Interpolate - PathPointWithLaneId interpolated_point{}; - - // pose interpolation - interpolated_point.point.pose = - autoware_utils::calc_interpolated_pose(curr_pt.point, next_pt.point, clamped_ratio); - - // twist interpolation - if (use_zero_order_hold_for_twist) { - interpolated_point.point.longitudinal_velocity_mps = curr_pt.point.longitudinal_velocity_mps; - interpolated_point.point.lateral_velocity_mps = curr_pt.point.lateral_velocity_mps; - } else { - interpolated_point.point.longitudinal_velocity_mps = autoware::interpolation::lerp( - curr_pt.point.longitudinal_velocity_mps, next_pt.point.longitudinal_velocity_mps, - clamped_ratio); - interpolated_point.point.lateral_velocity_mps = autoware::interpolation::lerp( - curr_pt.point.lateral_velocity_mps, next_pt.point.lateral_velocity_mps, clamped_ratio); - } - - // heading rate interpolation - interpolated_point.point.heading_rate_rps = autoware::interpolation::lerp( - curr_pt.point.heading_rate_rps, next_pt.point.heading_rate_rps, clamped_ratio); - - return interpolated_point; -} -} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/trajectory/path_shift.cpp b/common/autoware_motion_utils/src/trajectory/path_shift.cpp deleted file mode 100644 index 8864128f94bd9..0000000000000 --- a/common/autoware_motion_utils/src/trajectory/path_shift.cpp +++ /dev/null @@ -1,108 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/motion_utils/trajectory/path_shift.hpp" - -#include "autoware/motion_utils/trajectory/trajectory.hpp" - -#include -#include - -namespace autoware::motion_utils -{ -double calc_feasible_velocity_from_jerk( - const double lateral, const double jerk, const double longitudinal_distance) -{ - const double j = std::abs(jerk); - const double l = std::abs(lateral); - const double d = std::abs(longitudinal_distance); - if (j < 1.0e-8 || l < 1.0e-8) { - const std::string error_message( - std::string(__func__) + ": Failed to calculate velocity due to invalid arg"); - RCLCPP_WARN(get_logger(), "%s", error_message.c_str()); - return 1.0e10; - } - return d / (4.0 * std::pow(0.5 * l / j, 1.0 / 3.0)); -} - -double calc_lateral_dist_from_jerk( - const double longitudinal, const double jerk, const double velocity) -{ - const double j = std::abs(jerk); - const double d = std::abs(longitudinal); - const double v = std::abs(velocity); - - if (j < 1.0e-8 || v < 1.0e-8) { - const std::string error_message( - std::string(__func__) + ": Failed to calculate lateral distance due to invalid arg"); - RCLCPP_WARN(get_logger(), "%s", error_message.c_str()); - return 1.0e10; - } - return 2.0 * std::pow(d / (4.0 * v), 3.0) * j; -} - -double calc_longitudinal_dist_from_jerk( - const double lateral, const double jerk, const double velocity) -{ - const double j = std::abs(jerk); - const double l = std::abs(lateral); - const double v = std::abs(velocity); - if (j < 1.0e-8) { - const std::string error_message( - std::string(__func__) + ": Failed to calculate longitudinal distance due to invalid arg"); - RCLCPP_WARN(get_logger(), "%s", error_message.c_str()); - return 1.0e10; - } - return 4.0 * std::pow(0.5 * l / j, 1.0 / 3.0) * v; -} - -double calc_shift_time_from_jerk(const double lateral, const double jerk, const double acc) -{ - const double j = std::abs(jerk); - const double a = std::abs(acc); - const double l = std::abs(lateral); - if (j < 1.0e-8 || a < 1.0e-8) { - const std::string error_message( - std::string(__func__) + ": Failed to calculate shift time due to invalid arg"); - RCLCPP_WARN(get_logger(), "%s", error_message.c_str()); - return 1.0e10; // TODO(Horibe) maybe invalid arg? - } - - // time with constant jerk - double tj = a / j; - - // time with constant acceleration (zero jerk) - double ta = (std::sqrt(a * a + 4.0 * j * j * l / a) - 3.0 * a) / (2.0 * j); - - if (ta < 0.0) { - // it will not hit the acceleration limit this time - tj = std::pow(l / (2.0 * j), 1.0 / 3.0); - ta = 0.0; - } - - const double t_total = 4.0 * tj + 2.0 * ta; - return t_total; -} - -double calc_jerk_from_lat_lon_distance( - const double lateral, const double longitudinal, const double velocity) -{ - constexpr double ep = 1.0e-3; - const double lat = std::abs(lateral); - const double lon = std::max(std::abs(longitudinal), ep); - const double v = std::abs(velocity); - return 0.5 * lat * std::pow(4.0 * v / lon, 3); -} - -} // namespace autoware::motion_utils 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 deleted file mode 100644 index 4f9c329960b54..0000000000000 --- a/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp +++ /dev/null @@ -1,139 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" - -#include "autoware/interpolation/spline_interpolation_points_2d.hpp" -#include "autoware/motion_utils/trajectory/trajectory.hpp" - -#include -#include -#include - -namespace autoware::motion_utils -{ - -std::optional> getPathIndexRangeWithLaneId( - const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) -{ - size_t start_idx = 0; // NOTE: to prevent from maybe-uninitialized error - size_t end_idx = 0; // NOTE: to prevent from maybe-uninitialized error - - bool found_first_idx = false; - for (size_t i = 0; i < path.points.size(); ++i) { - const auto & p = path.points.at(i); - for (const auto & id : p.lane_ids) { - if (id == target_lane_id) { - if (!found_first_idx) { - start_idx = i; - found_first_idx = true; - } - end_idx = i; - } - } - } - - if (found_first_idx) { - // NOTE: In order to fully cover lanes with target_lane_id, start_idx and end_idx are expanded. - start_idx = static_cast(std::max(0, static_cast(start_idx) - 1)); - end_idx = std::min(path.points.size() - 1, end_idx + 1); - - return std::make_pair(start_idx, end_idx); - } - - return {}; -} - -size_t findNearestIndexFromLaneId( - const autoware_internal_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id) -{ - const auto opt_range = getPathIndexRangeWithLaneId(path, lane_id); - if (opt_range) { - const size_t start_idx = opt_range->first; - const size_t end_idx = opt_range->second; - - validateNonEmpty(path.points); - - const auto sub_points = std::vector{ - path.points.begin() + start_idx, path.points.begin() + end_idx + 1}; - validateNonEmpty(sub_points); - - return start_idx + findNearestIndex(sub_points, pos); - } - - return findNearestIndex(path.points, pos); -} - -size_t findNearestSegmentIndexFromLaneId( - const autoware_internal_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id) -{ - const size_t nearest_idx = findNearestIndexFromLaneId(path, pos, lane_id); - - if (nearest_idx == 0) { - return 0; - } - if (nearest_idx == path.points.size() - 1) { - return path.points.size() - 2; - } - - const double signed_length = calcLongitudinalOffsetToSegment(path.points, nearest_idx, pos); - - if (signed_length <= 0) { - return nearest_idx - 1; - } - - return nearest_idx; -} - -// NOTE: rear_to_cog is supposed to be positive -autoware_internal_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( - const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, - const bool enable_last_point_compensation) -{ - auto cog_path = path; - - // calculate curvature and yaw from spline interpolation - const auto spline = autoware::interpolation::SplineInterpolationPoints2d(path.points); - const auto curvature_vec = spline.getSplineInterpolatedCurvatures(); - const auto yaw_vec = spline.getSplineInterpolatedYaws(); - - for (size_t i = 0; i < path.points.size(); ++i) { - // calculate beta, which is CoG's velocity direction - const double beta = std::atan(rear_to_cog * curvature_vec.at(i)); - - // apply beta to CoG pose - geometry_msgs::msg::Pose cog_pose_with_beta; - cog_pose_with_beta.position = autoware_utils::get_point(path.points.at(i)); - cog_pose_with_beta.orientation = - autoware_utils::create_quaternion_from_yaw(yaw_vec.at(i) - beta); - - const auto rear_pose = - autoware_utils::calc_offset_pose(cog_pose_with_beta, -rear_to_cog, 0.0, 0.0); - - // update pose - autoware_utils::set_pose(rear_pose, cog_path.points.at(i)); - } - - // compensate for the last pose - if (enable_last_point_compensation) { - cog_path.points.back() = path.points.back(); - } - - insertOrientation(cog_path.points, true); - - return cog_path; -} -} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/trajectory/trajectory.cpp b/common/autoware_motion_utils/src/trajectory/trajectory.cpp deleted file mode 100644 index aa8d1c6b066cb..0000000000000 --- a/common/autoware_motion_utils/src/trajectory/trajectory.cpp +++ /dev/null @@ -1,645 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/motion_utils/trajectory/trajectory.hpp" - -#include -#include -#include - -namespace autoware::motion_utils -{ - -// -template void validateNonEmpty>( - const std::vector &); -template void -validateNonEmpty>( - const std::vector &); -template void validateNonEmpty>( - const std::vector &); - -// -template std::optional isDrivingForward>( - const std::vector &); -template std::optional -isDrivingForward>( - const std::vector &); -template std::optional -isDrivingForward>( - const std::vector &); - -// -template std::optional -isDrivingForwardWithTwist>( - const std::vector &); -template std::optional -isDrivingForwardWithTwist>( - const std::vector &); -template std::optional -isDrivingForwardWithTwist>( - const std::vector &); - -// -template std::vector -removeOverlapPoints>( - const std::vector & points, const size_t start_idx); -template std::vector -removeOverlapPoints>( - const std::vector & points, - const size_t start_idx); -template std::vector -removeOverlapPoints>( - const std::vector & points, const size_t start_idx); - -// -template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist, - const size_t src_idx, const size_t dst_idx); - -// -template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist, - const size_t src_idx); - -// -template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist); - -// -template size_t findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); -template size_t -findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); -template size_t findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); - -// -template std::optional -findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); -template std::optional -findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); -template std::optional -findNearestIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); - -// -template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, const size_t seg_idx, - const geometry_msgs::msg::Point & p_target, const bool throw_exception); -template double calcLongitudinalOffsetToSegment< - std::vector>( - const std::vector & points, - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); -template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, const size_t seg_idx, - const geometry_msgs::msg::Point & p_target, const bool throw_exception); - -// -template size_t findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); -template size_t -findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); -template size_t findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Point & point); - -// -template std::optional -findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); -template std::optional -findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); -template std::optional -findNearestSegmentIndex>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); - -// -template double calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); -template double -calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); -template double calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); - -// -template double calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const bool throw_exception); -template double -calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const bool throw_exception); -template double calcLateralOffset>( - const std::vector & points, - const geometry_msgs::msg::Point & p_target, const bool throw_exception); - -// -template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const size_t dst_idx); -template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const size_t dst_idx); - -// -template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, const size_t src_idx, - const size_t dst_idx); -template std::vector calcSignedArcLengthPartialSum< - std::vector>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); -template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, const size_t src_idx, - const size_t dst_idx); - -// -template double calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t dst_idx); -template double -calcSignedArcLength>( - const std::vector &, - const geometry_msgs::msg::Point & src_point, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector &, - const geometry_msgs::msg::Point & src_point, const size_t dst_idx); - -// -template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const geometry_msgs::msg::Point & dst_point); -template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point); -template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const geometry_msgs::msg::Point & dst_point); - -// -template double calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); -template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); -template double calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); - -// -template double calcArcLength>( - const std::vector & points); -template double -calcArcLength>( - const std::vector & points); -template double calcArcLength>( - const std::vector & points); - -// -template std::vector calcCurvature>( - const std::vector & points); -template std::vector -calcCurvature>( - const std::vector & points); -template std::vector -calcCurvature>( - const std::vector & points); - -// -template std::vector>> -calcCurvatureAndSegmentLength>( - const std::vector & points); -template std::vector>> calcCurvatureAndSegmentLength< - std::vector>( - const std::vector & points); -template std::vector>> -calcCurvatureAndSegmentLength>( - const std::vector & points); - -// -template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, - const size_t src_idx); - -// -template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, const size_t src_idx, - const double offset, const bool throw_exception); -template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const size_t src_idx, const double offset, const bool throw_exception); -template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, const size_t src_idx, - const double offset, const bool throw_exception); - -// -template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset); -template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset); -template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset); - -// -template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, const size_t src_idx, - const double offset, const bool set_orientation_from_position_direction, - const bool throw_exception); -template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, - const bool throw_exception); -template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, const size_t src_idx, - const double offset, const bool set_orientation_from_position_direction, - const bool throw_exception); - -// -template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset, - const bool set_orientation_from_position_direction); -template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset, - const bool set_orientation_from_position_direction); -template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const double offset, - const bool set_orientation_from_position_direction); - -// -template std::optional -insertTargetPoint>( - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, const double overlap_threshold); -template std::optional -insertTargetPoint>( - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold); -template std::optional -insertTargetPoint>( - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold); - -// -template std::optional -insertTargetPoint>( - const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, const double overlap_threshold); -template std::optional -insertTargetPoint>( - const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold); -template std::optional -insertTargetPoint>( - const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold); - -// -template std::optional -insertTargetPoint>( - const size_t src_segment_idx, const double insert_point_length, - std::vector & points, const double overlap_threshold); -template std::optional -insertTargetPoint>( - const size_t src_segment_idx, const double insert_point_length, - std::vector & points, - const double overlap_threshold); -template std::optional -insertTargetPoint>( - const size_t src_segment_idx, const double insert_point_length, - std::vector & points, - const double overlap_threshold); - -// -template std::optional -insertTargetPoint>( - const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, const double max_dist, - const double max_yaw, const double overlap_threshold); -template std::optional -insertTargetPoint>( - const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, - const double max_dist, const double max_yaw, const double overlap_threshold); -template std::optional -insertTargetPoint>( - const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, const double max_dist, - const double max_yaw, const double overlap_threshold); - -// -template std::optional insertStopPoint>( - const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold = 1e-3); -template std::optional -insertStopPoint>( - const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold = 1e-3); -template std::optional -insertStopPoint>( - const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold = 1e-3); - -// -template std::optional insertStopPoint>( - const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold); -template std::optional -insertStopPoint>( - const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold); -template std::optional -insertStopPoint>( - const double distance_to_stop_point, - std::vector & points_with_twist, - const double overlap_threshold); - -// -template std::optional insertStopPoint>( - const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, const double max_dist, - const double max_yaw, const double overlap_threshold); -template std::optional -insertStopPoint>( - const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, - const double max_dist, const double max_yaw, const double overlap_threshold); -template std::optional -insertStopPoint>( - const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, - const double max_dist, const double max_yaw, const double overlap_threshold); - -// -template std::optional -insertStopPoint>( - const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, - std::vector & points_with_twist, - const double overlap_threshold); - -// -template std::optional -insertDecelPoint>( - const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, - const double velocity, - std::vector & points_with_twist); - -// -template void insertOrientation>( - std::vector & points, const bool is_driving_forward); -template void -insertOrientation>( - std::vector & points, - const bool is_driving_forward); -template void insertOrientation>( - std::vector & points, - const bool is_driving_forward); - -// -template double calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); - -// -template double calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); -template double -calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector & points, - const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); - -// -template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, - const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); - -// -template size_t -findFirstNearestIndexWithSoftConstraints>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); -template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); -template size_t -findFirstNearestIndexWithSoftConstraints>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); - -// -template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); -template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); -template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); - -// -template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, - const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); - -// -template std::vector -cropForwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length); -template std::vector -cropForwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length); -template std::vector -cropForwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length); - -// -template std::vector -cropBackwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double backward_length); -template std::vector -cropBackwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double backward_length); -template std::vector -cropBackwardPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double backward_length); - -// -template std::vector -cropPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length, const double backward_length); -template std::vector -cropPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length, const double backward_length); -template std::vector -cropPoints>( - const std::vector & points, - const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, - const double forward_length, const double backward_length); - -// -template double calcYawDeviation>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const bool throw_exception); -template double -calcYawDeviation>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const bool throw_exception); -template double calcYawDeviation>( - const std::vector & points, - const geometry_msgs::msg::Pose & pose, const bool throw_exception); - -// -template bool isTargetPointFront>( - const std::vector & points, - const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, - const double threshold); -template bool -isTargetPointFront>( - const std::vector & points, - const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, - const double threshold); -template bool isTargetPointFront>( - const std::vector & points, - const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, - const double threshold); - -void calculate_time_from_start( - std::vector & trajectory, - const geometry_msgs::msg::Point & current_ego_point, const float min_velocity) -{ - const auto nearest_segment_idx = findNearestSegmentIndex(trajectory, current_ego_point); - if (nearest_segment_idx + 1 == trajectory.size()) { - return; - } - for (auto & p : trajectory) { - p.time_from_start = rclcpp::Duration::from_seconds(0); - } - // TODO(Maxime): some points can have very low velocities which introduce huge time errors - // Temporary solution: use a minimum velocity - for (auto idx = nearest_segment_idx + 1; idx < trajectory.size(); ++idx) { - const auto & from = trajectory[idx - 1]; - const auto velocity = std::max(min_velocity, from.longitudinal_velocity_mps); - if (velocity != 0.0) { - auto & to = trajectory[idx]; - const auto t = autoware_utils::calc_distance2d(from, to) / velocity; - to.time_from_start = rclcpp::Duration::from_seconds(t) + from.time_from_start; - } - } -} -} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/vehicle/vehicle_state_checker.cpp b/common/autoware_motion_utils/src/vehicle/vehicle_state_checker.cpp deleted file mode 100644 index e2a4754a5653e..0000000000000 --- a/common/autoware_motion_utils/src/vehicle/vehicle_state_checker.cpp +++ /dev/null @@ -1,135 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/motion_utils/vehicle/vehicle_state_checker.hpp" - -#include "autoware/motion_utils/trajectory/trajectory.hpp" - -#include - -namespace autoware::motion_utils -{ -VehicleStopCheckerBase::VehicleStopCheckerBase(rclcpp::Node * node, double buffer_duration) -: clock_(node->get_clock()), logger_(node->get_logger()) -{ - buffer_duration_ = buffer_duration; -} - -void VehicleStopCheckerBase::addTwist(const TwistStamped & twist) -{ - twist_buffer_.push_front(twist); - - const auto now = clock_->now(); - while (!twist_buffer_.empty()) { - // Check oldest data time - const auto time_diff = now - twist_buffer_.back().header.stamp; - - // Finish when oldest data is newer than threshold - if (time_diff.seconds() <= buffer_duration_) { - break; - } - - // Remove old data - twist_buffer_.pop_back(); - } -} - -bool VehicleStopCheckerBase::isVehicleStopped(const double stop_duration) const -{ - if (twist_buffer_.empty()) { - return false; - } - - constexpr double squared_stop_velocity = 1e-3 * 1e-3; - const auto now = clock_->now(); - - const auto time_buffer_back = now - twist_buffer_.back().header.stamp; - if (time_buffer_back.seconds() < stop_duration) { - return false; - } - - // Get velocities within stop_duration - for (const auto & velocity : twist_buffer_) { - double x = velocity.twist.linear.x; - double y = velocity.twist.linear.y; - double z = velocity.twist.linear.z; - double v = (x * x) + (y * y) + (z * z); - if (squared_stop_velocity <= v) { - return false; - } - - const auto time_diff = now - velocity.header.stamp; - if (time_diff.seconds() >= stop_duration) { - break; - } - } - - return true; -} - -VehicleStopChecker::VehicleStopChecker(rclcpp::Node * node) -: VehicleStopCheckerBase(node, velocity_buffer_time_sec) -{ - using std::placeholders::_1; - - sub_odom_ = node->create_subscription( - "/localization/kinematic_state", rclcpp::QoS(1), - std::bind(&VehicleStopChecker::onOdom, this, _1)); -} - -void VehicleStopChecker::onOdom(const Odometry::ConstSharedPtr msg) -{ - odometry_ptr_ = msg; - - TwistStamped current_velocity; - current_velocity.header = msg->header; - current_velocity.twist = msg->twist.twist; - addTwist(current_velocity); -} - -VehicleArrivalChecker::VehicleArrivalChecker(rclcpp::Node * node) : VehicleStopChecker(node) -{ - using std::placeholders::_1; - - sub_trajectory_ = node->create_subscription( - "/planning/scenario_planning/trajectory", rclcpp::QoS(1), - std::bind(&VehicleArrivalChecker::onTrajectory, this, _1)); -} - -bool VehicleArrivalChecker::isVehicleStoppedAtStopPoint(const double stop_duration) const -{ - if (!odometry_ptr_ || !trajectory_ptr_) { - return false; - } - - if (!isVehicleStopped(stop_duration)) { - return false; - } - - const auto & p = odometry_ptr_->pose.pose.position; - const auto idx = autoware::motion_utils::searchZeroVelocityIndex(trajectory_ptr_->points); - - if (!idx) { - return false; - } - - return std::abs(autoware::motion_utils::calcSignedArcLength( - trajectory_ptr_->points, p, idx.value())) < th_arrived_distance_m; -} - -void VehicleArrivalChecker::onTrajectory(const Trajectory::ConstSharedPtr msg) -{ - trajectory_ptr_ = msg; -} -} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/test/src/distance/test_distance.cpp b/common/autoware_motion_utils/test/src/distance/test_distance.cpp deleted file mode 100644 index f6d6a9cc4dafd..0000000000000 --- a/common/autoware_motion_utils/test/src/distance/test_distance.cpp +++ /dev/null @@ -1,116 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/motion_utils/distance/distance.hpp" -#include "gtest/gtest.h" -namespace -{ -using autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints; - -constexpr double epsilon = 1e-3; - -TEST(distance, calcDecelDistWithJerkAndAccConstraints) -{ - // invalid velocity - { - constexpr double current_vel = 16.7; - constexpr double target_vel = 20.0; - constexpr double current_acc = 0.0; - constexpr double acc_min = -0.5; - constexpr double jerk_acc = 1.0; - constexpr double jerk_dec = -0.5; - - const auto dist = calcDecelDistWithJerkAndAccConstraints( - current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec); - - EXPECT_FALSE(dist); - } - - // normal stop - { - constexpr double current_vel = 16.7; - constexpr double target_vel = 0.0; - constexpr double current_acc = 0.0; - constexpr double acc_min = -0.5; - constexpr double jerk_acc = 1.0; - constexpr double jerk_dec = -0.5; - - constexpr double expected_dist = 287.224; - const auto dist = calcDecelDistWithJerkAndAccConstraints( - current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec); - EXPECT_NEAR(expected_dist, *dist, epsilon); - } - - // sudden stop - { - constexpr double current_vel = 16.7; - constexpr double target_vel = 0.0; - constexpr double current_acc = 0.0; - constexpr double acc_min = -2.5; - constexpr double jerk_acc = 1.5; - constexpr double jerk_dec = -1.5; - - constexpr double expected_dist = 69.6947; - const auto dist = calcDecelDistWithJerkAndAccConstraints( - current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec); - EXPECT_NEAR(expected_dist, *dist, epsilon); - } - - // normal deceleration - { - constexpr double current_vel = 16.7; - constexpr double target_vel = 10.0; - constexpr double current_acc = 0.0; - constexpr double acc_min = -0.5; - constexpr double jerk_acc = 1.0; - constexpr double jerk_dec = -0.5; - - constexpr double expected_dist = 189.724; - const auto dist = calcDecelDistWithJerkAndAccConstraints( - current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec); - EXPECT_NEAR(expected_dist, *dist, epsilon); - } - - // sudden deceleration - { - constexpr double current_vel = 16.7; - constexpr double target_vel = 10.0; - constexpr double current_acc = 0.0; - constexpr double acc_min = -2.5; - constexpr double jerk_acc = 1.5; - constexpr double jerk_dec = -1.5; - - constexpr double expected_dist = 58.028; - const auto dist = calcDecelDistWithJerkAndAccConstraints( - current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec); - EXPECT_NEAR(expected_dist, *dist, epsilon); - } - - // current_acc is lower than acc_min - { - constexpr double current_vel = 16.7; - constexpr double target_vel = 0.0; - constexpr double current_acc = -2.5; - constexpr double acc_min = -0.5; - constexpr double jerk_acc = 1.0; - constexpr double jerk_dec = -0.5; - - constexpr double expected_dist = 217.429; - const auto dist = calcDecelDistWithJerkAndAccConstraints( - current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec); - EXPECT_NEAR(expected_dist, *dist, epsilon); - } -} - -} // namespace diff --git a/common/autoware_motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp b/common/autoware_motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp deleted file mode 100644 index afce7f953e644..0000000000000 --- a/common/autoware_motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp +++ /dev/null @@ -1,117 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/motion_utils/marker/virtual_wall_marker_creator.hpp" -#include "gtest/gtest.h" - -#include - -namespace -{ -constexpr auto wall_ns_suffix = "_virtual_wall"; -constexpr auto text_ns_suffix = "_factor_text"; - -bool has_ns_id( - const visualization_msgs::msg::MarkerArray & marker_array, const std::string & ns, const int id) -{ - return std::find_if(marker_array.markers.begin(), marker_array.markers.end(), [&](const auto m) { - return m.id == id && m.ns == ns; - }) != marker_array.markers.end(); -} - -bool has_ns_id( - const visualization_msgs::msg::MarkerArray & marker_array, const std::string & ns, - const int id_from, const int id_to) -{ - for (auto id = id_from; id <= id_to; ++id) - if (!has_ns_id(marker_array, ns, id)) return false; - return true; -} - -TEST(VirtualWallMarkerCreator, oneWall) -{ - autoware::motion_utils::VirtualWall wall; - autoware::motion_utils::VirtualWallMarkerCreator creator; - wall.style = autoware::motion_utils::VirtualWallType::stop; - wall.pose.position.x = 1.0; - wall.pose.position.y = 2.0; - creator.add_virtual_wall(wall); - auto markers = creator.create_markers(); - ASSERT_EQ(markers.markers.size(), 2UL); - EXPECT_TRUE(has_ns_id(markers, std::string("stop") + wall_ns_suffix, 0)); - EXPECT_TRUE(has_ns_id(markers, std::string("stop") + text_ns_suffix, 0)); - for (const auto & marker : markers.markers) { - EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::ADD); - EXPECT_EQ(marker.pose.position.x, 1.0); - EXPECT_EQ(marker.pose.position.y, 2.0); - } - markers = creator.create_markers(); - ASSERT_EQ(markers.markers.size(), 2UL); - EXPECT_TRUE(has_ns_id(markers, std::string("stop") + wall_ns_suffix, 0)); - EXPECT_TRUE(has_ns_id(markers, std::string("stop") + text_ns_suffix, 0)); - for (const auto & marker : markers.markers) - EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::DELETE); -} - -TEST(VirtualWallMarkerCreator, manyWalls) -{ - autoware::motion_utils::VirtualWall wall; - autoware::motion_utils::VirtualWallMarkerCreator creator; - wall.style = autoware::motion_utils::VirtualWallType::stop; - wall.ns = "ns1_"; - creator.add_virtual_wall(wall); - creator.add_virtual_wall(wall); - creator.add_virtual_wall(wall); - wall.ns = "ns2_"; - creator.add_virtual_wall(wall); - wall.style = autoware::motion_utils::VirtualWallType::slowdown; - wall.ns = "ns2_"; - creator.add_virtual_wall(wall); - creator.add_virtual_wall(wall); - wall.ns = "ns3_"; - creator.add_virtual_wall(wall); - creator.add_virtual_wall(wall); - creator.add_virtual_wall(wall); - wall.style = autoware::motion_utils::VirtualWallType::deadline; - wall.ns = "ns1_"; - creator.add_virtual_wall(wall); - wall.ns = "ns2_"; - creator.add_virtual_wall(wall); - wall.ns = "ns3_"; - creator.add_virtual_wall(wall); - - auto markers = creator.create_markers(); - ASSERT_EQ(markers.markers.size(), 12UL * 2); - EXPECT_TRUE(has_ns_id(markers, std::string("ns1_stop") + wall_ns_suffix, 0, 2)); - EXPECT_TRUE(has_ns_id(markers, std::string("ns1_stop") + text_ns_suffix, 0, 2)); - EXPECT_TRUE(has_ns_id(markers, std::string("ns2_stop") + wall_ns_suffix, 0)); - EXPECT_TRUE(has_ns_id(markers, std::string("ns2_stop") + text_ns_suffix, 0)); - EXPECT_TRUE(has_ns_id(markers, std::string("ns2_slow_down") + wall_ns_suffix, 0, 1)); - EXPECT_TRUE(has_ns_id(markers, std::string("ns2_slow_down") + text_ns_suffix, 0, 1)); - EXPECT_TRUE(has_ns_id(markers, std::string("ns3_slow_down") + wall_ns_suffix, 0, 2)); - EXPECT_TRUE(has_ns_id(markers, std::string("ns3_slow_down") + text_ns_suffix, 0, 2)); - EXPECT_TRUE(has_ns_id(markers, std::string("ns1_dead_line") + wall_ns_suffix, 0)); - EXPECT_TRUE(has_ns_id(markers, std::string("ns1_dead_line") + text_ns_suffix, 0)); - EXPECT_TRUE(has_ns_id(markers, std::string("ns2_dead_line") + wall_ns_suffix, 0)); - EXPECT_TRUE(has_ns_id(markers, std::string("ns2_dead_line") + text_ns_suffix, 0)); - EXPECT_TRUE(has_ns_id(markers, std::string("ns3_dead_line") + wall_ns_suffix, 0)); - EXPECT_TRUE(has_ns_id(markers, std::string("ns3_dead_line") + text_ns_suffix, 0)); - markers = creator.create_markers(); - ASSERT_EQ(markers.markers.size(), 12UL * 2); - for (const auto & marker : markers.markers) - EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::DELETE); - markers = creator.create_markers(); - ASSERT_TRUE(markers.markers.empty()); -} -} // namespace diff --git a/common/autoware_motion_utils/test/src/resample/test_resample.cpp b/common/autoware_motion_utils/test/src/resample/test_resample.cpp deleted file mode 100644 index a7ff8037b7311..0000000000000 --- a/common/autoware_motion_utils/test/src/resample/test_resample.cpp +++ /dev/null @@ -1,3622 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/motion_utils/constants.hpp" -#include "autoware/motion_utils/resample/resample.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 -#include - -#include -#include - -namespace -{ -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; - -geometry_msgs::msg::Pose createPose( - double x, double y, double z, double roll, double pitch, double yaw) -{ - geometry_msgs::msg::Pose p; - p.position = create_point(x, y, z); - p.orientation = create_quaternion_from_rpy(roll, pitch, yaw); - return p; -} - -PathPointWithLaneId generateTestPathPointWithLaneId( - const double x, const double y, const double z, const double theta = 0.0, - const double vel_lon = 0.0, const double vel_lat = 0.0, const double heading_rate = 0.0, - const bool is_final = false, const std::vector & lane_ids = {}) -{ - PathPointWithLaneId p; - p.point.pose = createPose(x, y, z, 0.0, 0.0, theta); - p.point.longitudinal_velocity_mps = vel_lon; - p.point.lateral_velocity_mps = vel_lat; - p.point.heading_rate_rps = heading_rate; - p.point.is_final = is_final; - p.lane_ids = lane_ids; - return p; -} - -PathPoint generateTestPathPoint( - const double x, const double y, const double z, const double theta = 0.0, - const double vel_lon = 0.0, const double vel_lat = 0.0, const double heading_rate = 0.0) -{ - PathPoint p; - p.pose = createPose(x, y, z, 0.0, 0.0, theta); - p.longitudinal_velocity_mps = vel_lon; - p.lateral_velocity_mps = vel_lat; - p.heading_rate_rps = heading_rate; - return p; -} - -TrajectoryPoint generateTestTrajectoryPoint( - const double x, const double y, const double z, const double theta = 0.0, - const double vel_lon = 0.0, const double vel_lat = 0.0, const double heading_rate = 0.0, - const double acc = 0.0) -{ - TrajectoryPoint p; - p.pose = createPose(x, y, z, 0.0, 0.0, theta); - p.longitudinal_velocity_mps = vel_lon; - p.lateral_velocity_mps = vel_lat; - p.heading_rate_rps = heading_rate; - p.acceleration_mps2 = acc; - return p; -} - -PathWithLaneId generateTestPathWithLaneId( - const size_t num_points, const double point_interval, const double vel_lon = 0.0, - const double vel_lat = 0.0, const double heading_rate_rps = 0.0, const double init_theta = 0.0, - const double delta_theta = 0.0) -{ - PathWithLaneId path; - for (size_t i = 0; i < num_points; ++i) { - const double theta = init_theta + i * delta_theta; - const double x = i * point_interval * std::cos(theta); - const double y = i * point_interval * std::sin(theta); - - PathPointWithLaneId p; - p.point.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); - p.point.longitudinal_velocity_mps = vel_lon; - p.point.lateral_velocity_mps = vel_lat; - p.point.heading_rate_rps = heading_rate_rps; - p.point.is_final = (i == num_points - 1); - p.lane_ids = {static_cast(i)}; - path.points.push_back(p); - } - - return path; -} - -template -T generateTestPath( - const size_t num_points, const double point_interval, const double vel_lon = 0.0, - const double vel_lat = 0.0, const double heading_rate_rps = 0.0, const double init_theta = 0.0, - const double delta_theta = 0.0) -{ - using Point = typename T::_points_type::value_type; - - T traj; - for (size_t i = 0; i < num_points; ++i) { - const double theta = init_theta + i * delta_theta; - const double x = i * point_interval * std::cos(theta); - const double y = i * point_interval * std::sin(theta); - - Point p; - p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); - p.longitudinal_velocity_mps = vel_lon; - p.lateral_velocity_mps = vel_lat; - p.heading_rate_rps = heading_rate_rps; - traj.points.push_back(p); - } - - return traj; -} - -template -T generateTestTrajectory( - const size_t num_points, const double point_interval, const double vel_lon = 0.0, - const double vel_lat = 0.0, const double heading_rate_rps = 0.0, const double acc = 0.0, - const double init_theta = 0.0, const double delta_theta = 0.0) -{ - using Point = typename T::_points_type::value_type; - - T traj; - for (size_t i = 0; i < num_points; ++i) { - const double theta = init_theta + i * delta_theta; - const double x = i * point_interval * std::cos(theta); - const double y = i * point_interval * std::sin(theta); - - Point p; - p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); - p.longitudinal_velocity_mps = vel_lon; - p.lateral_velocity_mps = vel_lat; - p.heading_rate_rps = heading_rate_rps; - p.acceleration_mps2 = acc; - traj.points.push_back(p); - } - - return traj; -} - -std::vector generateArclength(const size_t num_points, const double interval) -{ - std::vector resampled_arclength(num_points); - for (size_t i = 0; i < num_points; ++i) { - resampled_arclength.at(i) = i * interval; - } - - return resampled_arclength; -} - -template -std::vector setZeroVelocityAfterStop(const std::vector & traj_points) -{ - std::vector resampled_traj_points; - bool stop_point_found = false; - for (auto p : traj_points) { - if (!stop_point_found && p.longitudinal_velocity_mps < std::numeric_limits::epsilon()) { - stop_point_found = true; - } - if (stop_point_found) { - p.longitudinal_velocity_mps = 0.0; - } - resampled_traj_points.push_back(p); - } - return resampled_traj_points; -} - -} // namespace - -TEST(resample_vector_pose, resample_by_same_interval) -{ - using autoware::motion_utils::resamplePoseVector; - using geometry_msgs::msg::Pose; - - std::vector path(10); - for (size_t i = 0; i < 10; ++i) { - path.at(i) = createPose(i * 1.0, 0.0, 0.0, 0.0, 0.0, 0.0); - } - - // same interval - { - const auto resampled_path = resamplePoseVector(path, 1.0); - EXPECT_EQ(path.size(), resampled_path.size()); - for (size_t i = 0; i < path.size(); ++i) { - const auto & p = resampled_path.at(i); - const auto & ans_p = path.at(i); - EXPECT_NEAR(p.position.x, ans_p.position.x, epsilon); - EXPECT_NEAR(p.position.y, ans_p.position.y, epsilon); - EXPECT_NEAR(p.position.z, ans_p.position.z, epsilon); - EXPECT_NEAR(p.orientation.x, ans_p.orientation.x, epsilon); - EXPECT_NEAR(p.orientation.y, ans_p.orientation.y, epsilon); - EXPECT_NEAR(p.orientation.z, ans_p.orientation.z, epsilon); - EXPECT_NEAR(p.orientation.w, ans_p.orientation.w, epsilon); - } - } - - // random - { - const auto resampled_path = resamplePoseVector(path, 0.5); - for (size_t i = 0; i < path.size(); ++i) { - const auto & p = resampled_path.at(i); - EXPECT_NEAR(p.position.x, 0.5 * i, epsilon); - EXPECT_NEAR(p.position.y, 0.0, epsilon); - EXPECT_NEAR(p.position.z, 0.0, epsilon); - EXPECT_NEAR(p.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.orientation.w, 1.0, epsilon); - } - } -} - -TEST(resample_path_with_lane_id, resample_path_by_vector) -{ - using autoware::motion_utils::resamplePath; - // Output is same as input - { - auto path = generateTestPathWithLaneId(10, 1.0, 3.0, 1.0, 0.01); - std::vector resampled_arclength = generateArclength(10, 1.0); - - { - const auto resampled_path = resamplePath(path, resampled_arclength); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - const auto p = resampled_path.points.at(i); - const auto ans_p = path.points.at(i); - 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); - EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon); - EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon); - EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon); - EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon); - EXPECT_NEAR( - p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); - EXPECT_EQ(p.point.is_final, ans_p.point.is_final); - for (size_t j = 0; j < p.lane_ids.size(); ++j) { - EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j)); - } - } - } - - // Change the last point orientation - path.points.back() = generateTestPathPointWithLaneId( - 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) { - const auto p = resampled_path.points.at(i); - const auto ans_p = path.points.at(i); - 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); - EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon); - EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon); - EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon); - EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon); - EXPECT_NEAR( - p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); - EXPECT_EQ(p.point.is_final, ans_p.point.is_final); - for (size_t j = 0; j < p.lane_ids.size(); ++j) { - EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j)); - } - } - - const auto p = resampled_path.points.back(); - const auto ans_p = path.points.back(); - 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); - EXPECT_NEAR(p.point.pose.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p.point.pose.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p.point.pose.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p.point.pose.orientation.w, ans_quat.w, epsilon); - EXPECT_NEAR( - p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); - EXPECT_EQ(p.point.is_final, ans_p.point.is_final); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), ans_p.lane_ids.at(i)); - } - } - } - - // Output key is not same as input - { - autoware_internal_planning_msgs::msg::PathWithLaneId path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); - } - path.points.back().point.is_final = true; - std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; - - const auto resampled_path = resamplePath(path, resampled_arclength); - - { - const auto p = resampled_path.points.at(0); - EXPECT_NEAR(p.point.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.0, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 0); - } - } - - { - const auto p = resampled_path.points.at(1); - EXPECT_NEAR(p.point.pose.position.x, 1.2, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.12, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 1); - } - } - - { - const auto p = resampled_path.points.at(2); - EXPECT_NEAR(p.point.pose.position.x, 1.5, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.15, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 1); - } - } - - { - const auto p = resampled_path.points.at(3); - EXPECT_NEAR(p.point.pose.position.x, 5.3, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 5.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 2.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.53, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 5); - } - } - - { - const auto p = resampled_path.points.at(4); - EXPECT_NEAR(p.point.pose.position.x, 7.5, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 7.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 3.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.75, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 7); - } - } - - { - const auto p = resampled_path.points.at(5); - EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 9.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 4.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon); - EXPECT_EQ(p.point.is_final, true); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 9); - } - } - - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - const auto p = resampled_path.points.at(i); - EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon); - } - } - - // Duplicated points in the original path - { - autoware_internal_planning_msgs::msg::PathWithLaneId path; - path.points.resize(11); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); - } - path.points.at(6) = path.points.at(5); - std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; - - const auto resampled_path = resamplePath(path, resampled_arclength); - - EXPECT_EQ(path.points.size(), resampled_path.points.size()); - for (size_t i = 0; i < path.points.size(); ++i) { - const auto p = path.points.at(i); - const auto resampled_p = resampled_path.points.at(i); - EXPECT_NEAR(p.point.pose.position.x, resampled_p.point.pose.position.x, epsilon); - EXPECT_NEAR(p.point.pose.position.y, resampled_p.point.pose.position.y, epsilon); - EXPECT_NEAR(p.point.pose.position.z, resampled_p.point.pose.position.z, epsilon); - EXPECT_NEAR( - p.point.longitudinal_velocity_mps, resampled_p.point.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, resampled_p.point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, resampled_p.point.heading_rate_rps, epsilon); - EXPECT_EQ(p.point.is_final, resampled_p.point.is_final); - EXPECT_NEAR(p.point.pose.orientation.x, resampled_p.point.pose.orientation.x, epsilon); - EXPECT_NEAR(p.point.pose.orientation.y, resampled_p.point.pose.orientation.y, epsilon); - EXPECT_NEAR(p.point.pose.orientation.z, resampled_p.point.pose.orientation.z, epsilon); - EXPECT_NEAR(p.point.pose.orientation.w, resampled_p.point.pose.orientation.w, epsilon); - } - } - - // No Interpolation - { - // Input path size is not enough for interpolation - { - autoware_internal_planning_msgs::msg::PathWithLaneId path; - path.points.resize(1); - for (size_t i = 0; i < 1; ++i) { - path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, true, {0}); - } - std::vector resampled_arclength = generateArclength(10, 1.0); - - const auto resampled_path = resamplePath(path, resampled_arclength); - EXPECT_EQ(resampled_path.points.size(), path.points.size()); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - const auto p = resampled_path.points.at(i); - const auto ans_p = path.points.at(i); - 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); - EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon); - EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon); - EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon); - EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon); - EXPECT_NEAR( - p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); - EXPECT_EQ(p.point.is_final, ans_p.point.is_final); - for (size_t j = 0; j < p.lane_ids.size(); ++j) { - EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j)); - } - } - } - - // Resampled Arclength size is not enough for interpolation - { - autoware_internal_planning_msgs::msg::PathWithLaneId path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, true, {static_cast(i)}); - } - path.points.back().point.is_final = false; - std::vector resampled_arclength = generateArclength(1, 1.0); - - const auto resampled_path = resamplePath(path, resampled_arclength); - EXPECT_EQ(resampled_path.points.size(), path.points.size()); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - const auto p = resampled_path.points.at(i); - const auto ans_p = path.points.at(i); - 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); - EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon); - EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon); - EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon); - EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon); - EXPECT_NEAR( - p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); - EXPECT_EQ(p.point.is_final, ans_p.point.is_final); - for (size_t j = 0; j < p.lane_ids.size(); ++j) { - EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j)); - } - } - } - - // Resampled Arclength is longer than input path - { - autoware_internal_planning_msgs::msg::PathWithLaneId path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, true, {static_cast(i)}); - } - path.points.back().point.is_final = false; - std::vector resampled_arclength = generateArclength(3, 5.0); - - const auto resampled_path = resamplePath(path, resampled_arclength); - EXPECT_EQ(resampled_path.points.size(), path.points.size()); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - const auto p = resampled_path.points.at(i); - const auto ans_p = path.points.at(i); - 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); - EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon); - EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon); - EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon); - EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon); - EXPECT_NEAR( - p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); - EXPECT_EQ(p.point.is_final, ans_p.point.is_final); - for (size_t j = 0; j < p.lane_ids.size(); ++j) { - EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j)); - } - } - } - } -} - -TEST(resample_path_with_lane_id, resample_path_by_vector_backward) -{ - using autoware::motion_utils::resamplePath; - - { - autoware_internal_planning_msgs::msg::PathWithLaneId path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, autoware_utils::pi, i * 1.0, i * 0.5, i * 0.1, false, - {static_cast(i)}); - } - path.points.back().point.is_final = true; - std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; - - const auto resampled_path = resamplePath(path, resampled_arclength); - - { - const auto p = resampled_path.points.at(0); - EXPECT_NEAR(p.point.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.0, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 0); - } - } - - { - const auto p = resampled_path.points.at(1); - EXPECT_NEAR(p.point.pose.position.x, 1.2, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.12, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 1); - } - } - - { - const auto p = resampled_path.points.at(2); - EXPECT_NEAR(p.point.pose.position.x, 1.5, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.15, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 1); - } - } - - { - const auto p = resampled_path.points.at(3); - EXPECT_NEAR(p.point.pose.position.x, 5.3, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 5.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 2.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.53, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 5); - } - } - - { - const auto p = resampled_path.points.at(4); - EXPECT_NEAR(p.point.pose.position.x, 7.5, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 7.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 3.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.75, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 7); - } - } - - { - const auto p = resampled_path.points.at(5); - EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 9.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 4.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon); - EXPECT_EQ(p.point.is_final, true); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 9); - } - } - - 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); - EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); - } - } - - // change initial orientation - { - autoware_internal_planning_msgs::msg::PathWithLaneId path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); - } - path.points.back().point.is_final = true; - path.points.at(0).point.pose.orientation = - 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); - - { - const auto p = resampled_path.points.at(0); - EXPECT_NEAR(p.point.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.0, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 0); - } - } - - { - const auto p = resampled_path.points.at(1); - EXPECT_NEAR(p.point.pose.position.x, 1.2, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.12, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 1); - } - } - - { - const auto p = resampled_path.points.at(2); - EXPECT_NEAR(p.point.pose.position.x, 1.5, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.15, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 1); - } - } - - { - const auto p = resampled_path.points.at(3); - EXPECT_NEAR(p.point.pose.position.x, 5.3, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 5.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 2.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.53, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 5); - } - } - - { - const auto p = resampled_path.points.at(4); - EXPECT_NEAR(p.point.pose.position.x, 7.5, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 7.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 3.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.75, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 7); - } - } - - { - const auto p = resampled_path.points.at(5); - EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 9.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 4.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon); - EXPECT_EQ(p.point.is_final, true); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 9); - } - } - - // Initial Orientation - { - 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); - EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); - } - - 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); - EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); - } - } -} - -TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) -{ - using autoware::motion_utils::resamplePath; - - // Lerp x, y - { - autoware_internal_planning_msgs::msg::PathWithLaneId path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); - } - path.points.back().point.is_final = true; - std::vector resampled_arclength = {0.0, 1.2, 5.3, 9.0}; - - const auto resampled_path = resamplePath(path, resampled_arclength, true); - { - const auto p = resampled_path.points.at(0); - EXPECT_NEAR(p.point.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.0, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 0); - } - } - - { - const auto p = resampled_path.points.at(1); - EXPECT_NEAR(p.point.pose.position.x, 1.2, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.12, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 1); - } - } - - { - const auto p = resampled_path.points.at(2); - EXPECT_NEAR(p.point.pose.position.x, 5.3, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 5.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 2.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.53, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 5); - } - } - - { - const auto p = resampled_path.points.at(3); - EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 9.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 4.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon); - EXPECT_EQ(p.point.is_final, true); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 9); - } - } - - 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, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - } - } - - // Slerp z - { - autoware_internal_planning_msgs::msg::PathWithLaneId path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, i * 1.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); - } - path.points.back().point.is_final = true; - std::vector resampled_arclength = {0.0, 1.2, 5.3, 9.0}; - - const auto resampled_path = resamplePath(path, resampled_arclength, false, false); - { - const auto p = resampled_path.points.at(0); - EXPECT_NEAR(p.point.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.0, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 0); - } - } - - { - const auto p = resampled_path.points.at(1); - EXPECT_NEAR(p.point.pose.position.x, 1.2, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 1.2, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.12, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 1); - } - } - - { - const auto p = resampled_path.points.at(2); - EXPECT_NEAR(p.point.pose.position.x, 5.3, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 5.3, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 5.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 2.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.53, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 5); - } - } - - { - const auto p = resampled_path.points.at(3); - EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 9.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 9.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 4.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon); - EXPECT_EQ(p.point.is_final, true); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 9); - } - } - - const double pitch = std::atan(1.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); - EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); - } - } - - // Lerp v - { - autoware_internal_planning_msgs::msg::PathWithLaneId path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); - } - path.points.back().point.is_final = true; - std::vector resampled_arclength = {0.0, 1.2, 5.3, 9.0}; - - const auto resampled_path = resamplePath(path, resampled_arclength, false, true, false); - { - const auto p = resampled_path.points.at(0); - EXPECT_NEAR(p.point.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.0, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 0); - } - } - - { - const auto p = resampled_path.points.at(1); - EXPECT_NEAR(p.point.pose.position.x, 1.2, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.2, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 0.6, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.12, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 1); - } - } - - { - const auto p = resampled_path.points.at(2); - EXPECT_NEAR(p.point.pose.position.x, 5.3, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 5.3, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 2.65, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.53, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 5); - } - } - - { - const auto p = resampled_path.points.at(3); - EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 9.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 4.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon); - EXPECT_EQ(p.point.is_final, true); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 9); - } - } - - 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, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - } - } -} - -TEST(resample_path_with_lane_id, resample_path_by_same_interval) -{ - using autoware::motion_utils::resamplePath; - - // Same point resampling - { - PathWithLaneId path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); - } - path.points.back().point.is_final = true; - - { - const auto resampled_path = resamplePath(path, 1.0); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - const auto p = resampled_path.points.at(i); - const auto ans_p = path.points.at(i); - 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); - EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon); - EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon); - EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon); - EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon); - EXPECT_NEAR( - p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); - EXPECT_EQ(p.point.is_final, ans_p.point.is_final); - for (size_t j = 0; j < p.lane_ids.size(); ++j) { - EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j)); - } - } - } - // Change the last point orientation - path.points.back() = generateTestPathPointWithLaneId( - 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) { - const auto p = resampled_path.points.at(i); - const auto ans_p = path.points.at(i); - 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); - EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon); - EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon); - EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon); - EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon); - EXPECT_NEAR( - p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); - EXPECT_EQ(p.point.is_final, ans_p.point.is_final); - for (size_t j = 0; j < p.lane_ids.size(); ++j) { - EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j)); - } - } - - const auto p = resampled_path.points.back(); - const auto ans_p = path.points.back(); - 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); - EXPECT_NEAR(p.point.pose.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p.point.pose.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p.point.pose.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p.point.pose.orientation.w, ans_quat.w, epsilon); - EXPECT_NEAR( - p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); - EXPECT_EQ(p.point.is_final, ans_p.point.is_final); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), ans_p.lane_ids.at(i)); - } - } - } - - // Normal Case without zero point - { - PathWithLaneId path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); - } - path.points.at(0).point.longitudinal_velocity_mps = 5.0; - - const auto resampled_path = resamplePath(path, 0.1); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - const auto p = resampled_path.points.at(i); - EXPECT_NEAR(p.point.pose.position.x, 0.1 * i, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon); - - const size_t idx = i / 10; - EXPECT_NEAR( - p.point.longitudinal_velocity_mps, path.points.at(idx).point.longitudinal_velocity_mps, - epsilon); - EXPECT_NEAR( - p.point.lateral_velocity_mps, path.points.at(idx).point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.01 * i, epsilon); - EXPECT_EQ(p.point.is_final, path.points.at(idx).point.is_final); - for (size_t j = 0; j < p.lane_ids.size(); ++j) { - EXPECT_EQ(p.lane_ids.at(j), path.points.at(idx).lane_ids.at(j)); - } - } - } - - // Normal Case without stop point but with terminal point - { - PathWithLaneId path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); - } - path.points.at(0).point.longitudinal_velocity_mps = 5.0; - path.points.back().point.is_final = true; - - const auto resampled_traj = resamplePath(path, 0.4); - for (size_t i = 0; i < resampled_traj.points.size() - 1; ++i) { - const auto p = resampled_traj.points.at(i); - EXPECT_NEAR(p.point.pose.position.x, 0.4 * i, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon); - - const size_t idx = i / 2.5; - EXPECT_NEAR( - p.point.longitudinal_velocity_mps, path.points.at(idx).point.longitudinal_velocity_mps, - epsilon); - EXPECT_NEAR( - p.point.lateral_velocity_mps, path.points.at(idx).point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.04 * i, epsilon); - EXPECT_EQ(p.point.is_final, path.points.at(idx).point.is_final); - for (size_t j = 0; j < p.lane_ids.size(); ++j) { - EXPECT_EQ(p.lane_ids.at(j), path.points.at(idx).lane_ids.at(j)); - } - } - - { - const auto p = resampled_traj.points.at(23); - EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon); - - const size_t idx = 9; - EXPECT_NEAR( - p.point.longitudinal_velocity_mps, path.points.at(idx).point.longitudinal_velocity_mps, - epsilon); - EXPECT_NEAR( - p.point.lateral_velocity_mps, path.points.at(idx).point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon); - EXPECT_EQ(p.point.is_final, path.points.at(idx).point.is_final); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), path.points.at(idx).lane_ids.at(i)); - } - } - } - - // Normal Case without stop point but with terminal point (Boundary Condition) - { - PathWithLaneId path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); - } - path.points.at(0).point.longitudinal_velocity_mps = 5.0; - path.points.back().point.is_final = true; - - const double ds = 1.0 - autoware::motion_utils::overlap_threshold; - const auto resampled_path = resamplePath(path, ds); - for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { - const auto p = resampled_path.points.at(i); - EXPECT_NEAR(p.point.pose.position.x, ds * i, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon); - - const size_t idx = i == 0 ? 0 : i - 1; - EXPECT_NEAR( - p.point.longitudinal_velocity_mps, path.points.at(idx).point.longitudinal_velocity_mps, - epsilon); - EXPECT_NEAR( - p.point.lateral_velocity_mps, path.points.at(idx).point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, ds / 10 * i, epsilon); - EXPECT_EQ(p.point.is_final, path.points.at(idx).point.is_final); - for (size_t j = 0; j < p.lane_ids.size(); ++j) { - EXPECT_EQ(p.lane_ids.at(j), path.points.at(idx).lane_ids.at(j)); - } - } - - { - const auto p = resampled_path.points.at(10); - EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon); - - const size_t idx = 9; - EXPECT_NEAR( - p.point.longitudinal_velocity_mps, path.points.at(idx).point.longitudinal_velocity_mps, - epsilon); - EXPECT_NEAR( - p.point.lateral_velocity_mps, path.points.at(idx).point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon); - EXPECT_EQ(p.point.is_final, path.points.at(idx).point.is_final); - for (size_t j = 0; j < p.lane_ids.size(); ++j) { - EXPECT_EQ(p.lane_ids.at(j), path.points.at(idx).lane_ids.at(j)); - } - } - } - - // Normal Case with duplicated zero point - { - PathWithLaneId path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); - } - path.points.at(0).point.longitudinal_velocity_mps = 5.0; - path.points.at(5).point.longitudinal_velocity_mps = 0.0; - path.points.back().point.is_final = true; - - const auto resampled_path = resamplePath(path, 0.1); - EXPECT_EQ(resampled_path.points.size(), static_cast(91)); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - const auto p = resampled_path.points.at(i); - EXPECT_NEAR(p.point.pose.position.x, 0.1 * i, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon); - - const size_t idx = i / 10; - EXPECT_NEAR( - p.point.longitudinal_velocity_mps, path.points.at(idx).point.longitudinal_velocity_mps, - epsilon); - EXPECT_NEAR( - p.point.lateral_velocity_mps, path.points.at(idx).point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.01 * i, epsilon); - for (size_t j = 0; j < p.lane_ids.size(); ++j) { - EXPECT_EQ(p.lane_ids.at(j), path.points.at(idx).lane_ids.at(j)); - } - } - } - - // Normal Case with zero point - { - PathWithLaneId path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); - } - path.points.at(0).point.longitudinal_velocity_mps = 8.0; - path.points.at(5).point.longitudinal_velocity_mps = 0.0; - path.points.back().point.is_final = true; - - const auto resampled_path = resamplePath(path, 1.5); - EXPECT_EQ(resampled_path.points.size(), static_cast(8)); - { - const auto p = resampled_path.points.at(0).point; - const auto lane_ids = resampled_path.points.at(0).lane_ids; - EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR( - p.longitudinal_velocity_mps, path.points.at(0).point.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(0).point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); - EXPECT_EQ(p.is_final, false); - for (size_t j = 0; j < lane_ids.size(); ++j) { - EXPECT_EQ(lane_ids.at(j), path.points.at(0).lane_ids.at(j)); - } - } - - { - const auto p = resampled_path.points.at(1).point; - const auto lane_ids = resampled_path.points.at(1).lane_ids; - EXPECT_NEAR(p.pose.position.x, 1.5, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR( - p.longitudinal_velocity_mps, path.points.at(1).point.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(1).point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.15, epsilon); - EXPECT_EQ(p.is_final, false); - for (size_t j = 0; j < lane_ids.size(); ++j) { - EXPECT_EQ(lane_ids.at(j), path.points.at(1).lane_ids.at(j)); - } - } - - { - const auto p = resampled_path.points.at(2).point; - const auto lane_ids = resampled_path.points.at(2).lane_ids; - EXPECT_NEAR(p.pose.position.x, 3.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR( - p.longitudinal_velocity_mps, path.points.at(3).point.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(3).point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.30, epsilon); - EXPECT_EQ(p.is_final, false); - for (size_t j = 0; j < lane_ids.size(); ++j) { - EXPECT_EQ(lane_ids.at(j), path.points.at(3).lane_ids.at(j)); - } - } - - { - const auto p = resampled_path.points.at(3).point; - const auto lane_ids = resampled_path.points.at(3).lane_ids; - EXPECT_NEAR(p.pose.position.x, 4.5, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR( - p.longitudinal_velocity_mps, path.points.at(4).point.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(4).point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.45, epsilon); - EXPECT_EQ(p.is_final, false); - for (size_t j = 0; j < lane_ids.size(); ++j) { - EXPECT_EQ(lane_ids.at(j), path.points.at(4).lane_ids.at(j)); - } - } - - { - const auto p = resampled_path.points.at(4).point; - const auto lane_ids = resampled_path.points.at(4).lane_ids; - EXPECT_NEAR(p.pose.position.x, 5.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(5).point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.50, epsilon); - EXPECT_EQ(p.is_final, false); - for (size_t j = 0; j < lane_ids.size(); ++j) { - EXPECT_EQ(lane_ids.at(j), path.points.at(5).lane_ids.at(j)); - } - } - - { - const auto p = resampled_path.points.at(5).point; - const auto lane_ids = resampled_path.points.at(5).lane_ids; - EXPECT_NEAR(p.pose.position.x, 6.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR( - p.longitudinal_velocity_mps, path.points.at(6).point.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(6).point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.60, epsilon); - EXPECT_EQ(p.is_final, false); - for (size_t j = 0; j < lane_ids.size(); ++j) { - EXPECT_EQ(lane_ids.at(j), path.points.at(6).lane_ids.at(j)); - } - } - - { - const auto p = resampled_path.points.at(6).point; - const auto lane_ids = resampled_path.points.at(6).lane_ids; - EXPECT_NEAR(p.pose.position.x, 7.5, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR( - p.longitudinal_velocity_mps, path.points.at(7).point.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(7).point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.75, epsilon); - EXPECT_EQ(p.is_final, false); - for (size_t j = 0; j < lane_ids.size(); ++j) { - EXPECT_EQ(lane_ids.at(j), path.points.at(7).lane_ids.at(j)); - } - } - - { - const auto p = resampled_path.points.at(7).point; - const auto lane_ids = resampled_path.points.at(7).lane_ids; - EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR( - p.longitudinal_velocity_mps, path.points.at(9).point.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(9).point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.90, epsilon); - EXPECT_EQ(p.is_final, true); - for (size_t j = 0; j < lane_ids.size(); ++j) { - EXPECT_EQ(lane_ids.at(j), path.points.at(9).lane_ids.at(j)); - } - } - } - - // No Resample - { - // Input path size is not enough for resample - { - PathWithLaneId path; - path.points.resize(1); - for (size_t i = 0; i < 1; ++i) { - path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {0}); - } - - const auto resampled_path = resamplePath(path, 1.0); - EXPECT_EQ(resampled_path.points.size(), path.points.size()); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - const auto p = resampled_path.points.at(i); - const auto ans_p = path.points.at(i); - 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); - EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon); - EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon); - EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon); - EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon); - EXPECT_NEAR( - p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); - EXPECT_EQ(p.point.is_final, ans_p.point.is_final); - for (size_t j = 0; j < p.lane_ids.size(); ++j) { - EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j)); - } - } - } - - // Resample interval is invalid - { - PathWithLaneId path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); - } - - const auto resampled_path = resamplePath(path, 1e-4); - EXPECT_EQ(resampled_path.points.size(), path.points.size()); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - const auto p = resampled_path.points.at(i); - const auto ans_p = path.points.at(i); - 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); - EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon); - EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon); - EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon); - EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon); - EXPECT_NEAR( - p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); - EXPECT_EQ(p.point.is_final, ans_p.point.is_final); - for (size_t j = 0; j < p.lane_ids.size(); ++j) { - EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j)); - } - } - } - - // Resample interval is invalid (Negative value) - { - PathWithLaneId path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPointWithLaneId( - i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); - } - - const auto resampled_path = resamplePath(path, -5.0); - EXPECT_EQ(resampled_path.points.size(), path.points.size()); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - const auto p = resampled_path.points.at(i); - const auto ans_p = path.points.at(i); - 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); - EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon); - EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon); - EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon); - EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon); - EXPECT_NEAR( - p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); - EXPECT_EQ(p.point.is_final, ans_p.point.is_final); - for (size_t j = 0; j < p.lane_ids.size(); ++j) { - EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j)); - } - } - } - } -} - -TEST(resample_path, resample_path_by_vector) -{ - using autoware::motion_utils::resamplePath; - // Output is same as input - { - auto path = generateTestPath(10, 1.0, 3.0, 1.0, 0.01); - std::vector resampled_arclength = generateArclength(10, 1.0); - - { - const auto resampled_path = resamplePath(path, resampled_arclength); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - const auto p = resampled_path.points.at(i); - const auto ans_p = path.points.at(i); - 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); - EXPECT_NEAR(p.pose.orientation.x, ans_p.pose.orientation.x, epsilon); - EXPECT_NEAR(p.pose.orientation.y, ans_p.pose.orientation.y, epsilon); - EXPECT_NEAR(p.pose.orientation.z, ans_p.pose.orientation.z, epsilon); - EXPECT_NEAR(p.pose.orientation.w, ans_p.pose.orientation.w, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); - } - } - - // Change the last point orientation - path.points.back() = - 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) { - const auto p = resampled_path.points.at(i); - const auto ans_p = path.points.at(i); - 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); - EXPECT_NEAR(p.pose.orientation.x, ans_p.pose.orientation.x, epsilon); - EXPECT_NEAR(p.pose.orientation.y, ans_p.pose.orientation.y, epsilon); - EXPECT_NEAR(p.pose.orientation.z, ans_p.pose.orientation.z, epsilon); - EXPECT_NEAR(p.pose.orientation.w, ans_p.pose.orientation.w, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); - } - - const auto p = resampled_path.points.back(); - const auto ans_p = path.points.back(); - 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); - EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); - } - } - - // Output key is not same as input - { - autoware_planning_msgs::msg::Path path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); - } - std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; - - const auto resampled_path = resamplePath(path, resampled_arclength); - - { - const auto p = resampled_path.points.at(0); - EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); - } - - { - const auto p = resampled_path.points.at(1); - EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); - } - - { - const auto p = resampled_path.points.at(2); - EXPECT_NEAR(p.pose.position.x, 1.5, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.15, epsilon); - } - - { - const auto p = resampled_path.points.at(3); - EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 5.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); - } - - { - const auto p = resampled_path.points.at(4); - EXPECT_NEAR(p.pose.position.x, 7.5, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 7.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 3.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.75, epsilon); - } - - { - const auto p = resampled_path.points.at(5); - EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 9.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); - } - - 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, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - } - } - - // No Interpolation - { - // Input path size is not enough for interpolation - { - autoware_planning_msgs::msg::Path path; - path.points.resize(1); - for (size_t i = 0; i < 1; ++i) { - path.points.at(i) = - generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); - } - std::vector resampled_arclength = generateArclength(10, 1.0); - - const auto resampled_path = resamplePath(path, resampled_arclength); - EXPECT_EQ(resampled_path.points.size(), path.points.size()); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - EXPECT_NEAR( - resampled_path.points.at(i).pose.position.x, path.points.at(i).pose.position.x, epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.position.y, path.points.at(i).pose.position.y, epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.position.z, path.points.at(i).pose.position.z, epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.x, path.points.at(i).pose.orientation.x, - epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.y, path.points.at(i).pose.orientation.y, - epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.z, path.points.at(i).pose.orientation.z, - epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.w, path.points.at(i).pose.orientation.w, - epsilon); - } - } - - // Resampled Arclength size is not enough for interpolation - { - autoware_planning_msgs::msg::Path path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = - generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); - } - std::vector resampled_arclength = generateArclength(1, 1.0); - - const auto resampled_path = resamplePath(path, resampled_arclength); - EXPECT_EQ(resampled_path.points.size(), path.points.size()); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - EXPECT_NEAR( - resampled_path.points.at(i).pose.position.x, path.points.at(i).pose.position.x, epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.position.y, path.points.at(i).pose.position.y, epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.position.z, path.points.at(i).pose.position.z, epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.x, path.points.at(i).pose.orientation.x, - epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.y, path.points.at(i).pose.orientation.y, - epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.z, path.points.at(i).pose.orientation.z, - epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.w, path.points.at(i).pose.orientation.w, - epsilon); - } - } - - // Resampled Arclength is longer than input path - { - autoware_planning_msgs::msg::Path path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = - generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); - } - std::vector resampled_arclength = generateArclength(3, 5.0); - - const auto resampled_path = resamplePath(path, resampled_arclength); - EXPECT_EQ(resampled_path.points.size(), path.points.size()); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - EXPECT_NEAR( - resampled_path.points.at(i).pose.position.x, path.points.at(i).pose.position.x, epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.position.y, path.points.at(i).pose.position.y, epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.position.z, path.points.at(i).pose.position.z, epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.x, path.points.at(i).pose.orientation.x, - epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.y, path.points.at(i).pose.orientation.y, - epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.z, path.points.at(i).pose.orientation.z, - epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.w, path.points.at(i).pose.orientation.w, - epsilon); - } - } - } -} - -TEST(resample_path, resample_path_by_vector_backward) -{ - using autoware::motion_utils::resamplePath; - - { - autoware_planning_msgs::msg::Path path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1); - } - std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; - - const auto resampled_path = resamplePath(path, resampled_arclength); - - { - const auto p = resampled_path.points.at(0); - EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); - } - - { - const auto p = resampled_path.points.at(1); - EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); - } - - { - const auto p = resampled_path.points.at(2); - EXPECT_NEAR(p.pose.position.x, 1.5, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.15, epsilon); - } - - { - const auto p = resampled_path.points.at(3); - EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 5.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); - } - - { - const auto p = resampled_path.points.at(4); - EXPECT_NEAR(p.pose.position.x, 7.5, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 7.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 3.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.75, epsilon); - } - - { - const auto p = resampled_path.points.at(5); - EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 9.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); - } - - 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); - EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); - } - } - - // change initial orientation - { - autoware_planning_msgs::msg::Path path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1); - } - path.points.at(0).pose.orientation = - 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); - - { - const auto p = resampled_path.points.at(0); - EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); - } - - { - const auto p = resampled_path.points.at(1); - EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); - } - - { - const auto p = resampled_path.points.at(2); - EXPECT_NEAR(p.pose.position.x, 1.5, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.15, epsilon); - } - - { - const auto p = resampled_path.points.at(3); - EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 5.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); - } - - { - const auto p = resampled_path.points.at(4); - EXPECT_NEAR(p.pose.position.x, 7.5, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 7.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 3.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.75, epsilon); - } - - { - const auto p = resampled_path.points.at(5); - EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 9.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); - } - - // Initial Orientation - { - 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); - EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); - } - - 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); - EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); - } - } -} - -TEST(resample_path, resample_path_by_vector_non_default) -{ - using autoware::motion_utils::resamplePath; - - // Lerp x, y - { - autoware_planning_msgs::msg::Path path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); - } - std::vector resampled_arclength = {0.0, 1.2, 5.3, 9.0}; - - const auto resampled_path = resamplePath(path, resampled_arclength, true); - { - const auto p = resampled_path.points.at(0); - EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); - } - - { - const auto p = resampled_path.points.at(1); - EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); - } - - { - const auto p = resampled_path.points.at(2); - EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 5.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); - } - - { - const auto p = resampled_path.points.at(3); - EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 9.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); - } - - 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, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - } - } - - // Slerp z - { - autoware_planning_msgs::msg::Path path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = - generateTestPathPoint(i * 1.0, 0.0, i * 1.0, 0.0, i * 1.0, i * 0.5, i * 0.1); - } - std::vector resampled_arclength = {0.0, 1.2, 5.3, 9.0}; - - const auto resampled_path = resamplePath(path, resampled_arclength, false, false); - { - const auto p = resampled_path.points.at(0); - EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); - } - - { - const auto p = resampled_path.points.at(1); - EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 1.2, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); - } - - { - const auto p = resampled_path.points.at(2); - EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 5.3, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 5.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); - } - - { - const auto p = resampled_path.points.at(3); - EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 9.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 9.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); - } - - const double pitch = std::atan(1.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); - EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); - } - } - - // Lerp v - { - autoware_planning_msgs::msg::Path path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); - } - std::vector resampled_arclength = {0.0, 1.2, 5.3, 9.0}; - - const auto resampled_path = resamplePath(path, resampled_arclength, false, true, false); - { - const auto p = resampled_path.points.at(0); - EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); - } - - { - const auto p = resampled_path.points.at(1); - EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 1.2, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.6, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); - } - - { - const auto p = resampled_path.points.at(2); - EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 5.3, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 2.65, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); - } - - { - const auto p = resampled_path.points.at(3); - EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 9.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); - } - - 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, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - } - } -} - -TEST(resample_path, resample_path_by_same_interval) -{ - using autoware::motion_utils::resamplePath; - - // Same point resampling - { - autoware_planning_msgs::msg::Path path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); - } - - { - const auto resampled_path = resamplePath(path, 1.0); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - const auto p = resampled_path.points.at(i); - const auto ans_p = path.points.at(i); - 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); - EXPECT_NEAR(p.pose.orientation.x, ans_p.pose.orientation.x, epsilon); - EXPECT_NEAR(p.pose.orientation.y, ans_p.pose.orientation.y, epsilon); - EXPECT_NEAR(p.pose.orientation.z, ans_p.pose.orientation.z, epsilon); - EXPECT_NEAR(p.pose.orientation.w, ans_p.pose.orientation.w, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); - } - } - - // Change the last point orientation - path.points.back() = - 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) { - const auto p = resampled_path.points.at(i); - const auto ans_p = path.points.at(i); - 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); - EXPECT_NEAR(p.pose.orientation.x, ans_p.pose.orientation.x, epsilon); - EXPECT_NEAR(p.pose.orientation.y, ans_p.pose.orientation.y, epsilon); - EXPECT_NEAR(p.pose.orientation.z, ans_p.pose.orientation.z, epsilon); - EXPECT_NEAR(p.pose.orientation.w, ans_p.pose.orientation.w, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); - } - - const auto p = resampled_path.points.back(); - const auto ans_p = path.points.back(); - 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); - EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); - } - } - - // Normal Case without zero point - { - autoware_planning_msgs::msg::Path path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); - } - path.points.at(0).longitudinal_velocity_mps = 5.0; - - const auto resampled_path = resamplePath(path, 0.1); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - const auto p = resampled_path.points.at(i); - EXPECT_NEAR(p.pose.position.x, 0.1 * i, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - const size_t idx = i / 10; - EXPECT_NEAR( - p.longitudinal_velocity_mps, path.points.at(idx).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(idx).lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.01 * i, epsilon); - } - } - - // Normal Case without stop point but with terminal point - { - autoware_planning_msgs::msg::Path path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); - } - path.points.at(0).longitudinal_velocity_mps = 5.0; - - const auto resampled_path = resamplePath(path, 0.4); - for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { - const auto p = resampled_path.points.at(i); - EXPECT_NEAR(p.pose.position.x, 0.4 * i, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - const size_t idx = i / 2.5; - EXPECT_NEAR( - p.longitudinal_velocity_mps, path.points.at(idx).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(idx).lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.04 * i, epsilon); - } - - { - const auto p = resampled_path.points.at(23); - EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - const size_t idx = 9; - EXPECT_NEAR( - p.longitudinal_velocity_mps, path.points.at(idx).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(idx).lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); - } - } - - // Normal Case without stop point but with terminal point (Boundary Condition) - { - autoware_planning_msgs::msg::Path path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); - } - path.points.at(0).longitudinal_velocity_mps = 5.0; - - const double ds = 1.0 - autoware::motion_utils::overlap_threshold; - const auto resampled_path = resamplePath(path, ds); - for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { - const auto p = resampled_path.points.at(i); - EXPECT_NEAR(p.pose.position.x, ds * i, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - const size_t idx = i == 0 ? 0 : i - 1; - EXPECT_NEAR( - p.longitudinal_velocity_mps, path.points.at(idx).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(idx).lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, ds / 10.0 * i, epsilon); - } - - { - const auto p = resampled_path.points.at(10); - EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - const size_t idx = 9; - EXPECT_NEAR( - p.longitudinal_velocity_mps, path.points.at(idx).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(idx).lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); - } - } - - // Normal Case with duplicated zero point - { - autoware_planning_msgs::msg::Path path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); - } - path.points.at(0).longitudinal_velocity_mps = 5.0; - path.points.at(5).longitudinal_velocity_mps = 0.0; - - const auto resampled_path = resamplePath(path, 0.1); - EXPECT_EQ(resampled_path.points.size(), static_cast(91)); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - const auto p = resampled_path.points.at(i); - EXPECT_NEAR(p.pose.position.x, 0.1 * i, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - const size_t idx = i / 10; - EXPECT_NEAR( - p.longitudinal_velocity_mps, path.points.at(idx).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(idx).lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.01 * i, epsilon); - } - } - - // Normal Case with zero point - { - autoware_planning_msgs::msg::Path path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); - } - path.points.at(0).longitudinal_velocity_mps = 8.0; - path.points.at(5).longitudinal_velocity_mps = 0.0; - - const auto resampled_path = resamplePath(path, 1.5); - EXPECT_EQ(resampled_path.points.size(), static_cast(8)); - { - const auto p = resampled_path.points.at(0); - EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR( - p.longitudinal_velocity_mps, path.points.at(0).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(0).lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); - } - - { - const auto p = resampled_path.points.at(1); - EXPECT_NEAR(p.pose.position.x, 1.5, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR( - p.longitudinal_velocity_mps, path.points.at(1).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(1).lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.15, epsilon); - } - - { - const auto p = resampled_path.points.at(2); - EXPECT_NEAR(p.pose.position.x, 3.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR( - p.longitudinal_velocity_mps, path.points.at(3).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(3).lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.30, epsilon); - } - - { - const auto p = resampled_path.points.at(3); - EXPECT_NEAR(p.pose.position.x, 4.5, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR( - p.longitudinal_velocity_mps, path.points.at(4).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(4).lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.45, epsilon); - } - - { - const auto p = resampled_path.points.at(4); - EXPECT_NEAR(p.pose.position.x, 5.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(5).lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.50, epsilon); - } - - { - const auto p = resampled_path.points.at(5); - EXPECT_NEAR(p.pose.position.x, 6.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR( - p.longitudinal_velocity_mps, path.points.at(6).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(6).lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.60, epsilon); - } - - { - const auto p = resampled_path.points.at(6); - EXPECT_NEAR(p.pose.position.x, 7.5, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR( - p.longitudinal_velocity_mps, path.points.at(7).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(7).lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.75, epsilon); - } - - { - const auto p = resampled_path.points.at(7); - EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR( - p.longitudinal_velocity_mps, path.points.at(9).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(9).lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.90, epsilon); - } - } - - // No Resample - { - // Input path size is not enough for resample - { - autoware_planning_msgs::msg::Path path; - path.points.resize(1); - for (size_t i = 0; i < 1; ++i) { - path.points.at(i) = - generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); - } - - const auto resampled_path = resamplePath(path, 1.0); - EXPECT_EQ(resampled_path.points.size(), path.points.size()); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - EXPECT_NEAR( - resampled_path.points.at(i).pose.position.x, path.points.at(i).pose.position.x, epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.position.y, path.points.at(i).pose.position.y, epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.position.z, path.points.at(i).pose.position.z, epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.x, path.points.at(i).pose.orientation.x, - epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.y, path.points.at(i).pose.orientation.y, - epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.z, path.points.at(i).pose.orientation.z, - epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.w, path.points.at(i).pose.orientation.w, - epsilon); - } - } - - // Resample interval is invalid - { - autoware_planning_msgs::msg::Path path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = - generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); - } - - const auto resampled_path = resamplePath(path, 1e-4); - EXPECT_EQ(resampled_path.points.size(), path.points.size()); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - EXPECT_NEAR( - resampled_path.points.at(i).pose.position.x, path.points.at(i).pose.position.x, epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.position.y, path.points.at(i).pose.position.y, epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.position.z, path.points.at(i).pose.position.z, epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.x, path.points.at(i).pose.orientation.x, - epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.y, path.points.at(i).pose.orientation.y, - epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.z, path.points.at(i).pose.orientation.z, - epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.w, path.points.at(i).pose.orientation.w, - epsilon); - } - } - - // Resample interval is invalid (Negative value) - { - autoware_planning_msgs::msg::Path path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = - generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); - } - - const auto resampled_path = resamplePath(path, -5.0); - EXPECT_EQ(resampled_path.points.size(), path.points.size()); - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - EXPECT_NEAR( - resampled_path.points.at(i).pose.position.x, path.points.at(i).pose.position.x, epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.position.y, path.points.at(i).pose.position.y, epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.position.z, path.points.at(i).pose.position.z, epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.x, path.points.at(i).pose.orientation.x, - epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.y, path.points.at(i).pose.orientation.y, - epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.z, path.points.at(i).pose.orientation.z, - epsilon); - EXPECT_NEAR( - resampled_path.points.at(i).pose.orientation.w, path.points.at(i).pose.orientation.w, - epsilon); - } - } - } -} - -TEST(resample_trajectory, resample_trajectory_by_vector) -{ - using autoware::motion_utils::resampleTrajectory; - // Output is same as input - { - auto traj = generateTestTrajectory(10, 1.0, 3.0, 1.0, 0.01, 0.5); - std::vector resampled_arclength = generateArclength(10, 1.0); - - { - const auto resampled_traj = resampleTrajectory(traj, resampled_arclength); - for (size_t i = 0; i < resampled_traj.points.size(); ++i) { - const auto p = resampled_traj.points.at(i); - const auto ans_p = setZeroVelocityAfterStop(traj.points).at(i); - 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); - EXPECT_NEAR(p.pose.orientation.x, ans_p.pose.orientation.x, epsilon); - EXPECT_NEAR(p.pose.orientation.y, ans_p.pose.orientation.y, epsilon); - EXPECT_NEAR(p.pose.orientation.z, ans_p.pose.orientation.z, epsilon); - EXPECT_NEAR(p.pose.orientation.w, ans_p.pose.orientation.w, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); - EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); - } - } - - // Change the last point orientation - 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) { - const auto p = resampled_path.points.at(i); - const auto ans_p = setZeroVelocityAfterStop(traj.points).at(i); - 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); - EXPECT_NEAR(p.pose.orientation.x, ans_p.pose.orientation.x, epsilon); - EXPECT_NEAR(p.pose.orientation.y, ans_p.pose.orientation.y, epsilon); - EXPECT_NEAR(p.pose.orientation.z, ans_p.pose.orientation.z, epsilon); - EXPECT_NEAR(p.pose.orientation.w, ans_p.pose.orientation.w, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); - EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); - } - - const auto p = resampled_path.points.back(); - const auto ans_p = setZeroVelocityAfterStop(traj.points).back(); - 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); - EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); - EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); - } - } - - // Output key is not same as input - { - autoware_planning_msgs::msg::Trajectory traj; - traj.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - traj.points.at(i) = - generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); - } - std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; - - const auto resampled_path = resampleTrajectory(traj, resampled_arclength); - - { - const auto p = resampled_path.points.at(0); - EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); - EXPECT_NEAR(p.acceleration_mps2, 0.0, epsilon); - } - - { - const auto p = resampled_path.points.at(1); - EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); - EXPECT_NEAR(p.acceleration_mps2, 0.05, epsilon); - } - - { - const auto p = resampled_path.points.at(2); - EXPECT_NEAR(p.pose.position.x, 1.5, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.15, epsilon); - EXPECT_NEAR(p.acceleration_mps2, 0.05, epsilon); - } - - { - const auto p = resampled_path.points.at(3); - EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); - EXPECT_NEAR(p.acceleration_mps2, 0.25, epsilon); - } - - { - const auto p = resampled_path.points.at(4); - EXPECT_NEAR(p.pose.position.x, 7.5, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 3.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.75, epsilon); - EXPECT_NEAR(p.acceleration_mps2, 0.35, epsilon); - } - - { - const auto p = resampled_path.points.at(5); - EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); - EXPECT_NEAR(p.acceleration_mps2, 0.45, epsilon); - } - - 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, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - } - } - - // No Interpolation - { - // Input path size is not enough for interpolation - { - autoware_planning_msgs::msg::Trajectory traj; - traj.points.resize(1); - for (size_t i = 0; i < 1; ++i) { - traj.points.at(i) = - generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); - } - std::vector resampled_arclength = generateArclength(10, 1.0); - - const auto resampled_traj = resampleTrajectory(traj, resampled_arclength); - EXPECT_EQ(resampled_traj.points.size(), traj.points.size()); - for (size_t i = 0; i < resampled_traj.points.size(); ++i) { - EXPECT_NEAR( - resampled_traj.points.at(i).pose.position.x, traj.points.at(i).pose.position.x, epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.position.y, traj.points.at(i).pose.position.y, epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.position.z, traj.points.at(i).pose.position.z, epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.x, traj.points.at(i).pose.orientation.x, - epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.y, traj.points.at(i).pose.orientation.y, - epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.z, traj.points.at(i).pose.orientation.z, - epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.w, traj.points.at(i).pose.orientation.w, - epsilon); - } - } - - // Resampled Arclength size is not enough for interpolation - { - autoware_planning_msgs::msg::Trajectory traj; - traj.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - traj.points.at(i) = - generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); - } - std::vector resampled_arclength = generateArclength(1, 1.0); - - const auto resampled_traj = resampleTrajectory(traj, resampled_arclength); - EXPECT_EQ(resampled_traj.points.size(), traj.points.size()); - for (size_t i = 0; i < resampled_traj.points.size(); ++i) { - EXPECT_NEAR( - resampled_traj.points.at(i).pose.position.x, traj.points.at(i).pose.position.x, epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.position.y, traj.points.at(i).pose.position.y, epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.position.z, traj.points.at(i).pose.position.z, epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.x, traj.points.at(i).pose.orientation.x, - epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.y, traj.points.at(i).pose.orientation.y, - epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.z, traj.points.at(i).pose.orientation.z, - epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.w, traj.points.at(i).pose.orientation.w, - epsilon); - } - } - - // Resampled Arclength is longer than input path - { - autoware_planning_msgs::msg::Trajectory traj; - traj.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - traj.points.at(i) = - generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); - } - std::vector resampled_arclength = generateArclength(3, 5.0); - - const auto resampled_traj = resampleTrajectory(traj, resampled_arclength); - EXPECT_EQ(resampled_traj.points.size(), traj.points.size()); - for (size_t i = 0; i < resampled_traj.points.size(); ++i) { - EXPECT_NEAR( - resampled_traj.points.at(i).pose.position.x, traj.points.at(i).pose.position.x, epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.position.y, traj.points.at(i).pose.position.y, epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.position.z, traj.points.at(i).pose.position.z, epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.x, traj.points.at(i).pose.orientation.x, - epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.y, traj.points.at(i).pose.orientation.y, - epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.z, traj.points.at(i).pose.orientation.z, - epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.w, traj.points.at(i).pose.orientation.w, - epsilon); - } - } - } -} - -TEST(resample_trajectory, resample_trajectory_by_vector_non_default) -{ - using autoware::motion_utils::resampleTrajectory; - - // Lerp x, y - { - autoware_planning_msgs::msg::Trajectory traj; - traj.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - traj.points.at(i) = - generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); - } - std::vector resampled_arclength = {0.0, 1.2, 5.3, 9.0}; - - const auto resampled_traj = resampleTrajectory(traj, resampled_arclength, true); - { - const auto p = resampled_traj.points.at(0); - EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); - EXPECT_NEAR(p.acceleration_mps2, 0.0, epsilon); - } - - { - const auto p = resampled_traj.points.at(1); - EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); - EXPECT_NEAR(p.acceleration_mps2, 0.05, epsilon); - } - - { - const auto p = resampled_traj.points.at(2); - EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); - EXPECT_NEAR(p.acceleration_mps2, 0.25, epsilon); - } - - { - const auto p = resampled_traj.points.at(3); - EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); - EXPECT_NEAR(p.acceleration_mps2, 0.45, epsilon); - } - - 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, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - } - } - - // Slerp z - { - autoware_planning_msgs::msg::Trajectory traj; - traj.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - traj.points.at(i) = generateTestTrajectoryPoint( - i * 1.0, 0.0, i * 1.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); - } - std::vector resampled_arclength = {0.0, 1.2, 5.3, 9.0}; - - const auto resampled_traj = resampleTrajectory(traj, resampled_arclength, false, false); - { - const auto p = resampled_traj.points.at(0); - EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); - EXPECT_NEAR(p.acceleration_mps2, 0.0, epsilon); - } - - { - const auto p = resampled_traj.points.at(1); - EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 1.2, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); - EXPECT_NEAR(p.acceleration_mps2, 0.05, epsilon); - } - - { - const auto p = resampled_traj.points.at(2); - EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 5.3, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); - EXPECT_NEAR(p.acceleration_mps2, 0.25, epsilon); - } - - { - const auto p = resampled_traj.points.at(3); - EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 9.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); - EXPECT_NEAR(p.acceleration_mps2, 0.45, epsilon); - } - - const double pitch = std::atan(1.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); - EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); - } - } - - // Lerp twist - { - autoware_planning_msgs::msg::Trajectory traj; - traj.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - traj.points.at(i) = - generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); - } - std::vector resampled_arclength = {0.0, 1.2, 5.3, 9.0}; - - const auto resampled_traj = resampleTrajectory(traj, resampled_arclength, false, true, false); - { - const auto p = resampled_traj.points.at(0); - EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); - EXPECT_NEAR(p.acceleration_mps2, 0.0, epsilon); - } - - { - const auto p = resampled_traj.points.at(1); - EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 0.6, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); - EXPECT_NEAR(p.acceleration_mps2, 0.06, epsilon); - } - - { - const auto p = resampled_traj.points.at(2); - EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 2.65, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); - EXPECT_NEAR(p.acceleration_mps2, 0.265, epsilon); - } - - { - const auto p = resampled_traj.points.at(3); - EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); - EXPECT_NEAR(p.acceleration_mps2, 0.45, epsilon); - } - - 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, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - } - } -} - -TEST(resample_trajectory, resample_trajectory_by_same_interval) -{ - using autoware::motion_utils::resampleTrajectory; - - // Same point resampling - { - autoware_planning_msgs::msg::Trajectory traj; - traj.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - traj.points.at(i) = - generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); - } - - { - const auto resampled_traj = resampleTrajectory(traj, 1.0); - for (size_t i = 0; i < resampled_traj.points.size(); ++i) { - const auto p = resampled_traj.points.at(i); - const auto ans_p = setZeroVelocityAfterStop(traj.points).at(i); - 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); - EXPECT_NEAR(p.pose.orientation.x, ans_p.pose.orientation.x, epsilon); - EXPECT_NEAR(p.pose.orientation.y, ans_p.pose.orientation.y, epsilon); - EXPECT_NEAR(p.pose.orientation.z, ans_p.pose.orientation.z, epsilon); - EXPECT_NEAR(p.pose.orientation.w, ans_p.pose.orientation.w, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); - EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); - } - } - - // Change the last point orientation - 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) { - const auto p = resampled_path.points.at(i); - const auto ans_p = setZeroVelocityAfterStop(traj.points).at(i); - 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); - EXPECT_NEAR(p.pose.orientation.x, ans_p.pose.orientation.x, epsilon); - EXPECT_NEAR(p.pose.orientation.y, ans_p.pose.orientation.y, epsilon); - EXPECT_NEAR(p.pose.orientation.z, ans_p.pose.orientation.z, epsilon); - EXPECT_NEAR(p.pose.orientation.w, ans_p.pose.orientation.w, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); - EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); - } - - const auto p = resampled_path.points.back(); - const auto ans_p = setZeroVelocityAfterStop(traj.points).back(); - 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); - EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); - EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); - EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); - } - } - - // Normal Case without zero point - { - autoware_planning_msgs::msg::Trajectory traj; - traj.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - traj.points.at(i) = - generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); - } - traj.points.at(0).longitudinal_velocity_mps = 5.0; - - const auto resampled_traj = resampleTrajectory(traj, 0.1); - for (size_t i = 0; i < resampled_traj.points.size(); ++i) { - const auto p = resampled_traj.points.at(i); - EXPECT_NEAR(p.pose.position.x, 0.1 * i, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - const size_t idx = i / 10; - auto ans_p = setZeroVelocityAfterStop(traj.points).at(idx); - EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.01 * i, epsilon); - EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); - } - } - - // Normal Case without stop point but with terminal point - { - autoware_planning_msgs::msg::Trajectory traj; - traj.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - traj.points.at(i) = - generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); - } - traj.points.at(0).longitudinal_velocity_mps = 5.0; - - const auto resampled_traj = resampleTrajectory(traj, 0.4); - for (size_t i = 0; i < resampled_traj.points.size() - 1; ++i) { - const auto p = resampled_traj.points.at(i); - EXPECT_NEAR(p.pose.position.x, 0.4 * i, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - const size_t idx = i / 2.5; - auto ans_p = setZeroVelocityAfterStop(traj.points).at(idx); - EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.04 * i, epsilon); - EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); - } - - { - const auto p = resampled_traj.points.at(23); - EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - const size_t idx = 9; - auto ans_p = setZeroVelocityAfterStop(traj.points).at(idx); - EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); - EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); - } - } - - // Normal Case without stop point but with terminal point (Boundary Condition) - { - autoware_planning_msgs::msg::Trajectory traj; - traj.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - traj.points.at(i) = - generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); - } - traj.points.at(0).longitudinal_velocity_mps = 5.0; - - const double ds = 1.0 - autoware::motion_utils::overlap_threshold; - const auto resampled_traj = resampleTrajectory(traj, ds); - for (size_t i = 0; i < resampled_traj.points.size() - 1; ++i) { - const auto p = resampled_traj.points.at(i); - EXPECT_NEAR(p.pose.position.x, ds * i, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - const size_t idx = i == 0 ? 0 : i - 1; - auto ans_p = setZeroVelocityAfterStop(traj.points).at(idx); - EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, ds / 10.0 * i, epsilon); - EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); - } - - { - const auto p = resampled_traj.points.at(10); - EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - const size_t idx = 9; - auto ans_p = setZeroVelocityAfterStop(traj.points).at(idx); - EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); - EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); - } - } - - // Normal Case with duplicated zero point - { - autoware_planning_msgs::msg::Trajectory traj; - traj.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - traj.points.at(i) = - generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); - } - traj.points.at(0).longitudinal_velocity_mps = 5.0; - traj.points.at(5).longitudinal_velocity_mps = 0.0; - - const auto resampled_traj = resampleTrajectory(traj, 0.1); - EXPECT_EQ(resampled_traj.points.size(), static_cast(91)); - for (size_t i = 0; i < resampled_traj.points.size(); ++i) { - const auto p = resampled_traj.points.at(i); - EXPECT_NEAR(p.pose.position.x, 0.1 * i, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - const size_t idx = i / 10; - auto ans_p = setZeroVelocityAfterStop(traj.points).at(idx); - EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.01 * i, epsilon); - EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); - } - } - - // Normal Case with zero point - { - autoware_planning_msgs::msg::Trajectory traj; - traj.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - traj.points.at(i) = - generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); - } - traj.points.at(0).longitudinal_velocity_mps = 8.0; - traj.points.at(5).longitudinal_velocity_mps = 0.0; - - const auto resampled_traj = resampleTrajectory(traj, 1.5); - EXPECT_EQ(resampled_traj.points.size(), static_cast(8)); - { - const auto p = resampled_traj.points.at(0); - EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR( - p.longitudinal_velocity_mps, traj.points.at(0).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, traj.points.at(0).lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); - EXPECT_NEAR(p.acceleration_mps2, traj.points.at(0).acceleration_mps2, epsilon); - } - - { - const auto p = resampled_traj.points.at(1); - EXPECT_NEAR(p.pose.position.x, 1.5, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR( - p.longitudinal_velocity_mps, traj.points.at(1).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, traj.points.at(1).lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.15, epsilon); - EXPECT_NEAR(p.acceleration_mps2, traj.points.at(1).acceleration_mps2, epsilon); - } - - { - const auto p = resampled_traj.points.at(2); - EXPECT_NEAR(p.pose.position.x, 3.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR( - p.longitudinal_velocity_mps, traj.points.at(3).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, traj.points.at(3).lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.30, epsilon); - EXPECT_NEAR(p.acceleration_mps2, traj.points.at(3).acceleration_mps2, epsilon); - } - - { - const auto p = resampled_traj.points.at(3); - EXPECT_NEAR(p.pose.position.x, 4.5, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR( - p.longitudinal_velocity_mps, traj.points.at(4).longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, traj.points.at(4).lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.45, epsilon); - EXPECT_NEAR(p.acceleration_mps2, traj.points.at(4).acceleration_mps2, epsilon); - } - - { - const auto p = resampled_traj.points.at(4); - EXPECT_NEAR(p.pose.position.x, 5.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, traj.points.at(5).lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.50, epsilon); - EXPECT_NEAR(p.acceleration_mps2, traj.points.at(5).acceleration_mps2, epsilon); - } - - { - const auto p = resampled_traj.points.at(5); - auto ans_p = setZeroVelocityAfterStop(traj.points).at(6); - EXPECT_NEAR(p.pose.position.x, 6.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.60, epsilon); - EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); - } - - { - const auto p = resampled_traj.points.at(6); - auto ans_p = setZeroVelocityAfterStop(traj.points).at(7); - EXPECT_NEAR(p.pose.position.x, 7.5, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.75, epsilon); - EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); - } - - { - const auto p = resampled_traj.points.at(7); - const auto ans_p = setZeroVelocityAfterStop(traj.points).at(9); - EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); - - EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); - EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.90, epsilon); - EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); - } - } - - // No Resample - { - // Input path size is not enough for resample - { - autoware_planning_msgs::msg::Trajectory traj; - traj.points.resize(1); - for (size_t i = 0; i < 1; ++i) { - traj.points.at(i) = - generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); - } - - const auto resampled_traj = resampleTrajectory(traj, 1.0); - EXPECT_EQ(resampled_traj.points.size(), traj.points.size()); - for (size_t i = 0; i < resampled_traj.points.size(); ++i) { - EXPECT_NEAR( - resampled_traj.points.at(i).pose.position.x, traj.points.at(i).pose.position.x, epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.position.y, traj.points.at(i).pose.position.y, epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.position.z, traj.points.at(i).pose.position.z, epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.x, traj.points.at(i).pose.orientation.x, - epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.y, traj.points.at(i).pose.orientation.y, - epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.z, traj.points.at(i).pose.orientation.z, - epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.w, traj.points.at(i).pose.orientation.w, - epsilon); - } - } - - // Resample interval is invalid - { - autoware_planning_msgs::msg::Trajectory traj; - traj.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - traj.points.at(i) = - generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); - } - - const auto resampled_traj = resampleTrajectory(traj, 1e-4); - EXPECT_EQ(resampled_traj.points.size(), traj.points.size()); - for (size_t i = 0; i < resampled_traj.points.size(); ++i) { - EXPECT_NEAR( - resampled_traj.points.at(i).pose.position.x, traj.points.at(i).pose.position.x, epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.position.y, traj.points.at(i).pose.position.y, epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.position.z, traj.points.at(i).pose.position.z, epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.x, traj.points.at(i).pose.orientation.x, - epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.y, traj.points.at(i).pose.orientation.y, - epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.z, traj.points.at(i).pose.orientation.z, - epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.w, traj.points.at(i).pose.orientation.w, - epsilon); - } - } - - // Resample interval is invalid (Negative value) - { - autoware_planning_msgs::msg::Trajectory traj; - traj.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - traj.points.at(i) = - generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); - } - - const auto resampled_traj = resampleTrajectory(traj, -5.0); - EXPECT_EQ(resampled_traj.points.size(), traj.points.size()); - for (size_t i = 0; i < resampled_traj.points.size(); ++i) { - EXPECT_NEAR( - resampled_traj.points.at(i).pose.position.x, traj.points.at(i).pose.position.x, epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.position.y, traj.points.at(i).pose.position.y, epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.position.z, traj.points.at(i).pose.position.z, epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.x, traj.points.at(i).pose.orientation.x, - epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.y, traj.points.at(i).pose.orientation.y, - epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.z, traj.points.at(i).pose.orientation.z, - epsilon); - EXPECT_NEAR( - resampled_traj.points.at(i).pose.orientation.w, traj.points.at(i).pose.orientation.w, - epsilon); - } - } - } -} - -TEST(resample_trajectory, resample_with_middle_stop_point) -{ - // This test is to check the behavior when the stop point is unstably resampled by zero-order hold - // interpolation. - - using autoware::motion_utils::resampleTrajectory; - - autoware_planning_msgs::msg::Trajectory traj; - traj.points.reserve(10); - - traj.points.push_back(generateTestTrajectoryPoint(0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0)); - traj.points.push_back(generateTestTrajectoryPoint(1.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0)); - traj.points.push_back(generateTestTrajectoryPoint(2.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0)); - traj.points.push_back(generateTestTrajectoryPoint(3.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - traj.points.push_back(generateTestTrajectoryPoint(3.1, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0)); - traj.points.push_back(generateTestTrajectoryPoint(4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - traj.points.push_back(generateTestTrajectoryPoint(5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - traj.points.push_back(generateTestTrajectoryPoint(6.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - traj.points.push_back(generateTestTrajectoryPoint(7.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - - std::vector interpolated_axis = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0}; - - const auto resampled_traj = resampleTrajectory(traj, interpolated_axis); - - EXPECT_NEAR(resampled_traj.points.at(0).longitudinal_velocity_mps, 10.0, epsilon); - EXPECT_NEAR(resampled_traj.points.at(1).longitudinal_velocity_mps, 10.0, epsilon); - EXPECT_NEAR(resampled_traj.points.at(2).longitudinal_velocity_mps, 10.0, epsilon); - EXPECT_NEAR(resampled_traj.points.at(3).longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(resampled_traj.points.at(4).longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(resampled_traj.points.at(5).longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(resampled_traj.points.at(6).longitudinal_velocity_mps, 0.0, epsilon); -} diff --git a/common/autoware_motion_utils/test/src/test_motion_utils.cpp b/common/autoware_motion_utils/test/src/test_motion_utils.cpp deleted file mode 100644 index d8b9b0ac78981..0000000000000 --- a/common/autoware_motion_utils/test/src/test_motion_utils.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2022 TierIV -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include - -int main(int argc, char * argv[]) -{ - testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); - bool result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -} diff --git a/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp b/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp deleted file mode 100644 index f40159d7dd833..0000000000000 --- a/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/motion_utils/trajectory/trajectory.hpp" - -#include -#include -#include - -#include - -namespace -{ -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 = create_point(x, y, z); - p.orientation = create_quaternion_from_rpy(roll, pitch, yaw); - return p; -} - -template -T generateTestTrajectory( - const size_t num_points, const double point_interval, const double vel = 0.0, - const double init_theta = 0.0, const double delta_theta = 0.0) -{ - using Point = typename T::_points_type::value_type; - - T traj; - for (size_t i = 0; i < num_points; ++i) { - const double theta = init_theta + i * delta_theta; - const double x = i * point_interval * std::cos(theta); - const double y = i * point_interval * std::sin(theta); - - Point p; - p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); - p.longitudinal_velocity_mps = vel; - traj.points.push_back(p); - } - - return traj; -} -} // namespace - -TEST(trajectory_benchmark, DISABLED_calcLateralOffset) -{ - std::random_device r; - std::default_random_engine e1(r()); - std::uniform_real_distribution uniform_dist(0.0, 1000.0); - - using autoware::motion_utils::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 = 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 deleted file mode 100644 index 7d05d69de4e1a..0000000000000 --- a/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp +++ /dev/null @@ -1,664 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/motion_utils/trajectory/conversion.hpp" -#include "autoware/motion_utils/trajectory/interpolation.hpp" -#include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware_utils/geometry/boost_geometry.hpp" -#include "autoware_utils/math/unit_conversion.hpp" - -#include -#include -#include - -#include -#include - -namespace -{ -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; - -geometry_msgs::msg::Pose createPose( - double x, double y, double z, double roll, double pitch, double yaw) -{ - geometry_msgs::msg::Pose p; - p.position = create_point(x, y, z); - p.orientation = create_quaternion_from_rpy(roll, pitch, yaw); - return p; -} - -TrajectoryPoint generateTestTrajectoryPoint( - const double x, const double y, const double z, const double theta = 0.0, - const double vel_lon = 0.0, const double vel_lat = 0.0, const double heading_rate = 0.0, - const double acc = 0.0) -{ - TrajectoryPoint p; - p.pose = createPose(x, y, z, 0.0, 0.0, theta); - p.longitudinal_velocity_mps = vel_lon; - p.lateral_velocity_mps = vel_lat; - p.heading_rate_rps = heading_rate; - p.acceleration_mps2 = acc; - return p; -} - -template -T generateTestTrajectory( - const size_t num_points, const double point_interval, const double vel_lon = 0.0, - const double vel_lat = 0.0, const double heading_rate_rps = 0.0, const double acc = 0.0, - const double init_theta = 0.0, const double delta_theta = 0.0) -{ - using Point = typename T::_points_type::value_type; - - T traj; - for (size_t i = 0; i < num_points; ++i) { - const double theta = init_theta + i * delta_theta; - const double x = i * point_interval * std::cos(theta); - const double y = i * point_interval * std::sin(theta); - - Point p; - p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); - p.longitudinal_velocity_mps = vel_lon; - p.lateral_velocity_mps = vel_lat; - p.heading_rate_rps = heading_rate_rps; - p.acceleration_mps2 = acc; - traj.points.push_back(p); - } - return traj; -} - -PathPointWithLaneId generateTestPathPoint( - const double x, const double y, const double z, const double theta = 0.0, - const double vel_lon = 0.0, const double vel_lat = 0.0, const double heading_rate = 0.0) -{ - PathPointWithLaneId p; - p.point.pose = createPose(x, y, z, 0.0, 0.0, theta); - p.point.longitudinal_velocity_mps = vel_lon; - p.point.lateral_velocity_mps = vel_lat; - p.point.heading_rate_rps = heading_rate; - return p; -} - -template -T generateTestPath( - const size_t num_points, const double point_interval, const double vel_lon = 0.0, - const double vel_lat = 0.0, const double heading_rate_rps = 0.0, const double init_theta = 0.0, - const double delta_theta = 0.0) -{ - using Point = typename T::_points_type::value_type; - - T traj; - for (size_t i = 0; i < num_points; ++i) { - const double theta = init_theta + i * delta_theta; - const double x = i * point_interval * std::cos(theta); - const double y = i * point_interval * std::sin(theta); - - Point p; - p.point.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); - p.point.longitudinal_velocity_mps = vel_lon; - p.point.lateral_velocity_mps = vel_lat; - p.point.heading_rate_rps = heading_rate_rps; - traj.points.push_back(p); - } - return traj; -} -} // namespace - -TEST(Interpolation, interpolate_path_for_trajectory) -{ - using autoware::motion_utils::calcInterpolatedPoint; - - { - autoware_planning_msgs::msg::Trajectory traj; - traj.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - traj.points.at(i) = - generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); - } - - // Same points as the trajectory point - { - const auto target_pose = createPose(3.0, 0.0, 0.0, 0.0, 0.0, 0.0); - const auto result = calcInterpolatedPoint(traj, target_pose); - - EXPECT_NEAR(result.pose.position.x, 3.0, epsilon); - EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR(result.longitudinal_velocity_mps, 3.0, epsilon); - EXPECT_NEAR(result.lateral_velocity_mps, 1.5, epsilon); - EXPECT_NEAR(result.heading_rate_rps, 0.3, epsilon); - EXPECT_NEAR(result.acceleration_mps2, 0.15, epsilon); - EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); - EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); - } - - // Random Point - { - const auto target_pose = createPose(2.5, 5.0, 0.0, 0.0, 0.0, 0.0); - const auto result = calcInterpolatedPoint(traj, target_pose); - - EXPECT_NEAR(result.pose.position.x, 2.5, epsilon); - EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR(result.longitudinal_velocity_mps, 2.5, epsilon); - EXPECT_NEAR(result.lateral_velocity_mps, 1.25, epsilon); - EXPECT_NEAR(result.heading_rate_rps, 0.25, epsilon); - EXPECT_NEAR(result.acceleration_mps2, 0.125, epsilon); - EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); - EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); - } - - // Random Point with zero order hold - { - const auto target_pose = createPose(2.5, 5.0, 0.0, 0.0, 0.0, 0.0); - const auto result = calcInterpolatedPoint(traj, target_pose, true); - - EXPECT_NEAR(result.pose.position.x, 2.5, epsilon); - EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR(result.longitudinal_velocity_mps, 2.0, epsilon); - EXPECT_NEAR(result.lateral_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(result.heading_rate_rps, 0.25, epsilon); - EXPECT_NEAR(result.acceleration_mps2, 0.1, epsilon); - EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); - EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); - } - - // Initial Point - { - const auto target_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - const auto result = calcInterpolatedPoint(traj, target_pose); - - EXPECT_NEAR(result.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR(result.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(result.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(result.heading_rate_rps, 0.0, epsilon); - EXPECT_NEAR(result.acceleration_mps2, 0.0, epsilon); - EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); - EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); - } - - // Terminal Point - { - const auto target_pose = createPose(9.0, 0.0, 0.0, 0.0, 0.0, 0.0); - const auto result = calcInterpolatedPoint(traj, target_pose); - - EXPECT_NEAR(result.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR(result.longitudinal_velocity_mps, 9.0, epsilon); - EXPECT_NEAR(result.lateral_velocity_mps, 4.5, epsilon); - EXPECT_NEAR(result.heading_rate_rps, 0.9, epsilon); - EXPECT_NEAR(result.acceleration_mps2, 0.45, epsilon); - EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); - EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); - } - - // Outside of initial point - { - const auto target_pose = createPose(-2.0, -9.0, 0.0, 0.0, 0.0, 0.0); - const auto result = calcInterpolatedPoint(traj, target_pose); - - EXPECT_NEAR(result.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR(result.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(result.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(result.heading_rate_rps, 0.0, epsilon); - EXPECT_NEAR(result.acceleration_mps2, 0.0, epsilon); - EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); - EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); - } - - // Outside of terminal point - { - const auto target_pose = createPose(10.0, 5.0, 0.0, 0.0, 0.0, 0.0); - const auto result = calcInterpolatedPoint(traj, target_pose); - - EXPECT_NEAR(result.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR(result.longitudinal_velocity_mps, 9.0, epsilon); - EXPECT_NEAR(result.lateral_velocity_mps, 4.5, epsilon); - EXPECT_NEAR(result.heading_rate_rps, 0.9, epsilon); - EXPECT_NEAR(result.acceleration_mps2, 0.45, epsilon); - EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); - EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); - } - } - - // Empty Point - { - const Trajectory traj; - const auto target_pose = createPose(3.0, 5.0, 0.0, 0.0, 0.0, 0.0); - const auto result = calcInterpolatedPoint(traj, target_pose); - - EXPECT_NEAR(result.pose.position.x, 3.0, epsilon); - EXPECT_NEAR(result.pose.position.y, 5.0, epsilon); - EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR(result.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(result.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(result.heading_rate_rps, 0.0, epsilon); - EXPECT_NEAR(result.acceleration_mps2, 0.0, epsilon); - EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); - EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); - } - - // One point - { - const auto traj = generateTestTrajectory(1, 1.0, 1.0, 0.5, 0.1, 0.05); - const auto target_pose = createPose(3.0, 5.0, 0.0, 0.0, 0.0, 0.0); - const auto result = calcInterpolatedPoint(traj, target_pose); - - EXPECT_NEAR(result.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR(result.longitudinal_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(result.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(result.heading_rate_rps, 0.1, epsilon); - EXPECT_NEAR(result.acceleration_mps2, 0.05, epsilon); - EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); - EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); - } - - // Duplicated Points - { - autoware_planning_msgs::msg::Trajectory traj; - traj.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - traj.points.at(i) = - generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); - } - traj.points.at(4) = traj.points.at(3); - - const auto target_pose = createPose(3.2, 5.0, 0.0, 0.0, 0.0, 0.0); - const auto result = calcInterpolatedPoint(traj, target_pose); - - EXPECT_NEAR(result.pose.position.x, 3.0, epsilon); - EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR(result.longitudinal_velocity_mps, 3.0, epsilon); - EXPECT_NEAR(result.lateral_velocity_mps, 1.5, epsilon); - EXPECT_NEAR(result.heading_rate_rps, 0.3, epsilon); - EXPECT_NEAR(result.acceleration_mps2, 0.15, epsilon); - EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); - EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); - } -} - -TEST(Interpolation, interpolate_path_for_path) -{ - using autoware::motion_utils::calcInterpolatedPoint; - - { - autoware_internal_planning_msgs::msg::PathWithLaneId path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); - } - - // Same points as the path point - { - const auto target_pose = createPose(3.0, 0.0, 0.0, 0.0, 0.0, 0.0); - const auto result = calcInterpolatedPoint(path, target_pose); - - EXPECT_NEAR(result.point.pose.position.x, 3.0, epsilon); - EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR(result.point.longitudinal_velocity_mps, 3.0, epsilon); - EXPECT_NEAR(result.point.lateral_velocity_mps, 1.5, epsilon); - EXPECT_NEAR(result.point.heading_rate_rps, 0.3, epsilon); - } - - // Random Point - { - const auto target_pose = createPose(2.5, 5.0, 0.0, 0.0, 0.0, 0.0); - const auto result = calcInterpolatedPoint(path, target_pose); - - EXPECT_NEAR(result.point.pose.position.x, 2.5, epsilon); - EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR(result.point.longitudinal_velocity_mps, 2.5, epsilon); - EXPECT_NEAR(result.point.lateral_velocity_mps, 1.25, epsilon); - EXPECT_NEAR(result.point.heading_rate_rps, 0.25, epsilon); - } - - // Random Point with zero order hold - { - const auto target_pose = createPose(2.5, 5.0, 0.0, 0.0, 0.0, 0.0); - const auto result = calcInterpolatedPoint(path, target_pose, true); - - EXPECT_NEAR(result.point.pose.position.x, 2.5, epsilon); - EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR(result.point.longitudinal_velocity_mps, 2.0, epsilon); - EXPECT_NEAR(result.point.lateral_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(result.point.heading_rate_rps, 0.25, epsilon); - } - - // Initial Point - { - const auto target_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - const auto result = calcInterpolatedPoint(path, target_pose); - - EXPECT_NEAR(result.point.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR(result.point.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(result.point.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(result.point.heading_rate_rps, 0.0, epsilon); - } - - // Terminal Point - { - const auto target_pose = createPose(9.0, 0.0, 0.0, 0.0, 0.0, 0.0); - const auto result = calcInterpolatedPoint(path, target_pose); - - EXPECT_NEAR(result.point.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR(result.point.longitudinal_velocity_mps, 9.0, epsilon); - EXPECT_NEAR(result.point.lateral_velocity_mps, 4.5, epsilon); - EXPECT_NEAR(result.point.heading_rate_rps, 0.9, epsilon); - } - - // Outside of initial point - { - const auto target_pose = createPose(-2.0, -9.0, 0.0, 0.0, 0.0, 0.0); - const auto result = calcInterpolatedPoint(path, target_pose); - - EXPECT_NEAR(result.point.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR(result.point.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(result.point.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(result.point.heading_rate_rps, 0.0, epsilon); - } - - // Outside of terminal point - { - const auto target_pose = createPose(10.0, 5.0, 0.0, 0.0, 0.0, 0.0); - const auto result = calcInterpolatedPoint(path, target_pose); - - EXPECT_NEAR(result.point.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR(result.point.longitudinal_velocity_mps, 9.0, epsilon); - EXPECT_NEAR(result.point.lateral_velocity_mps, 4.5, epsilon); - EXPECT_NEAR(result.point.heading_rate_rps, 0.9, epsilon); - } - } - - // Empty Point - { - const PathWithLaneId path; - const auto target_pose = createPose(3.0, 5.0, 0.0, 0.0, 0.0, 0.0); - const auto result = calcInterpolatedPoint(path, target_pose); - - EXPECT_NEAR(result.point.pose.position.x, 3.0, epsilon); - EXPECT_NEAR(result.point.pose.position.y, 5.0, epsilon); - EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR(result.point.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(result.point.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(result.point.heading_rate_rps, 0.0, epsilon); - } - - // One point - { - const auto path = generateTestPath(1, 1.0, 1.0, 0.5, 0.1); - const auto target_pose = createPose(3.0, 5.0, 0.0, 0.0, 0.0, 0.0); - const auto result = calcInterpolatedPoint(path, target_pose); - - EXPECT_NEAR(result.point.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR(result.point.longitudinal_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(result.point.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(result.point.heading_rate_rps, 0.1, epsilon); - } - - // Duplicated Points - { - autoware_internal_planning_msgs::msg::PathWithLaneId path; - path.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); - } - path.points.at(4) = path.points.at(3); - - const auto target_pose = createPose(3.2, 5.0, 0.0, 0.0, 0.0, 0.0); - const auto result = calcInterpolatedPoint(path, target_pose); - - EXPECT_NEAR(result.point.pose.position.x, 3.0, epsilon); - EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon); - EXPECT_NEAR(result.point.longitudinal_velocity_mps, 3.0, epsilon); - EXPECT_NEAR(result.point.lateral_velocity_mps, 1.5, epsilon); - EXPECT_NEAR(result.point.heading_rate_rps, 0.3, epsilon); - } -} - -TEST(Interpolation, interpolate_points_with_length) -{ - using autoware::motion_utils::calcInterpolatedPose; - - { - autoware_planning_msgs::msg::Trajectory traj; - traj.points.resize(10); - for (size_t i = 0; i < 10; ++i) { - traj.points.at(i) = - generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); - } - - // Same points as the trajectory point - { - const auto result = calcInterpolatedPose(traj.points, 3.0); - - EXPECT_NEAR(result.position.x, 3.0, epsilon); - EXPECT_NEAR(result.position.y, 0.0, epsilon); - EXPECT_NEAR(result.position.z, 0.0, epsilon); - EXPECT_NEAR(result.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.orientation.w, 1.0, epsilon); - } - - // Random Point - { - const auto result = calcInterpolatedPose(traj.points, 2.5); - - EXPECT_NEAR(result.position.x, 2.5, epsilon); - EXPECT_NEAR(result.position.y, 0.0, epsilon); - EXPECT_NEAR(result.position.z, 0.0, epsilon); - EXPECT_NEAR(result.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.orientation.w, 1.0, epsilon); - } - - // Negative length - { - const auto result = calcInterpolatedPose(traj.points, -3.0); - - EXPECT_NEAR(result.position.x, 0.0, epsilon); - EXPECT_NEAR(result.position.y, 0.0, epsilon); - EXPECT_NEAR(result.position.z, 0.0, epsilon); - EXPECT_NEAR(result.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.orientation.w, 1.0, epsilon); - } - - // Boundary value (0.0) - { - const auto result = calcInterpolatedPose(traj.points, 0.0); - - EXPECT_NEAR(result.position.x, 0.0, epsilon); - EXPECT_NEAR(result.position.y, 0.0, epsilon); - EXPECT_NEAR(result.position.z, 0.0, epsilon); - EXPECT_NEAR(result.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.orientation.w, 1.0, epsilon); - } - - // Terminal Point - { - const auto result = calcInterpolatedPose(traj.points, 9.0); - - EXPECT_NEAR(result.position.x, 9.0, epsilon); - EXPECT_NEAR(result.position.y, 0.0, epsilon); - EXPECT_NEAR(result.position.z, 0.0, epsilon); - EXPECT_NEAR(result.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.orientation.w, 1.0, epsilon); - } - - // Outside of terminal point - { - const auto result = calcInterpolatedPose(traj.points, 10.0); - - EXPECT_NEAR(result.position.x, 9.0, epsilon); - EXPECT_NEAR(result.position.y, 0.0, epsilon); - EXPECT_NEAR(result.position.z, 0.0, epsilon); - EXPECT_NEAR(result.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.orientation.w, 1.0, epsilon); - } - } - - // one point - { - autoware_planning_msgs::msg::Trajectory traj; - traj.points.resize(1); - for (size_t i = 0; i < 1; ++i) { - traj.points.at(i) = generateTestTrajectoryPoint( - (i + 1) * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); - } - - const auto result = calcInterpolatedPose(traj.points, 2.0); - EXPECT_NEAR(result.position.x, 1.0, epsilon); - EXPECT_NEAR(result.position.y, 0.0, epsilon); - EXPECT_NEAR(result.position.z, 0.0, epsilon); - EXPECT_NEAR(result.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.orientation.w, 1.0, epsilon); - } - - // Empty Point - { - const Trajectory traj; - const auto result = calcInterpolatedPose(traj.points, 2.0); - - EXPECT_NEAR(result.position.x, 0.0, epsilon); - EXPECT_NEAR(result.position.y, 0.0, epsilon); - EXPECT_NEAR(result.position.z, 0.0, epsilon); - EXPECT_NEAR(result.orientation.x, 0.0, epsilon); - EXPECT_NEAR(result.orientation.y, 0.0, epsilon); - EXPECT_NEAR(result.orientation.z, 0.0, epsilon); - EXPECT_NEAR(result.orientation.w, 1.0, epsilon); - } -} diff --git a/common/autoware_motion_utils/test/src/trajectory/test_path_shift.cpp b/common/autoware_motion_utils/test/src/trajectory/test_path_shift.cpp deleted file mode 100644 index fbdcc7a8f6f81..0000000000000 --- a/common/autoware_motion_utils/test/src/trajectory/test_path_shift.cpp +++ /dev/null @@ -1,163 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/motion_utils/trajectory/path_shift.hpp" - -#include - -TEST(path_shift_test, calc_feasible_velocity_from_jerk) -{ - using autoware::motion_utils::calc_feasible_velocity_from_jerk; - - double longitudinal_distance = 0.0; - double lateral_distance = 0.0; - double lateral_jerk = 0.0; - - // Condition: zero lateral jerk - EXPECT_DOUBLE_EQ( - calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), - 1.0e10); - - // Condition: zero lateral distance - lateral_jerk = 1.0; - EXPECT_DOUBLE_EQ( - calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), - 1.0e10); - - // Condition: zero longitudinal distance - lateral_distance = 2.0; - EXPECT_DOUBLE_EQ( - calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), 0.0); - - // Condition: random condition - longitudinal_distance = 50.0; - EXPECT_DOUBLE_EQ( - calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), 12.5); -} - -TEST(path_shift_test, calc_lateral_dist_from_jerk) -{ - using autoware::motion_utils::calc_lateral_dist_from_jerk; - - double longitudinal_distance = 0.0; - double lateral_jerk = 0.0; - double velocity = 0.0; - - // Condition: zero lateral jerk - EXPECT_DOUBLE_EQ( - calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 1.0e10); - - // Condition: zero velocity - lateral_jerk = 2.0; - EXPECT_DOUBLE_EQ( - calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 1.0e10); - - // Condition: zero longitudinal distance - velocity = 10.0; - EXPECT_DOUBLE_EQ(calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 0.0); - - // Condition: random condition - longitudinal_distance = 100.0; - EXPECT_DOUBLE_EQ( - calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 62.5); -} - -TEST(path_shift_test, calc_longitudinal_dist_from_jerk) -{ - using autoware::motion_utils::calc_longitudinal_dist_from_jerk; - - double lateral_distance = 0.0; - double lateral_jerk = 0.0; - double velocity = 0.0; - - // Condition: zero lateral jerk - EXPECT_DOUBLE_EQ( - calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 1.0e10); - - // Condition: zero lateral distance - lateral_jerk = -1.0; - velocity = 10.0; - EXPECT_DOUBLE_EQ(calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 0.0); - - // Condition: zero velocity - velocity = 0.0; - lateral_distance = 54.0; - EXPECT_DOUBLE_EQ(calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 0.0); - - // Condition: random - velocity = 8.0; - EXPECT_DOUBLE_EQ( - calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 96.0); -} - -TEST(path_shift_test, calc_shift_time_from_jerk) -{ - constexpr double epsilon = 1e-6; - - using autoware::motion_utils::calc_shift_time_from_jerk; - - double lateral_distance = 0.0; - double lateral_jerk = 0.0; - double lateral_acceleration = 0.0; - - // Condition: zero lateral jerk - EXPECT_DOUBLE_EQ( - calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 1.0e10); - - // Condition: zero lateral acceleration - lateral_jerk = -2.0; - EXPECT_DOUBLE_EQ( - calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 1.0e10); - - // Condition: zero lateral distance - lateral_acceleration = -4.0; - EXPECT_DOUBLE_EQ( - calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 0.0); - - // Condition: random (TODO: use DOUBLE_EQ in future. currently not precise enough) - lateral_distance = 80.0; - EXPECT_NEAR( - calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 11.16515139, - epsilon); -} - -TEST(path_shift_test, calc_jerk_from_lat_lon_distance) -{ - using autoware::motion_utils::calc_jerk_from_lat_lon_distance; - - double lateral_distance = 0.0; - double longitudinal_distance = 100.0; - double velocity = 10.0; - - // Condition: zero lateral distance - EXPECT_DOUBLE_EQ( - calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 0.0); - - // Condition: zero velocity - lateral_distance = 5.0; - velocity = 0.0; - EXPECT_DOUBLE_EQ( - calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 0.0); - - // Condition: zero longitudinal distance - longitudinal_distance = 0.0; - velocity = 10.0; - EXPECT_DOUBLE_EQ( - calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 1.6 * 1e14); - - // Condition: random - longitudinal_distance = 100.0; - EXPECT_DOUBLE_EQ( - calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 0.16); -} 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 deleted file mode 100644 index 1315a0c6b155a..0000000000000 --- a/common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp +++ /dev/null @@ -1,190 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" -#include "autoware_utils/geometry/geometry.hpp" - -#include - -#include -#include - -namespace -{ -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 = create_point(x, y, z); - p.orientation = autoware_utils::create_quaternion_from_rpy(roll, pitch, yaw); - return p; -} - -PathWithLaneId generateTestPathWithLaneId(const size_t num_points, const double point_interval) -{ - PathWithLaneId path; - for (size_t i = 0; i < num_points; ++i) { - const double x = i * point_interval; - const double y = 0.0; - - PathPointWithLaneId p; - p.point.pose = createPose(x, y, 0.0, 0.0, 0.0, 0.0); - p.lane_ids.push_back(i); - path.points.push_back(p); - } - - return path; -} -} // namespace - -TEST(path_with_lane_id, getPathIndexRangeWithLaneId) -{ - using autoware::motion_utils::getPathIndexRangeWithLaneId; - using autoware_internal_planning_msgs::msg::PathWithLaneId; - - // Usual cases - { - PathWithLaneId points; - points.points.resize(6); - points.points.at(0).lane_ids.push_back(3); - points.points.at(1).lane_ids.push_back(3); - points.points.at(2).lane_ids.push_back(1); - points.points.at(3).lane_ids.push_back(2); - points.points.at(4).lane_ids.push_back(2); - points.points.at(5).lane_ids.push_back(2); - - { - const auto res = getPathIndexRangeWithLaneId(points, 3); - EXPECT_EQ(res->first, 0U); - EXPECT_EQ(res->second, 2U); - } - { - const auto res = getPathIndexRangeWithLaneId(points, 1); - EXPECT_EQ(res->first, 1U); - EXPECT_EQ(res->second, 3U); - } - { - const auto res = getPathIndexRangeWithLaneId(points, 2); - EXPECT_EQ(res->first, 2U); - EXPECT_EQ(res->second, 5U); - } - { - const auto res = getPathIndexRangeWithLaneId(points, 4); - EXPECT_EQ(res, std::nullopt); - } - } - - // Empty points - { - PathWithLaneId points; - const auto res = getPathIndexRangeWithLaneId(points, 4); - EXPECT_EQ(res, std::nullopt); - } -} - -TEST(path_with_lane_id, findNearestIndexFromLaneId) -{ - using autoware::motion_utils::findNearestIndexFromLaneId; - using autoware::motion_utils::findNearestSegmentIndexFromLaneId; - - const auto path = generateTestPathWithLaneId(10, 1.0); - - // Normal cases - { - auto modified_path = path; - for (size_t i = 0; i < 10; ++i) { - modified_path.points.at(i).lane_ids = {100}; - } - EXPECT_EQ(findNearestIndexFromLaneId(modified_path, create_point(2.4, 1.3, 0.0), 100), 2U); - EXPECT_EQ( - findNearestSegmentIndexFromLaneId(modified_path, create_point(2.4, 1.3, 0.0), 100), 2U); - } - - { - auto modified_path = path; - for (size_t i = 3; i < 6; ++i) { - modified_path.points.at(i).lane_ids = {100}; - } - EXPECT_EQ(findNearestIndexFromLaneId(modified_path, create_point(4.1, 0.3, 0.0), 100), 4U); - EXPECT_EQ( - findNearestSegmentIndexFromLaneId(modified_path, create_point(4.1, 0.3, 0.0), 100), 4U); - } - - { - auto modified_path = path; - for (size_t i = 8; i < 9; ++i) { - modified_path.points.at(i).lane_ids = {100}; - } - EXPECT_EQ(findNearestIndexFromLaneId(modified_path, create_point(8.5, -0.5, 0.0), 100), 8U); - EXPECT_EQ( - findNearestSegmentIndexFromLaneId(modified_path, create_point(8.5, -0.5, 0.0), 100), 8U); - } - - // Nearest is not within range - { - auto modified_path = path; - for (size_t i = 3; i < 9; ++i) { - modified_path.points.at(i).lane_ids = {100}; - } - EXPECT_EQ(findNearestIndexFromLaneId(modified_path, create_point(2.4, 1.3, 0.0), 100), 2U); - EXPECT_EQ( - findNearestSegmentIndexFromLaneId(modified_path, create_point(2.4, 1.3, 0.0), 100), 2U); - } - - // Path does not contain lane_id. - { - 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{}, create_point(2.4, 1.3, 0.0), 100), - std::invalid_argument); - EXPECT_THROW( - findNearestSegmentIndexFromLaneId(PathWithLaneId{}, create_point(2.4, 1.3, 0.0), 100), - std::invalid_argument); -} - -// NOTE: This test is temporary for the current implementation. -TEST(path_with_lane_id, convertToRearWheelCenter) -{ - using autoware::motion_utils::convertToRearWheelCenter; - - PathWithLaneId path; - - PathPointWithLaneId p1; - p1.point.pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - path.points.push_back(p1); - PathPointWithLaneId p2; - p2.point.pose = createPose(1.0, 2.0, 0.0, 0.0, 0.0, 0.0); - path.points.push_back(p2); - PathPointWithLaneId p3; - p3.point.pose = createPose(2.0, 4.0, 0.0, 0.0, 0.0, 0.0); - path.points.push_back(p3); - - const auto cog_path = convertToRearWheelCenter(path, 1.0); - - constexpr double epsilon = 1e-6; - EXPECT_NEAR(cog_path.points.at(0).point.pose.position.x, -0.4472136, epsilon); - EXPECT_NEAR(cog_path.points.at(0).point.pose.position.y, -0.8944272, epsilon); - EXPECT_NEAR(cog_path.points.at(1).point.pose.position.x, 0.5527864, epsilon); - EXPECT_NEAR(cog_path.points.at(1).point.pose.position.y, 1.1055728, epsilon); - EXPECT_NEAR(cog_path.points.at(2).point.pose.position.x, 2.0, epsilon); - EXPECT_NEAR(cog_path.points.at(2).point.pose.position.y, 4.0, epsilon); -} diff --git a/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp b/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp deleted file mode 100644 index a2a931741a235..0000000000000 --- a/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp +++ /dev/null @@ -1,5517 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/motion_utils/trajectory/conversion.hpp" -#include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware_utils/geometry/boost_geometry.hpp" -#include "autoware_utils/math/unit_conversion.hpp" - -#include -#include -#include - -#include -#include -#include - -namespace -{ -using autoware_planning_msgs::msg::Trajectory; -using TrajectoryPointArray = std::vector; -using autoware_utils::create_point; -using autoware_utils::create_quaternion_from_rpy; -using autoware_utils::transform_point; - -constexpr double epsilon = 1e-6; - -geometry_msgs::msg::Pose createPose( - double x, double y, double z, double roll, double pitch, double yaw) -{ - geometry_msgs::msg::Pose p; - p.position = create_point(x, y, z); - p.orientation = create_quaternion_from_rpy(roll, pitch, yaw); - return p; -} - -template -T generateTestTrajectory( - const size_t num_points, const double point_interval, const double vel = 0.0, - const double init_theta = 0.0, const double delta_theta = 0.0) -{ - using Point = typename T::_points_type::value_type; - - T traj; - for (size_t i = 0; i < num_points; ++i) { - const double theta = init_theta + i * delta_theta; - const double x = i * point_interval * std::cos(theta); - const double y = i * point_interval * std::sin(theta); - - Point p; - p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); - p.longitudinal_velocity_mps = vel; - traj.points.push_back(p); - } - - return traj; -} - -TrajectoryPointArray generateTestTrajectoryPointArray( - const size_t num_points, const double point_interval, const double vel = 0.0, - const double init_theta = 0.0, const double delta_theta = 0.0) -{ - using autoware_planning_msgs::msg::TrajectoryPoint; - TrajectoryPointArray traj; - for (size_t i = 0; i < num_points; ++i) { - const double theta = init_theta + i * delta_theta; - const double x = i * point_interval * std::cos(theta); - const double y = i * point_interval * std::sin(theta); - - TrajectoryPoint p; - p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); - p.longitudinal_velocity_mps = vel; - traj.push_back(p); - } - - return traj; -} - -template -void updateTrajectoryVelocityAt(T & points, const size_t idx, const double vel) -{ - points.at(idx).longitudinal_velocity_mps = vel; -} -} // namespace - -TEST(trajectory, validateNonEmpty) -{ - using autoware::motion_utils::validateNonEmpty; - - // Empty - EXPECT_THROW(validateNonEmpty(Trajectory{}.points), std::invalid_argument); - - // Non-empty - const auto traj = generateTestTrajectory(10, 1.0); - EXPECT_NO_THROW(validateNonEmpty(traj.points)); -} - -TEST(trajectory, validateNonSharpAngle_DefaultThreshold) -{ - using autoware::motion_utils::validateNonSharpAngle; - using autoware_planning_msgs::msg::TrajectoryPoint; - - TrajectoryPoint p1; - p1.pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - p1.longitudinal_velocity_mps = 0.0; - - TrajectoryPoint p2; - p2.pose = createPose(1.0, 1.0, 0.0, 0.0, 0.0, 0.0); - p2.longitudinal_velocity_mps = 0.0; - - TrajectoryPoint p3; - p3.pose = createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0); - p3.longitudinal_velocity_mps = 0.0; - - // Non sharp angle - { - EXPECT_NO_THROW(validateNonSharpAngle(p1, p2, p3)); - } - - // Sharp angle - { - EXPECT_THROW(validateNonSharpAngle(p2, p1, p3), std::invalid_argument); - EXPECT_THROW(validateNonSharpAngle(p1, p3, p2), std::invalid_argument); - } -} - -TEST(trajectory, validateNonSharpAngle_SetThreshold) -{ - using autoware::motion_utils::validateNonSharpAngle; - 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); - p1.longitudinal_velocity_mps = 0.0; - - TrajectoryPoint p2; - p2.pose = createPose(1.73205080756887729, 0.0, 0.0, 0.0, 0.0, 0.0); - p2.longitudinal_velocity_mps = 0.0; - - TrajectoryPoint p3; - p3.pose = createPose(1.73205080756887729, 1.0, 0.0, 0.0, 0.0, 0.0); - p3.longitudinal_velocity_mps = 0.0; - - // Non sharp angle - { - EXPECT_NO_THROW(validateNonSharpAngle(p1, p2, p3, pi / 6)); - EXPECT_NO_THROW(validateNonSharpAngle(p2, p3, p1, pi / 6)); - } - - // Sharp angle - { - EXPECT_THROW(validateNonSharpAngle(p3, p1, p2, pi / 6), std::invalid_argument); - } -} - -TEST(trajectory, searchZeroVelocityIndex) -{ - using autoware::motion_utils::searchZeroVelocityIndex; - - // Empty - EXPECT_FALSE(searchZeroVelocityIndex(Trajectory{}.points)); - - // No zero velocity point - { - const auto traj = generateTestTrajectory(10, 1.0, 1.0); - EXPECT_FALSE(searchZeroVelocityIndex(traj.points)); - } - - // Only start point is zero - { - const size_t idx_ans = 0; - - auto traj = generateTestTrajectory(10, 1.0, 1.0); - updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); - - EXPECT_EQ(*searchZeroVelocityIndex(traj.points), idx_ans); - } - - // Only end point is zero - { - const size_t idx_ans = 9; - - auto traj = generateTestTrajectory(10, 1.0, 1.0); - updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); - - EXPECT_EQ(*searchZeroVelocityIndex(traj.points), idx_ans); - } - - // Only middle point is zero - { - const size_t idx_ans = 5; - - auto traj = generateTestTrajectory(10, 1.0, 1.0); - updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); - - EXPECT_EQ(*searchZeroVelocityIndex(traj.points), idx_ans); - } - - // Two points are zero - { - const size_t idx_ans = 3; - - auto traj = generateTestTrajectory(10, 1.0, 1.0); - updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); - updateTrajectoryVelocityAt(traj.points, 6, 0.0); - - EXPECT_EQ(*searchZeroVelocityIndex(traj.points), idx_ans); - } - - // Negative velocity point is before zero velocity point - { - const size_t idx_ans = 3; - - auto traj = generateTestTrajectory(10, 1.0, 1.0); - updateTrajectoryVelocityAt(traj.points, 2, -1.0); - updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); - - EXPECT_EQ(*searchZeroVelocityIndex(traj.points), idx_ans); - } - - // Search from src_idx to dst_idx - { - const size_t idx_ans = 3; - - auto traj = generateTestTrajectory(10, 1.0, 1.0); - updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); - - EXPECT_FALSE(searchZeroVelocityIndex(traj.points, 0, 3)); - EXPECT_EQ(*searchZeroVelocityIndex(traj.points, 0, 4), idx_ans); - EXPECT_EQ(*searchZeroVelocityIndex(traj.points, 3, 10), idx_ans); - EXPECT_FALSE(searchZeroVelocityIndex(traj.points, 4, 10)); - } -} - -TEST(trajectory, searchZeroVelocityIndex_from_pose) -{ - using autoware::motion_utils::searchZeroVelocityIndex; - - // No zero velocity point - { - const auto traj = generateTestTrajectory(10, 1.0, 1.0); - EXPECT_FALSE(searchZeroVelocityIndex(traj.points, 0)); - } - - // Only start point is zero - { - const size_t idx_ans = 0; - - auto traj = generateTestTrajectory(10, 1.0, 1.0); - updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); - - EXPECT_EQ(*searchZeroVelocityIndex(traj.points, 0), idx_ans); - } - - // Only end point is zero - { - const size_t idx_ans = 9; - - auto traj = generateTestTrajectory(10, 1.0, 1.0); - updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); - - EXPECT_EQ(*searchZeroVelocityIndex(traj.points, 0), idx_ans); - } - - // Only middle point is zero - { - const size_t idx_ans = 5; - - auto traj = generateTestTrajectory(10, 1.0, 1.0); - updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); - - EXPECT_EQ(*searchZeroVelocityIndex(traj.points, 0), idx_ans); - } - - // Two points are zero - { - const size_t idx_ans = 3; - - auto traj = generateTestTrajectory(10, 1.0, 1.0); - updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); - updateTrajectoryVelocityAt(traj.points, 6, 0.0); - - EXPECT_EQ(*searchZeroVelocityIndex(traj.points, 0), idx_ans); - } - - // Negative velocity point is before zero velocity point - { - const size_t idx_ans = 3; - - auto traj = generateTestTrajectory(10, 1.0, 1.0); - updateTrajectoryVelocityAt(traj.points, 2, -1.0); - updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); - - EXPECT_EQ(*searchZeroVelocityIndex(traj.points, 0), idx_ans); - } -} - -TEST(trajectory, findNearestIndex_Pos_StraightTrajectory) -{ - using autoware::motion_utils::findNearestIndex; - - const auto traj = generateTestTrajectory(10, 1.0); - - // Empty - EXPECT_THROW( - findNearestIndex(Trajectory{}.points, geometry_msgs::msg::Point{}), std::invalid_argument); - - // Start point - EXPECT_EQ(findNearestIndex(traj.points, create_point(0.0, 0.0, 0.0)), 0U); - - // End point - EXPECT_EQ(findNearestIndex(traj.points, create_point(9.0, 0.0, 0.0)), 9U); - - // Boundary conditions - 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, create_point(-4.0, 5.0, 0.0)), 0U); - - // Point after end point - EXPECT_EQ(findNearestIndex(traj.points, create_point(100.0, -3.0, 0.0)), 9U); - - // Random cases - 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) -{ - using autoware::motion_utils::findNearestIndex; - - const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); - - // Random cases - EXPECT_EQ(findNearestIndex(traj.points, create_point(5.1, 3.4, 0.0)), 6U); -} - -TEST(trajectory, findNearestIndex_Pose_NoThreshold) -{ - using autoware::motion_utils::findNearestIndex; - - const auto traj = generateTestTrajectory(10, 1.0); - - // Empty - EXPECT_FALSE(findNearestIndex(Trajectory{}.points, geometry_msgs::msg::Pose{}, {})); - - // Start point - EXPECT_EQ(*findNearestIndex(traj.points, createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)), 0U); - - // End point - EXPECT_EQ(*findNearestIndex(traj.points, createPose(9.0, 0.0, 0.0, 0.0, 0.0, 0.0)), 9U); - - // Boundary conditions - EXPECT_EQ(*findNearestIndex(traj.points, createPose(0.5, 0.0, 0.0, 0.0, 0.0, 0.0)), 0U); - EXPECT_EQ(*findNearestIndex(traj.points, createPose(0.51, 0.0, 0.0, 0.0, 0.0, 0.0)), 1U); - - // Point before start point - EXPECT_EQ(*findNearestIndex(traj.points, createPose(-4.0, 5.0, 0.0, 0.0, 0.0, 0.0)), 0U); - - // Point after end point - EXPECT_EQ(*findNearestIndex(traj.points, createPose(100.0, -3.0, 0.0, 0.0, 0.0, 0.0)), 9U); -} - -TEST(trajectory, findNearestIndex_Pose_DistThreshold) -{ - using autoware::motion_utils::findNearestIndex; - - const auto traj = generateTestTrajectory(10, 1.0); - - // Out of threshold - EXPECT_FALSE(findNearestIndex(traj.points, createPose(3.0, 0.6, 0.0, 0.0, 0.0, 0.0), 0.5)); - - // On threshold - EXPECT_EQ(*findNearestIndex(traj.points, createPose(3.0, 0.5, 0.0, 0.0, 0.0, 0.0), 0.5), 3U); - - // Within threshold - EXPECT_EQ(*findNearestIndex(traj.points, createPose(3.0, 0.4, 0.0, 0.0, 0.0, 0.0), 0.5), 3U); -} - -TEST(trajectory, findNearestIndex_Pose_YawThreshold) -{ - using autoware::motion_utils::findNearestIndex; - - const auto traj = generateTestTrajectory(10, 1.0); - const auto max_d = std::numeric_limits::max(); - - // Out of threshold - EXPECT_FALSE(findNearestIndex(traj.points, createPose(3.0, 0.0, 0.0, 0.0, 0.0, 1.1), max_d, 1.0)); - - // On threshold - EXPECT_EQ( - *findNearestIndex(traj.points, createPose(3.0, 0.0, 0.0, 0.0, 0.0, 1.0), max_d, 1.0), 3U); - - // Within threshold - EXPECT_EQ( - *findNearestIndex(traj.points, createPose(3.0, 0.0, 0.0, 0.0, 0.0, 0.9), max_d, 1.0), 3U); -} - -TEST(trajectory, findNearestIndex_Pose_DistAndYawThreshold) -{ - using autoware::motion_utils::findNearestIndex; - - const auto traj = generateTestTrajectory(10, 1.0); - - // Random cases - EXPECT_EQ(*findNearestIndex(traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 2.0, 0.4), 2U); - EXPECT_EQ( - *findNearestIndex(traj.points, createPose(4.1, 0.3, 0.0, 0.0, 0.0, -0.8), 0.5, 1.0), 4U); - EXPECT_EQ( - *findNearestIndex(traj.points, createPose(8.5, -0.5, 0.0, 0.0, 0.0, 0.0), 1.0, 0.1), 8U); -} - -TEST(trajectory, findNearestSegmentIndex) -{ - using autoware::motion_utils::findNearestSegmentIndex; - - const auto traj = generateTestTrajectory(10, 1.0); - - // Empty - EXPECT_THROW( - findNearestSegmentIndex(Trajectory{}.points, geometry_msgs::msg::Point{}), - std::invalid_argument); - - // Start point - EXPECT_EQ(findNearestSegmentIndex(traj.points, create_point(0.0, 0.0, 0.0)), 0U); - - // End point - EXPECT_EQ(findNearestSegmentIndex(traj.points, create_point(9.0, 0.0, 0.0)), 8U); - - // Boundary conditions - 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, create_point(-4.0, 5.0, 0.0)), 0U); - - // Point after end point - EXPECT_EQ(findNearestSegmentIndex(traj.points, create_point(100.0, -3.0, 0.0)), 8U); - - // Random cases - 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{ - 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, create_point(9.0, 1.0, 0.0)), 0U); -} - -TEST(trajectory, calcLongitudinalOffsetToSegment_StraightTrajectory) -{ - using autoware::motion_utils::calcLongitudinalOffsetToSegment; - - const auto traj = generateTestTrajectory(10, 1.0); - const bool throw_exception = true; - - // Empty - EXPECT_THROW( - calcLongitudinalOffsetToSegment( - Trajectory{}.points, {}, geometry_msgs::msg::Point{}, throw_exception), - std::invalid_argument); - - // Out of range - EXPECT_THROW( - calcLongitudinalOffsetToSegment(traj.points, -1, geometry_msgs::msg::Point{}, throw_exception), - std::out_of_range); - EXPECT_THROW( - calcLongitudinalOffsetToSegment( - traj.points, traj.points.size() - 1, geometry_msgs::msg::Point{}, throw_exception), - std::out_of_range); - - // Same close points in trajectory - { - const auto invalid_traj = generateTestTrajectory(10, 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); - } - - // Same point - EXPECT_NEAR( - 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, create_point(-3.9, 3.0, 0.0)), -9.9, epsilon); - - // Point after start point - EXPECT_NEAR( - calcLongitudinalOffsetToSegment(traj.points, 7, create_point(13.3, -10.0, 0.0)), 6.3, epsilon); - - // Random cases - EXPECT_NEAR( - calcLongitudinalOffsetToSegment(traj.points, 2, create_point(4.3, 7.0, 0.0)), 2.3, epsilon); - EXPECT_NEAR( - calcLongitudinalOffsetToSegment(traj.points, 8, create_point(1.0, 3.0, 0.0)), -7, epsilon); -} - -TEST(trajectory, calcLongitudinalOffsetToSegment_CurveTrajectory) -{ - using autoware::motion_utils::calcLongitudinalOffsetToSegment; - - const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); - - // Random cases - EXPECT_NEAR( - calcLongitudinalOffsetToSegment(traj.points, 2, create_point(2.0, 0.5, 0.0)), 0.083861449, - epsilon); -} - -TEST(trajectory, calcLateralOffset) -{ - using autoware::motion_utils::calcLateralOffset; - - const auto traj = generateTestTrajectory(10, 1.0); - const bool throw_exception = true; - - // Empty - EXPECT_THROW( - calcLateralOffset(Trajectory{}.points, geometry_msgs::msg::Point{}, throw_exception), - std::invalid_argument); - - // Trajectory size is 1 - { - const auto one_point_traj = generateTestTrajectory(1, 1.0); - EXPECT_THROW( - calcLateralOffset(one_point_traj.points, geometry_msgs::msg::Point{}, throw_exception), - std::runtime_error); - } - - // Same close points in trajectory - { - const auto invalid_traj = generateTestTrajectory(10, 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, create_point(3.1, 0.0, 0.0)), 0.0, epsilon); - - // Point before start point - 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, create_point(13.3, -10.0, 0.0)), -10.0, epsilon); - - // Random cases - 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) -{ - using autoware::motion_utils::calcLateralOffset; - - const auto traj = generateTestTrajectory(10, 1.0); - const bool throw_exception = true; - - // Empty - EXPECT_THROW( - calcLateralOffset(Trajectory{}.points, geometry_msgs::msg::Point{}, throw_exception), - std::invalid_argument); - - // Trajectory size is 1 - { - const auto one_point_traj = generateTestTrajectory(1, 1.0); - EXPECT_THROW( - calcLateralOffset( - one_point_traj.points, geometry_msgs::msg::Point{}, static_cast(0), - throw_exception), - std::runtime_error); - } - - // Same close points in trajectory - { - const auto invalid_traj = generateTestTrajectory(10, 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); - EXPECT_THROW( - calcLateralOffset(invalid_traj.points, p, static_cast(3), throw_exception), - std::runtime_error); - } - - // Point on trajectory - EXPECT_NEAR( - 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, create_point(-3.9, 3.0, 0.0), static_cast(0)), 3.0, - epsilon); - - // Point after start point - EXPECT_NEAR( - calcLateralOffset(traj.points, create_point(13.3, -10.0, 0.0), static_cast(8)), -10.0, - epsilon); - - // Random cases - EXPECT_NEAR( - calcLateralOffset(traj.points, create_point(4.3, 7.0, 0.0), static_cast(4)), 7.0, - epsilon); - EXPECT_NEAR( - calcLateralOffset(traj.points, create_point(1.0, -3.0, 0.0), static_cast(0)), -3.0, - epsilon); - EXPECT_NEAR( - calcLateralOffset(traj.points, create_point(1.0, -3.0, 0.0), static_cast(1)), -3.0, - epsilon); -} - -TEST(trajectory, calcLateralOffset_CurveTrajectory) -{ - using autoware::motion_utils::calcLateralOffset; - - const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); - - // Random cases - 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) -{ - using autoware::motion_utils::calcSignedArcLength; - - const auto traj = generateTestTrajectory(10, 1.0); - - // Empty - EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); - - // Out of range - EXPECT_THROW(calcSignedArcLength(traj.points, -1, 1), std::out_of_range); - EXPECT_THROW(calcSignedArcLength(traj.points, 0, traj.points.size() + 1), std::out_of_range); - - // Same point - EXPECT_NEAR(calcSignedArcLength(traj.points, 3, 3), 0, epsilon); - - // Forward - EXPECT_NEAR(calcSignedArcLength(traj.points, 0, 3), 3, epsilon); - - // Backward - EXPECT_NEAR(calcSignedArcLength(traj.points, 9, 5), -4, epsilon); -} - -TEST(trajectory, calcSignedArcLengthFromPointToIndex) -{ - using autoware::motion_utils::calcSignedArcLength; - - const auto traj = generateTestTrajectory(10, 1.0); - - // Empty - EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); - - // Same point - EXPECT_NEAR(calcSignedArcLength(traj.points, create_point(3.0, 0.0, 0.0), 3), 0, epsilon); - - // Forward - EXPECT_NEAR(calcSignedArcLength(traj.points, create_point(0.0, 0.0, 0.0), 3), 3, epsilon); - - // Backward - 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, create_point(-3.9, 3.0, 0.0), 6), 9.9, epsilon); - - // Point after end point - EXPECT_NEAR(calcSignedArcLength(traj.points, create_point(13.3, -10.0, 0.0), 7), -6.3, epsilon); - - // Random cases - 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) -{ - using autoware::motion_utils::calcSignedArcLength; - - const auto traj = generateTestTrajectory(10, 1.0); - - // Empty - EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); - - // Same point - EXPECT_NEAR(calcSignedArcLength(traj.points, 3, create_point(3.0, 0.0, 0.0)), 0, epsilon); - - // Forward - EXPECT_NEAR(calcSignedArcLength(traj.points, 0, create_point(3.0, 0.0, 0.0)), 3, epsilon); - - // Backward - 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, create_point(-3.9, 3.0, 0.0)), -9.9, epsilon); - - // Point after end point - 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, 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) -{ - using autoware::motion_utils::calcSignedArcLength; - - const auto traj = generateTestTrajectory(10, 1.0); - - // Empty - EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); - - // Same point - { - const auto p = create_point(3.0, 0.0, 0.0); - EXPECT_NEAR(calcSignedArcLength(traj.points, p, p), 0, epsilon); - } - - // Forward - { - 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 = 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 = 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 = 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 = 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 = 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 = 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); - } -} - -TEST(trajectory, calcArcLength) -{ - using autoware::motion_utils::calcArcLength; - - const auto traj = generateTestTrajectory(10, 1.0); - - // Empty - EXPECT_DOUBLE_EQ(calcArcLength(Trajectory{}.points), 0.0); - - // Whole Length - EXPECT_NEAR(calcArcLength(traj.points), 9.0, epsilon); -} - -TEST(trajectory, convertToTrajectory) -{ - using autoware::motion_utils::convertToTrajectory; - - // Size check - { - const auto traj_input = generateTestTrajectoryPointArray(50, 1.0); - const auto traj = convertToTrajectory(traj_input); - EXPECT_EQ(traj.points.size(), traj_input.size()); - } -} - -TEST(trajectory, convertToTrajectoryPointArray) -{ - using autoware::motion_utils::convertToTrajectoryPointArray; - - const auto traj_input = generateTestTrajectory(100, 1.0); - const auto traj = convertToTrajectoryPointArray(traj_input); - - // Size check - EXPECT_EQ(traj.size(), traj_input.points.size()); - - // Value check - for (size_t i = 0; i < traj.size(); ++i) { - EXPECT_EQ(traj.at(i), traj_input.points.at(i)); - } -} - -TEST(trajectory, calcDistanceToForwardStopPointFromIndex) -{ - using autoware::motion_utils::calcDistanceToForwardStopPoint; - - auto traj_input = generateTestTrajectory(100, 1.0, 3.0); - traj_input.points.at(50).longitudinal_velocity_mps = 0.0; - - // Empty - { - EXPECT_FALSE(calcDistanceToForwardStopPoint(Trajectory{}.points)); - } - - // No Stop Point - { - const auto traj_no_stop_point = generateTestTrajectory(10, 1.0, 3.0); - for (size_t i = 0; i < 10; ++i) { - const auto dist = calcDistanceToForwardStopPoint(traj_no_stop_point.points, i); - EXPECT_FALSE(dist); - } - } - - // Boundary1 (Edge of the input trajectory) - { - const auto dist = calcDistanceToForwardStopPoint(traj_input.points, 0); - EXPECT_NEAR(dist.value(), 50.0, epsilon); - } - - // Boundary2 (Edge of the input trajectory) - { - const auto dist = calcDistanceToForwardStopPoint(traj_input.points, 99); - EXPECT_FALSE(dist); - } - - // Boundary3 (On the Stop Point) - { - const auto dist = calcDistanceToForwardStopPoint(traj_input.points, 50); - EXPECT_NEAR(dist.value(), 0.0, epsilon); - } - - // Boundary4 (Right before the stop point) - { - const auto dist = calcDistanceToForwardStopPoint(traj_input.points, 49); - EXPECT_NEAR(dist.value(), 1.0, epsilon); - } - - // Boundary5 (Right behind the stop point) - { - const auto dist = calcDistanceToForwardStopPoint(traj_input.points, 51); - EXPECT_FALSE(dist); - } - - // Random - { - const auto dist = calcDistanceToForwardStopPoint(traj_input.points, 20); - EXPECT_NEAR(dist.value(), 30.0, epsilon); - } -} - -TEST(trajectory, calcDistanceToForwardStopPointFromPose) -{ - using autoware::motion_utils::calcDistanceToForwardStopPoint; - - auto traj_input = generateTestTrajectory(100, 1.0, 3.0); - traj_input.points.at(50).longitudinal_velocity_mps = 0.0; - - // Empty - { - EXPECT_FALSE(calcDistanceToForwardStopPoint(Trajectory{}.points, geometry_msgs::msg::Pose{})); - } - - // No Stop Point - { - const auto traj_no_stop_point = generateTestTrajectory(100, 1.0, 3.0); - const auto pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - const auto dist = calcDistanceToForwardStopPoint(traj_no_stop_point.points, pose); - EXPECT_FALSE(dist); - } - - // Trajectory Edge1 - { - const auto pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); - EXPECT_NEAR(dist.value(), 50.0, epsilon); - } - - // Trajectory Edge2 - { - const auto pose = createPose(99.0, 0.0, 0.0, 0.0, 0.0, 0.0); - const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); - EXPECT_FALSE(dist); - } - - // Out of Trajectory1 - { - const auto pose = createPose(-10.0, 0.0, 0.0, 0.0, 0.0, 0.0); - const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); - EXPECT_NEAR(dist.value(), 60.0, epsilon); - } - - // Out of Trajectory2 - { - const auto pose = createPose(200.0, 0.0, 0.0, 0.0, 0.0, 0.0); - const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); - EXPECT_FALSE(dist); - } - - // Out of Trajectory3 - { - const auto pose = createPose(-30.0, 50.0, 0.0, 0.0, 0.0, 0.0); - const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); - EXPECT_NEAR(dist.value(), 80.0, epsilon); - } - - // Boundary Condition1 - { - const auto pose = createPose(50.0, 0.0, 0.0, 0.0, 0.0, 0.0); - const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); - EXPECT_NEAR(dist.value(), 0.0, epsilon); - } - - // Boundary Condition2 - { - const auto pose = createPose(50.1, 0.0, 0.0, 0.0, 0.0, 0.0); - const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); - EXPECT_FALSE(dist); - } - - // Boundary Condition3 - { - const auto pose = createPose(49.9, 0.0, 0.0, 0.0, 0.0, 0.0); - const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); - EXPECT_NEAR(dist.value(), 0.1, epsilon); - } - - // Random - { - const auto pose = createPose(3.0, 2.0, 0.0, 0.0, 0.0, 0.0); - const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); - EXPECT_NEAR(dist.value(), 47.0, epsilon); - } -} - -TEST(trajectory, calcDistanceToForwardStopPoint_DistThreshold) -{ - using autoware::motion_utils::calcDistanceToForwardStopPoint; - - auto traj_input = generateTestTrajectory(100, 1.0, 3.0); - traj_input.points.at(50).longitudinal_velocity_mps = 0.0; - - // Boundary Condition1 - { - const auto pose = createPose(-4.9, 0.0, 0.0, 0.0, 0.0, 0.0); - const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, 5.0); - EXPECT_NEAR(dist.value(), 54.9, epsilon); - } - - // Boundary Condition2 - { - const auto pose = createPose(-5.0, 0.0, 0.0, 0.0, 0.0, 0.0); - const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, 5.0); - EXPECT_NEAR(dist.value(), 55.0, epsilon); - } - - // Boundary Condition3 - { - const auto pose = createPose(-5.1, 0.0, 0.0, 0.0, 0.0, 0.0); - const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, 5.0); - EXPECT_FALSE(dist); - } - - // Random - { - { - const auto pose = createPose(3.0, 2.0, 0.0, 0.0, 0.0, 0.0); - const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, 5.0); - EXPECT_NEAR(dist.value(), 47.0, epsilon); - } - - { - const auto pose = createPose(200.0, 0.0, 0.0, 0.0, 0.0, 0.0); - const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, 5.0); - EXPECT_FALSE(dist); - } - } -} - -TEST(trajectory, calcDistanceToForwardStopPoint_YawThreshold) -{ - using autoware::motion_utils::calcDistanceToForwardStopPoint; - using autoware_utils::deg2rad; - - const auto max_d = std::numeric_limits::max(); - auto traj_input = generateTestTrajectory(100, 1.0, 3.0); - traj_input.points.at(50).longitudinal_velocity_mps = 0.0; - - // Boundary Condition - { - const double x = 2.0; - { - const auto pose = createPose(x, 0.0, 0.0, 0.0, 0.0, deg2rad(29.9)); - const auto dist = - calcDistanceToForwardStopPoint(traj_input.points, pose, max_d, deg2rad(30.0)); - EXPECT_NEAR(dist.value(), 48.0, epsilon); - } - - { - const auto pose = createPose(x, 0.0, 0.0, 0.0, 0.0, deg2rad(30.0)); - const auto dist = - calcDistanceToForwardStopPoint(traj_input.points, pose, max_d, deg2rad(30.0)); - EXPECT_NEAR(dist.value(), 48.0, epsilon); - } - - { - const auto pose = createPose(x, 0.0, 0.0, 0.0, 0.0, deg2rad(30.1)); - const auto dist = - calcDistanceToForwardStopPoint(traj_input.points, pose, max_d, deg2rad(30.0)); - EXPECT_FALSE(dist); - } - } - - // Random - { - { - const auto pose = createPose(3.0, 2.0, 0.0, 0.0, 0.0, deg2rad(15.0)); - const auto dist = - calcDistanceToForwardStopPoint(traj_input.points, pose, max_d, deg2rad(20.0)); - EXPECT_NEAR(dist.value(), 47.0, epsilon); - } - - { - const auto pose = createPose(15.0, 30.0, 0.0, 0.0, 0.0, deg2rad(45.0)); - const auto dist = - calcDistanceToForwardStopPoint(traj_input.points, pose, max_d, deg2rad(10.0)); - EXPECT_FALSE(dist); - } - } -} - -TEST(trajectory, calcLongitudinalOffsetPointFromIndex) -{ - using autoware::motion_utils::calcArcLength; - using autoware::motion_utils::calcLongitudinalOffsetPoint; - using autoware::motion_utils::calcSignedArcLength; - using autoware_utils::get_point; - - const auto traj = generateTestTrajectory(10, 1.0); - const auto total_length = calcArcLength(traj.points); - - // Empty - EXPECT_FALSE(calcLongitudinalOffsetPoint(Trajectory{}.points, {}, {})); - - // Out of range - EXPECT_FALSE(calcLongitudinalOffsetPoint(traj.points, traj.points.size() + 1, 1.0)); - EXPECT_FALSE(calcLongitudinalOffsetPoint(traj.points, -1, 1.0)); - - // Found Pose(forward) - for (size_t i = 0; i < traj.points.size(); ++i) { - double x_ans = get_point(traj.points.at(i)).x; - - const auto d_back = calcSignedArcLength(traj.points, i, traj.points.size() - 1); - - for (double len = 0.0; len < d_back + epsilon; len += 0.1) { - const auto p_out = calcLongitudinalOffsetPoint(traj.points, i, std::min(len, d_back)); - - EXPECT_NE(p_out, std::nullopt); - EXPECT_NEAR(p_out.value().x, x_ans, epsilon); - EXPECT_NEAR(p_out.value().y, 0.0, epsilon); - EXPECT_NEAR(p_out.value().z, 0.0, epsilon); - - x_ans += 0.1; - } - } - - // Found Pose(backward) - for (size_t i = 0; i < traj.points.size(); ++i) { - double x_ans = get_point(traj.points.at(i)).x; - - const auto d_front = calcSignedArcLength(traj.points, i, 0); - - for (double len = 0.0; d_front - epsilon < len; len -= 0.1) { - const auto p_out = calcLongitudinalOffsetPoint(traj.points, i, std::max(len, d_front)); - - EXPECT_NE(p_out, std::nullopt); - EXPECT_NEAR(p_out.value().x, x_ans, epsilon); - EXPECT_NEAR(p_out.value().y, 0.0, epsilon); - EXPECT_NEAR(p_out.value().z, 0.0, epsilon); - - x_ans -= 0.1; - } - } - - // No found - { - const auto p_out = calcLongitudinalOffsetPoint(traj.points, 0, total_length + epsilon); - - EXPECT_EQ(p_out, std::nullopt); - } - - // No found - { - const auto p_out = calcLongitudinalOffsetPoint(traj.points, 9, -total_length - epsilon); - - EXPECT_EQ(p_out, std::nullopt); - } - - // No found(Trajectory size is 1) - { - const auto one_point_traj = generateTestTrajectory(1, 1.0); - const auto p_out = calcLongitudinalOffsetPoint(one_point_traj.points, 0.0, 0.0); - - EXPECT_EQ(p_out, std::nullopt); - } -} - -TEST(trajectory, calcLongitudinalOffsetPointFromPoint) -{ - using autoware::motion_utils::calcArcLength; - using autoware::motion_utils::calcLongitudinalOffsetPoint; - using autoware::motion_utils::calcSignedArcLength; - using autoware_utils::create_point; - using autoware_utils::get_point; - - const auto traj = generateTestTrajectory(10, 1.0); - const auto total_length = calcArcLength(traj.points); - - // Empty - EXPECT_FALSE(calcLongitudinalOffsetPoint(Trajectory{}.points, {}, {})); - - // Found Pose(forward) - for (double x_start = 0.0; x_start < total_length + epsilon; x_start += 0.1) { - constexpr double lateral_deviation = 0.5; - double x_ans = x_start; - - const auto p_src = 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) { - const auto p_out = calcLongitudinalOffsetPoint(traj.points, p_src, std::min(len, d_back)); - - EXPECT_NE(p_out, std::nullopt); - EXPECT_NEAR(p_out.value().x, x_ans, epsilon); - EXPECT_NEAR(p_out.value().y, 0.0, epsilon); - EXPECT_NEAR(p_out.value().z, 0.0, epsilon); - - x_ans += 0.1; - } - } - - // Found Pose(backward) - for (double x_start = 0.0; x_start < total_length + epsilon; x_start += 0.1) { - constexpr double lateral_deviation = 0.5; - double x_ans = x_start; - - const auto p_src = 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) { - const auto p_out = calcLongitudinalOffsetPoint(traj.points, p_src, std::max(len, d_front)); - - EXPECT_NE(p_out, std::nullopt); - EXPECT_NEAR(p_out.value().x, x_ans, epsilon); - EXPECT_NEAR(p_out.value().y, 0.0, epsilon); - EXPECT_NEAR(p_out.value().z, 0.0, epsilon); - - x_ans -= 0.1; - } - } - - // No found - { - 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); - } - - // No found - { - 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); - } - - // Out of range(Trajectory size is 1) - { - const auto one_point_traj = generateTestTrajectory(1, 1.0); - EXPECT_FALSE( - calcLongitudinalOffsetPoint(one_point_traj.points, geometry_msgs::msg::Point{}, {})); - } -} - -TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) -{ - using autoware::motion_utils::calcArcLength; - using autoware::motion_utils::calcLongitudinalOffsetPose; - using autoware::motion_utils::calcSignedArcLength; - using autoware_utils::get_point; - - const auto traj = generateTestTrajectory(10, 1.0); - const auto total_length = calcArcLength(traj.points); - - // Empty - EXPECT_FALSE(calcLongitudinalOffsetPose(Trajectory{}.points, {}, {})); - - // Out of range - EXPECT_FALSE(calcLongitudinalOffsetPose(traj.points, traj.points.size() + 1, 1.0)); - EXPECT_FALSE(calcLongitudinalOffsetPose(traj.points, -1, 1.0)); - - // Found Pose(forward) - for (size_t i = 0; i < traj.points.size(); ++i) { - double x_ans = get_point(traj.points.at(i)).x; - - const auto d_back = calcSignedArcLength(traj.points, i, traj.points.size() - 1); - - for (double len = 0.0; len < d_back + epsilon; len += 0.1) { - const auto p_out = calcLongitudinalOffsetPose(traj.points, i, std::min(len, d_back)); - - EXPECT_NE(p_out, std::nullopt); - EXPECT_NEAR(p_out.value().position.x, x_ans, epsilon); - EXPECT_NEAR(p_out.value().position.y, 0.0, epsilon); - EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.x, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.y, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.z, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.w, 1.0, epsilon); - - x_ans += 0.1; - } - } - - // Found Pose(backward) - for (size_t i = 0; i < traj.points.size(); ++i) { - double x_ans = get_point(traj.points.at(i)).x; - - const auto d_front = calcSignedArcLength(traj.points, i, 0); - - for (double len = 0.0; d_front - epsilon < len; len -= 0.1) { - const auto p_out = calcLongitudinalOffsetPose(traj.points, i, std::max(len, d_front)); - - EXPECT_NE(p_out, std::nullopt); - EXPECT_NEAR(p_out.value().position.x, x_ans, epsilon); - EXPECT_NEAR(p_out.value().position.y, 0.0, epsilon); - EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.x, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.y, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.z, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.w, 1.0, epsilon); - - x_ans -= 0.1; - } - } - - // No found - { - const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, total_length + epsilon); - - EXPECT_EQ(p_out, std::nullopt); - } - - // No found - { - const auto p_out = calcLongitudinalOffsetPose(traj.points, 9, -total_length - epsilon); - - EXPECT_EQ(p_out, std::nullopt); - } - - // No found(Trajectory size is 1) - { - const auto one_point_traj = generateTestTrajectory(1, 1.0); - const auto p_out = calcLongitudinalOffsetPose(one_point_traj.points, 0.0, 0.0); - - EXPECT_EQ(p_out, std::nullopt); - } -} - -TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) -{ - using autoware::motion_utils::calcArcLength; - using autoware::motion_utils::calcLongitudinalOffsetPose; - using autoware_planning_msgs::msg::TrajectoryPoint; - using autoware_utils::deg2rad; - - Trajectory traj{}; - - { - TrajectoryPoint p; - p.pose = createPose(0.0, 0.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); - p.longitudinal_velocity_mps = 0.0; - traj.points.push_back(p); - } - - { - TrajectoryPoint p; - p.pose = createPose(1.0, 1.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - p.longitudinal_velocity_mps = 0.0; - traj.points.push_back(p); - } - - const auto total_length = calcArcLength(traj.points); - - // Found pose(forward) - for (double len = 0.0; len < total_length; len += 0.1) { - const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, len); - const auto ans_quat = 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); - EXPECT_NEAR(p_out.value().position.y, len * std::sin(deg2rad(45.0)), epsilon); - EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); - } - - // Found pose(backward) - for (double len = total_length; 0.0 < len; len -= 0.1) { - const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, -len); - const auto ans_quat = 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); - EXPECT_NEAR(p_out.value().position.y, 1.0 - len * std::sin(deg2rad(45.0)), epsilon); - EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); - } - - // Boundary condition - { - const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, total_length); - 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); - EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); - EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); - } - - // Boundary condition - { - const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, 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); - EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); - EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); - } -} - -TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) -{ - using autoware::motion_utils::calcArcLength; - using autoware::motion_utils::calcLongitudinalOffsetPose; - using autoware_planning_msgs::msg::TrajectoryPoint; - using autoware_utils::deg2rad; - - Trajectory traj{}; - - { - TrajectoryPoint p; - p.pose = createPose(0.0, 0.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); - p.longitudinal_velocity_mps = 0.0; - traj.points.push_back(p); - } - - { - TrajectoryPoint p; - p.pose = createPose(1.0, 1.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - p.longitudinal_velocity_mps = 0.0; - traj.points.push_back(p); - } - - const auto total_length = calcArcLength(traj.points); - - // Found pose(forward) - for (double len = 0.0; len < total_length; len += 0.1) { - const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, len, false); - // ratio between two points - const auto ratio = len / total_length; - const auto ans_quat = - 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); - EXPECT_NEAR(p_out.value().position.y, len * std::sin(deg2rad(45.0)), epsilon); - EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); - } - - // Found pose(backward) - for (double len = total_length; 0.0 < len; len -= 0.1) { - const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, -len, false); - // ratio between two points - const auto ratio = len / total_length; - const auto ans_quat = - 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); - EXPECT_NEAR(p_out.value().position.y, 1.0 - len * std::sin(deg2rad(45.0)), epsilon); - EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); - } - - // Boundary condition - { - const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, total_length, false); - 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); - EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); - EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); - } - - // Boundary condition - { - const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, 0.0, false); - 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); - EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); - EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); - } -} - -TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) -{ - using autoware::motion_utils::calcArcLength; - using autoware::motion_utils::calcLongitudinalOffsetPose; - using autoware::motion_utils::calcSignedArcLength; - using autoware_utils::create_point; - using autoware_utils::get_point; - - const auto traj = generateTestTrajectory(10, 1.0); - const auto total_length = calcArcLength(traj.points); - - // Empty - EXPECT_FALSE(calcLongitudinalOffsetPose(Trajectory{}.points, {}, {})); - - // Found Pose(forward) - for (double x_start = 0.0; x_start < total_length + epsilon; x_start += 0.1) { - constexpr double lateral_deviation = 0.5; - double x_ans = x_start; - - const auto p_src = 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) { - const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, std::min(len, d_back)); - - EXPECT_NE(p_out, std::nullopt); - EXPECT_NEAR(p_out.value().position.x, x_ans, epsilon); - EXPECT_NEAR(p_out.value().position.y, 0.0, epsilon); - EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.x, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.y, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.z, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.w, 1.0, epsilon); - - x_ans += 0.1; - } - } - - // Found Pose(backward) - for (double x_start = 0.0; x_start < total_length + epsilon; x_start += 0.1) { - constexpr double lateral_deviation = 0.5; - double x_ans = x_start; - - const auto p_src = 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) { - const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, std::max(len, d_front)); - - EXPECT_NE(p_out, std::nullopt); - EXPECT_NEAR(p_out.value().position.x, x_ans, epsilon); - EXPECT_NEAR(p_out.value().position.y, 0.0, epsilon); - EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.x, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.y, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.z, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.w, 1.0, epsilon); - - x_ans -= 0.1; - } - } - - // No found - { - 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); - } - - // No found - { - 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); - } - - // Out of range(Trajectory size is 1) - { - const auto one_point_traj = generateTestTrajectory(1, 1.0); - EXPECT_FALSE( - calcLongitudinalOffsetPose(one_point_traj.points, geometry_msgs::msg::Point{}, {})); - } -} - -TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) -{ - using autoware::motion_utils::calcArcLength; - using autoware::motion_utils::calcLongitudinalOffsetPose; - using autoware::motion_utils::calcLongitudinalOffsetToSegment; - using autoware_planning_msgs::msg::TrajectoryPoint; - using autoware_utils::create_point; - using autoware_utils::deg2rad; - - Trajectory traj{}; - - { - TrajectoryPoint p; - p.pose = createPose(0.0, 0.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); - p.longitudinal_velocity_mps = 0.0; - traj.points.push_back(p); - } - - { - TrajectoryPoint p; - p.pose = createPose(1.0, 1.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - p.longitudinal_velocity_mps = 0.0; - traj.points.push_back(p); - } - - const auto total_length = calcArcLength(traj.points); - - // Found pose - for (double len_start = 0.0; len_start < total_length; len_start += 0.1) { - constexpr double deviation = 0.1; - - const auto p_src = 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 = 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, p_src.x + len * std::cos(deg2rad(45.0)) - deviation, epsilon); - EXPECT_NEAR( - p_out.value().position.y, p_src.y + len * std::sin(deg2rad(45.0)) + deviation, epsilon); - EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); - } - } - - // Boundary condition - { - constexpr double deviation = 0.1; - - 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 = 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); - EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); - EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); - } -} - -TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) -{ - using autoware::motion_utils::calcArcLength; - using autoware::motion_utils::calcLongitudinalOffsetPose; - using autoware::motion_utils::calcLongitudinalOffsetToSegment; - using autoware_planning_msgs::msg::TrajectoryPoint; - using autoware_utils::create_point; - using autoware_utils::deg2rad; - - Trajectory traj{}; - - { - TrajectoryPoint p; - p.pose = createPose(0.0, 0.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); - p.longitudinal_velocity_mps = 0.0; - traj.points.push_back(p); - } - - { - TrajectoryPoint p; - p.pose = createPose(1.0, 1.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - p.longitudinal_velocity_mps = 0.0; - traj.points.push_back(p); - } - - const auto total_length = calcArcLength(traj.points); - - // Found pose - for (double len_start = 0.0; len_start < total_length; len_start += 0.1) { - constexpr double deviation = 0.1; - - const auto p_src = 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, false); - // ratio between two points - const auto ratio = (src_offset + len) / total_length; - const auto ans_quat = - 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, p_src.x + len * std::cos(deg2rad(45.0)) - deviation, epsilon); - EXPECT_NEAR( - p_out.value().position.y, p_src.y + len * std::sin(deg2rad(45.0)) + deviation, epsilon); - EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); - } - } - - // Boundary condition - { - constexpr double deviation = 0.1; - - 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 = 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); - EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); - EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); - } -} - -TEST(trajectory, insertTargetPoint) -{ - using autoware::motion_utils::calcArcLength; - using autoware::motion_utils::findNearestSegmentIndex; - using autoware::motion_utils::insertTargetPoint; - 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); - - // Insert - for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { - auto traj_out = traj; - - 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Insert(Boundary condition) - for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Insert(Quaternion interpolation) - for (double x_start = 0.25; x_start < total_length; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Not insert(Overlap base_idx point) - for (double x_start = 0.0; x_start < total_length - 1.0 + epsilon; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - } - - // Not insert(Overlap base_idx + 1 point) - for (double x_start = 1.0; x_start < total_length + epsilon; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - } - - // Invalid target point(In front of begin point) - { - auto traj_out = traj; - - 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); - - EXPECT_EQ(insert_idx, std::nullopt); - } - - // Invalid target point(Behind of end point) - { - auto traj_out = traj; - - 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); - - EXPECT_EQ(insert_idx, std::nullopt); - } - - // Invalid target point(Huge lateral offset) - { - auto traj_out = traj; - - 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); - - EXPECT_EQ(insert_idx, std::nullopt); - } - - // Invalid base index - { - auto traj_out = traj; - - const size_t segment_idx = 9U; - 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); - } - - // Empty - { - auto empty_traj = generateTestTrajectory(0, 1.0); - const size_t segment_idx = 0; - EXPECT_FALSE(insertTargetPoint(segment_idx, geometry_msgs::msg::Point{}, empty_traj.points)); - } -} - -TEST(trajectory, insertTargetPoint_Reverse) -{ - using autoware::motion_utils::calcArcLength; - using autoware::motion_utils::findNearestSegmentIndex; - using autoware::motion_utils::insertTargetPoint; - 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 = 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 = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE( - calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } -} - -TEST(trajectory, insertTargetPoint_OverlapThreshold) -{ - using autoware::motion_utils::calcArcLength; - using autoware::motion_utils::findNearestSegmentIndex; - using autoware::motion_utils::insertTargetPoint; - 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); - const auto total_length = calcArcLength(traj.points); - - // Insert(Boundary condition) - for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE( - calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Not insert(Overlap base_idx point) - for (double x_start = 0.0; x_start < total_length - 1.0 + epsilon; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE( - calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold); - } - } - - // Not insert(Overlap base_idx + 1 point) - for (double x_start = 1.0; x_start < total_length + epsilon; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE( - calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold); - } - } -} - -TEST(trajectory, insertTargetPoint_Length) -{ - using autoware::motion_utils::calcArcLength; - using autoware::motion_utils::findNearestSegmentIndex; - using autoware::motion_utils::insertTargetPoint; - 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); - - // Insert - for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { - auto traj_out = traj; - - 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Insert(Boundary condition) - for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Right on the terminal point - { - auto traj_out = traj; - - 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Insert(Quaternion interpolation) - for (double x_start = 0.25; x_start < total_length; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Not insert(Overlap base_idx point) - for (double x_start = 0.0; x_start < total_length - 1.0 + epsilon; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - } - - // Not insert(Overlap base_idx + 1 point) - for (double x_start = 1.0; x_start < total_length + epsilon; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - } - - // Invalid target point(In front of the beginning point) - { - auto traj_out = traj; - - 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); - } - - // Invalid target point(Behind the end point) - { - auto traj_out = traj; - - 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); - } - - // Invalid target point(Huge lateral offset) - { - auto traj_out = traj; - - 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); - } - - // Empty - { - auto empty_traj = generateTestTrajectory(0, 1.0); - EXPECT_THROW( - insertTargetPoint(0.0, geometry_msgs::msg::Point{}, empty_traj.points), - std::invalid_argument); - } -} - -TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) -{ - using autoware::motion_utils::calcArcLength; - using autoware::motion_utils::findNearestSegmentIndex; - using autoware::motion_utils::insertTargetPoint; - 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); - - // Insert - for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { - auto traj_out = traj; - - 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Insert(Boundary condition) - for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Right on the terminal point - { - auto traj_out = traj; - - 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Not insert(Overlap base_idx point) - for (double x_start = 0.0; x_start < total_length - 1.0 + epsilon; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - } - - // Not insert(Overlap base_idx + 1 point) - for (double x_start = 1.0; x_start < total_length + epsilon; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - } - - // Invalid target point(In front of the beginning point) - { - auto traj_out = traj; - - const auto insert_idx = insertTargetPoint(0, -1.0, traj_out.points); - - EXPECT_EQ(insert_idx, std::nullopt); - } - - // Invalid target point(Behind the end point) - { - auto traj_out = traj; - - const auto insert_idx = insertTargetPoint(0, 10.0, traj_out.points); - - EXPECT_EQ(insert_idx, std::nullopt); - } - - // Empty - { - auto empty_traj = generateTestTrajectory(0, 1.0); - EXPECT_THROW(insertTargetPoint(0, 0.0, empty_traj.points), std::invalid_argument); - } -} - -TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Idx) -{ - using autoware::motion_utils::calcArcLength; - using autoware::motion_utils::findNearestSegmentIndex; - using autoware::motion_utils::insertTargetPoint; - 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); - - // Insert - for (double x_start = 0.5; x_start < 5.0; x_start += 1.0) { - auto traj_out = traj; - - const size_t start_idx = 2; - 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Boundary Condition(Before End Point) - { - auto traj_out = traj; - const double x_start = 1.0 - 1e-2; - - const size_t start_idx = 8; - 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Boundary Condition(Right on End Point) - { - auto traj_out = traj; - const double x_start = 1.0; - - const size_t start_idx = 8; - 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Boundary Condition(Insert on end point) - { - auto traj_out = traj; - const double x_start = 4.0; - - const size_t start_idx = 5; - 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Boundary Condition(start point) - { - auto traj_out = traj; - const double x_start = 0.0; - - const size_t start_idx = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // No Insert (Index Out of range) - { - auto traj_out = traj; - EXPECT_EQ(insertTargetPoint(9, 0.0, traj_out.points), std::nullopt); - EXPECT_EQ(insertTargetPoint(9, 1.0, traj_out.points), std::nullopt); - EXPECT_EQ(insertTargetPoint(10, 0.0, traj_out.points), std::nullopt); - EXPECT_EQ(insertTargetPoint(10, 1.0, traj_out.points), std::nullopt); - } - - // No Insert (Length out of range) - { - auto traj_out = traj; - EXPECT_EQ(insertTargetPoint(0, 10.0, traj_out.points), std::nullopt); - EXPECT_EQ(insertTargetPoint(0, 9.0001, traj_out.points), std::nullopt); - EXPECT_EQ(insertTargetPoint(5, 5.0, traj_out.points), std::nullopt); - EXPECT_EQ(insertTargetPoint(8, 1.0001, traj_out.points), std::nullopt); - } -} - -TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero_Start_Idx) -{ - using autoware::motion_utils::calcArcLength; - using autoware::motion_utils::findNearestSegmentIndex; - using autoware::motion_utils::insertTargetPoint; - 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); - - // Insert - for (double x_start = -0.5; x_start < -5.0; x_start -= 1.0) { - auto traj_out = traj; - - const size_t start_idx = 7; - 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Boundary Condition(Before End Point) - { - auto traj_out = traj; - const double x_start = -1.0 - 1e-2; - - const size_t start_idx = 8; - 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Boundary Condition(Right on End Point) - { - auto traj_out = traj; - const double x_start = -1.0; - - const size_t start_idx = 8; - 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Boundary Condition(start point) - { - auto traj_out = traj; - const double x_start = -5.0; - - const size_t start_idx = 5; - 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // No Insert (Index Out of range) - { - auto traj_out = traj; - EXPECT_EQ(insertTargetPoint(0, -1.0, traj_out.points), std::nullopt); - EXPECT_EQ(insertTargetPoint(0, -1.0, traj_out.points), std::nullopt); - } - - // No Insert (Length out of range) - { - auto traj_out = traj; - EXPECT_EQ(insertTargetPoint(9, -10.0, traj_out.points), std::nullopt); - EXPECT_EQ(insertTargetPoint(9, -9.0001, traj_out.points), std::nullopt); - EXPECT_EQ(insertTargetPoint(1, -1.0001, traj_out.points), std::nullopt); - } -} - -TEST(trajectory, insertTargetPoint_Length_from_a_pose) -{ - using autoware::motion_utils::calcArcLength; - using autoware::motion_utils::findNearestSegmentIndex; - using autoware::motion_utils::insertTargetPoint; - 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); - - // Insert (From Zero Point) - for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { - 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 = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Insert (From Non-Zero Point) - { - const double src_pose_x = 5.0; - const double src_pose_y = 3.0; - for (double x_start = 0.5; x_start < total_length - src_pose_x; x_start += 1.0) { - auto traj_out = traj; - - 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 = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - } - - // No Insert - { - const double src_pose_x = 2.0; - const double src_pose_y = 3.0; - for (double x_start = 1e-3; x_start < total_length - src_pose_x; x_start += 1.0) { - auto traj_out = traj; - - 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 = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - } - } - - // Insert (Boundary Condition) - { - const double src_pose_x = 2.0; - const double src_pose_y = 3.0; - for (double x_start = 1e-2; x_start < total_length - src_pose_x; x_start += 1.0) { - auto traj_out = traj; - - 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 = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - } - - // In Front of the beginning point of the trajectory - { - const double src_pose_x = -1.0; - const double src_pose_y = 0.0; - for (double x_start = 0.5 - src_pose_x; x_start < total_length - src_pose_x; x_start += 1.0) { - auto traj_out = traj; - - 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 = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - } - - // Insert from the point in front of the trajectory (Invalid) - { - auto traj_out = traj; - const double src_pose_x = -1.0; - const double src_pose_y = 0.0; - const geometry_msgs::msg::Pose src_pose = - createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertTargetPoint(src_pose, 0.0, traj_out.points), std::nullopt); - EXPECT_EQ(insertTargetPoint(src_pose, 0.5, traj_out.points), std::nullopt); - } - - // Insert from the point behind the trajectory (Invalid) - { - auto traj_out = traj; - const double src_pose_x = 10.0; - const double src_pose_y = 3.0; - const geometry_msgs::msg::Pose src_pose = - createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertTargetPoint(src_pose, 0.0, traj_out.points), std::nullopt); - EXPECT_EQ(insertTargetPoint(src_pose, 1.0, traj_out.points), std::nullopt); - EXPECT_EQ(insertTargetPoint(src_pose, 10.0, traj_out.points), std::nullopt); - } - - // Insert from the point in front of the trajectory (Boundary Condition) - { - auto traj_out = traj; - const double src_pose_x = -1.0; - const double src_pose_y = 0.0; - 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 = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Insert from the end point (Boundary Condition) - { - auto traj_out = traj; - const double src_pose_x = 9.0; - const double src_pose_y = 0.0; - 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 = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // No Insert (Negative Insert Length) - { - auto traj_out = traj; - const double src_pose_x = 5.0; - const double src_pose_y = 3.0; - const geometry_msgs::msg::Pose src_pose = - createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertTargetPoint(src_pose, -1.0, traj_out.points), std::nullopt); - EXPECT_EQ(insertTargetPoint(src_pose, -10.0, traj_out.points), std::nullopt); - } - - // No Insert (Too Far from the source point) - { - auto traj_out = traj; - const double src_pose_x = 5.0; - const double src_pose_y = 3.0; - const geometry_msgs::msg::Pose src_pose = - createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertTargetPoint(src_pose, 1.0, traj_out.points, 1.0), std::nullopt); - EXPECT_EQ(insertTargetPoint(src_pose, 10.0, traj_out.points, 1.0), std::nullopt); - } - - // No Insert (Too large yaw deviation) - { - auto traj_out = traj; - const double src_pose_x = 5.0; - const double src_pose_y = 3.0; - const double src_yaw = deg2rad(60.0); - const geometry_msgs::msg::Pose src_pose = - createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, src_yaw); - const double max_dist = std::numeric_limits::max(); - EXPECT_EQ( - insertTargetPoint(src_pose, 1.0, traj_out.points, max_dist, deg2rad(45)), std::nullopt); - EXPECT_EQ( - insertTargetPoint(src_pose, 10.0, traj_out.points, max_dist, deg2rad(45)), std::nullopt); - } -} - -TEST(trajectory, insertStopPoint_from_a_source_index) -{ - using autoware::motion_utils::calcArcLength; - using autoware::motion_utils::findNearestSegmentIndex; - using autoware::motion_utils::insertStopPoint; - 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); - - // Insert - for (double x_start = 0.5; x_start < 5.0; x_start += 1.0) { - auto traj_out = traj; - - const size_t start_idx = 2; - 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(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 { - EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); - } - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Boundary Condition(Before End Point) - { - auto traj_out = traj; - const double x_start = 1.0 - 1e-2; - - const size_t start_idx = 8; - 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(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 { - EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); - } - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Boundary Condition(Right on End Point) - { - auto traj_out = traj; - const double x_start = 1.0; - - const size_t start_idx = 8; - 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(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 { - EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); - } - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Boundary Condition(Insert on end point) - { - auto traj_out = traj; - const double x_start = 4.0; - - const size_t start_idx = 5; - 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(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 { - EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); - } - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Boundary Condition(start point) - { - auto traj_out = traj; - const double x_start = 0.0; - - const size_t start_idx = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(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 { - EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); - } - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // No Insert (Index Out of range) - { - auto traj_out = traj; - EXPECT_EQ(insertStopPoint(9, 0.0, traj_out.points), std::nullopt); - EXPECT_EQ(insertStopPoint(9, 1.0, traj_out.points), std::nullopt); - EXPECT_EQ(insertStopPoint(10, 0.0, traj_out.points), std::nullopt); - EXPECT_EQ(insertStopPoint(10, 1.0, traj_out.points), std::nullopt); - } - - // No Insert (Length out of range) - { - auto traj_out = traj; - EXPECT_EQ(insertStopPoint(0, 10.0, traj_out.points), std::nullopt); - EXPECT_EQ(insertStopPoint(0, 9.0001, traj_out.points), std::nullopt); - EXPECT_EQ(insertStopPoint(5, 5.0, traj_out.points), std::nullopt); - EXPECT_EQ(insertStopPoint(8, 1.0001, traj_out.points), std::nullopt); - } -} - -TEST(trajectory, insertStopPoint_from_front_point) -{ - using autoware::motion_utils::calcArcLength; - using autoware::motion_utils::findNearestSegmentIndex; - using autoware::motion_utils::insertStopPoint; - 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); - - // Insert - for (double x_start = 0.5; x_start < 5.0; x_start += 1.0) { - auto traj_out = traj; - - 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(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 { - EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); - } - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Boundary Condition(Before End Point) - { - auto traj_out = traj; - const double x_start = 1.0 - 1e-2; - - 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(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 { - EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); - } - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Boundary Condition(Right on End Point) - { - auto traj_out = traj; - - 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(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 { - EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); - } - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Boundary Condition(start point) - { - auto traj_out = traj; - const double x_start = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(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 { - EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); - } - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // No Insert (Length out of range) - { - auto traj_out = traj; - EXPECT_EQ(insertStopPoint(9.0 + 1e-2, traj_out.points), std::nullopt); - EXPECT_EQ(insertStopPoint(10.0, traj_out.points), std::nullopt); - EXPECT_EQ( - insertStopPoint(-std::numeric_limits::epsilon(), traj_out.points), std::nullopt); - EXPECT_EQ(insertStopPoint(-3.0, traj_out.points), std::nullopt); - } -} - -TEST(trajectory, insertStopPoint_from_a_pose) -{ - using autoware::motion_utils::calcArcLength; - using autoware::motion_utils::findNearestSegmentIndex; - using autoware::motion_utils::insertStopPoint; - 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); - - // Insert (From Zero Point) - for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { - 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 = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(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 { - EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); - } - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Insert (From Non-Zero Point) - { - const double src_pose_x = 5.0; - const double src_pose_y = 3.0; - for (double x_start = 0.5; x_start < total_length - src_pose_x; x_start += 1.0) { - auto traj_out = traj; - - 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 = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(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 { - EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); - } - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - } - - // No Insert - { - const double src_pose_x = 2.0; - const double src_pose_y = 3.0; - for (double x_start = 1e-3; x_start < total_length - src_pose_x; x_start += 1.0) { - auto traj_out = traj; - - 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 = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(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 { - EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); - } - } - } - } - - // Insert (Boundary Condition) - { - const double src_pose_x = 2.0; - const double src_pose_y = 3.0; - for (double x_start = 1e-2; x_start < total_length - src_pose_x; x_start += 1.0) { - auto traj_out = traj; - - 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 = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(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 { - EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); - } - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - } - - // In Front of the beginning point of the trajectory - { - const double src_pose_x = -1.0; - const double src_pose_y = 0.0; - for (double x_start = 0.5 - src_pose_x; x_start < total_length - src_pose_x; x_start += 1.0) { - auto traj_out = traj; - - 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 = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(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 { - EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); - } - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - } - - // Insert from the point in front of the trajectory (Invalid) - { - auto traj_out = traj; - const double src_pose_x = -1.0; - const double src_pose_y = 0.0; - const geometry_msgs::msg::Pose src_pose = - createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertStopPoint(src_pose, 0.0, traj_out.points), std::nullopt); - EXPECT_EQ(insertStopPoint(src_pose, 0.5, traj_out.points), std::nullopt); - } - - // Insert from the point behind the trajectory (Invalid) - { - auto traj_out = traj; - const double src_pose_x = 10.0; - const double src_pose_y = 3.0; - const geometry_msgs::msg::Pose src_pose = - createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertStopPoint(src_pose, 0.0, traj_out.points), std::nullopt); - EXPECT_EQ(insertStopPoint(src_pose, 1.0, traj_out.points), std::nullopt); - EXPECT_EQ(insertStopPoint(src_pose, 10.0, traj_out.points), std::nullopt); - } - - // Insert from the point in front of the trajectory (Boundary Condition) - { - auto traj_out = traj; - const double src_pose_x = -1.0; - const double src_pose_y = 0.0; - 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 = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(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 { - EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); - } - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Insert from the end point (Boundary Condition) - { - auto traj_out = traj; - const double src_pose_x = 9.0; - const double src_pose_y = 0.0; - 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 = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(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 { - EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); - } - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // No Insert (Negative Insert Length) - { - auto traj_out = traj; - const double src_pose_x = 5.0; - const double src_pose_y = 3.0; - const geometry_msgs::msg::Pose src_pose = - createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertStopPoint(src_pose, -1.0, traj_out.points), std::nullopt); - EXPECT_EQ(insertStopPoint(src_pose, -10.0, traj_out.points), std::nullopt); - } - - // No Insert (Too Far from the source point) - { - auto traj_out = traj; - const double src_pose_x = 5.0; - const double src_pose_y = 3.0; - const geometry_msgs::msg::Pose src_pose = - createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertStopPoint(src_pose, 1.0, traj_out.points, 1.0), std::nullopt); - EXPECT_EQ(insertStopPoint(src_pose, 10.0, traj_out.points, 1.0), std::nullopt); - } - - // No Insert (Too large yaw deviation) - { - auto traj_out = traj; - const double src_pose_x = 5.0; - const double src_pose_y = 3.0; - const double src_yaw = deg2rad(60.0); - const geometry_msgs::msg::Pose src_pose = - createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, src_yaw); - const double max_dist = std::numeric_limits::max(); - EXPECT_EQ(insertStopPoint(src_pose, 1.0, traj_out.points, max_dist, deg2rad(45)), std::nullopt); - EXPECT_EQ( - insertStopPoint(src_pose, 10.0, traj_out.points, max_dist, deg2rad(45)), std::nullopt); - } -} - -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_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); - - // Insert - for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { - auto traj_out = traj; - - 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(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); - } - for (size_t i = insert_idx.value(); i < traj_out.points.size(); ++i) { - EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Insert(Boundary condition) - for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(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); - } - for (size_t i = insert_idx.value(); i < traj_out.points.size(); ++i) { - EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Insert(Quaternion interpolation) - for (double x_start = 0.25; x_start < total_length; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(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); - } - for (size_t i = insert_idx.value(); i < traj_out.points.size(); ++i) { - EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); - } - - { - 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); - EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); - } - - { - const auto p_base = 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); - EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); - } - } - - // Not insert(Overlap base_idx point) - for (double x_start = 0.0; x_start < total_length - 1.0 + epsilon; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(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); - } - for (size_t i = insert_idx.value(); i < traj_out.points.size(); ++i) { - EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); - } - } - - // Not insert(Overlap base_idx + 1 point) - for (double x_start = 1.0; x_start < total_length + epsilon; x_start += 1.0) { - auto traj_out = traj; - - const auto p_target = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(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); - } - for (size_t i = insert_idx.value(); i < traj_out.points.size(); ++i) { - EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); - } - } - - // Invalid target point(In front of begin point) - { - auto traj_out = traj; - - 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); - - EXPECT_EQ(insert_idx, std::nullopt); - } - - // Invalid target point(Behind of end point) - { - auto traj_out = traj; - - 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); - - EXPECT_EQ(insert_idx, std::nullopt); - } - - // Invalid target point(Huge lateral offset) - { - auto traj_out = traj; - - 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); - - EXPECT_EQ(insert_idx, std::nullopt); - } - - // Invalid base index - { - auto traj_out = traj; - - const size_t segment_idx = 9U; - 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); - } - - // Empty - { - auto empty_traj = generateTestTrajectory(0, 1.0); - const size_t segment_idx = 0; - EXPECT_FALSE(insertStopPoint(segment_idx, geometry_msgs::msg::Point{}, empty_traj.points)); - } -} - -TEST(trajectory, insertDecelPoint_from_a_point) -{ - using autoware::motion_utils::calcArcLength; - using autoware::motion_utils::findNearestSegmentIndex; - using autoware::motion_utils::insertDecelPoint; - 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); - const double decel_velocity = 5.0; - - // Insert (From Zero 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 = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.value()) { - EXPECT_NEAR(get_longitudinal_velocity(traj_out.points.at(i)), 10.0, epsilon); - } else { - EXPECT_NEAR(get_longitudinal_velocity(traj_out.points.at(i)), decel_velocity, epsilon); - } - } - } - } - - // Insert (From Non-Zero Point) - { - const double src_point_x = 5.0; - 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 = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx + 1); - EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.value()) { - EXPECT_NEAR(get_longitudinal_velocity(traj_out.points.at(i)), 10.0, epsilon); - } else { - EXPECT_NEAR(get_longitudinal_velocity(traj_out.points.at(i)), decel_velocity, epsilon); - } - } - } - } - - // No Insert - { - const double src_point_x = 2.0; - 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 = 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); - - EXPECT_NE(insert_idx, std::nullopt); - EXPECT_EQ(insert_idx.value(), base_idx); - EXPECT_EQ(traj_out.points.size(), traj.points.size()); - - for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { - EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.value()) { - EXPECT_NEAR(get_longitudinal_velocity(traj_out.points.at(i)), 10.0, epsilon); - } else { - EXPECT_NEAR(get_longitudinal_velocity(traj_out.points.at(i)), decel_velocity, epsilon); - } - } - } - } -} - -TEST(trajectory, findFirstNearestIndexWithSoftConstraints) -{ - using autoware::motion_utils::findFirstNearestIndexWithSoftConstraints; - using autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; - using autoware_utils::pi; - - const auto traj = generateTestTrajectory(10, 1.0); - - // Non overlapped points - { - // 1. Dist and yaw thresholds are given - // Normal cases - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 2.0, 0.4), - 2U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 2.0, 0.4), - 2U); - - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - traj.points, createPose(4.1, 0.3, 0.0, 0.0, 0.0, -0.8), 0.5, 1.0), - 4U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - traj.points, createPose(4.1, 0.3, 0.0, 0.0, 0.0, -0.8), 0.5, 1.0), - 4U); - - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - traj.points, createPose(8.5, -0.5, 0.0, 0.0, 0.0, 0.0), 1.0, 0.1), - 8U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - traj.points, createPose(8.5, -0.5, 0.0, 0.0, 0.0, 0.0), 1.0, 0.1), - 8U); - - // Dist is out of range - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.4), - 2U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.4), - 2U); - - // Yaw is out of range - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 2.0, 0.2), - 2U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 2.0, 0.2), - 2U); - - // Dist and yaw is out of range - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.2), - 2U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.2), - 2U); - - // Empty points - EXPECT_THROW( - findFirstNearestIndexWithSoftConstraints( - Trajectory{}.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.2), - std::invalid_argument); - EXPECT_THROW( - findFirstNearestSegmentIndexWithSoftConstraints( - Trajectory{}.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.2), - std::invalid_argument); - - // 2. Dist threshold is given - // Normal cases - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 2.0), - 2U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 2.0), - 2U); - - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - traj.points, createPose(4.1, 0.3, 0.0, 0.0, 0.0, -0.8), 0.5), - 4U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - traj.points, createPose(4.1, 0.3, 0.0, 0.0, 0.0, -0.8), 0.5), - 4U); - - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - traj.points, createPose(8.5, -0.5, 0.0, 0.0, 0.0, 0.0), 1.0), - 8U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - traj.points, createPose(8.5, -0.5, 0.0, 0.0, 0.0, 0.0), 1.0), - 8U); - - // Dist is out of range - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0), - 2U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0), - 2U); - - // 3. No threshold is given - // Normal cases - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3)), - 2U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3)), - 2U); - - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - traj.points, createPose(4.1, 0.3, 0.0, 0.0, 0.0, -0.8)), - 4U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - traj.points, createPose(4.1, 0.3, 0.0, 0.0, 0.0, -0.8)), - 4U); - - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - traj.points, createPose(8.5, -0.5, 0.0, 0.0, 0.0, 0.0)), - 8U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - traj.points, createPose(8.5, -0.5, 0.0, 0.0, 0.0, 0.0)), - 8U); - } - - // Vertically crossing points - { - // ___ - // | | - // S__|__| - // | - // | - // G - std::vector poses; - poses.push_back(createPose(-2.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - poses.push_back(createPose(-1.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - poses.push_back(createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - poses.push_back(createPose(1.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - poses.push_back(createPose(2.0, 0.0, 0.0, 0.0, 0.0, pi / 2.0)); - poses.push_back(createPose(2.0, 1.0, 0.0, 0.0, 0.0, pi / 2.0)); - poses.push_back(createPose(2.0, 2.0, 0.0, 0.0, 0.0, pi)); - poses.push_back(createPose(1.0, 2.0, 0.0, 0.0, 0.0, pi)); - poses.push_back(createPose(0.0, 2.0, 0.0, 0.0, 0.0, -pi / 2.0)); - poses.push_back(createPose(0.0, 1.0, 0.0, 0.0, 0.0, -pi / 2.0)); - poses.push_back(createPose(0.0, 0.0, 0.0, 0.0, 0.0, -pi / 2.0)); - poses.push_back(createPose(0.0, -1.0, 0.0, 0.0, 0.0, -pi / 2.0)); - poses.push_back(createPose(0.0, -2.0, 0.0, 0.0, 0.0, -pi / 2.0)); - - // 1. Dist and yaw thresholds are given - { - // Normal cases - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 1.0, 0.4), - 2U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 1.0, 0.4), - 2U); - - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, -pi / 2.0), 1.0, 0.4), - 10U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, -pi / 2.0), 1.0, 0.4), - 9U); - - // Several nearest index within threshold - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 10.0, pi * 2.0), - 2U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 10.0, pi * 2.0), - 2U); - - // Dist is out of range - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 0.0, 0.4), - 2U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 0.0, 0.4), - 2U); - - // Yaw is out of range - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.0), - 2U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.0), - 2U); - - // Dist and yaw is out of range - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.3), 0.0, 0.0), - 2U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.3), 0.0, 0.0), - 2U); - } - - // 2. Dist threshold is given - { - // Normal cases - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 1.0), - 2U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 1.0), - 2U); - - // Several nearest index within threshold - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 10.0), - 2U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 10.0), - 2U); - - // Dist is out of range - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 0.0), - 2U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 0.0), - 2U); - } - - // 3. No threshold is given - { - // Normal cases - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints(poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0)), - 2U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0)), - 2U); - } - } - - { - // Points has a loop structure with the opposite direction (= u-turn) - // __ - // S/G ___|_| - - std::vector poses; - poses.push_back(createPose(-3.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - poses.push_back(createPose(-2.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - poses.push_back(createPose(-1.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - poses.push_back(createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - poses.push_back(createPose(1.0, 0.0, 0.0, 0.0, 0.0, pi / 2.0)); - poses.push_back(createPose(1.0, 1.0, 0.0, 0.0, 0.0, pi)); - poses.push_back(createPose(0.0, 1.0, 0.0, 0.0, 0.0, -pi / 2.0)); - poses.push_back(createPose(0.0, 0.0, 0.0, 0.0, 0.0, pi)); - poses.push_back(createPose(-1.0, 0.0, 0.0, 0.0, 0.0, pi)); - poses.push_back(createPose(-2.0, 0.0, 0.0, 0.0, 0.0, pi)); - poses.push_back(createPose(-3.0, 0.0, 0.0, 0.0, 0.0, pi)); - - // 1. Dist and yaw thresholds are given - { - // Normal cases - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0, 0.4), - 1U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0, 0.4), - 0U); - - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 1.0, 0.4), - 9U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 1.0, 0.4), - 9U); - - // Several nearest index within threshold - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 10.0, pi * 2.0), - 1U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 10.0, pi * 2.0), - 0U); - - // Dist is out of range - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 0.0, 0.4), - 1U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 0.0, 0.4), - 0U); - - // Yaw is out of range - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi * 0.9), 1.0, 0.0), - 1U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi * 0.9), 1.0, 0.0), - 0U); - - // Dist and yaw is out of range - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi * 0.9), 0.0, 0.0), - 1U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi * 0.9), 0.0, 0.0), - 0U); - } - - // 2. Dist threshold is given - { - // Normal cases - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0), - 1U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0), - 0U); - - // Several nearest index within threshold - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 10.0), - 1U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 10.0), - 0U); - - // Dist is out of range - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0), - 1U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0), - 0U); - } - - // 3. No threshold is given - { - // Normal cases - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints(poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0)), - 1U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0)), - 0U); - } - } - - { // Points has a loop structure with the same direction - // ___ - // | | - // S__|__|__G - std::vector poses; - poses.push_back(createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - poses.push_back(createPose(1.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - poses.push_back(createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - poses.push_back(createPose(3.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - poses.push_back(createPose(4.0, 0.0, 0.0, 0.0, 0.0, pi / 2.0)); - poses.push_back(createPose(4.0, 1.0, 0.0, 0.0, 0.0, pi / 2.0)); - poses.push_back(createPose(4.0, 2.0, 0.0, 0.0, 0.0, pi)); - poses.push_back(createPose(3.0, 2.0, 0.0, 0.0, 0.0, pi)); - poses.push_back(createPose(2.0, 2.0, 0.0, 0.0, 0.0, -pi / 2.0)); - poses.push_back(createPose(2.0, 1.0, 0.0, 0.0, 0.0, -pi / 2.0)); - poses.push_back(createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - poses.push_back(createPose(3.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - poses.push_back(createPose(4.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - poses.push_back(createPose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - poses.push_back(createPose(6.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - - // 1. Dist and yaw thresholds are given - { - // Normal cases - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0, 0.4), - 3U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0, 0.4), - 3U); - - // Several nearest index within threshold - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 10.0, pi * 2.0), - 3U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 10.0, pi * 2.0), - 3U); - - // Dist is out of range - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0, 0.4), - 3U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0, 0.4), - 3U); - - // Yaw is out of range - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0, 0.0), - 3U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0, 0.0), - 3U); - - // Dist and yaw is out of range - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0, 0.0), - 3U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0, 0.0), - 3U); - } - - // 2. Dist threshold is given - { - // Normal cases - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0), - 3U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0), - 3U); - - // Several nearest index within threshold - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 10.0), - 3U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 10.0), - 3U); - - // Dist is out of range - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints( - poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0), - 3U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0), - 3U); - } - - // 3. No threshold is given - { - // Normal cases - EXPECT_EQ( - findFirstNearestIndexWithSoftConstraints(poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0)), - 3U); - EXPECT_EQ( - findFirstNearestSegmentIndexWithSoftConstraints( - poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0)), - 3U); - } - } -} - -TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointAndSegmentIndex) -{ - using autoware::motion_utils::calcSignedArcLength; - - const auto traj = generateTestTrajectory(10, 1.0); - - // Empty - EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); - - // Same point - { - 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 = 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 = 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 = 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 = 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 = 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 = 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 = 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); - } -} - -TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointIndex) -{ - using autoware::motion_utils::calcSignedArcLength; - - const auto traj = generateTestTrajectory(10, 1.0); - - // Empty - EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); - - // Same point - { - 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 = 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); - } - - // Backward - { - 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); - } - - // Point before start point - { - 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 = create_point(13.3, -10.0, 0.0); - EXPECT_NEAR(calcSignedArcLength(traj.points, 7, p2, 8), 6.3, epsilon); - } - - // Start point - { - 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 = 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 = 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 = 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); - } -} - -TEST(trajectory, removeOverlapPoints) -{ - using autoware::motion_utils::removeOverlapPoints; - - const auto traj = generateTestTrajectory(10, 1.0, 1.0); - const auto removed_traj = removeOverlapPoints(traj.points, 0); - EXPECT_EQ(traj.points.size(), removed_traj.size()); - for (size_t i = 0; i < traj.points.size(); ++i) { - EXPECT_NEAR(traj.points.at(i).pose.position.x, removed_traj.at(i).pose.position.x, epsilon); - EXPECT_NEAR(traj.points.at(i).pose.position.y, removed_traj.at(i).pose.position.y, epsilon); - EXPECT_NEAR(traj.points.at(i).pose.position.z, removed_traj.at(i).pose.position.z, epsilon); - EXPECT_NEAR( - traj.points.at(i).pose.orientation.x, removed_traj.at(i).pose.orientation.x, epsilon); - EXPECT_NEAR( - traj.points.at(i).pose.orientation.y, removed_traj.at(i).pose.orientation.y, epsilon); - EXPECT_NEAR( - traj.points.at(i).pose.orientation.z, removed_traj.at(i).pose.orientation.z, epsilon); - EXPECT_NEAR( - traj.points.at(i).pose.orientation.w, removed_traj.at(i).pose.orientation.w, epsilon); - EXPECT_NEAR( - traj.points.at(i).longitudinal_velocity_mps, removed_traj.at(i).longitudinal_velocity_mps, - epsilon); - } - - // No overlap points - { - const auto traj = generateTestTrajectory(10, 1.0, 1.0); - for (size_t start_idx = 0; start_idx < 10; ++start_idx) { - const auto removed_traj = removeOverlapPoints(traj.points, start_idx); - EXPECT_EQ(traj.points.size(), removed_traj.size()); - for (size_t i = 0; i < traj.points.size(); ++i) { - EXPECT_NEAR(traj.points.at(i).pose.position.x, removed_traj.at(i).pose.position.x, epsilon); - EXPECT_NEAR(traj.points.at(i).pose.position.y, removed_traj.at(i).pose.position.y, epsilon); - EXPECT_NEAR(traj.points.at(i).pose.position.z, removed_traj.at(i).pose.position.z, epsilon); - EXPECT_NEAR( - traj.points.at(i).pose.orientation.x, removed_traj.at(i).pose.orientation.x, epsilon); - EXPECT_NEAR( - traj.points.at(i).pose.orientation.y, removed_traj.at(i).pose.orientation.y, epsilon); - EXPECT_NEAR( - traj.points.at(i).pose.orientation.z, removed_traj.at(i).pose.orientation.z, epsilon); - EXPECT_NEAR( - traj.points.at(i).pose.orientation.w, removed_traj.at(i).pose.orientation.w, epsilon); - EXPECT_NEAR( - traj.points.at(i).longitudinal_velocity_mps, removed_traj.at(i).longitudinal_velocity_mps, - epsilon); - } - } - } - - // Overlap points from certain point - { - auto traj = generateTestTrajectory(10, 1.0, 1.0); - traj.points.at(5) = traj.points.at(6); - const auto removed_traj = removeOverlapPoints(traj.points); - - EXPECT_EQ(traj.points.size() - 1, removed_traj.size()); - for (size_t i = 0; i < 6; ++i) { - EXPECT_NEAR(traj.points.at(i).pose.position.x, removed_traj.at(i).pose.position.x, epsilon); - EXPECT_NEAR(traj.points.at(i).pose.position.y, removed_traj.at(i).pose.position.y, epsilon); - EXPECT_NEAR(traj.points.at(i).pose.position.z, removed_traj.at(i).pose.position.z, epsilon); - EXPECT_NEAR( - traj.points.at(i).pose.orientation.x, removed_traj.at(i).pose.orientation.x, epsilon); - EXPECT_NEAR( - traj.points.at(i).pose.orientation.y, removed_traj.at(i).pose.orientation.y, epsilon); - EXPECT_NEAR( - traj.points.at(i).pose.orientation.z, removed_traj.at(i).pose.orientation.z, epsilon); - EXPECT_NEAR( - traj.points.at(i).pose.orientation.w, removed_traj.at(i).pose.orientation.w, epsilon); - EXPECT_NEAR( - traj.points.at(i).longitudinal_velocity_mps, removed_traj.at(i).longitudinal_velocity_mps, - epsilon); - } - - for (size_t i = 6; i < 9; ++i) { - EXPECT_NEAR( - traj.points.at(i + 1).pose.position.x, removed_traj.at(i).pose.position.x, epsilon); - EXPECT_NEAR( - traj.points.at(i + 1).pose.position.y, removed_traj.at(i).pose.position.y, epsilon); - EXPECT_NEAR( - traj.points.at(i + 1).pose.position.z, removed_traj.at(i).pose.position.z, epsilon); - EXPECT_NEAR( - traj.points.at(i + 1).pose.orientation.x, removed_traj.at(i).pose.orientation.x, epsilon); - EXPECT_NEAR( - traj.points.at(i + 1).pose.orientation.y, removed_traj.at(i).pose.orientation.y, epsilon); - EXPECT_NEAR( - traj.points.at(i + 1).pose.orientation.z, removed_traj.at(i).pose.orientation.z, epsilon); - EXPECT_NEAR( - traj.points.at(i + 1).pose.orientation.w, removed_traj.at(i).pose.orientation.w, epsilon); - EXPECT_NEAR( - traj.points.at(i + 1).longitudinal_velocity_mps, - removed_traj.at(i).longitudinal_velocity_mps, epsilon); - } - } - - // Overlap points from certain point - { - auto traj = generateTestTrajectory(10, 1.0, 1.0); - traj.points.at(5) = traj.points.at(6); - const auto removed_traj = removeOverlapPoints(traj.points, 6); - - EXPECT_EQ(traj.points.size(), removed_traj.size()); - for (size_t i = 0; i < traj.points.size(); ++i) { - EXPECT_NEAR(traj.points.at(i).pose.position.x, removed_traj.at(i).pose.position.x, epsilon); - EXPECT_NEAR(traj.points.at(i).pose.position.y, removed_traj.at(i).pose.position.y, epsilon); - EXPECT_NEAR(traj.points.at(i).pose.position.z, removed_traj.at(i).pose.position.z, epsilon); - EXPECT_NEAR( - traj.points.at(i).pose.orientation.x, removed_traj.at(i).pose.orientation.x, epsilon); - EXPECT_NEAR( - traj.points.at(i).pose.orientation.y, removed_traj.at(i).pose.orientation.y, epsilon); - EXPECT_NEAR( - traj.points.at(i).pose.orientation.z, removed_traj.at(i).pose.orientation.z, epsilon); - EXPECT_NEAR( - traj.points.at(i).pose.orientation.w, removed_traj.at(i).pose.orientation.w, epsilon); - EXPECT_NEAR( - traj.points.at(i).longitudinal_velocity_mps, removed_traj.at(i).longitudinal_velocity_mps, - epsilon); - } - } - - // Empty Points - { - const Trajectory traj; - const auto removed_traj = removeOverlapPoints(traj.points); - EXPECT_TRUE(removed_traj.empty()); - } -} - -TEST(trajectory, cropForwardPoints) -{ - using autoware::motion_utils::cropForwardPoints; - - const auto traj = generateTestTrajectory(10, 1.0, 1.0); - - { // Normal case - const auto cropped_traj_points = - 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_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_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_utils::create_point(1.0, 1.0, 0.0), 1, 2.5); - EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); - } -} - -TEST(trajectory, cropBackwardPoints) -{ - using autoware::motion_utils::cropBackwardPoints; - - const auto traj = generateTestTrajectory(10, 1.0, 1.0); - - { // Normal case - const auto cropped_traj_points = - 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_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_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_utils::create_point(8.0, 8.0, 0.0), 8, 2.5); - EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); - } -} - -TEST(trajectory, cropPoints) -{ - using autoware::motion_utils::cropPoints; - - const auto traj = generateTestTrajectory(10, 1.0, 1.0); - - { // Normal case - const auto cropped_traj_points = - 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_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_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_utils::create_point(3.0, 3.0, 0.0), 3, 2.2, 1.9); - EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); - } -} - -TEST(Trajectory, removeFirstInvalidOrientationPoints) -{ - using autoware::motion_utils::insertOrientation; - using autoware::motion_utils::removeFirstInvalidOrientationPoints; - - const double max_yaw_diff = M_PI_2; - - auto testRemoveInvalidOrientationPoints = [&]( - const Trajectory & traj, - std::function modifyTrajectory, - size_t expected_size) { - auto modified_traj = traj; - insertOrientation(modified_traj.points, true); - modifyTrajectory(modified_traj); - removeFirstInvalidOrientationPoints(modified_traj.points, max_yaw_diff); - EXPECT_EQ(modified_traj.points.size(), expected_size); - for (size_t i = 0; i < modified_traj.points.size() - 1; ++i) { - 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_utils::normalize_radian(yaw1 - yaw2)); - EXPECT_LE(yaw_diff, max_yaw_diff); - } - }; - - auto traj = generateTestTrajectory(10, 1.0, 1.0); - - // no invalid points - testRemoveInvalidOrientationPoints(traj, [](Trajectory &) {}, traj.points.size()); - - // invalid point at the end - testRemoveInvalidOrientationPoints( - traj, - [&](Trajectory & t) { - auto invalid_point = t.points.back(); - invalid_point.pose.orientation = - tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_4)); - t.points.push_back(invalid_point); - }, - traj.points.size()); - - // invalid point in the middle - testRemoveInvalidOrientationPoints( - traj, - [&](Trajectory & t) { - auto invalid_point = t.points[4]; - invalid_point.pose.orientation = - tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_4)); - t.points.insert(t.points.begin() + 4, invalid_point); - }, - traj.points.size()); -} - -TEST(trajectory, calcYawDeviation) -{ - using autoware::motion_utils::calcYawDeviation; - using autoware_planning_msgs::msg::TrajectoryPoint; - - constexpr double tolerance = 1e-3; - - // Generate test trajectory - const auto trajectory = generateTestTrajectory(4, 10.0, 0.0, 0.0, M_PI / 8); - - // check with fist point - { - const auto input_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - const double yaw_deviation = calcYawDeviation(trajectory.points, input_pose); - constexpr double expected_yaw_deviation = -M_PI / 8; - EXPECT_NEAR(yaw_deviation, expected_yaw_deviation, tolerance); - } - - // check with middle point - { - const auto input_pose = createPose(10.0, 10.0, 0.0, 0.0, 0.0, M_PI / 8); - const double yaw_deviation = calcYawDeviation(trajectory.points, input_pose); - constexpr double expected_yaw_deviation = -0.734; - EXPECT_NEAR(yaw_deviation, expected_yaw_deviation, tolerance); - } -} - -TEST(trajectory, isTargetPointFront) -{ - using autoware::motion_utils::isTargetPointFront; - 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 = 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 = 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 = 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 = 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 = 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 = 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 = 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)); - } - - // empty point - { - const Trajectory traj; - 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)); - } - - // non-default threshold - { - const double threshold = 3.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 = 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 = 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 deleted file mode 100644 index e2e884045ee1f..0000000000000 --- a/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp +++ /dev/null @@ -1,529 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/motion_utils/vehicle/vehicle_state_checker.hpp" -#include "autoware_utils/geometry/geometry.hpp" -#include "test_vehicle_state_checker_helper.hpp" - -#include - -#include - -#include -#include - -constexpr double ODOMETRY_HISTORY_500_MS = 0.5; -constexpr double ODOMETRY_HISTORY_1000_MS = 1.0; -constexpr double STOP_DURATION_THRESHOLD_0_MS = 0.0; -constexpr double STOP_DURATION_THRESHOLD_200_MS = 0.2; -constexpr double STOP_DURATION_THRESHOLD_400_MS = 0.4; -constexpr double STOP_DURATION_THRESHOLD_600_MS = 0.6; -constexpr double STOP_DURATION_THRESHOLD_800_MS = 0.8; -constexpr double STOP_DURATION_THRESHOLD_1000_MS = 1.0; - -using autoware::motion_utils::VehicleArrivalChecker; -using autoware::motion_utils::VehicleStopChecker; -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 -{ -public: - CheckerNode() : Node("test_checker_node") - { - vehicle_stop_checker_ = std::make_unique(this); - vehicle_arrival_checker_ = std::make_unique(this); - } - - std::unique_ptr vehicle_stop_checker_; - std::unique_ptr vehicle_arrival_checker_; -}; - -class PubManager : public rclcpp::Node -{ -public: - PubManager() : Node("test_pub_node") - { - pub_odom_ = create_publisher("/localization/kinematic_state", 1); - pub_traj_ = create_publisher("/planning/scenario_planning/trajectory", 1); - } - - rclcpp::Publisher::SharedPtr pub_odom_; - rclcpp::Publisher::SharedPtr pub_traj_; - - void publishStoppedOdometry(const geometry_msgs::msg::Pose & pose, const double publish_duration) - { - const auto start_time = this->now(); - while (true) { - const auto now = this->now(); - - const auto time_diff = now - start_time; - if (publish_duration < time_diff.seconds()) { - break; - } - - Odometry odometry; - odometry.header.stamp = now; - odometry.pose.pose = pose; - 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(); - } - } - - void publishStoppedOdometry(const double publish_duration) - { - const auto start_time = this->now(); - while (true) { - const auto now = this->now(); - - const auto time_diff = now - start_time; - if (publish_duration < time_diff.seconds()) { - break; - } - - Odometry odometry; - odometry.header.stamp = now; - 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(); - } - } - - void publishStoppingOdometry(const double publish_duration) - { - const auto start_time = this->now(); - while (true) { - const auto now = this->now(); - - const auto time_diff = now - start_time; - if (publish_duration < time_diff.seconds()) { - break; - } - - Odometry odometry; - odometry.header.stamp = now; - 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 - ? 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(); - } - } - - void publishOldOdometry(const double publish_duration) - { - const auto start_time = this->now(); - while (true) { - const auto now = this->now(); - - const auto time_diff = now - start_time; - if (publish_duration < time_diff.seconds()) { - break; - } - - Odometry odometry; - odometry.header.stamp = now - rclcpp::Duration(15, 0); // 15 seconds old data - 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(); - } - } -}; - -TEST(vehicle_stop_checker, isVehicleStopped) -{ - { - auto checker = std::make_shared(); - auto manager = std::make_shared(); - EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected."; - - EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped()); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(checker); - executor.add_node(manager); - - std::thread spin_thread = - std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); - - manager->publishStoppedOdometry(ODOMETRY_HISTORY_500_MS); - - EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped()); - EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS)); - EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_200_MS)); - EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_400_MS)); - EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_600_MS)); - EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_800_MS)); - EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_1000_MS)); - - executor.cancel(); - spin_thread.join(); - checker.reset(); - manager.reset(); - } - - { - auto checker = std::make_shared(); - auto manager = std::make_shared(); - EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected."; - - EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped()); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(checker); - executor.add_node(manager); - - std::thread spin_thread = - std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); - - manager->publishStoppedOdometry(ODOMETRY_HISTORY_1000_MS); - - EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped()); - EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS)); - EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_200_MS)); - EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_400_MS)); - EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_600_MS)); - EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_800_MS)); - EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_1000_MS)); - - executor.cancel(); - spin_thread.join(); - checker.reset(); - manager.reset(); - } - - { - auto checker = std::make_shared(); - auto manager = std::make_shared(); - EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected."; - - EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped()); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(checker); - executor.add_node(manager); - - std::thread spin_thread = - std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); - - manager->publishStoppingOdometry(ODOMETRY_HISTORY_1000_MS); - - EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped()); - EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS)); - EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_200_MS)); - EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_400_MS)); - EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_600_MS)); - EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_800_MS)); - EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_1000_MS)); - - executor.cancel(); - spin_thread.join(); - checker.reset(); - manager.reset(); - } - - // check if the old data will be discarded - { - auto checker = std::make_shared(); - auto manager = std::make_shared(); - EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected."; - - EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped()); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(checker); - executor.add_node(manager); - - std::thread spin_thread = - std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); - - manager->publishOldOdometry(ODOMETRY_HISTORY_500_MS); - - EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped()); - EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS)); - EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_200_MS)); - EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_400_MS)); - EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_600_MS)); - EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_800_MS)); - EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_1000_MS)); - - executor.cancel(); - spin_thread.join(); - checker.reset(); - manager.reset(); - } -} - -TEST(vehicle_arrival_checker, isVehicleStopped) -{ - { - auto checker = std::make_shared(); - auto manager = std::make_shared(); - EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected."; - - EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStopped()); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(checker); - executor.add_node(manager); - - std::thread spin_thread = - std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); - - manager->publishStoppedOdometry(ODOMETRY_HISTORY_500_MS); - - EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStopped()); - EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS)); - EXPECT_TRUE( - checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_200_MS)); - EXPECT_TRUE( - checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_400_MS)); - EXPECT_FALSE( - checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_600_MS)); - EXPECT_FALSE( - checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_800_MS)); - EXPECT_FALSE( - checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_1000_MS)); - - executor.cancel(); - spin_thread.join(); - checker.reset(); - manager.reset(); - } - - { - auto checker = std::make_shared(); - auto manager = std::make_shared(); - EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected."; - - EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStopped()); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(checker); - executor.add_node(manager); - - std::thread spin_thread = - std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); - - manager->publishStoppedOdometry(ODOMETRY_HISTORY_1000_MS); - - EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStopped()); - EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS)); - EXPECT_TRUE( - checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_200_MS)); - EXPECT_TRUE( - checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_400_MS)); - EXPECT_TRUE( - checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_600_MS)); - EXPECT_TRUE( - checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_800_MS)); - EXPECT_TRUE( - checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_1000_MS)); - - executor.cancel(); - spin_thread.join(); - checker.reset(); - manager.reset(); - } - - { - auto checker = std::make_shared(); - auto manager = std::make_shared(); - EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected."; - - EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStopped()); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(checker); - executor.add_node(manager); - - std::thread spin_thread = - std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); - - manager->publishStoppingOdometry(ODOMETRY_HISTORY_1000_MS); - - EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStopped()); - EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS)); - EXPECT_TRUE( - checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_200_MS)); - EXPECT_TRUE( - checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_400_MS)); - EXPECT_FALSE( - checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_600_MS)); - EXPECT_FALSE( - checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_800_MS)); - EXPECT_FALSE( - checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_1000_MS)); - - executor.cancel(); - spin_thread.join(); - checker.reset(); - manager.reset(); - } -} - -TEST(vehicle_arrival_checker, isVehicleStoppedAtStopPoint) -{ - { - auto checker = std::make_shared(); - auto manager = std::make_shared(); - EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected."; - - EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStopped()); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(checker); - executor.add_node(manager); - - std::thread spin_thread = - std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); - - geometry_msgs::msg::Pose odom_pose; - 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 = 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); - - EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint()); - EXPECT_TRUE( - checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint(STOP_DURATION_THRESHOLD_0_MS)); - EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( - STOP_DURATION_THRESHOLD_200_MS)); - EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( - STOP_DURATION_THRESHOLD_400_MS)); - EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( - STOP_DURATION_THRESHOLD_600_MS)); - EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( - STOP_DURATION_THRESHOLD_800_MS)); - EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( - STOP_DURATION_THRESHOLD_1000_MS)); - - executor.cancel(); - spin_thread.join(); - checker.reset(); - manager.reset(); - } - - { - auto checker = std::make_shared(); - auto manager = std::make_shared(); - EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected."; - - EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint()); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(checker); - executor.add_node(manager); - - std::thread spin_thread = - std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); - - geometry_msgs::msg::Pose odom_pose; - 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 = 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); - - EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint()); - EXPECT_FALSE( - checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint(STOP_DURATION_THRESHOLD_0_MS)); - EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( - STOP_DURATION_THRESHOLD_200_MS)); - EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( - STOP_DURATION_THRESHOLD_400_MS)); - EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( - STOP_DURATION_THRESHOLD_600_MS)); - EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( - STOP_DURATION_THRESHOLD_800_MS)); - EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( - STOP_DURATION_THRESHOLD_1000_MS)); - - executor.cancel(); - spin_thread.join(); - checker.reset(); - manager.reset(); - } - - { - auto checker = std::make_shared(); - auto manager = std::make_shared(); - EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected."; - - EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint()); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(checker); - executor.add_node(manager); - - std::thread spin_thread = - std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); - - geometry_msgs::msg::Pose odom_pose; - 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 = 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); - - EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint()); - EXPECT_FALSE( - checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint(STOP_DURATION_THRESHOLD_0_MS)); - EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( - STOP_DURATION_THRESHOLD_200_MS)); - EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( - STOP_DURATION_THRESHOLD_400_MS)); - EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( - STOP_DURATION_THRESHOLD_600_MS)); - EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( - STOP_DURATION_THRESHOLD_800_MS)); - EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( - STOP_DURATION_THRESHOLD_1000_MS)); - - executor.cancel(); - spin_thread.join(); - checker.reset(); - manager.reset(); - } -} diff --git a/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp b/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp deleted file mode 100644 index 29802e70bfd5f..0000000000000 --- a/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp +++ /dev/null @@ -1,59 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef VEHICLE__TEST_VEHICLE_STATE_CHECKER_HELPER_HPP_ -#define VEHICLE__TEST_VEHICLE_STATE_CHECKER_HELPER_HPP_ - -#include -#include - -using autoware_planning_msgs::msg::Trajectory; -using autoware_planning_msgs::msg::TrajectoryPoint; - -inline Trajectory generateTrajectoryWithStopPoint(const geometry_msgs::msg::Pose & goal_pose) -{ - constexpr double interval_distance = 1.0; - - Trajectory traj; - for (double s = 0.0; s <= 10.0 * interval_distance; s += interval_distance) { - TrajectoryPoint p; - p.pose = goal_pose; - p.pose.position.x += s; - p.longitudinal_velocity_mps = 1.0; - traj.points.push_back(p); - } - - traj.points.front().longitudinal_velocity_mps = 0.0; - std::reverse(traj.points.begin(), traj.points.end()); - return traj; -} - -inline Trajectory generateTrajectoryWithoutStopPoint(const geometry_msgs::msg::Pose & goal_pose) -{ - constexpr double interval_distance = 1.0; - - Trajectory traj; - for (double s = 0.0; s <= 10.0 * interval_distance; s += interval_distance) { - TrajectoryPoint p; - p.pose = goal_pose; - p.pose.position.x += s; - p.longitudinal_velocity_mps = 1.0; - traj.points.push_back(p); - } - - std::reverse(traj.points.begin(), traj.points.end()); - return traj; -} - -#endif // VEHICLE__TEST_VEHICLE_STATE_CHECKER_HELPER_HPP_