From c152f5739ab2ac68108f071a4d26a65714366296 Mon Sep 17 00:00:00 2001 From: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Date: Wed, 15 Dec 2021 19:05:29 +0900 Subject: [PATCH] feat: add autoware auto dependencies (#185) * Back port .auto control packages (#571) * Implement Lateral and Longitudinal Control Muxer * [#570] Porting wf_simulator * [#1189] Deactivate flaky test in 'trajectory_follower_nodes' * [#1189] Fix flacky test in 'trajectory_follower_nodes/latlon_muxer' * [#1057] Add osqp_interface package * [#1057] Add library code for MPC-based lateral control * [#1271] Use std::abs instead of abs * [#1057] Implement Lateral Controller for Cargo ODD * [#1246] Resolve "Test case names currently use snake_case but should be CamelCase" * [#1325] Deactivate flaky smoke test in 'trajectory_follower_nodes' * [#1058] Add library code of longitudinal controller * Fix build error for trajectory follower Signed-off-by: wep21 * Fix build error for trajectory follower nodes Signed-off-by: wep21 * [#1272] Add AckermannControlCommand support to simple_planning_simulator * [#1058] Add Longitudinal Controller node * [#1058] Rename velocity_controller -> longitudinal_controller * [#1058] Update CMakeLists.txt for the longitudinal_controller_node * [#1058] Add smoke test python launch file * [#1058] Use LowPassFilter1d from trajectory_follower * [#1058] Use autoware_auto_msgs * [#1058] Changes for .auto (debug msg tmp fix, common func, tf listener) * [#1058] Remove unused parameters * [#1058] Fix ros test * [#1058] Rm default params from declare_parameters + use autoware types * [#1058] Use default param file to setup NodeOptions in the ros test * [#1058] Fix docstring * [#1058] Replace receiving a Twist with a VehicleKinematicState * [#1058] Change class variables format to m_ prefix * [#1058] Fix plugin name of LongitudinalController in CMakeLists.txt * [#1058] Fix copyright dates * [#1058] Reorder includes * [#1058] Add some tests (~89% coverage without disabling flaky tests) * [#1058] Add more tests (90+% coverage without disabling flaky tests) * [#1058] Use Float32MultiArrayDiagnostic message for debug and slope * [#1058] Calculate wheel_base value from vehicle parameters * [#1058] Cleanup redundant logger setting in tests * [#1058] Set ROS_DOMAIN_ID when running tests to prevent CI failures * [#1058] Remove TF listener and use published vehicle state instead * [#1058] Change smoke tests to use autoware_testing * [#1058] Add plotjuggler cfg for both lateral and longitudinal control * [#1058] Improve design documents * [#1058] Disable flaky test * [#1058] Properly transform vehicle state in longitudinal node * [#1058] Fix TF buffer of lateral controller * [#1058] Tuning of lateral controller for LGSVL * [#1058] Fix formating * [#1058] Fix /tf_static sub to be transient_local * [#1058] Fix yaw recalculation of reverse trajs in the lateral controller * modify trajectory_follower for galactic build Signed-off-by: Takamasa Horibe * [#1379] Update trajectory_follower * [#1379] Update simple_planning_simulator * [#1379] Update trajectory_follower_nodes * apply trajectory msg modification in control Signed-off-by: Takamasa Horibe * move directory Signed-off-by: Takamasa Horibe * remote control/trajectory_follower level dorectpry Signed-off-by: Takamasa Horibe * remove .iv trajectory follower Signed-off-by: Takamasa Horibe * use .auto trajectory_follower Signed-off-by: Takamasa Horibe * remove .iv simple_planning_simulator & osqp_interface Signed-off-by: Takamasa Horibe * use .iv simple_planning_simulator & osqp_interface Signed-off-by: Takamasa Horibe * add tmp_autoware_auto_dependencies Signed-off-by: Takamasa Horibe * tmporally add autoware_auto_msgs Signed-off-by: Takamasa Horibe * apply .auto message split Signed-off-by: Takamasa Horibe * fix build depend Signed-off-by: Takamasa Horibe * fix packages using osqp * fix autoware_auto_geometry * ignore lint of some packages * ignore ament_lint of some packages * ignore lint/pre-commit of trajectory_follower_nodes * disable unit tests of some packages Co-authored-by: Maxime CLEMENT Co-authored-by: Joshua Whitley Co-authored-by: Igor Bogoslavskyi Co-authored-by: MIURA Yasuyuki Co-authored-by: wep21 Co-authored-by: tomoya.kimura * Port parking planner packages from .Auto (#600) * Copy code of 'vehicle_constants_manager' * Fix vehicle_constants_manager for ROS galactic * Rm .iv costmap_generator freespace_planner freespace_planning_aglorihtms * Add astar_search (from .Auto) * Copy freespace_planner from .Auto * Update freespace_planner for .IV * Copy costmap_generator from .Auto * Copy and update had_map_utils from .Auto * Update costmap_generator * Copy costmap_generator_nodes * Update costmap_generator_nodes * Comment out all tests * Move vehicle_constant_managers to tmp_autoware_auto_dependencies * ignore pre-commit for back-ported packages Signed-off-by: Takamasa Horibe * ignore testing Signed-off-by: Takamasa Horibe Co-authored-by: Takamasa Horibe * fix: fix pre-commit * fix: fix markdownlint * fix: fix cpplint * feat: remove autoware_auto_dependencies Co-authored-by: Takamasa Horibe Co-authored-by: Maxime CLEMENT Co-authored-by: Joshua Whitley Co-authored-by: Igor Bogoslavskyi Co-authored-by: MIURA Yasuyuki Co-authored-by: wep21 Co-authored-by: tomoya.kimura Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> --- .pre-commit-config.yaml | 2 +- build_depends.repos | 4 - common/autoware_auto_cmake/CMakeLists.txt | 33 + .../autoware_auto_cmake-extras.cmake | 16 + .../cmake/autoware_auto_cmake.cmake | 111 + .../design/autoware_auto_cmake-design.md | 29 + common/autoware_auto_cmake/package.xml | 29 + common/autoware_auto_common/CMakeLists.txt | 75 + .../design/comparisons.md | 66 + .../include/common/filesystem.hpp | 5558 +++++++++++++++++ .../include/common/type_traits.hpp | 206 + .../include/common/types.hpp | 136 + .../include/common/visibility_control.hpp | 38 + .../include/helper_functions/angle_utils.hpp | 62 + .../helper_functions/bool_comparisons.hpp | 50 + .../include/helper_functions/byte_reader.hpp | 77 + .../include/helper_functions/crtp.hpp | 52 + .../helper_functions/float_comparisons.hpp | 151 + .../helper_functions/mahalanobis_distance.hpp | 75 + .../helper_functions/message_adapters.hpp | 111 + .../helper_functions/template_utils.hpp | 67 + .../include/helper_functions/type_name.hpp | 56 + common/autoware_auto_common/package.xml | 21 + .../autoware_auto_common/test/gtest_main.cpp | 23 + .../test/test_angle_utils.cpp | 37 + .../test/test_bool_comparisons.cpp | 44 + .../test/test_byte_reader.cpp | 54 + .../test/test_float_comparisons.cpp | 151 + .../test/test_mahalanobis_distance.cpp | 41 + .../test/test_message_field_adapters.cpp | 77 + .../test/test_template_utils.cpp | 134 + .../test/test_type_name.cpp | 37 + .../test/test_type_traits.cpp | 95 + common/autoware_auto_geometry/CMakeLists.txt | 64 + .../autoware_auto_geometry/design/interval.md | 112 + .../design/polygon_intersection_2d-design.md | 50 + .../design/spatial-hash-design.md | 102 + .../bounding_box/bounding_box_common.hpp | 188 + .../geometry/bounding_box/eigenbox_2d.hpp | 254 + .../include/geometry/bounding_box/lfit.hpp | 284 + .../bounding_box/rotating_calipers.hpp | 290 + .../include/geometry/bounding_box_2d.hpp | 34 + .../include/geometry/common_2d.hpp | 595 ++ .../include/geometry/common_3d.hpp | 77 + .../include/geometry/convex_hull.hpp | 194 + .../include/geometry/hull_pockets.hpp | 118 + .../include/geometry/intersection.hpp | 324 + .../include/geometry/interval.hpp | 365 ++ .../include/geometry/lookup_table.hpp | 184 + .../include/geometry/spatial_hash.hpp | 390 ++ .../include/geometry/spatial_hash_config.hpp | 494 ++ .../include/geometry/visibility_control.hpp | 42 + common/autoware_auto_geometry/package.xml | 26 + .../src/bounding_box.cpp | 154 + .../src/spatial_hash.cpp | 118 + .../test/include/test_bounding_box.hpp | 669 ++ .../test/include/test_spatial_hash.hpp | 266 + .../test/src/lookup_table.cpp | 164 + .../test/src/test_area.cpp | 137 + .../test/src/test_common_2d.cpp | 198 + .../test/src/test_convex_hull.cpp | 372 ++ .../test/src/test_geometry.cpp | 27 + .../test/src/test_hull_pockets.cpp | 208 + .../test/src/test_intersection.cpp | 178 + .../test/src/test_interval.cpp | 263 + common/autoware_auto_tf2/CMakeLists.txt | 46 + .../design/autoware-auto-tf2-design.md | 175 + .../tf2_autoware_auto_msgs.hpp | 199 + common/autoware_auto_tf2/package.xml | 27 + .../test/test_tf2_autoware_auto_msgs.cpp | 325 + common/autoware_testing/CMakeLists.txt | 33 + .../autoware_testing-extras.cmake | 17 + .../autoware_testing/__init__.py | 0 .../autoware_testing/smoke_test.py | 89 + .../cmake/add_smoke_test.cmake | 46 + .../design/autoware_testing-design.md | 65 + common/autoware_testing/package.xml | 22 + .../resource/autoware_testing | 0 common/autoware_testing/setup.cfg | 4 + common/autoware_testing/setup.py | 25 + common/fake_test_node/CMakeLists.txt | 41 + .../design/fake_test_node-design.md | 66 + .../include/fake_test_node/fake_test_node.hpp | 253 + .../fake_test_node/visibility_control.hpp | 42 + common/fake_test_node/package.xml | 25 + common/fake_test_node/src/fake_test_node.cpp | 81 + .../test/test_fake_test_node.cpp | 109 + common/had_map_utils/CMakeLists.txt | 38 + .../had_map_utils/had_map_computation.hpp | 38 + .../had_map_utils/had_map_conversion.hpp | 42 + .../include/had_map_utils/had_map_query.hpp | 61 + .../include/had_map_utils/had_map_utils.hpp | 45 + .../had_map_utils/had_map_visualization.hpp | 275 + .../had_map_utils/visibility_control.hpp | 38 + common/had_map_utils/package.xml | 33 + .../had_map_utils/src/had_map_computation.cpp | 135 + .../had_map_utils/src/had_map_conversion.cpp | 73 + common/had_map_utils/src/had_map_query.cpp | 132 + common/had_map_utils/src/had_map_utils.cpp | 175 + .../src/had_map_visualization.cpp | 558 ++ common/motion_common/CMakeLists.txt | 42 + .../include/motion_common/config.hpp | 182 + .../include/motion_common/motion_common.hpp | 279 + .../motion_common/trajectory_common.hpp | 170 + .../motion_common/visibility_control.hpp | 36 + common/motion_common/package.xml | 33 + .../kinematic_bicycle.snippet.hpp | 50 + .../linear_tire.snippet.hpp | 59 + .../src/motion_common/config.cpp | 264 + .../src/motion_common/motion_common.cpp | 270 + .../src/motion_common/trajectory_common.cpp | 219 + common/motion_common/test/gtest_main.cpp | 21 + common/motion_common/test/interpolation.cpp | 271 + common/motion_common/test/sanity_checks.cpp | 57 + common/motion_common/test/trajectory.cpp | 405 ++ common/motion_testing/CMakeLists.txt | 29 + .../include/motion_testing/motion_testing.hpp | 127 + .../motion_testing/visibility_control.hpp | 36 + common/motion_testing/package.xml | 30 + .../src/motion_testing/motion_testing.cpp | 319 + .../test/constant_trajectory.cpp | 125 + common/motion_testing/test/gtest_main.cpp | 21 + .../motion_testing/test/trajectory_checks.cpp | 40 + common/time_utils/CMakeLists.txt | 21 + .../include/time_utils/stopwatch.hpp | 84 + .../include/time_utils/time_utils.hpp | 50 + .../include/time_utils/visibility_control.hpp | 36 + common/time_utils/package.xml | 23 + .../time_utils/src/time_utils/time_utils.cpp | 108 + .../vehicle_constants_manager/CMakeLists.txt | 50 + .../vehicle_constants_manager-design.md | 145 + .../vehicle_constants_manager.hpp | 186 + .../visibility_control.hpp | 39 + common/vehicle_constants_manager/package.xml | 25 + .../param/params_lexus_rx_hybrid_2016.yaml | 20 + .../src/vehicle_constants_manager.cpp | 196 + .../test/test_vehicle_constants_manager.cpp | 124 + 137 files changed, 22179 insertions(+), 5 deletions(-) create mode 100644 common/autoware_auto_cmake/CMakeLists.txt create mode 100644 common/autoware_auto_cmake/autoware_auto_cmake-extras.cmake create mode 100644 common/autoware_auto_cmake/cmake/autoware_auto_cmake.cmake create mode 100644 common/autoware_auto_cmake/design/autoware_auto_cmake-design.md create mode 100644 common/autoware_auto_cmake/package.xml create mode 100644 common/autoware_auto_common/CMakeLists.txt create mode 100644 common/autoware_auto_common/design/comparisons.md create mode 100644 common/autoware_auto_common/include/common/filesystem.hpp create mode 100644 common/autoware_auto_common/include/common/type_traits.hpp create mode 100644 common/autoware_auto_common/include/common/types.hpp create mode 100644 common/autoware_auto_common/include/common/visibility_control.hpp create mode 100644 common/autoware_auto_common/include/helper_functions/angle_utils.hpp create mode 100644 common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp create mode 100644 common/autoware_auto_common/include/helper_functions/byte_reader.hpp create mode 100644 common/autoware_auto_common/include/helper_functions/crtp.hpp create mode 100644 common/autoware_auto_common/include/helper_functions/float_comparisons.hpp create mode 100644 common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp create mode 100644 common/autoware_auto_common/include/helper_functions/message_adapters.hpp create mode 100644 common/autoware_auto_common/include/helper_functions/template_utils.hpp create mode 100644 common/autoware_auto_common/include/helper_functions/type_name.hpp create mode 100644 common/autoware_auto_common/package.xml create mode 100644 common/autoware_auto_common/test/gtest_main.cpp create mode 100644 common/autoware_auto_common/test/test_angle_utils.cpp create mode 100644 common/autoware_auto_common/test/test_bool_comparisons.cpp create mode 100644 common/autoware_auto_common/test/test_byte_reader.cpp create mode 100644 common/autoware_auto_common/test/test_float_comparisons.cpp create mode 100644 common/autoware_auto_common/test/test_mahalanobis_distance.cpp create mode 100644 common/autoware_auto_common/test/test_message_field_adapters.cpp create mode 100644 common/autoware_auto_common/test/test_template_utils.cpp create mode 100644 common/autoware_auto_common/test/test_type_name.cpp create mode 100644 common/autoware_auto_common/test/test_type_traits.cpp create mode 100644 common/autoware_auto_geometry/CMakeLists.txt create mode 100644 common/autoware_auto_geometry/design/interval.md create mode 100644 common/autoware_auto_geometry/design/polygon_intersection_2d-design.md create mode 100644 common/autoware_auto_geometry/design/spatial-hash-design.md create mode 100644 common/autoware_auto_geometry/include/geometry/bounding_box/bounding_box_common.hpp create mode 100644 common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp create mode 100644 common/autoware_auto_geometry/include/geometry/bounding_box/lfit.hpp create mode 100644 common/autoware_auto_geometry/include/geometry/bounding_box/rotating_calipers.hpp create mode 100644 common/autoware_auto_geometry/include/geometry/bounding_box_2d.hpp create mode 100644 common/autoware_auto_geometry/include/geometry/common_2d.hpp create mode 100644 common/autoware_auto_geometry/include/geometry/common_3d.hpp create mode 100644 common/autoware_auto_geometry/include/geometry/convex_hull.hpp create mode 100644 common/autoware_auto_geometry/include/geometry/hull_pockets.hpp create mode 100644 common/autoware_auto_geometry/include/geometry/intersection.hpp create mode 100644 common/autoware_auto_geometry/include/geometry/interval.hpp create mode 100644 common/autoware_auto_geometry/include/geometry/lookup_table.hpp create mode 100644 common/autoware_auto_geometry/include/geometry/spatial_hash.hpp create mode 100644 common/autoware_auto_geometry/include/geometry/spatial_hash_config.hpp create mode 100644 common/autoware_auto_geometry/include/geometry/visibility_control.hpp create mode 100644 common/autoware_auto_geometry/package.xml create mode 100644 common/autoware_auto_geometry/src/bounding_box.cpp create mode 100644 common/autoware_auto_geometry/src/spatial_hash.cpp create mode 100644 common/autoware_auto_geometry/test/include/test_bounding_box.hpp create mode 100644 common/autoware_auto_geometry/test/include/test_spatial_hash.hpp create mode 100644 common/autoware_auto_geometry/test/src/lookup_table.cpp create mode 100644 common/autoware_auto_geometry/test/src/test_area.cpp create mode 100644 common/autoware_auto_geometry/test/src/test_common_2d.cpp create mode 100644 common/autoware_auto_geometry/test/src/test_convex_hull.cpp create mode 100644 common/autoware_auto_geometry/test/src/test_geometry.cpp create mode 100644 common/autoware_auto_geometry/test/src/test_hull_pockets.cpp create mode 100644 common/autoware_auto_geometry/test/src/test_intersection.cpp create mode 100644 common/autoware_auto_geometry/test/src/test_interval.cpp create mode 100755 common/autoware_auto_tf2/CMakeLists.txt create mode 100644 common/autoware_auto_tf2/design/autoware-auto-tf2-design.md create mode 100644 common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp create mode 100644 common/autoware_auto_tf2/package.xml create mode 100755 common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp create mode 100644 common/autoware_testing/CMakeLists.txt create mode 100644 common/autoware_testing/autoware_testing-extras.cmake create mode 100644 common/autoware_testing/autoware_testing/__init__.py create mode 100644 common/autoware_testing/autoware_testing/smoke_test.py create mode 100644 common/autoware_testing/cmake/add_smoke_test.cmake create mode 100644 common/autoware_testing/design/autoware_testing-design.md create mode 100644 common/autoware_testing/package.xml create mode 100644 common/autoware_testing/resource/autoware_testing create mode 100644 common/autoware_testing/setup.cfg create mode 100644 common/autoware_testing/setup.py create mode 100644 common/fake_test_node/CMakeLists.txt create mode 100644 common/fake_test_node/design/fake_test_node-design.md create mode 100644 common/fake_test_node/include/fake_test_node/fake_test_node.hpp create mode 100644 common/fake_test_node/include/fake_test_node/visibility_control.hpp create mode 100644 common/fake_test_node/package.xml create mode 100644 common/fake_test_node/src/fake_test_node.cpp create mode 100644 common/fake_test_node/test/test_fake_test_node.cpp create mode 100644 common/had_map_utils/CMakeLists.txt create mode 100644 common/had_map_utils/include/had_map_utils/had_map_computation.hpp create mode 100644 common/had_map_utils/include/had_map_utils/had_map_conversion.hpp create mode 100644 common/had_map_utils/include/had_map_utils/had_map_query.hpp create mode 100644 common/had_map_utils/include/had_map_utils/had_map_utils.hpp create mode 100644 common/had_map_utils/include/had_map_utils/had_map_visualization.hpp create mode 100644 common/had_map_utils/include/had_map_utils/visibility_control.hpp create mode 100644 common/had_map_utils/package.xml create mode 100644 common/had_map_utils/src/had_map_computation.cpp create mode 100644 common/had_map_utils/src/had_map_conversion.cpp create mode 100644 common/had_map_utils/src/had_map_query.cpp create mode 100644 common/had_map_utils/src/had_map_utils.cpp create mode 100644 common/had_map_utils/src/had_map_visualization.cpp create mode 100644 common/motion_common/CMakeLists.txt create mode 100644 common/motion_common/include/motion_common/config.hpp create mode 100644 common/motion_common/include/motion_common/motion_common.hpp create mode 100644 common/motion_common/include/motion_common/trajectory_common.hpp create mode 100644 common/motion_common/include/motion_common/visibility_control.hpp create mode 100644 common/motion_common/package.xml create mode 100644 common/motion_common/scripts/autogeneration_code_snippets/kinematic_bicycle.snippet.hpp create mode 100644 common/motion_common/scripts/autogeneration_code_snippets/linear_tire.snippet.hpp create mode 100644 common/motion_common/src/motion_common/config.cpp create mode 100644 common/motion_common/src/motion_common/motion_common.cpp create mode 100644 common/motion_common/src/motion_common/trajectory_common.cpp create mode 100644 common/motion_common/test/gtest_main.cpp create mode 100644 common/motion_common/test/interpolation.cpp create mode 100644 common/motion_common/test/sanity_checks.cpp create mode 100644 common/motion_common/test/trajectory.cpp create mode 100644 common/motion_testing/CMakeLists.txt create mode 100644 common/motion_testing/include/motion_testing/motion_testing.hpp create mode 100644 common/motion_testing/include/motion_testing/visibility_control.hpp create mode 100644 common/motion_testing/package.xml create mode 100644 common/motion_testing/src/motion_testing/motion_testing.cpp create mode 100644 common/motion_testing/test/constant_trajectory.cpp create mode 100644 common/motion_testing/test/gtest_main.cpp create mode 100644 common/motion_testing/test/trajectory_checks.cpp create mode 100644 common/time_utils/CMakeLists.txt create mode 100644 common/time_utils/include/time_utils/stopwatch.hpp create mode 100644 common/time_utils/include/time_utils/time_utils.hpp create mode 100644 common/time_utils/include/time_utils/visibility_control.hpp create mode 100644 common/time_utils/package.xml create mode 100644 common/time_utils/src/time_utils/time_utils.cpp create mode 100644 common/vehicle_constants_manager/CMakeLists.txt create mode 100644 common/vehicle_constants_manager/design/vehicle_constants_manager-design.md create mode 100644 common/vehicle_constants_manager/include/vehicle_constants_manager/vehicle_constants_manager.hpp create mode 100644 common/vehicle_constants_manager/include/vehicle_constants_manager/visibility_control.hpp create mode 100644 common/vehicle_constants_manager/package.xml create mode 100644 common/vehicle_constants_manager/param/params_lexus_rx_hybrid_2016.yaml create mode 100644 common/vehicle_constants_manager/src/vehicle_constants_manager.cpp create mode 100644 common/vehicle_constants_manager/test/test_vehicle_constants_manager.cpp diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 7e8233e8aeae2..85a9e63eacafc 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -90,4 +90,4 @@ repos: args: [--quiet] exclude: .cu -exclude: .svg|control/trajectory_follower|control/trajectory_follower_nodes|common/autoware_auto_dependencies|common/autoware_auto_perception_rviz_plugin|common/osqp_interface|simulator/simple_planning_simulator|planning/freespace_planner|planning/astar_search|planning/costmap_generator +exclude: .svg|control/trajectory_follower|control/trajectory_follower_nodes|common/autoware_auto_cmake|common/autoware_auto_common|common/autoware_auto_geometry|common/autoware_auto_tf2|common/autoware_testing|common/fake_test_node|common/had_map_utils|common/motion_common|common/motion_testing|common/time_utils|common/vehicle_constants_manager|common/autoware_auto_perception_rviz_plugin|common/osqp_interface|simulator/simple_planning_simulator|planning/freespace_planner|planning/astar_search|planning/costmap_generator diff --git a/build_depends.repos b/build_depends.repos index 51917f0b47b8b..c7f979549889b 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -7,10 +7,6 @@ repositories: type: git url: https://github.com/tier4/tier4_ament_lint version: main - common/autoware_auto_dependencies: - type: git - url: https://github.com/tier4/AutowareArchitectureProposal.iv.git - version: only-autoware-auto-dependencies messages/autoware_auto_msgs: type: git url: https://github.com/tier4/autoware_auto_msgs.git diff --git a/common/autoware_auto_cmake/CMakeLists.txt b/common/autoware_auto_cmake/CMakeLists.txt new file mode 100644 index 0000000000000..7fa136c91f9b7 --- /dev/null +++ b/common/autoware_auto_cmake/CMakeLists.txt @@ -0,0 +1,33 @@ +# Copyright 2018 the Autoware Foundation +# +# 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. +# +# Co-developed by Tier IV, Inc. and Apex.AI, Inc. +cmake_minimum_required(VERSION 3.5) + +project(autoware_auto_cmake NONE) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +list(APPEND ${PROJECT_NAME}_CONFIG_EXTRAS + "autoware_auto_cmake-extras.cmake" +) + +if(BUILD_TESTING) + ament_lint_cmake(${CMAKE_CURRENT_SOURCE_DIR}) +endif() + +ament_auto_package(INSTALL_TO_SHARE + cmake +) diff --git a/common/autoware_auto_cmake/autoware_auto_cmake-extras.cmake b/common/autoware_auto_cmake/autoware_auto_cmake-extras.cmake new file mode 100644 index 0000000000000..4f8308d038fcc --- /dev/null +++ b/common/autoware_auto_cmake/autoware_auto_cmake-extras.cmake @@ -0,0 +1,16 @@ +# Copyright 2018 the Autoware Foundation +# Co-developed by Tier IV, Inc. and Apex.AI, 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_auto_cmake_DIR}/autoware_auto_cmake.cmake") diff --git a/common/autoware_auto_cmake/cmake/autoware_auto_cmake.cmake b/common/autoware_auto_cmake/cmake/autoware_auto_cmake.cmake new file mode 100644 index 0000000000000..58a96fae973a7 --- /dev/null +++ b/common/autoware_auto_cmake/cmake/autoware_auto_cmake.cmake @@ -0,0 +1,111 @@ +# Copyright 2021 the Autoware Foundation +# Co-developed by Tier IV, Inc. and Apex.AI, 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. + +# Set lanuage standard with variables instead of per target so even targets that +# don't use autoware_set_compile_options() have this set when importing +# autoware_auto_cmake via ament_auto_find_build_dependencies() +if(NOT CMAKE_C_STANDARD) + # Default to C11 (rcutils uses C11 thread local storage) + set(CMAKE_C_STANDARD 11) + set(CMAKE_C_STANDARD_REQUIRED ON) + set(CMAKE_C_EXTENSIONS OFF) +endif() + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +# Get user-provided variables +set(DOWNLOAD_ARTIFACTS OFF CACHE BOOL "enable artifact download") + +# Clang tidy +if(TIDY_WITH_CLANG) + string(CONCAT CMAKE_CXX_CLANG_TIDY + "clang-tidy;" + "-checks=-*," + "bugprone-*," + "cert-*," + "cppcoreguidelines-*," + "clang-analyze-*," + "google-*," + "hicpp-*," + "modernize-*," + "performance-*," + "readability-*") +endif() + +# Try to adhere to strict ISO C++ as much as possible: +# from https://lefticus.gitbooks.io/cpp-best-practices/content/02-Use_the_Tools_Available.html +function(autoware_set_compile_options target) + + if(CMAKE_CXX_COMPILER_ID MATCHES "MSVC") + # Causes the visibility macros to use dllexport rather than dllimport, + # which is appropriate when building the dll but not consuming it. + string(TOUPPER ${target} PROJECT_NAME_UPPER) + target_compile_options(${target} PRIVATE "/bigobj") + target_compile_definitions(${target} PRIVATE + ${PROJECT_NAME_UPPER}_BUILDING_DLL + -D_CRT_NONSTDC_NO_WARNINGS + -D_CRT_SECURE_NO_WARNINGS + -D_WINSOCK_DEPRECATED_NO_WARNINGS) + endif() + + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + target_compile_options(${target} PRIVATE + -Wall + -Werror + -Wextra + #-Wshadow # causes issues with ROS 2 headers + #-Wnon-virtual-dtor # causes issues with ROS 2 headers + -pedantic + -Wcast-align + -Wunused + -Wconversion + -Wsign-conversion + -Wdouble-promotion + -Waddress + #-Wnull-dereference # gcc6 + #-Wduplicated-branches # gcc7 + #-Wduplicated-cond # gcc6 + #-Wrestrict # gcc7 + -fvisibility=hidden) + # C++-only options + target_compile_options(${target} + PRIVATE $<$: -Woverloaded-virtual -Wold-style-cast>) + endif() + + if(CMAKE_COMPILER_IS_GNUCXX) + target_compile_options(${target} + PUBLIC $<$: -Wuseless-cast>) + target_compile_options(${target} PRIVATE -Wlogical-op -frecord-gcc-switches) + endif() + +endfunction() + +# Turn off optimization compiler flags for `target` unless the variable `AUTOWARE_OPTIMIZATION_OF_SLOW_TARGETS` is +# defined and evaluates to `true` in a boolean context. +function(autoware_turn_off_optimization target) + + if(AUTOWARE_OPTIMIZATION_OF_SLOW_TARGETS) + # do nothing + else() + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + target_compile_options(${target} PRIVATE -O0) + endif() + endif() + +endfunction() diff --git a/common/autoware_auto_cmake/design/autoware_auto_cmake-design.md b/common/autoware_auto_cmake/design/autoware_auto_cmake-design.md new file mode 100644 index 0000000000000..62c82d6dd7db2 --- /dev/null +++ b/common/autoware_auto_cmake/design/autoware_auto_cmake-design.md @@ -0,0 +1,29 @@ +autoware_auto_cmake {#autoware-auto-cmake-design} +=========== + +This is the design document for the `autoware_auto_cmake` package. + + +# Purpose + +Provide common CMake variables and functions to Autoware packages. + +Those include: + +- Setting the language standard +- Getting user-provided variables +- Providing functions to: + + set compiler flags + + turn off optimizations + +# Design + +## Usage + +Add `autoware_auto_cmake` as a "build_depend" in the dependent packages. + +### CMake variables {#cmake-config-variables} + +|Name|Type|Descritpion|Default| +|----|----|-----------|-------| +|`DOWNLOAD_ARTIFACTS`|*BOOL*|Allow downloading artifacts at build time.|`OFF`| diff --git a/common/autoware_auto_cmake/package.xml b/common/autoware_auto_cmake/package.xml new file mode 100644 index 0000000000000..6d8163eae6018 --- /dev/null +++ b/common/autoware_auto_cmake/package.xml @@ -0,0 +1,29 @@ + + + + autoware_auto_cmake + 1.0.0 + Import the compiler settings defined for Autoware.Auto + Apex.AI, Inc. + Apache 2.0 + + ament_cmake_auto + ament_cmake_lint_cmake + + ros_environment + + ament_cmake_core + ament_cmake_lint_cmake + ament_cmake_copyright + ament_cmake_cppcheck + ament_cmake_cpplint + ament_cmake_uncrustify + + ament_cmake_lint_cmake + + + + + ament_cmake + + diff --git a/common/autoware_auto_common/CMakeLists.txt b/common/autoware_auto_common/CMakeLists.txt new file mode 100644 index 0000000000000..acfa3cbb7ef08 --- /dev/null +++ b/common/autoware_auto_common/CMakeLists.txt @@ -0,0 +1,75 @@ +# Copyright 2019 the Autoware Foundation +# Co-developed by Tier IV, Inc. and Apex.AI, 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. +# +# Co-developed by Tier IV, Inc. and Apex.AI, Inc. +cmake_minimum_required(VERSION 3.5) + +### Export headers +project(autoware_auto_common) + +## dependencies +find_package(ament_cmake_auto REQUIRED) +find_package(Eigen3 REQUIRED) +ament_auto_find_build_dependencies() + +include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # Temporarily disable cpplint and uncrustify + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_cpplint + ament_cmake_uncrustify + ) + + ament_lint_auto_find_test_dependencies() + + file(GLOB_RECURSE FILES_MINUS_SOME + ${CMAKE_CURRENT_SOURCE_DIR}/include/* + ${CMAKE_CURRENT_SOURCE_DIR}/test/* + ) + + list(FILTER FILES_MINUS_SOME EXCLUDE REGEX ".*filesystem.hpp") + + # Re-enable cpplint + find_package(ament_cmake_cpplint) + ament_cpplint(${FILES_MINUS_SOME}) + + # Re-enable uncrustify + find_package(ament_cmake_uncrustify) + ament_uncrustify(${FILES_MINUS_SOME}) + + # Unit tests + set(TEST_COMMON test_common_gtest) + ament_add_gtest(${TEST_COMMON} + test/gtest_main.cpp + test/test_bool_comparisons.cpp + test/test_byte_reader.cpp + test/test_float_comparisons.cpp + test/test_mahalanobis_distance.cpp + test/test_message_field_adapters.cpp + test/test_template_utils.cpp + test/test_angle_utils.cpp + test/test_type_name.cpp + test/test_type_traits.cpp) + autoware_set_compile_options(${TEST_COMMON}) + target_compile_options(${TEST_COMMON} PRIVATE -Wno-sign-conversion) + target_include_directories(${TEST_COMMON} PRIVATE include) + ament_target_dependencies(${TEST_COMMON} builtin_interfaces Eigen3) +endif() + +# Ament Exporting +ament_auto_package() diff --git a/common/autoware_auto_common/design/comparisons.md b/common/autoware_auto_common/design/comparisons.md new file mode 100644 index 0000000000000..6e1d2eb9db35e --- /dev/null +++ b/common/autoware_auto_common/design/comparisons.md @@ -0,0 +1,66 @@ +Comparisons {#helper-comparisons} +=========== + +The `float_comparisons.hpp` library is a simple set of functions for performing approximate numerical comparisons. +There are separate functions for performing comparisons using absolute bounds and relative bounds. Absolute comparison checks are prefixed with `abs_` and relative checks are prefixed with `rel_`. + +The `bool_comparisons.hpp` library additionally contains an XOR operator. + +The intent of the library is to improve readability of code and reduce likelihood of typographical errors when using numerical and boolean comparisons. + +# Target use cases + +The approximate comparisons are intended to be used to check whether two numbers lie within some absolute or relative interval. +The `exclusive_or` function will test whether two values cast to different boolean values. + +# Assumptions + +* The approximate comparisons all take an `epsilon` parameter. +The value of this parameter must be >= 0. +* The library is only intended to be used with floating point types. +A static assertion will be thrown if the library is used with a non-floating point type. + +# Example Usage + +```c++ +#include "common/bool_comparisons.hpp" +#include "common/float_comparisons.hpp" + +#include + +// using-directive is just for illustration; don't do this in practice +using namespace autoware::common::helper_functions::comparisons; + +static constexpr auto epsilon = 0.2; +static constexpr auto relative_epsilon = 0.01; + +std::cout << exclusive_or(true, false) << "\n"; +// Prints: true + +std::cout << rel_eq(1.0, 1.1, relative_epsilon)) << "\n"; +// Prints: false + +std::cout << approx_eq(10000.0, 10010.0, epsilon, relative_epsilon)) << "\n"; +// Prints: true + +std::cout << abs_eq(4.0, 4.2, epsilon) << "\n"; +// Prints: true + +std::cout << abs_ne(4.0, 4.2, epsilon) << "\n"; +// Prints: false + +std::cout << abs_eq_zero(0.2, epsilon) << "\n"; +// Prints: false + +std::cout << abs_lt(4.0, 4.25, epsilon) << "\n"; +// Prints: true + +std::cout << abs_lte(1.0, 1.2, epsilon) << "\n"; +// Prints: true + +std::cout << abs_gt(1.25, 1.0, epsilon) << "\n"; +// Prints: true + +std::cout << abs_gte(0.75, 1.0, epsilon) << "\n"; +// Prints: false +``` diff --git a/common/autoware_auto_common/include/common/filesystem.hpp b/common/autoware_auto_common/include/common/filesystem.hpp new file mode 100644 index 0000000000000..7fe16d8df8084 --- /dev/null +++ b/common/autoware_auto_common/include/common/filesystem.hpp @@ -0,0 +1,5558 @@ +//--------------------------------------------------------------------------------------- +// +// ghc::filesystem - A C++17-like filesystem implementation for C++11/C++14/C++17 +// +//--------------------------------------------------------------------------------------- +// +// Copyright (c) 2018, Steffen Schümann +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +//--------------------------------------------------------------------------------------- +// +// To dynamically select std::filesystem where available, you could use: +// +// #if defined(__cplusplus) && __cplusplus >= 201703L && defined(__has_include) && __has_include() +// #include +// namespace fs = std::filesystem; +// #else +// #include +// namespace fs = ghc::filesystem; +// #endif +// +//--------------------------------------------------------------------------------------- +#ifndef GHC_FILESYSTEM_H +#define GHC_FILESYSTEM_H + +// #define BSD manifest constant only in +// sys/param.h +#ifndef _WIN32 +#include +#endif + +#ifndef GHC_OS_DETECTED +#if defined(__APPLE__) && defined(__MACH__) +#define GHC_OS_MACOS +#elif defined(__linux__) +#define GHC_OS_LINUX +#if defined(__ANDROID__) +#define GHC_OS_ANDROID +#endif +#elif defined(_WIN64) +#define GHC_OS_WINDOWS +#define GHC_OS_WIN64 +#elif defined(_WIN32) +#define GHC_OS_WINDOWS +#define GHC_OS_WIN32 +#elif defined(__svr4__) +#define GHC_OS_SYS5R4 +#elif defined(BSD) +#define GHC_OS_BSD +#elif defined(__EMSCRIPTEN__) +#define GHC_OS_WEB +#include +#else +#error "Operating system currently not supported!" +#endif +#define GHC_OS_DETECTED +#endif + +#if defined(GHC_FILESYSTEM_IMPLEMENTATION) +#define GHC_EXPAND_IMPL +#define GHC_INLINE +#ifdef GHC_OS_WINDOWS +#define GHC_FS_API +#define GHC_FS_API_CLASS +#else +#define GHC_FS_API __attribute__((visibility("default"))) +#define GHC_FS_API_CLASS __attribute__((visibility("default"))) +#endif +#elif defined(GHC_FILESYSTEM_FWD) +#define GHC_INLINE +#ifdef GHC_OS_WINDOWS +#define GHC_FS_API extern +#define GHC_FS_API_CLASS +#else +#define GHC_FS_API extern +#define GHC_FS_API_CLASS +#endif +#else +#define GHC_EXPAND_IMPL +#define GHC_INLINE inline +#define GHC_FS_API +#define GHC_FS_API_CLASS +#endif + +#ifdef GHC_EXPAND_IMPL + +#ifdef GHC_OS_WINDOWS +#include +// additional includes +#include +#include +#include +#include +#include +#else +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef GHC_OS_ANDROID +#include +#if __ANDROID_API__ < 12 +#include +#endif +#include +#define statvfs statfs +#else +#include +#endif +#if !defined(__ANDROID__) || __ANDROID_API__ >= 26 +#include +#endif +#endif +#ifdef GHC_OS_MACOS +#include +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#else // GHC_EXPAND_IMPL +#include +#include +#include +#include +#include +#include +#include +#ifdef GHC_OS_WINDOWS +#include +#endif +#endif // GHC_EXPAND_IMPL + +//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +// Behaviour Switches (see README.md, should match the config in test/filesystem_test.cpp): +//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +// LWG #2682 disables the since then invalid use of the copy option create_symlinks on directories +// configure LWG conformance () +#define LWG_2682_BEHAVIOUR +//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +// LWG #2395 makes crate_directory/create_directories not emit an error if there is a regular +// file with that name, it is superceded by P1164R1, so only activate if really needed +// #define LWG_2935_BEHAVIOUR +//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +// LWG #2937 enforces that fs::equivalent emits an error, if !fs::exists(p1)||!exists(p2) +#define LWG_2937_BEHAVIOUR +//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +// UTF8-Everywhere is the original behaviour of ghc::filesystem. With this define you can +// enable the more standard conforming implementation option that uses wstring on Windows +// as ghc::filesystem::string_type. +// #define GHC_WIN_WSTRING_STRING_TYPE +//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +// Raise errors/exceptions when invalid unicode codepoints or UTF-8 sequences are found, +// instead of replacing them with the unicode replacement character (U+FFFD). +// #define GHC_RAISE_UNICODE_ERRORS +//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + +// ghc::filesystem version in decimal (major * 10000 + minor * 100 + patch) +#define GHC_FILESYSTEM_VERSION 10304L + +#if !defined(GHC_WITH_EXCEPTIONS) && (defined(__EXCEPTIONS) || defined(__cpp_exceptions) || defined(_CPPUNWIND)) +#define GHC_WITH_EXCEPTIONS +#endif +#if !defined(GHC_WITH_EXCEPTIONS) && defined(GHC_RAISE_UNICODE_ERRORS) +#error "Can't raise unicode errors whith exception support disabled" +#endif + +namespace ghc { +namespace filesystem { + +// temporary existing exception type for yet unimplemented parts +class GHC_FS_API_CLASS not_implemented_exception : public std::logic_error +{ +public: + not_implemented_exception() + : std::logic_error("function not implemented yet.") + { + } +}; + +template +class path_helper_base +{ +public: + using value_type = char_type; +#ifdef GHC_OS_WINDOWS + static constexpr value_type preferred_separator = '\\'; +#else + static constexpr value_type preferred_separator = '/'; +#endif +}; + +#if __cplusplus < 201703L +template +constexpr char_type path_helper_base::preferred_separator; +#endif + +// 30.10.8 class path +class GHC_FS_API_CLASS path +#if defined(GHC_OS_WINDOWS) && defined(GHC_WIN_WSTRING_STRING_TYPE) +#define GHC_USE_WCHAR_T + : private path_helper_base +{ +public: + using path_helper_base::value_type; +#else + : private path_helper_base +{ +public: + using path_helper_base::value_type; +#endif + using string_type = std::basic_string; + using path_helper_base::preferred_separator; + + // 30.10.10.1 enumeration format + /// The path format in wich the constructor argument is given. + enum format { + generic_format, ///< The generic format, internally used by + ///< ghc::filesystem::path with slashes + native_format, ///< The format native to the current platform this code + ///< is build for + auto_format, ///< Try to auto-detect the format, fallback to native + }; + + template + struct _is_basic_string : std::false_type + { + }; + template + struct _is_basic_string> : std::true_type + { + }; +#ifdef __cpp_lib_string_view + template + struct _is_basic_string> : std::true_type + { + }; +#endif + + template + using path_type = typename std::enable_if::value, path>::type; +#ifdef GHC_USE_WCHAR_T + template + using path_from_string = typename std::enable_if<_is_basic_string::value || std::is_same::type>::value || std::is_same::type>::value || + std::is_same::type>::value || std::is_same::type>::value, + path>::type; + template + using path_type_EcharT = typename std::enable_if::value || std::is_same::value || std::is_same::value, path>::type; +#else + template + using path_from_string = typename std::enable_if<_is_basic_string::value || std::is_same::type>::value || std::is_same::type>::value, path>::type; + template + using path_type_EcharT = typename std::enable_if::value || std::is_same::value || std::is_same::value || std::is_same::value, path>::type; +#endif + // 30.10.8.4.1 constructors and destructor + path() noexcept; + path(const path& p); + path(path&& p) noexcept; + path(string_type&& source, format fmt = auto_format); + template > + path(const Source& source, format fmt = auto_format); + template + path(InputIterator first, InputIterator last, format fmt = auto_format); +#ifdef GHC_WITH_EXCEPTIONS + template > + path(const Source& source, const std::locale& loc, format fmt = auto_format); + template + path(InputIterator first, InputIterator last, const std::locale& loc, format fmt = auto_format); +#endif + ~path(); + + // 30.10.8.4.2 assignments + path& operator=(const path& p); + path& operator=(path&& p) noexcept; + path& operator=(string_type&& source); + path& assign(string_type&& source); + template + path& operator=(const Source& source); + template + path& assign(const Source& source); + template + path& assign(InputIterator first, InputIterator last); + + // 30.10.8.4.3 appends + path& operator/=(const path& p); + template + path& operator/=(const Source& source); + template + path& append(const Source& source); + template + path& append(InputIterator first, InputIterator last); + + // 30.10.8.4.4 concatenation + path& operator+=(const path& x); + path& operator+=(const string_type& x); +#ifdef __cpp_lib_string_view + path& operator+=(std::basic_string_view x); +#endif + path& operator+=(const value_type* x); + path& operator+=(value_type x); + template + path_from_string& operator+=(const Source& x); + template + path_type_EcharT& operator+=(EcharT x); + template + path& concat(const Source& x); + template + path& concat(InputIterator first, InputIterator last); + + // 30.10.8.4.5 modifiers + void clear() noexcept; + path& make_preferred(); + path& remove_filename(); + path& replace_filename(const path& replacement); + path& replace_extension(const path& replacement = path()); + void swap(path& rhs) noexcept; + + // 30.10.8.4.6 native format observers + const string_type& native() const; // this implementation doesn't support noexcept for native() + const value_type* c_str() const; // this implementation doesn't support noexcept for c_str() + operator string_type() const; + template , class Allocator = std::allocator> + std::basic_string string(const Allocator& a = Allocator()) const; + std::string string() const; + std::wstring wstring() const; + std::string u8string() const; + std::u16string u16string() const; + std::u32string u32string() const; + + // 30.10.8.4.7 generic format observers + template , class Allocator = std::allocator> + std::basic_string generic_string(const Allocator& a = Allocator()) const; + const std::string& generic_string() const; // this is different from the standard, that returns by value + std::wstring generic_wstring() const; + std::string generic_u8string() const; + std::u16string generic_u16string() const; + std::u32string generic_u32string() const; + + // 30.10.8.4.8 compare + int compare(const path& p) const noexcept; + int compare(const string_type& s) const; +#ifdef __cpp_lib_string_view + int compare(std::basic_string_view s) const; +#endif + int compare(const value_type* s) const; + + // 30.10.8.4.9 decomposition + path root_name() const; + path root_directory() const; + path root_path() const; + path relative_path() const; + path parent_path() const; + path filename() const; + path stem() const; + path extension() const; + + // 30.10.8.4.10 query + bool empty() const noexcept; + bool has_root_name() const; + bool has_root_directory() const; + bool has_root_path() const; + bool has_relative_path() const; + bool has_parent_path() const; + bool has_filename() const; + bool has_stem() const; + bool has_extension() const; + bool is_absolute() const; + bool is_relative() const; + + // 30.10.8.4.11 generation + path lexically_normal() const; + path lexically_relative(const path& base) const; + path lexically_proximate(const path& base) const; + + // 30.10.8.5 iterators + class iterator; + using const_iterator = iterator; + iterator begin() const; + iterator end() const; + +private: + using impl_value_type = std::string::value_type; + using impl_string_type = std::basic_string; + friend class directory_iterator; + void append_name(const char* name); + static constexpr impl_value_type generic_separator = '/'; + template + class input_iterator_range + { + public: + typedef InputIterator iterator; + typedef InputIterator const_iterator; + typedef typename InputIterator::difference_type difference_type; + + input_iterator_range(const InputIterator& first, const InputIterator& last) + : _first(first) + , _last(last) + { + } + + InputIterator begin() const { return _first; } + InputIterator end() const { return _last; } + + private: + InputIterator _first; + InputIterator _last; + }; + friend void swap(path& lhs, path& rhs) noexcept; + friend size_t hash_value(const path& p) noexcept; + static void postprocess_path_with_format(impl_string_type& p, format fmt); + impl_string_type _path; +#ifdef GHC_OS_WINDOWS + impl_string_type native_impl() const; + mutable string_type _native_cache; +#else + const impl_string_type& native_impl() const; +#endif +}; + +// 30.10.8.6 path non-member functions +GHC_FS_API void swap(path& lhs, path& rhs) noexcept; +GHC_FS_API size_t hash_value(const path& p) noexcept; +GHC_FS_API bool operator==(const path& lhs, const path& rhs) noexcept; +GHC_FS_API bool operator!=(const path& lhs, const path& rhs) noexcept; +GHC_FS_API bool operator<(const path& lhs, const path& rhs) noexcept; +GHC_FS_API bool operator<=(const path& lhs, const path& rhs) noexcept; +GHC_FS_API bool operator>(const path& lhs, const path& rhs) noexcept; +GHC_FS_API bool operator>=(const path& lhs, const path& rhs) noexcept; + +GHC_FS_API path operator/(const path& lhs, const path& rhs); + +// 30.10.8.6.1 path inserter and extractor +template +std::basic_ostream& operator<<(std::basic_ostream& os, const path& p); +template +std::basic_istream& operator>>(std::basic_istream& is, path& p); + +// 30.10.8.6.2 path factory functions +template > +path u8path(const Source& source); +template +path u8path(InputIterator first, InputIterator last); + +// 30.10.9 class filesystem_error +class GHC_FS_API_CLASS filesystem_error : public std::system_error +{ +public: + filesystem_error(const std::string& what_arg, std::error_code ec); + filesystem_error(const std::string& what_arg, const path& p1, std::error_code ec); + filesystem_error(const std::string& what_arg, const path& p1, const path& p2, std::error_code ec); + const path& path1() const noexcept; + const path& path2() const noexcept; + const char* what() const noexcept override; + +private: + std::string _what_arg; + std::error_code _ec; + path _p1, _p2; +}; + +class GHC_FS_API_CLASS path::iterator +{ +public: + using value_type = const path; + using difference_type = std::ptrdiff_t; + using pointer = const path*; + using reference = const path&; + using iterator_category = std::bidirectional_iterator_tag; + + iterator(); + iterator(const impl_string_type::const_iterator& first, const impl_string_type::const_iterator& last, const impl_string_type::const_iterator& pos); + iterator& operator++(); + iterator operator++(int); + iterator& operator--(); + iterator operator--(int); + bool operator==(const iterator& other) const; + bool operator!=(const iterator& other) const; + reference operator*() const; + pointer operator->() const; + +private: + impl_string_type::const_iterator increment(const std::string::const_iterator& pos) const; + impl_string_type::const_iterator decrement(const std::string::const_iterator& pos) const; + void updateCurrent(); + impl_string_type::const_iterator _first; + impl_string_type::const_iterator _last; + impl_string_type::const_iterator _root; + impl_string_type::const_iterator _iter; + path _current; +}; + +struct space_info +{ + uintmax_t capacity; + uintmax_t free; + uintmax_t available; +}; + +// 30.10.10, enumerations +enum class file_type { + none, + not_found, + regular, + directory, + symlink, + block, + character, + fifo, + socket, + unknown, +}; + +enum class perms : uint16_t { + none = 0, + + owner_read = 0400, + owner_write = 0200, + owner_exec = 0100, + owner_all = 0700, + + group_read = 040, + group_write = 020, + group_exec = 010, + group_all = 070, + + others_read = 04, + others_write = 02, + others_exec = 01, + others_all = 07, + + all = 0777, + set_uid = 04000, + set_gid = 02000, + sticky_bit = 01000, + + mask = 07777, + unknown = 0xffff +}; + +enum class perm_options : uint16_t { + replace = 3, + add = 1, + remove = 2, + nofollow = 4, +}; + +enum class copy_options : uint16_t { + none = 0, + + skip_existing = 1, + overwrite_existing = 2, + update_existing = 4, + + recursive = 8, + + copy_symlinks = 0x10, + skip_symlinks = 0x20, + + directories_only = 0x40, + create_symlinks = 0x80, +#ifndef GHC_OS_WEB + create_hard_links = 0x100 +#endif +}; + +enum class directory_options : uint16_t { + none = 0, + follow_directory_symlink = 1, + skip_permission_denied = 2, +}; + +// 30.10.11 class file_status +class GHC_FS_API_CLASS file_status +{ +public: + // 30.10.11.1 constructors and destructor + file_status() noexcept; + explicit file_status(file_type ft, perms prms = perms::unknown) noexcept; + file_status(const file_status&) noexcept; + file_status(file_status&&) noexcept; + ~file_status(); + // assignments: + file_status& operator=(const file_status&) noexcept; + file_status& operator=(file_status&&) noexcept; + // 30.10.11.3 modifiers + void type(file_type ft) noexcept; + void permissions(perms prms) noexcept; + // 30.10.11.2 observers + file_type type() const noexcept; + perms permissions() const noexcept; + +private: + file_type _type; + perms _perms; +}; + +using file_time_type = std::chrono::time_point; + +// 30.10.12 Class directory_entry +class GHC_FS_API_CLASS directory_entry +{ +public: + // 30.10.12.1 constructors and destructor + directory_entry() noexcept = default; + directory_entry(const directory_entry&) = default; + directory_entry(directory_entry&&) noexcept = default; +#ifdef GHC_WITH_EXCEPTIONS + explicit directory_entry(const path& p); +#endif + directory_entry(const path& p, std::error_code& ec); + ~directory_entry(); + + // assignments: + directory_entry& operator=(const directory_entry&) = default; + directory_entry& operator=(directory_entry&&) noexcept = default; + + // 30.10.12.2 modifiers +#ifdef GHC_WITH_EXCEPTIONS + void assign(const path& p); +#endif + void assign(const path& p, std::error_code& ec); +#ifdef GHC_WITH_EXCEPTIONS + void replace_filename(const path& p); +#endif + void replace_filename(const path& p, std::error_code& ec); +#ifdef GHC_WITH_EXCEPTIONS + void refresh(); +#endif + void refresh(std::error_code& ec) noexcept; + + // 30.10.12.3 observers + const filesystem::path& path() const noexcept; + operator const filesystem::path&() const noexcept; +#ifdef GHC_WITH_EXCEPTIONS + bool exists() const; +#endif + bool exists(std::error_code& ec) const noexcept; +#ifdef GHC_WITH_EXCEPTIONS + bool is_block_file() const; +#endif + bool is_block_file(std::error_code& ec) const noexcept; +#ifdef GHC_WITH_EXCEPTIONS + bool is_character_file() const; +#endif + bool is_character_file(std::error_code& ec) const noexcept; +#ifdef GHC_WITH_EXCEPTIONS + bool is_directory() const; +#endif + bool is_directory(std::error_code& ec) const noexcept; +#ifdef GHC_WITH_EXCEPTIONS + bool is_fifo() const; +#endif + bool is_fifo(std::error_code& ec) const noexcept; +#ifdef GHC_WITH_EXCEPTIONS + bool is_other() const; +#endif + bool is_other(std::error_code& ec) const noexcept; +#ifdef GHC_WITH_EXCEPTIONS + bool is_regular_file() const; +#endif + bool is_regular_file(std::error_code& ec) const noexcept; +#ifdef GHC_WITH_EXCEPTIONS + bool is_socket() const; +#endif + bool is_socket(std::error_code& ec) const noexcept; +#ifdef GHC_WITH_EXCEPTIONS + bool is_symlink() const; +#endif + bool is_symlink(std::error_code& ec) const noexcept; +#ifdef GHC_WITH_EXCEPTIONS + uintmax_t file_size() const; +#endif + uintmax_t file_size(std::error_code& ec) const noexcept; + +#ifndef GHC_OS_WEB +#ifdef GHC_WITH_EXCEPTIONS + uintmax_t hard_link_count() const; +#endif + uintmax_t hard_link_count(std::error_code& ec) const noexcept; +#endif + +#ifdef GHC_WITH_EXCEPTIONS + file_time_type last_write_time() const; +#endif + file_time_type last_write_time(std::error_code& ec) const noexcept; + +#ifdef GHC_WITH_EXCEPTIONS + file_status status() const; +#endif + file_status status(std::error_code& ec) const noexcept; + +#ifdef GHC_WITH_EXCEPTIONS + file_status symlink_status() const; +#endif + file_status symlink_status(std::error_code& ec) const noexcept; + bool operator<(const directory_entry& rhs) const noexcept; + bool operator==(const directory_entry& rhs) const noexcept; + bool operator!=(const directory_entry& rhs) const noexcept; + bool operator<=(const directory_entry& rhs) const noexcept; + bool operator>(const directory_entry& rhs) const noexcept; + bool operator>=(const directory_entry& rhs) const noexcept; + +private: + friend class directory_iterator; + filesystem::path _path; + file_status _status; + file_status _symlink_status; + uintmax_t _file_size = 0; +#ifndef GHC_OS_WINDOWS + uintmax_t _hard_link_count = 0; +#endif + time_t _last_write_time = 0; +}; + +// 30.10.13 Class directory_iterator +class GHC_FS_API_CLASS directory_iterator +{ +public: + class GHC_FS_API_CLASS proxy + { + public: + const directory_entry& operator*() const& noexcept { return _dir_entry; } + directory_entry operator*() && noexcept { return std::move(_dir_entry); } + + private: + explicit proxy(const directory_entry& dir_entry) + : _dir_entry(dir_entry) + { + } + friend class directory_iterator; + friend class recursive_directory_iterator; + directory_entry _dir_entry; + }; + using iterator_category = std::input_iterator_tag; + using value_type = directory_entry; + using difference_type = std::ptrdiff_t; + using pointer = const directory_entry*; + using reference = const directory_entry&; + + // 30.10.13.1 member functions + directory_iterator() noexcept; +#ifdef GHC_WITH_EXCEPTIONS + explicit directory_iterator(const path& p); + directory_iterator(const path& p, directory_options options); +#endif + directory_iterator(const path& p, std::error_code& ec) noexcept; + directory_iterator(const path& p, directory_options options, std::error_code& ec) noexcept; + directory_iterator(const directory_iterator& rhs); + directory_iterator(directory_iterator&& rhs) noexcept; + ~directory_iterator(); + directory_iterator& operator=(const directory_iterator& rhs); + directory_iterator& operator=(directory_iterator&& rhs) noexcept; + const directory_entry& operator*() const; + const directory_entry* operator->() const; +#ifdef GHC_WITH_EXCEPTIONS + directory_iterator& operator++(); +#endif + directory_iterator& increment(std::error_code& ec) noexcept; + + // other members as required by 27.2.3, input iterators +#ifdef GHC_WITH_EXCEPTIONS + proxy operator++(int) + { + proxy p{**this}; + ++*this; + return p; + } +#endif + bool operator==(const directory_iterator& rhs) const; + bool operator!=(const directory_iterator& rhs) const; + +private: + friend class recursive_directory_iterator; + class impl; + std::shared_ptr _impl; +}; + +// 30.10.13.2 directory_iterator non-member functions +GHC_FS_API directory_iterator begin(directory_iterator iter) noexcept; +GHC_FS_API directory_iterator end(const directory_iterator&) noexcept; + +// 30.10.14 class recursive_directory_iterator +class GHC_FS_API_CLASS recursive_directory_iterator +{ +public: + using iterator_category = std::input_iterator_tag; + using value_type = directory_entry; + using difference_type = std::ptrdiff_t; + using pointer = const directory_entry*; + using reference = const directory_entry&; + + // 30.10.14.1 constructors and destructor + recursive_directory_iterator() noexcept; +#ifdef GHC_WITH_EXCEPTIONS + explicit recursive_directory_iterator(const path& p); + recursive_directory_iterator(const path& p, directory_options options); +#endif + recursive_directory_iterator(const path& p, directory_options options, std::error_code& ec) noexcept; + recursive_directory_iterator(const path& p, std::error_code& ec) noexcept; + recursive_directory_iterator(const recursive_directory_iterator& rhs); + recursive_directory_iterator(recursive_directory_iterator&& rhs) noexcept; + ~recursive_directory_iterator(); + + // 30.10.14.1 observers + directory_options options() const; + int depth() const; + bool recursion_pending() const; + + const directory_entry& operator*() const; + const directory_entry* operator->() const; + + // 30.10.14.1 modifiers recursive_directory_iterator& + recursive_directory_iterator& operator=(const recursive_directory_iterator& rhs); + recursive_directory_iterator& operator=(recursive_directory_iterator&& rhs) noexcept; +#ifdef GHC_WITH_EXCEPTIONS + recursive_directory_iterator& operator++(); +#endif + recursive_directory_iterator& increment(std::error_code& ec) noexcept; + +#ifdef GHC_WITH_EXCEPTIONS + void pop(); +#endif + void pop(std::error_code& ec); + void disable_recursion_pending(); + + // other members as required by 27.2.3, input iterators +#ifdef GHC_WITH_EXCEPTIONS + directory_iterator::proxy operator++(int) + { + directory_iterator::proxy proxy{**this}; + ++*this; + return proxy; + } +#endif + bool operator==(const recursive_directory_iterator& rhs) const; + bool operator!=(const recursive_directory_iterator& rhs) const; + +private: + struct recursive_directory_iterator_impl + { + directory_options _options; + bool _recursion_pending; + std::stack _dir_iter_stack; + recursive_directory_iterator_impl(directory_options options, bool recursion_pending) + : _options(options) + , _recursion_pending(recursion_pending) + { + } + }; + std::shared_ptr _impl; +}; + +// 30.10.14.2 directory_iterator non-member functions +GHC_FS_API recursive_directory_iterator begin(recursive_directory_iterator iter) noexcept; +GHC_FS_API recursive_directory_iterator end(const recursive_directory_iterator&) noexcept; + +// 30.10.15 filesystem operations +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API path absolute(const path& p); +#endif +GHC_FS_API path absolute(const path& p, std::error_code& ec); + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API path canonical(const path& p); +#endif +GHC_FS_API path canonical(const path& p, std::error_code& ec); + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API void copy(const path& from, const path& to); +#endif +GHC_FS_API void copy(const path& from, const path& to, std::error_code& ec) noexcept; +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API void copy(const path& from, const path& to, copy_options options); +#endif +GHC_FS_API void copy(const path& from, const path& to, copy_options options, std::error_code& ec) noexcept; + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API bool copy_file(const path& from, const path& to); +#endif +GHC_FS_API bool copy_file(const path& from, const path& to, std::error_code& ec) noexcept; +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API bool copy_file(const path& from, const path& to, copy_options option); +#endif +GHC_FS_API bool copy_file(const path& from, const path& to, copy_options option, std::error_code& ec) noexcept; + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API void copy_symlink(const path& existing_symlink, const path& new_symlink); +#endif +GHC_FS_API void copy_symlink(const path& existing_symlink, const path& new_symlink, std::error_code& ec) noexcept; + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API bool create_directories(const path& p); +#endif +GHC_FS_API bool create_directories(const path& p, std::error_code& ec) noexcept; + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API bool create_directory(const path& p); +#endif +GHC_FS_API bool create_directory(const path& p, std::error_code& ec) noexcept; + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API bool create_directory(const path& p, const path& attributes); +#endif +GHC_FS_API bool create_directory(const path& p, const path& attributes, std::error_code& ec) noexcept; + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API void create_directory_symlink(const path& to, const path& new_symlink); +#endif +GHC_FS_API void create_directory_symlink(const path& to, const path& new_symlink, std::error_code& ec) noexcept; + +#ifndef GHC_OS_WEB +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API void create_hard_link(const path& to, const path& new_hard_link); +#endif +GHC_FS_API void create_hard_link(const path& to, const path& new_hard_link, std::error_code& ec) noexcept; +#endif + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API void create_symlink(const path& to, const path& new_symlink); +#endif +GHC_FS_API void create_symlink(const path& to, const path& new_symlink, std::error_code& ec) noexcept; + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API path current_path(); +#endif +GHC_FS_API path current_path(std::error_code& ec); +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API void current_path(const path& p); +#endif +GHC_FS_API void current_path(const path& p, std::error_code& ec) noexcept; + +GHC_FS_API bool exists(file_status s) noexcept; +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API bool exists(const path& p); +#endif +GHC_FS_API bool exists(const path& p, std::error_code& ec) noexcept; + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API bool equivalent(const path& p1, const path& p2); +#endif +GHC_FS_API bool equivalent(const path& p1, const path& p2, std::error_code& ec) noexcept; + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API uintmax_t file_size(const path& p); +#endif +GHC_FS_API uintmax_t file_size(const path& p, std::error_code& ec) noexcept; + +#ifndef GHC_OS_WEB +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API uintmax_t hard_link_count(const path& p); +#endif +GHC_FS_API uintmax_t hard_link_count(const path& p, std::error_code& ec) noexcept; +#endif + +GHC_FS_API bool is_block_file(file_status s) noexcept; +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API bool is_block_file(const path& p); +#endif +GHC_FS_API bool is_block_file(const path& p, std::error_code& ec) noexcept; +GHC_FS_API bool is_character_file(file_status s) noexcept; +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API bool is_character_file(const path& p); +#endif +GHC_FS_API bool is_character_file(const path& p, std::error_code& ec) noexcept; +GHC_FS_API bool is_directory(file_status s) noexcept; +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API bool is_directory(const path& p); +#endif +GHC_FS_API bool is_directory(const path& p, std::error_code& ec) noexcept; +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API bool is_empty(const path& p); +#endif +GHC_FS_API bool is_empty(const path& p, std::error_code& ec) noexcept; +GHC_FS_API bool is_fifo(file_status s) noexcept; +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API bool is_fifo(const path& p); +#endif +GHC_FS_API bool is_fifo(const path& p, std::error_code& ec) noexcept; +GHC_FS_API bool is_other(file_status s) noexcept; +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API bool is_other(const path& p); +#endif +GHC_FS_API bool is_other(const path& p, std::error_code& ec) noexcept; +GHC_FS_API bool is_regular_file(file_status s) noexcept; +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API bool is_regular_file(const path& p); +#endif +GHC_FS_API bool is_regular_file(const path& p, std::error_code& ec) noexcept; +GHC_FS_API bool is_socket(file_status s) noexcept; +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API bool is_socket(const path& p); +#endif +GHC_FS_API bool is_socket(const path& p, std::error_code& ec) noexcept; +GHC_FS_API bool is_symlink(file_status s) noexcept; +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API bool is_symlink(const path& p); +#endif +GHC_FS_API bool is_symlink(const path& p, std::error_code& ec) noexcept; + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API file_time_type last_write_time(const path& p); +#endif +GHC_FS_API file_time_type last_write_time(const path& p, std::error_code& ec) noexcept; +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API void last_write_time(const path& p, file_time_type new_time); +#endif +GHC_FS_API void last_write_time(const path& p, file_time_type new_time, std::error_code& ec) noexcept; + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API void permissions(const path& p, perms prms, perm_options opts = perm_options::replace); +#endif +GHC_FS_API void permissions(const path& p, perms prms, std::error_code& ec) noexcept; +GHC_FS_API void permissions(const path& p, perms prms, perm_options opts, std::error_code& ec); + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API path proximate(const path& p, std::error_code& ec); +GHC_FS_API path proximate(const path& p, const path& base = current_path()); +#endif +GHC_FS_API path proximate(const path& p, const path& base, std::error_code& ec); + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API path read_symlink(const path& p); +#endif +GHC_FS_API path read_symlink(const path& p, std::error_code& ec); + +GHC_FS_API path relative(const path& p, std::error_code& ec); +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API path relative(const path& p, const path& base = current_path()); +#endif +GHC_FS_API path relative(const path& p, const path& base, std::error_code& ec); + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API bool remove(const path& p); +#endif +GHC_FS_API bool remove(const path& p, std::error_code& ec) noexcept; + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API uintmax_t remove_all(const path& p); +#endif +GHC_FS_API uintmax_t remove_all(const path& p, std::error_code& ec) noexcept; + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API void rename(const path& from, const path& to); +#endif +GHC_FS_API void rename(const path& from, const path& to, std::error_code& ec) noexcept; + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API void resize_file(const path& p, uintmax_t size); +#endif +GHC_FS_API void resize_file(const path& p, uintmax_t size, std::error_code& ec) noexcept; + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API space_info space(const path& p); +#endif +GHC_FS_API space_info space(const path& p, std::error_code& ec) noexcept; + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API file_status status(const path& p); +#endif +GHC_FS_API file_status status(const path& p, std::error_code& ec) noexcept; + +GHC_FS_API bool status_known(file_status s) noexcept; + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API file_status symlink_status(const path& p); +#endif +GHC_FS_API file_status symlink_status(const path& p, std::error_code& ec) noexcept; + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API path temp_directory_path(); +#endif +GHC_FS_API path temp_directory_path(std::error_code& ec) noexcept; + +#ifdef GHC_WITH_EXCEPTIONS +GHC_FS_API path weakly_canonical(const path& p); +#endif +GHC_FS_API path weakly_canonical(const path& p, std::error_code& ec) noexcept; + +// Non-C++17 add-on std::fstream wrappers with path +template > +class basic_filebuf : public std::basic_filebuf +{ +public: + basic_filebuf() {} + ~basic_filebuf() override {} + basic_filebuf(const basic_filebuf&) = delete; + const basic_filebuf& operator=(const basic_filebuf&) = delete; + basic_filebuf* open(const path& p, std::ios_base::openmode mode) + { +#if defined(GHC_OS_WINDOWS) && !defined(__GLIBCXX__) + return std::basic_filebuf::open(p.wstring().c_str(), mode) ? this : 0; +#else + return std::basic_filebuf::open(p.string().c_str(), mode) ? this : 0; +#endif + } +}; + +template > +class basic_ifstream : public std::basic_ifstream +{ +public: + basic_ifstream() {} +#if defined(GHC_OS_WINDOWS) && !defined(__GLIBCXX__) + explicit basic_ifstream(const path& p, std::ios_base::openmode mode = std::ios_base::in) + : std::basic_ifstream(p.wstring().c_str(), mode) + { + } + void open(const path& p, std::ios_base::openmode mode = std::ios_base::in) { std::basic_ifstream::open(p.wstring().c_str(), mode); } +#else + explicit basic_ifstream(const path& p, std::ios_base::openmode mode = std::ios_base::in) + : std::basic_ifstream(p.string().c_str(), mode) + { + } + void open(const path& p, std::ios_base::openmode mode = std::ios_base::in) { std::basic_ifstream::open(p.string().c_str(), mode); } +#endif + basic_ifstream(const basic_ifstream&) = delete; + const basic_ifstream& operator=(const basic_ifstream&) = delete; + ~basic_ifstream() override {} +}; + +template > +class basic_ofstream : public std::basic_ofstream +{ +public: + basic_ofstream() {} +#if defined(GHC_OS_WINDOWS) && !defined(__GLIBCXX__) + explicit basic_ofstream(const path& p, std::ios_base::openmode mode = std::ios_base::out) + : std::basic_ofstream(p.wstring().c_str(), mode) + { + } + void open(const path& p, std::ios_base::openmode mode = std::ios_base::out) { std::basic_ofstream::open(p.wstring().c_str(), mode); } +#else + explicit basic_ofstream(const path& p, std::ios_base::openmode mode = std::ios_base::out) + : std::basic_ofstream(p.string().c_str(), mode) + { + } + void open(const path& p, std::ios_base::openmode mode = std::ios_base::out) { std::basic_ofstream::open(p.string().c_str(), mode); } +#endif + basic_ofstream(const basic_ofstream&) = delete; + const basic_ofstream& operator=(const basic_ofstream&) = delete; + ~basic_ofstream() override {} +}; + +template > +class basic_fstream : public std::basic_fstream +{ +public: + basic_fstream() {} +#if defined(GHC_OS_WINDOWS) && !defined(__GLIBCXX__) + explicit basic_fstream(const path& p, std::ios_base::openmode mode = std::ios_base::in | std::ios_base::out) + : std::basic_fstream(p.wstring().c_str(), mode) + { + } + void open(const path& p, std::ios_base::openmode mode = std::ios_base::in | std::ios_base::out) { std::basic_fstream::open(p.wstring().c_str(), mode); } +#else + explicit basic_fstream(const path& p, std::ios_base::openmode mode = std::ios_base::in | std::ios_base::out) + : std::basic_fstream(p.string().c_str(), mode) + { + } + void open(const path& p, std::ios_base::openmode mode = std::ios_base::in | std::ios_base::out) { std::basic_fstream::open(p.string().c_str(), mode); } +#endif + basic_fstream(const basic_fstream&) = delete; + const basic_fstream& operator=(const basic_fstream&) = delete; + ~basic_fstream() override {} +}; + +typedef basic_filebuf filebuf; +typedef basic_filebuf wfilebuf; +typedef basic_ifstream ifstream; +typedef basic_ifstream wifstream; +typedef basic_ofstream ofstream; +typedef basic_ofstream wofstream; +typedef basic_fstream fstream; +typedef basic_fstream wfstream; + +class GHC_FS_API_CLASS u8arguments +{ +public: + u8arguments(int& argc, char**& argv); + ~u8arguments() + { + _refargc = _argc; + _refargv = _argv; + } + + bool valid() const { return _isvalid; } + +private: + int _argc; + char** _argv; + int& _refargc; + char**& _refargv; + bool _isvalid; +#ifdef GHC_OS_WINDOWS + std::vector _args; + std::vector _argp; +#endif +}; + +//------------------------------------------------------------------------------------------------- +// Implementation +//------------------------------------------------------------------------------------------------- + +namespace detail { +// GHC_FS_API void postprocess_path_with_format(path::impl_string_type& p, path::format fmt); +enum utf8_states_t { S_STRT = 0, S_RJCT = 8 }; +GHC_FS_API void appendUTF8(std::string& str, uint32_t unicode); +GHC_FS_API bool is_surrogate(uint32_t c); +GHC_FS_API bool is_high_surrogate(uint32_t c); +GHC_FS_API bool is_low_surrogate(uint32_t c); +GHC_FS_API unsigned consumeUtf8Fragment(const unsigned state, const uint8_t fragment, uint32_t& codepoint); +enum class portable_error { + none = 0, + exists, + not_found, + not_supported, + not_implemented, + invalid_argument, + is_a_directory, +}; +GHC_FS_API std::error_code make_error_code(portable_error err); +#ifdef GHC_OS_WINDOWS +GHC_FS_API std::error_code make_system_error(uint32_t err = 0); +#else +GHC_FS_API std::error_code make_system_error(int err = 0); +#endif +} // namespace detail + +namespace detail { + +#ifdef GHC_EXPAND_IMPL + +GHC_INLINE std::error_code make_error_code(portable_error err) +{ +#ifdef GHC_OS_WINDOWS + switch (err) { + case portable_error::none: + return std::error_code(); + case portable_error::exists: + return std::error_code(ERROR_ALREADY_EXISTS, std::system_category()); + case portable_error::not_found: + return std::error_code(ERROR_PATH_NOT_FOUND, std::system_category()); + case portable_error::not_supported: + return std::error_code(ERROR_NOT_SUPPORTED, std::system_category()); + case portable_error::not_implemented: + return std::error_code(ERROR_CALL_NOT_IMPLEMENTED, std::system_category()); + case portable_error::invalid_argument: + return std::error_code(ERROR_INVALID_PARAMETER, std::system_category()); + case portable_error::is_a_directory: +#ifdef ERROR_DIRECTORY_NOT_SUPPORTED + return std::error_code(ERROR_DIRECTORY_NOT_SUPPORTED, std::system_category()); +#else + return std::error_code(ERROR_NOT_SUPPORTED, std::system_category()); +#endif + } +#else + switch (err) { + case portable_error::none: + return std::error_code(); + case portable_error::exists: + return std::error_code(EEXIST, std::system_category()); + case portable_error::not_found: + return std::error_code(ENOENT, std::system_category()); + case portable_error::not_supported: + return std::error_code(ENOTSUP, std::system_category()); + case portable_error::not_implemented: + return std::error_code(ENOSYS, std::system_category()); + case portable_error::invalid_argument: + return std::error_code(EINVAL, std::system_category()); + case portable_error::is_a_directory: + return std::error_code(EISDIR, std::system_category()); + } +#endif + return std::error_code(); +} + +#ifdef GHC_OS_WINDOWS +GHC_INLINE std::error_code make_system_error(uint32_t err) +{ + return std::error_code(err ? static_cast(err) : static_cast(::GetLastError()), std::system_category()); +} +#else +GHC_INLINE std::error_code make_system_error(int err) +{ + return std::error_code(err ? err : errno, std::system_category()); +} +#endif + +#endif // GHC_EXPAND_IMPL + +template +using EnableBitmask = typename std::enable_if::value || std::is_same::value || std::is_same::value || std::is_same::value, Enum>::type; +} // namespace detail + +template +detail::EnableBitmask operator&(Enum X, Enum Y) +{ + using underlying = typename std::underlying_type::type; + return static_cast(static_cast(X) & static_cast(Y)); +} + +template +detail::EnableBitmask operator|(Enum X, Enum Y) +{ + using underlying = typename std::underlying_type::type; + return static_cast(static_cast(X) | static_cast(Y)); +} + +template +detail::EnableBitmask operator^(Enum X, Enum Y) +{ + using underlying = typename std::underlying_type::type; + return static_cast(static_cast(X) ^ static_cast(Y)); +} + +template +detail::EnableBitmask operator~(Enum X) +{ + using underlying = typename std::underlying_type::type; + return static_cast(~static_cast(X)); +} + +template +detail::EnableBitmask& operator&=(Enum& X, Enum Y) +{ + X = X & Y; + return X; +} + +template +detail::EnableBitmask& operator|=(Enum& X, Enum Y) +{ + X = X | Y; + return X; +} + +template +detail::EnableBitmask& operator^=(Enum& X, Enum Y) +{ + X = X ^ Y; + return X; +} + +#ifdef GHC_EXPAND_IMPL + +namespace detail { + +GHC_INLINE bool in_range(uint32_t c, uint32_t lo, uint32_t hi) +{ + return (static_cast(c - lo) < (hi - lo + 1)); +} + +GHC_INLINE bool is_surrogate(uint32_t c) +{ + return in_range(c, 0xd800, 0xdfff); +} + +GHC_INLINE bool is_high_surrogate(uint32_t c) +{ + return (c & 0xfffffc00) == 0xd800; +} + +GHC_INLINE bool is_low_surrogate(uint32_t c) +{ + return (c & 0xfffffc00) == 0xdc00; +} + +GHC_INLINE void appendUTF8(std::string& str, uint32_t unicode) +{ + if (unicode <= 0x7f) { + str.push_back(static_cast(unicode)); + } + else if (unicode >= 0x80 && unicode <= 0x7ff) { + str.push_back(static_cast((unicode >> 6) + 192)); + str.push_back(static_cast((unicode & 0x3f) + 128)); + } + else if ((unicode >= 0x800 && unicode <= 0xd7ff) || (unicode >= 0xe000 && unicode <= 0xffff)) { + str.push_back(static_cast((unicode >> 12) + 224)); + str.push_back(static_cast(((unicode & 0xfff) >> 6) + 128)); + str.push_back(static_cast((unicode & 0x3f) + 128)); + } + else if (unicode >= 0x10000 && unicode <= 0x10ffff) { + str.push_back(static_cast((unicode >> 18) + 240)); + str.push_back(static_cast(((unicode & 0x3ffff) >> 12) + 128)); + str.push_back(static_cast(((unicode & 0xfff) >> 6) + 128)); + str.push_back(static_cast((unicode & 0x3f) + 128)); + } + else { +#ifdef GHC_RAISE_UNICODE_ERRORS + throw filesystem_error("Illegal code point for unicode character.", str, std::make_error_code(std::errc::illegal_byte_sequence)); +#else + appendUTF8(str, 0xfffd); +#endif + } +} + +// Thanks to Bjoern Hoehrmann (https://bjoern.hoehrmann.de/utf-8/decoder/dfa/) +// and Taylor R Campbell for the ideas to this DFA approach of UTF-8 decoding; +// Generating debugging and shrinking my own DFA from scratch was a day of fun! +GHC_INLINE unsigned consumeUtf8Fragment(const unsigned state, const uint8_t fragment, uint32_t& codepoint) +{ + static const uint32_t utf8_state_info[] = { + // encoded states + 0x11111111u, 0x11111111u, 0x77777777u, 0x77777777u, 0x88888888u, 0x88888888u, 0x88888888u, 0x88888888u, 0x22222299u, 0x22222222u, 0x22222222u, 0x22222222u, 0x3333333au, 0x33433333u, 0x9995666bu, 0x99999999u, + 0x88888880u, 0x22818108u, 0x88888881u, 0x88888882u, 0x88888884u, 0x88888887u, 0x88888886u, 0x82218108u, 0x82281108u, 0x88888888u, 0x88888883u, 0x88888885u, 0u, 0u, 0u, 0u, + }; + uint8_t category = fragment < 128 ? 0 : (utf8_state_info[(fragment >> 3) & 0xf] >> ((fragment & 7) << 2)) & 0xf; + codepoint = (state ? (codepoint << 6) | (fragment & 0x3fu) : (0xffu >> category) & fragment); + return state == S_RJCT ? static_cast(S_RJCT) : static_cast((utf8_state_info[category + 16] >> (state << 2)) & 0xf); +} + +GHC_INLINE bool validUtf8(const std::string& utf8String) +{ + std::string::const_iterator iter = utf8String.begin(); + unsigned utf8_state = S_STRT; + std::uint32_t codepoint = 0; + while (iter < utf8String.end()) { + if ((utf8_state = consumeUtf8Fragment(utf8_state, static_cast(*iter++), codepoint)) == S_RJCT) { + return false; + } + } + if (utf8_state) { + return false; + } + return true; +} + +} // namespace detail + +#endif + +namespace detail { + +template ::type* = nullptr> +inline StringType fromUtf8(const std::string& utf8String, const typename StringType::allocator_type& alloc = typename StringType::allocator_type()) +{ + return StringType(utf8String.begin(), utf8String.end(), alloc); +} + +template ::type* = nullptr> +inline StringType fromUtf8(const std::string& utf8String, const typename StringType::allocator_type& alloc = typename StringType::allocator_type()) +{ + StringType result(alloc); + result.reserve(utf8String.length()); + std::string::const_iterator iter = utf8String.begin(); + unsigned utf8_state = S_STRT; + std::uint32_t codepoint = 0; + while (iter < utf8String.end()) { + if ((utf8_state = consumeUtf8Fragment(utf8_state, static_cast(*iter++), codepoint)) == S_STRT) { + if (codepoint <= 0xffff) { + result += static_cast(codepoint); + } + else { + codepoint -= 0x10000; + result += static_cast((codepoint >> 10) + 0xd800); + result += static_cast((codepoint & 0x3ff) + 0xdc00); + } + codepoint = 0; + } + else if (utf8_state == S_RJCT) { +#ifdef GHC_RAISE_UNICODE_ERRORS + throw filesystem_error("Illegal byte sequence for unicode character.", utf8String, std::make_error_code(std::errc::illegal_byte_sequence)); +#else + result += static_cast(0xfffd); + utf8_state = S_STRT; + codepoint = 0; +#endif + } + } + if (utf8_state) { +#ifdef GHC_RAISE_UNICODE_ERRORS + throw filesystem_error("Illegal byte sequence for unicode character.", utf8String, std::make_error_code(std::errc::illegal_byte_sequence)); +#else + result += static_cast(0xfffd); +#endif + } + return result; +} + +template ::type* = nullptr> +inline StringType fromUtf8(const std::string& utf8String, const typename StringType::allocator_type& alloc = typename StringType::allocator_type()) +{ + StringType result(alloc); + result.reserve(utf8String.length()); + std::string::const_iterator iter = utf8String.begin(); + unsigned utf8_state = S_STRT; + std::uint32_t codepoint = 0; + while (iter < utf8String.end()) { + if ((utf8_state = consumeUtf8Fragment(utf8_state, static_cast(*iter++), codepoint)) == S_STRT) { + result += static_cast(codepoint); + codepoint = 0; + } + else if (utf8_state == S_RJCT) { +#ifdef GHC_RAISE_UNICODE_ERRORS + throw filesystem_error("Illegal byte sequence for unicode character.", utf8String, std::make_error_code(std::errc::illegal_byte_sequence)); +#else + result += static_cast(0xfffd); + utf8_state = S_STRT; + codepoint = 0; +#endif + } + } + if (utf8_state) { +#ifdef GHC_RAISE_UNICODE_ERRORS + throw filesystem_error("Illegal byte sequence for unicode character.", utf8String, std::make_error_code(std::errc::illegal_byte_sequence)); +#else + result += static_cast(0xfffd); +#endif + } + return result; +} + +template ::type size = 1> +inline std::string toUtf8(const std::basic_string& unicodeString) +{ + return std::string(unicodeString.begin(), unicodeString.end()); +} + +template ::type size = 2> +inline std::string toUtf8(const std::basic_string& unicodeString) +{ + std::string result; + for (auto iter = unicodeString.begin(); iter != unicodeString.end(); ++iter) { + char32_t c = *iter; + if (is_surrogate(c)) { + ++iter; + if (iter != unicodeString.end() && is_high_surrogate(c) && is_low_surrogate(*iter)) { + appendUTF8(result, (char32_t(c) << 10) + *iter - 0x35fdc00); + } + else { +#ifdef GHC_RAISE_UNICODE_ERRORS + throw filesystem_error("Illegal code point for unicode character.", result, std::make_error_code(std::errc::illegal_byte_sequence)); +#else + appendUTF8(result, 0xfffd); + if(iter == unicodeString.end()) { + break; + } +#endif + } + } + else { + appendUTF8(result, c); + } + } + return result; +} + +template ::type size = 4> +inline std::string toUtf8(const std::basic_string& unicodeString) +{ + std::string result; + for (auto c : unicodeString) { + appendUTF8(result, static_cast(c)); + } + return result; +} + +template +inline std::string toUtf8(const charT* unicodeString) +{ + return toUtf8(std::basic_string>(unicodeString)); +} + +} // namespace detail + +#ifdef GHC_EXPAND_IMPL + +namespace detail { + +GHC_INLINE bool startsWith(const std::string& what, const std::string& with) +{ + return with.length() <= what.length() && equal(with.begin(), with.end(), what.begin()); +} + +} // namespace detail + +GHC_INLINE void path::postprocess_path_with_format(path::impl_string_type& p, path::format fmt) +{ +#ifdef GHC_RAISE_UNICODE_ERRORS + if(!detail::validUtf8(p)) { + path t; + t._path = p; + throw filesystem_error("Illegal byte sequence for unicode character.", t, std::make_error_code(std::errc::illegal_byte_sequence)); + } +#endif + switch (fmt) { +#ifndef GHC_OS_WINDOWS + case path::auto_format: + case path::native_format: +#endif + case path::generic_format: + // nothing to do + break; +#ifdef GHC_OS_WINDOWS + case path::auto_format: + case path::native_format: + if (detail::startsWith(p, std::string("\\\\?\\"))) { + // remove Windows long filename marker + p.erase(0, 4); + if (detail::startsWith(p, std::string("UNC\\"))) { + p.erase(0, 2); + p[0] = '\\'; + } + } + for (auto& c : p) { + if (c == '\\') { + c = '/'; + } + } + break; +#endif + } + if (p.length() > 2 && p[0] == '/' && p[1] == '/' && p[2] != '/') { + std::string::iterator new_end = std::unique(p.begin() + 2, p.end(), [](path::value_type lhs, path::value_type rhs) { return lhs == rhs && lhs == '/'; }); + p.erase(new_end, p.end()); + } + else { + std::string::iterator new_end = std::unique(p.begin(), p.end(), [](path::value_type lhs, path::value_type rhs) { return lhs == rhs && lhs == '/'; }); + p.erase(new_end, p.end()); + } +} + +#endif // GHC_EXPAND_IMPL + +template +inline path::path(const Source& source, format fmt) + : _path(detail::toUtf8(source)) +{ + postprocess_path_with_format(_path, fmt); +} +template <> +inline path::path(const std::wstring& source, format fmt) +{ + _path = detail::toUtf8(source); + postprocess_path_with_format(_path, fmt); +} +template <> +inline path::path(const std::u16string& source, format fmt) +{ + _path = detail::toUtf8(source); + postprocess_path_with_format(_path, fmt); +} +template <> +inline path::path(const std::u32string& source, format fmt) +{ + _path = detail::toUtf8(source); + postprocess_path_with_format(_path, fmt); +} + +#ifdef __cpp_lib_string_view +template <> +inline path::path(const std::string_view& source, format fmt) +{ + _path = detail::toUtf8(std::string(source)); + postprocess_path_with_format(_path, fmt); +} +#ifdef GHC_USE_WCHAR_T +template <> +inline path::path(const std::wstring_view& source, format fmt) +{ + _path = detail::toUtf8(std::wstring(source).c_str()); + postprocess_path_with_format(_path, fmt); +} +#endif +#endif + +template +inline path u8path(const Source& source) +{ + return path(source); +} +template +inline path u8path(InputIterator first, InputIterator last) +{ + return path(first, last); +} + +template +inline path::path(InputIterator first, InputIterator last, format fmt) + : path(std::basic_string::value_type>(first, last), fmt) +{ + // delegated +} + +#ifdef GHC_EXPAND_IMPL + +namespace detail { + +GHC_INLINE bool equals_simple_insensitive(const char* str1, const char* str2) +{ +#ifdef GHC_OS_WINDOWS +#ifdef __GNUC__ + while (::tolower((unsigned char)*str1) == ::tolower((unsigned char)*str2++)) { + if (*str1++ == 0) + return true; + } + return false; +#else + return 0 == ::_stricmp(str1, str2); +#endif +#else + return 0 == ::strcasecmp(str1, str2); +#endif +} + +GHC_INLINE const char* strerror_adapter(char* gnu, char*) +{ + return gnu; +} + +GHC_INLINE const char* strerror_adapter(int posix, char* buffer) +{ + if(posix) { + return "Error in strerror_r!"; + } + return buffer; +} + +template +GHC_INLINE std::string systemErrorText(ErrorNumber code = 0) +{ +#if defined(GHC_OS_WINDOWS) + LPVOID msgBuf; + DWORD dw = code ? static_cast(code) : ::GetLastError(); + FormatMessageW(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS, NULL, dw, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), (LPWSTR)&msgBuf, 0, NULL); + std::string msg = toUtf8(std::wstring((LPWSTR)msgBuf)); + LocalFree(msgBuf); + return msg; +#else + char buffer[512]; + return strerror_adapter(strerror_r(code ? code : errno, buffer, sizeof(buffer)), buffer); +#endif +} + +#ifdef GHC_OS_WINDOWS +using CreateSymbolicLinkW_fp = BOOLEAN(WINAPI*)(LPCWSTR, LPCWSTR, DWORD); +using CreateHardLinkW_fp = BOOLEAN(WINAPI*)(LPCWSTR, LPCWSTR, LPSECURITY_ATTRIBUTES); + +GHC_INLINE void create_symlink(const path& target_name, const path& new_symlink, bool to_directory, std::error_code& ec) +{ + std::error_code tec; + auto fs = status(target_name, tec); + if ((fs.type() == file_type::directory && !to_directory) || (fs.type() == file_type::regular && to_directory)) { + ec = detail::make_error_code(detail::portable_error::not_supported); + return; + } +#if defined(__GNUC__) && __GNUC__ >= 8 +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-function-type" +#endif + static CreateSymbolicLinkW_fp api_call = reinterpret_cast(GetProcAddress(GetModuleHandleW(L"kernel32.dll"), "CreateSymbolicLinkW")); +#if defined(__GNUC__) && __GNUC__ >= 8 +#pragma GCC diagnostic pop +#endif + if (api_call) { + if (api_call(detail::fromUtf8(new_symlink.u8string()).c_str(), detail::fromUtf8(target_name.u8string()).c_str(), to_directory ? 1 : 0) == 0) { + auto result = ::GetLastError(); + if (result == ERROR_PRIVILEGE_NOT_HELD && api_call(detail::fromUtf8(new_symlink.u8string()).c_str(), detail::fromUtf8(target_name.u8string()).c_str(), to_directory ? 3 : 2) != 0) { + return; + } + ec = detail::make_system_error(result); + } + } + else { + ec = detail::make_system_error(ERROR_NOT_SUPPORTED); + } +} + +GHC_INLINE void create_hardlink(const path& target_name, const path& new_hardlink, std::error_code& ec) +{ +#if defined(__GNUC__) && __GNUC__ >= 8 +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-function-type" +#endif + static CreateHardLinkW_fp api_call = reinterpret_cast(GetProcAddress(GetModuleHandleW(L"kernel32.dll"), "CreateHardLinkW")); +#if defined(__GNUC__) && __GNUC__ >= 8 +#pragma GCC diagnostic pop +#endif + if (api_call) { + if (api_call(detail::fromUtf8(new_hardlink.u8string()).c_str(), detail::fromUtf8(target_name.u8string()).c_str(), NULL) == 0) { + ec = detail::make_system_error(); + } + } + else { + ec = detail::make_system_error(ERROR_NOT_SUPPORTED); + } +} +#else +GHC_INLINE void create_symlink(const path& target_name, const path& new_symlink, bool, std::error_code& ec) +{ + if (::symlink(target_name.c_str(), new_symlink.c_str()) != 0) { + ec = detail::make_system_error(); + } +} + +#ifndef GHC_OS_WEB +GHC_INLINE void create_hardlink(const path& target_name, const path& new_hardlink, std::error_code& ec) +{ + if (::link(target_name.c_str(), new_hardlink.c_str()) != 0) { + ec = detail::make_system_error(); + } +} +#endif +#endif + +template +GHC_INLINE file_status file_status_from_st_mode(T mode) +{ +#ifdef GHC_OS_WINDOWS + file_type ft = file_type::unknown; + if ((mode & _S_IFDIR) == _S_IFDIR) { + ft = file_type::directory; + } + else if ((mode & _S_IFREG) == _S_IFREG) { + ft = file_type::regular; + } + else if ((mode & _S_IFCHR) == _S_IFCHR) { + ft = file_type::character; + } + perms prms = static_cast(mode & 0xfff); + return file_status(ft, prms); +#else + file_type ft = file_type::unknown; + if (S_ISDIR(mode)) { + ft = file_type::directory; + } + else if (S_ISREG(mode)) { + ft = file_type::regular; + } + else if (S_ISCHR(mode)) { + ft = file_type::character; + } + else if (S_ISBLK(mode)) { + ft = file_type::block; + } + else if (S_ISFIFO(mode)) { + ft = file_type::fifo; + } + else if (S_ISLNK(mode)) { + ft = file_type::symlink; + } + else if (S_ISSOCK(mode)) { + ft = file_type::socket; + } + perms prms = static_cast(mode & 0xfff); + return file_status(ft, prms); +#endif +} + +GHC_INLINE path resolveSymlink(const path& p, std::error_code& ec) +{ +#ifdef GHC_OS_WINDOWS +#ifndef REPARSE_DATA_BUFFER_HEADER_SIZE + typedef struct _REPARSE_DATA_BUFFER + { + ULONG ReparseTag; + USHORT ReparseDataLength; + USHORT Reserved; + union + { + struct + { + USHORT SubstituteNameOffset; + USHORT SubstituteNameLength; + USHORT PrintNameOffset; + USHORT PrintNameLength; + ULONG Flags; + WCHAR PathBuffer[1]; + } SymbolicLinkReparseBuffer; + struct + { + USHORT SubstituteNameOffset; + USHORT SubstituteNameLength; + USHORT PrintNameOffset; + USHORT PrintNameLength; + WCHAR PathBuffer[1]; + } MountPointReparseBuffer; + struct + { + UCHAR DataBuffer[1]; + } GenericReparseBuffer; + } DUMMYUNIONNAME; + } REPARSE_DATA_BUFFER; +#ifndef MAXIMUM_REPARSE_DATA_BUFFER_SIZE +#define MAXIMUM_REPARSE_DATA_BUFFER_SIZE (16 * 1024) +#endif +#endif + + std::shared_ptr file(CreateFileW(p.wstring().c_str(), 0, FILE_SHARE_READ | FILE_SHARE_WRITE | FILE_SHARE_DELETE, 0, OPEN_EXISTING, FILE_FLAG_OPEN_REPARSE_POINT | FILE_FLAG_BACKUP_SEMANTICS, 0), CloseHandle); + if (file.get() == INVALID_HANDLE_VALUE) { + ec = detail::make_system_error(); + return path(); + } + + std::shared_ptr reparseData((REPARSE_DATA_BUFFER*)std::calloc(1, MAXIMUM_REPARSE_DATA_BUFFER_SIZE), std::free); + ULONG bufferUsed; + path result; + if (DeviceIoControl(file.get(), FSCTL_GET_REPARSE_POINT, 0, 0, reparseData.get(), MAXIMUM_REPARSE_DATA_BUFFER_SIZE, &bufferUsed, 0)) { + if (IsReparseTagMicrosoft(reparseData->ReparseTag)) { + switch (reparseData->ReparseTag) { + case IO_REPARSE_TAG_SYMLINK: + result = std::wstring(&reparseData->SymbolicLinkReparseBuffer.PathBuffer[reparseData->SymbolicLinkReparseBuffer.PrintNameOffset / sizeof(WCHAR)], reparseData->SymbolicLinkReparseBuffer.PrintNameLength / sizeof(WCHAR)); + break; + case IO_REPARSE_TAG_MOUNT_POINT: + result = std::wstring(&reparseData->MountPointReparseBuffer.PathBuffer[reparseData->MountPointReparseBuffer.PrintNameOffset / sizeof(WCHAR)], reparseData->MountPointReparseBuffer.PrintNameLength / sizeof(WCHAR)); + break; + default: + break; + } + } + } + else { + ec = detail::make_system_error(); + } + return result; +#else + size_t bufferSize = 256; + while (true) { + std::vector buffer(bufferSize, static_cast(0)); + auto rc = ::readlink(p.c_str(), buffer.data(), buffer.size()); + if (rc < 0) { + ec = detail::make_system_error(); + return path(); + } + else if (rc < static_cast(bufferSize)) { + return path(std::string(buffer.data(), static_cast(rc))); + } + bufferSize *= 2; + } + return path(); +#endif +} + +#ifdef GHC_OS_WINDOWS +GHC_INLINE time_t timeFromFILETIME(const FILETIME& ft) +{ + ULARGE_INTEGER ull; + ull.LowPart = ft.dwLowDateTime; + ull.HighPart = ft.dwHighDateTime; + return static_cast(ull.QuadPart / 10000000ULL - 11644473600ULL); +} + +GHC_INLINE void timeToFILETIME(time_t t, FILETIME& ft) +{ + LONGLONG ll; + ll = Int32x32To64(t, 10000000) + 116444736000000000; + ft.dwLowDateTime = static_cast(ll); + ft.dwHighDateTime = static_cast(ll >> 32); +} + +template +GHC_INLINE uintmax_t hard_links_from_INFO(const INFO* info) +{ + return static_cast(-1); +} + +template <> +GHC_INLINE uintmax_t hard_links_from_INFO(const BY_HANDLE_FILE_INFORMATION* info) +{ + return info->nNumberOfLinks; +} + +template +GHC_INLINE file_status status_from_INFO(const path& p, const INFO* info, std::error_code&, uintmax_t* sz = nullptr, time_t* lwt = nullptr) noexcept +{ + file_type ft = file_type::unknown; + if ((info->dwFileAttributes & FILE_ATTRIBUTE_REPARSE_POINT)) { + ft = file_type::symlink; + } + else { + if ((info->dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY)) { + ft = file_type::directory; + } + else { + ft = file_type::regular; + } + } + perms prms = perms::owner_read | perms::group_read | perms::others_read; + if (!(info->dwFileAttributes & FILE_ATTRIBUTE_READONLY)) { + prms = prms | perms::owner_write | perms::group_write | perms::others_write; + } + std::string ext = p.extension().generic_string(); + if (equals_simple_insensitive(ext.c_str(), ".exe") || equals_simple_insensitive(ext.c_str(), ".cmd") || equals_simple_insensitive(ext.c_str(), ".bat") || equals_simple_insensitive(ext.c_str(), ".com")) { + prms = prms | perms::owner_exec | perms::group_exec | perms::others_exec; + } + if (sz) { + *sz = static_cast(info->nFileSizeHigh) << (sizeof(info->nFileSizeHigh) * 8) | info->nFileSizeLow; + } + if (lwt) { + *lwt = detail::timeFromFILETIME(info->ftLastWriteTime); + } + return file_status(ft, prms); +} + +#endif + +GHC_INLINE bool is_not_found_error(std::error_code& ec) +{ +#ifdef GHC_OS_WINDOWS + return ec.value() == ERROR_FILE_NOT_FOUND || ec.value() == ERROR_PATH_NOT_FOUND || ec.value() == ERROR_INVALID_NAME; +#else + return ec.value() == ENOENT || ec.value() == ENOTDIR; +#endif +} + +GHC_INLINE file_status symlink_status_ex(const path& p, std::error_code& ec, uintmax_t* sz = nullptr, uintmax_t* nhl = nullptr, time_t* lwt = nullptr) noexcept +{ +#ifdef GHC_OS_WINDOWS + file_status fs; + WIN32_FILE_ATTRIBUTE_DATA attr; + if (!GetFileAttributesExW(detail::fromUtf8(p.u8string()).c_str(), GetFileExInfoStandard, &attr)) { + ec = detail::make_system_error(); + } + else { + ec.clear(); + fs = detail::status_from_INFO(p, &attr, ec, sz, lwt); + if (nhl) { + *nhl = 0; + } + if (attr.dwFileAttributes & FILE_ATTRIBUTE_REPARSE_POINT) { + fs.type(file_type::symlink); + } + } + if (detail::is_not_found_error(ec)) { + return file_status(file_type::not_found); + } + return ec ? file_status(file_type::none) : fs; +#else + (void)sz; + (void)nhl; + (void)lwt; + struct ::stat fs; + auto result = ::lstat(p.c_str(), &fs); + if (result == 0) { + ec.clear(); + file_status f_s = detail::file_status_from_st_mode(fs.st_mode); + return f_s; + } + ec = detail::make_system_error(); + if (detail::is_not_found_error(ec)) { + return file_status(file_type::not_found, perms::unknown); + } + return file_status(file_type::none); +#endif +} + +GHC_INLINE file_status status_ex(const path& p, std::error_code& ec, file_status* sls = nullptr, uintmax_t* sz = nullptr, uintmax_t* nhl = nullptr, time_t* lwt = nullptr, int recurse_count = 0) noexcept +{ + ec.clear(); +#ifdef GHC_OS_WINDOWS + if (recurse_count > 16) { + ec = detail::make_system_error(0x2A9 /*ERROR_STOPPED_ON_SYMLINK*/); + return file_status(file_type::unknown); + } + WIN32_FILE_ATTRIBUTE_DATA attr; + if (!::GetFileAttributesExW(p.wstring().c_str(), GetFileExInfoStandard, &attr)) { + ec = detail::make_system_error(); + } + else if (attr.dwFileAttributes & FILE_ATTRIBUTE_REPARSE_POINT) { + path target = resolveSymlink(p, ec); + file_status result; + if (!ec && !target.empty()) { + if (sls) { + *sls = status_from_INFO(p, &attr, ec); + } + return detail::status_ex(target, ec, nullptr, sz, nhl, lwt, recurse_count + 1); + } + return file_status(file_type::unknown); + } + if (ec) { + if (detail::is_not_found_error(ec)) { + return file_status(file_type::not_found); + } + return file_status(file_type::none); + } + if (nhl) { + *nhl = 0; + } + return detail::status_from_INFO(p, &attr, ec, sz, lwt); +#else + (void)recurse_count; + struct ::stat st; + auto result = ::lstat(p.c_str(), &st); + if (result == 0) { + ec.clear(); + file_status fs = detail::file_status_from_st_mode(st.st_mode); + if (fs.type() == file_type::symlink) { + result = ::stat(p.c_str(), &st); + if (result == 0) { + if (sls) { + *sls = fs; + } + fs = detail::file_status_from_st_mode(st.st_mode); + } + } + if (sz) { + *sz = static_cast(st.st_size); + } + if (nhl) { + *nhl = st.st_nlink; + } + if (lwt) { + *lwt = st.st_mtime; + } + return fs; + } + else { + ec = detail::make_system_error(); + if (detail::is_not_found_error(ec)) { + return file_status(file_type::not_found, perms::unknown); + } + return file_status(file_type::none); + } +#endif +} + +} // namespace detail + +GHC_INLINE u8arguments::u8arguments(int& argc, char**& argv) + : _argc(argc) + , _argv(argv) + , _refargc(argc) + , _refargv(argv) + , _isvalid(false) +{ +#ifdef GHC_OS_WINDOWS + LPWSTR* p; + p = ::CommandLineToArgvW(::GetCommandLineW(), &argc); + _args.reserve(static_cast(argc)); + _argp.reserve(static_cast(argc)); + for (size_t i = 0; i < static_cast(argc); ++i) { + _args.push_back(detail::toUtf8(std::wstring(p[i]))); + _argp.push_back((char*)_args[i].data()); + } + argv = _argp.data(); + ::LocalFree(p); + _isvalid = true; +#else + std::setlocale(LC_ALL, ""); +#if defined(__ANDROID__) && __ANDROID_API__ < 26 + _isvalid = true; +#else + if (detail::equals_simple_insensitive(::nl_langinfo(CODESET), "UTF-8")) { + _isvalid = true; + } +#endif +#endif +} + +//----------------------------------------------------------------------------- +// 30.10.8.4.1 constructors and destructor + +GHC_INLINE path::path() noexcept {} + +GHC_INLINE path::path(const path& p) + : _path(p._path) +{ +} + +GHC_INLINE path::path(path&& p) noexcept + : _path(std::move(p._path)) +{ +} + +GHC_INLINE path::path(string_type&& source, format fmt) +#ifdef GHC_USE_WCHAR_T + : _path(detail::toUtf8(source)) +#else + : _path(std::move(source)) +#endif +{ + postprocess_path_with_format(_path, fmt); +} + +#endif // GHC_EXPAND_IMPL + +#ifdef GHC_WITH_EXCEPTIONS +template +inline path::path(const Source& source, const std::locale& loc, format fmt) + : path(source, fmt) +{ + std::string locName = loc.name(); + if (!(locName.length() >= 5 && (locName.substr(locName.length() - 5) == "UTF-8" || locName.substr(locName.length() - 5) == "utf-8"))) { + throw filesystem_error("This implementation only supports UTF-8 locales!", path(_path), detail::make_error_code(detail::portable_error::not_supported)); + } +} + +template +inline path::path(InputIterator first, InputIterator last, const std::locale& loc, format fmt) + : path(std::basic_string::value_type>(first, last), fmt) +{ + std::string locName = loc.name(); + if (!(locName.length() >= 5 && (locName.substr(locName.length() - 5) == "UTF-8" || locName.substr(locName.length() - 5) == "utf-8"))) { + throw filesystem_error("This implementation only supports UTF-8 locales!", path(_path), detail::make_error_code(detail::portable_error::not_supported)); + } +} +#endif + +#ifdef GHC_EXPAND_IMPL + +GHC_INLINE path::~path() {} + +//----------------------------------------------------------------------------- +// 30.10.8.4.2 assignments + +GHC_INLINE path& path::operator=(const path& p) +{ + _path = p._path; + return *this; +} + +GHC_INLINE path& path::operator=(path&& p) noexcept +{ + _path = std::move(p._path); + return *this; +} + +GHC_INLINE path& path::operator=(path::string_type&& source) +{ + return assign(source); +} + +GHC_INLINE path& path::assign(path::string_type&& source) +{ +#ifdef GHC_USE_WCHAR_T + _path = detail::toUtf8(source); +#else + _path = std::move(source); +#endif + postprocess_path_with_format(_path, native_format); + return *this; +} + +#endif // GHC_EXPAND_IMPL + +template +inline path& path::operator=(const Source& source) +{ + return assign(source); +} + +template +inline path& path::assign(const Source& source) +{ + _path.assign(detail::toUtf8(source)); + postprocess_path_with_format(_path, native_format); + return *this; +} + +template <> +inline path& path::assign(const path& source) +{ + _path = source._path; + return *this; +} + +template +inline path& path::assign(InputIterator first, InputIterator last) +{ + _path.assign(first, last); + postprocess_path_with_format(_path, native_format); + return *this; +} + +#ifdef GHC_EXPAND_IMPL + +//----------------------------------------------------------------------------- +// 30.10.8.4.3 appends + +GHC_INLINE path& path::operator/=(const path& p) +{ + if (p.empty()) { + // was: if ((!has_root_directory() && is_absolute()) || has_filename()) + if (!_path.empty() && _path[_path.length() - 1] != '/' && _path[_path.length() - 1] != ':') { + _path += '/'; + } + return *this; + } + if ((p.is_absolute() && (_path != root_name() || p._path != "/")) || (p.has_root_name() && p.root_name() != root_name())) { + assign(p); + return *this; + } + if (p.has_root_directory()) { + assign(root_name()); + } + else if ((!has_root_directory() && is_absolute()) || has_filename()) { + _path += '/'; + } + auto iter = p.begin(); + bool first = true; + if (p.has_root_name()) { + ++iter; + } + while (iter != p.end()) { + if (!first && !(!_path.empty() && _path[_path.length() - 1] == '/')) { + _path += '/'; + } + first = false; + _path += (*iter++).generic_string(); + } + return *this; +} + +GHC_INLINE void path::append_name(const char* name) +{ + if (_path.empty()) { + this->operator/=(path(name)); + } + else { + if (_path.back() != path::generic_separator) { + _path.push_back(path::generic_separator); + } + _path += name; + } +} + +#endif // GHC_EXPAND_IMPL + +template +inline path& path::operator/=(const Source& source) +{ + return append(source); +} + +template +inline path& path::append(const Source& source) +{ + return this->operator/=(path(detail::toUtf8(source))); +} + +template <> +inline path& path::append(const path& p) +{ + return this->operator/=(p); +} + +template +inline path& path::append(InputIterator first, InputIterator last) +{ + std::basic_string::value_type> part(first, last); + return append(part); +} + +#ifdef GHC_EXPAND_IMPL + +//----------------------------------------------------------------------------- +// 30.10.8.4.4 concatenation + +GHC_INLINE path& path::operator+=(const path& x) +{ + return concat(x._path); +} + +GHC_INLINE path& path::operator+=(const string_type& x) +{ + return concat(x); +} + +#ifdef __cpp_lib_string_view +GHC_INLINE path& path::operator+=(std::basic_string_view x) +{ + return concat(x); +} +#endif + +GHC_INLINE path& path::operator+=(const value_type* x) +{ + return concat(string_type(x)); +} + +GHC_INLINE path& path::operator+=(value_type x) +{ +#ifdef GHC_OS_WINDOWS + if (x == '\\') { + x = generic_separator; + } +#endif + if (_path.empty() || _path.back() != generic_separator) { +#ifdef GHC_USE_WCHAR_T + _path += detail::toUtf8(string_type(1, x)); +#else + _path += x; +#endif + } + return *this; +} + +#endif // GHC_EXPAND_IMPL + +template +inline path::path_from_string& path::operator+=(const Source& x) +{ + return concat(x); +} + +template +inline path::path_type_EcharT& path::operator+=(EcharT x) +{ + std::basic_string part(1, x); + concat(detail::toUtf8(part)); + return *this; +} + +template +inline path& path::concat(const Source& x) +{ + path p(x); + postprocess_path_with_format(p._path, native_format); + _path += p._path; + return *this; +} +template +inline path& path::concat(InputIterator first, InputIterator last) +{ + _path.append(first, last); + postprocess_path_with_format(_path, native_format); + return *this; +} + +#ifdef GHC_EXPAND_IMPL + +//----------------------------------------------------------------------------- +// 30.10.8.4.5 modifiers +GHC_INLINE void path::clear() noexcept +{ + _path.clear(); +} + +GHC_INLINE path& path::make_preferred() +{ + // as this filesystem implementation only uses generic_format + // internally, this must be a no-op + return *this; +} + +GHC_INLINE path& path::remove_filename() +{ + if (has_filename()) { + _path.erase(_path.size() - filename()._path.size()); + } + return *this; +} + +GHC_INLINE path& path::replace_filename(const path& replacement) +{ + remove_filename(); + return append(replacement); +} + +GHC_INLINE path& path::replace_extension(const path& replacement) +{ + if (has_extension()) { + _path.erase(_path.size() - extension()._path.size()); + } + if (!replacement.empty() && replacement._path[0] != '.') { + _path += '.'; + } + return concat(replacement); +} + +GHC_INLINE void path::swap(path& rhs) noexcept +{ + _path.swap(rhs._path); +} + +//----------------------------------------------------------------------------- +// 30.10.8.4.6, native format observers +#ifdef GHC_OS_WINDOWS +GHC_INLINE path::impl_string_type path::native_impl() const +{ + impl_string_type result; + if (is_absolute() && _path.length() > MAX_PATH - 10) { + // expand long Windows filenames with marker + if (has_root_name() && _path[0] == '/') { + result = "\\\\?\\UNC" + _path.substr(1); + } + else { + result = "\\\\?\\" + _path; + } + } + else { + result = _path; + } + /*if (has_root_name() && root_name()._path[0] == '/') { + return _path; + }*/ + for (auto& c : result) { + if (c == '/') { + c = '\\'; + } + } + return result; +} +#else +GHC_INLINE const path::impl_string_type& path::native_impl() const +{ + return _path; +} +#endif + +GHC_INLINE const path::string_type& path::native() const +{ +#ifdef GHC_OS_WINDOWS +#ifdef GHC_USE_WCHAR_T + _native_cache = detail::fromUtf8(native_impl()); +#else + _native_cache = native_impl(); +#endif + return _native_cache; +#else + return _path; +#endif +} + +GHC_INLINE const path::value_type* path::c_str() const +{ + return native().c_str(); +} + +GHC_INLINE path::operator path::string_type() const +{ + return native(); +} + +#endif // GHC_EXPAND_IMPL + +template +inline std::basic_string path::string(const Allocator& a) const +{ + return detail::fromUtf8>(native_impl(), a); +} + +#ifdef GHC_EXPAND_IMPL + +GHC_INLINE std::string path::string() const +{ + return native_impl(); +} + +GHC_INLINE std::wstring path::wstring() const +{ +#ifdef GHC_USE_WCHAR_T + return native(); +#else + return detail::fromUtf8(native()); +#endif +} + +GHC_INLINE std::string path::u8string() const +{ + return native_impl(); +} + +GHC_INLINE std::u16string path::u16string() const +{ + return detail::fromUtf8(native_impl()); +} + +GHC_INLINE std::u32string path::u32string() const +{ + return detail::fromUtf8(native_impl()); +} + +#endif // GHC_EXPAND_IMPL + +//----------------------------------------------------------------------------- +// 30.10.8.4.7, generic format observers +template +inline std::basic_string path::generic_string(const Allocator& a) const +{ + return detail::fromUtf8>(_path, a); +} + +#ifdef GHC_EXPAND_IMPL + +GHC_INLINE const std::string& path::generic_string() const +{ + return _path; +} + +GHC_INLINE std::wstring path::generic_wstring() const +{ + return detail::fromUtf8(_path); +} + +GHC_INLINE std::string path::generic_u8string() const +{ + return _path; +} + +GHC_INLINE std::u16string path::generic_u16string() const +{ + return detail::fromUtf8(_path); +} + +GHC_INLINE std::u32string path::generic_u32string() const +{ + return detail::fromUtf8(_path); +} + +//----------------------------------------------------------------------------- +// 30.10.8.4.8, compare +GHC_INLINE int path::compare(const path& p) const noexcept +{ + return native().compare(p.native()); +} + +GHC_INLINE int path::compare(const string_type& s) const +{ + return native().compare(path(s).native()); +} + +#ifdef __cpp_lib_string_view +GHC_INLINE int path::compare(std::basic_string_view s) const +{ + return native().compare(path(s).native()); +} +#endif + +GHC_INLINE int path::compare(const value_type* s) const +{ + return native().compare(path(s).native()); +} + +//----------------------------------------------------------------------------- +// 30.10.8.4.9, decomposition +GHC_INLINE path path::root_name() const +{ +#ifdef GHC_OS_WINDOWS + if (_path.length() >= 2 && std::toupper(static_cast(_path[0])) >= 'A' && std::toupper(static_cast(_path[0])) <= 'Z' && _path[1] == ':') { + return path(_path.substr(0, 2)); + } +#endif + if (_path.length() > 2 && _path[0] == '/' && _path[1] == '/' && _path[2] != '/' && std::isprint(_path[2])) { + impl_string_type::size_type pos = _path.find_first_of("/\\", 3); + if (pos == impl_string_type::npos) { + return path(_path); + } + else { + return path(_path.substr(0, pos)); + } + } + return path(); +} + +GHC_INLINE path path::root_directory() const +{ + path root = root_name(); + if (_path.length() > root._path.length() && _path[root._path.length()] == '/') { + return path("/"); + } + return path(); +} + +GHC_INLINE path path::root_path() const +{ + return root_name().generic_string() + root_directory().generic_string(); +} + +GHC_INLINE path path::relative_path() const +{ + std::string root = root_path()._path; + return path(_path.substr((std::min)(root.length(), _path.length())), generic_format); +} + +GHC_INLINE path path::parent_path() const +{ + if (has_relative_path()) { + if (empty() || begin() == --end()) { + return path(); + } + else { + path pp; + for (string_type s : input_iterator_range(begin(), --end())) { + if (s == "/") { + // don't use append to join a path- + pp += s; + } + else { + pp /= s; + } + } + return pp; + } + } + else { + return *this; + } +} + +GHC_INLINE path path::filename() const +{ + return relative_path().empty() ? path() : path(*--end()); +} + +GHC_INLINE path path::stem() const +{ + impl_string_type fn = filename().string(); + if (fn != "." && fn != "..") { + impl_string_type::size_type n = fn.rfind('.'); + if (n != impl_string_type::npos && n != 0) { + return path{fn.substr(0, n)}; + } + } + return path{fn}; +} + +GHC_INLINE path path::extension() const +{ + impl_string_type fn = filename().string(); + impl_string_type::size_type pos = fn.find_last_of('.'); + if (pos == std::string::npos || pos == 0) { + return ""; + } + return fn.substr(pos); +} + +//----------------------------------------------------------------------------- +// 30.10.8.4.10, query +GHC_INLINE bool path::empty() const noexcept +{ + return _path.empty(); +} + +GHC_INLINE bool path::has_root_name() const +{ + return !root_name().empty(); +} + +GHC_INLINE bool path::has_root_directory() const +{ + return !root_directory().empty(); +} + +GHC_INLINE bool path::has_root_path() const +{ + return !root_path().empty(); +} + +GHC_INLINE bool path::has_relative_path() const +{ + return !relative_path().empty(); +} + +GHC_INLINE bool path::has_parent_path() const +{ + return !parent_path().empty(); +} + +GHC_INLINE bool path::has_filename() const +{ + return !filename().empty(); +} + +GHC_INLINE bool path::has_stem() const +{ + return !stem().empty(); +} + +GHC_INLINE bool path::has_extension() const +{ + return !extension().empty(); +} + +GHC_INLINE bool path::is_absolute() const +{ +#ifdef GHC_OS_WINDOWS + return has_root_name() && has_root_directory(); +#else + return has_root_directory(); +#endif +} + +GHC_INLINE bool path::is_relative() const +{ + return !is_absolute(); +} + +//----------------------------------------------------------------------------- +// 30.10.8.4.11, generation +GHC_INLINE path path::lexically_normal() const +{ + path dest; + bool lastDotDot = false; + for (string_type s : *this) { + if (s == ".") { + dest /= ""; + continue; + } + else if (s == ".." && !dest.empty()) { + auto root = root_path(); + if (dest == root) { + continue; + } + else if (*(--dest.end()) != "..") { + if (dest._path.back() == generic_separator) { + dest._path.pop_back(); + } + dest.remove_filename(); + continue; + } + } + if (!(s.empty() && lastDotDot)) { + dest /= s; + } + lastDotDot = s == ".."; + } + if (dest.empty()) { + dest = "."; + } + return dest; +} + +GHC_INLINE path path::lexically_relative(const path& base) const +{ + if (root_name() != base.root_name() || is_absolute() != base.is_absolute() || (!has_root_directory() && base.has_root_directory())) { + return path(); + } + const_iterator a = begin(), b = base.begin(); + while (a != end() && b != base.end() && *a == *b) { + ++a; + ++b; + } + if (a == end() && b == base.end()) { + return path("."); + } + int count = 0; + for (const auto& element : input_iterator_range(b, base.end())) { + if (element != "." && element != "" && element != "..") { + ++count; + } + else if (element == "..") { + --count; + } + } + if (count < 0) { + return path(); + } + path result; + for (int i = 0; i < count; ++i) { + result /= ".."; + } + for (const auto& element : input_iterator_range(a, end())) { + result /= element; + } + return result; +} + +GHC_INLINE path path::lexically_proximate(const path& base) const +{ + path result = lexically_relative(base); + return result.empty() ? *this : result; +} + +//----------------------------------------------------------------------------- +// 30.10.8.5, iterators +GHC_INLINE path::iterator::iterator() {} + +GHC_INLINE path::iterator::iterator(const path::impl_string_type::const_iterator& first, const path::impl_string_type::const_iterator& last, const path::impl_string_type::const_iterator& pos) + : _first(first) + , _last(last) + , _iter(pos) +{ + updateCurrent(); + // find the position of a potential root directory slash +#ifdef GHC_OS_WINDOWS + if (_last - _first >= 3 && std::toupper(static_cast(*first)) >= 'A' && std::toupper(static_cast(*first)) <= 'Z' && *(first + 1) == ':' && *(first + 2) == '/') { + _root = _first + 2; + } + else +#endif + { + if (_first != _last && *_first == '/') { + if (_last - _first >= 2 && *(_first + 1) == '/' && !(_last - _first >= 3 && *(_first + 2) == '/')) { + _root = increment(_first); + } + else { + _root = _first; + } + } + else { + _root = _last; + } + } +} + +GHC_INLINE path::impl_string_type::const_iterator path::iterator::increment(const path::impl_string_type::const_iterator& pos) const +{ + path::impl_string_type::const_iterator i = pos; + bool fromStart = i == _first; + if (i != _last) { + // we can only sit on a slash if it is a network name or a root + if (*i++ == '/') { + if (i != _last && *i == '/') { + if (fromStart && !(i + 1 != _last && *(i + 1) == '/')) { + // leadind double slashes detected, treat this and the + // following until a slash as one unit + i = std::find(++i, _last, '/'); + } + else { + // skip redundant slashes + while (i != _last && *i == '/') { + ++i; + } + } + } + } + else { + if (fromStart && i != _last && *i == ':') { + ++i; + } + else { + i = std::find(i, _last, '/'); + } + } + } + return i; +} + +GHC_INLINE path::impl_string_type::const_iterator path::iterator::decrement(const path::impl_string_type::const_iterator& pos) const +{ + path::impl_string_type::const_iterator i = pos; + if (i != _first) { + --i; + // if this is now the root slash or the trailing slash, we are done, + // else check for network name + if (i != _root && (pos != _last || *i != '/')) { +#ifdef GHC_OS_WINDOWS + static const std::string seps = "/:"; + i = std::find_first_of(std::reverse_iterator(i), std::reverse_iterator(_first), seps.begin(), seps.end()).base(); + if (i > _first && *i == ':') { + i++; + } +#else + i = std::find(std::reverse_iterator(i), std::reverse_iterator(_first), '/').base(); +#endif + // Now we have to check if this is a network name + if (i - _first == 2 && *_first == '/' && *(_first + 1) == '/') { + i -= 2; + } + } + } + return i; +} + +GHC_INLINE void path::iterator::updateCurrent() +{ + if (_iter != _first && _iter != _last && (*_iter == '/' && _iter != _root) && (_iter + 1 == _last)) { + _current = ""; + } + else { + _current.assign(_iter, increment(_iter)); + if (_current.generic_string().size() > 1 && _current.generic_string()[0] == '/' && _current.generic_string()[_current.generic_string().size() - 1] == '/') { + // shrink successive slashes to one + _current = "/"; + } + } +} + +GHC_INLINE path::iterator& path::iterator::operator++() +{ + _iter = increment(_iter); + while (_iter != _last && // we didn't reach the end + _iter != _root && // this is not a root position + *_iter == '/' && // we are on a slash + (_iter + 1) != _last // the slash is not the last char + ) { + ++_iter; + } + updateCurrent(); + return *this; +} + +GHC_INLINE path::iterator path::iterator::operator++(int) +{ + path::iterator i{*this}; + ++(*this); + return i; +} + +GHC_INLINE path::iterator& path::iterator::operator--() +{ + _iter = decrement(_iter); + updateCurrent(); + return *this; +} + +GHC_INLINE path::iterator path::iterator::operator--(int) +{ + auto i = *this; + --(*this); + return i; +} + +GHC_INLINE bool path::iterator::operator==(const path::iterator& other) const +{ + return _iter == other._iter; +} + +GHC_INLINE bool path::iterator::operator!=(const path::iterator& other) const +{ + return _iter != other._iter; +} + +GHC_INLINE path::iterator::reference path::iterator::operator*() const +{ + return _current; +} + +GHC_INLINE path::iterator::pointer path::iterator::operator->() const +{ + return &_current; +} + +GHC_INLINE path::iterator path::begin() const +{ + return iterator(_path.begin(), _path.end(), _path.begin()); +} + +GHC_INLINE path::iterator path::end() const +{ + return iterator(_path.begin(), _path.end(), _path.end()); +} + +//----------------------------------------------------------------------------- +// 30.10.8.6, path non-member functions +GHC_INLINE void swap(path& lhs, path& rhs) noexcept +{ + swap(lhs._path, rhs._path); +} + +GHC_INLINE size_t hash_value(const path& p) noexcept +{ + return std::hash()(p.generic_string()); +} + +GHC_INLINE bool operator==(const path& lhs, const path& rhs) noexcept +{ + return lhs.generic_string() == rhs.generic_string(); +} + +GHC_INLINE bool operator!=(const path& lhs, const path& rhs) noexcept +{ + return lhs.generic_string() != rhs.generic_string(); +} + +GHC_INLINE bool operator<(const path& lhs, const path& rhs) noexcept +{ + return lhs.generic_string() < rhs.generic_string(); +} + +GHC_INLINE bool operator<=(const path& lhs, const path& rhs) noexcept +{ + return lhs.generic_string() <= rhs.generic_string(); +} + +GHC_INLINE bool operator>(const path& lhs, const path& rhs) noexcept +{ + return lhs.generic_string() > rhs.generic_string(); +} + +GHC_INLINE bool operator>=(const path& lhs, const path& rhs) noexcept +{ + return lhs.generic_string() >= rhs.generic_string(); +} + +GHC_INLINE path operator/(const path& lhs, const path& rhs) +{ + path result(lhs); + result /= rhs; + return result; +} + +#endif // GHC_EXPAND_IMPL + +//----------------------------------------------------------------------------- +// 30.10.8.6.1 path inserter and extractor +template +inline std::basic_ostream& operator<<(std::basic_ostream& os, const path& p) +{ + os << "\""; + auto ps = p.string(); + for (auto c : ps) { + if (c == '"' || c == '\\') { + os << '\\'; + } + os << c; + } + os << "\""; + return os; +} + +template +inline std::basic_istream& operator>>(std::basic_istream& is, path& p) +{ + std::basic_string tmp; + charT c; + is >> c; + if (c == '"') { + auto sf = is.flags(); + is >> std::noskipws; + while (is) { + auto c2 = is.get(); + if (is) { + if (c2 == '\\') { + c2 = is.get(); + if (is) { + tmp += static_cast(c2); + } + } + else if (c2 == '"') { + break; + } + else { + tmp += static_cast(c2); + } + } + } + if ((sf & std::ios_base::skipws) == std::ios_base::skipws) { + is >> std::skipws; + } + p = path(tmp); + } + else { + is >> tmp; + p = path(static_cast(c) + tmp); + } + return is; +} + +#ifdef GHC_EXPAND_IMPL + +//----------------------------------------------------------------------------- +// 30.10.9 Class filesystem_error +GHC_INLINE filesystem_error::filesystem_error(const std::string& what_arg, std::error_code ec) + : std::system_error(ec, what_arg) + , _what_arg(what_arg) + , _ec(ec) +{ +} + +GHC_INLINE filesystem_error::filesystem_error(const std::string& what_arg, const path& p1, std::error_code ec) + : std::system_error(ec, what_arg) + , _what_arg(what_arg) + , _ec(ec) + , _p1(p1) +{ + if (!_p1.empty()) { + _what_arg += ": '" + _p1.u8string() + "'"; + } +} + +GHC_INLINE filesystem_error::filesystem_error(const std::string& what_arg, const path& p1, const path& p2, std::error_code ec) + : std::system_error(ec, what_arg) + , _what_arg(what_arg) + , _ec(ec) + , _p1(p1) + , _p2(p2) +{ + if (!_p1.empty()) { + _what_arg += ": '" + _p1.u8string() + "'"; + } + if (!_p2.empty()) { + _what_arg += ", '" + _p2.u8string() + "'"; + } +} + +GHC_INLINE const path& filesystem_error::path1() const noexcept +{ + return _p1; +} + +GHC_INLINE const path& filesystem_error::path2() const noexcept +{ + return _p2; +} + +GHC_INLINE const char* filesystem_error::what() const noexcept +{ + return _what_arg.c_str(); +} + +//----------------------------------------------------------------------------- +// 30.10.15, filesystem operations +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE path absolute(const path& p) +{ + std::error_code ec; + path result = absolute(p, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), p, ec); + } + return result; +} +#endif + +GHC_INLINE path absolute(const path& p, std::error_code& ec) +{ + ec.clear(); +#ifdef GHC_OS_WINDOWS + if (p.empty()) { + return absolute(current_path(ec), ec) / ""; + } + ULONG size = ::GetFullPathNameW(p.wstring().c_str(), 0, 0, 0); + if (size) { + std::vector buf(size, 0); + ULONG s2 = GetFullPathNameW(p.wstring().c_str(), size, buf.data(), nullptr); + if (s2 && s2 < size) { + path result = path(std::wstring(buf.data(), s2)); + if (p.filename() == ".") { + result /= "."; + } + return result; + } + } + ec = detail::make_system_error(); + return path(); +#else + path base = current_path(ec); + if (!ec) { + if (p.empty()) { + return base / p; + } + if (p.has_root_name()) { + if (p.has_root_directory()) { + return p; + } + else { + return p.root_name() / base.root_directory() / base.relative_path() / p.relative_path(); + } + } + else { + if (p.has_root_directory()) { + return base.root_name() / p; + } + else { + return base / p; + } + } + } + ec = detail::make_system_error(); + return path(); +#endif +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE path canonical(const path& p) +{ + std::error_code ec; + auto result = canonical(p, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), p, ec); + } + return result; +} +#endif + +GHC_INLINE path canonical(const path& p, std::error_code& ec) +{ + if (p.empty()) { + ec = detail::make_error_code(detail::portable_error::not_found); + return path(); + } + path work = p.is_absolute() ? p : absolute(p, ec); + path root = work.root_path(); + path result; + + auto fs = status(work, ec); + if (ec) { + return path(); + } + if (fs.type() == file_type::not_found) { + ec = detail::make_error_code(detail::portable_error::not_found); + return path(); + } + bool redo; + do { + redo = false; + result.clear(); + for (auto pe : work) { + if (pe.empty() || pe == ".") { + continue; + } + else if (pe == "..") { + result = result.parent_path(); + continue; + } + else if ((result / pe).string().length() <= root.string().length()) { + result /= pe; + continue; + } + auto sls = symlink_status(result / pe, ec); + if (ec) { + return path(); + } + if (is_symlink(sls)) { + redo = true; + auto target = read_symlink(result / pe, ec); + if (ec) { + return path(); + } + if (target.is_absolute()) { + result = target; + continue; + } + else { + result /= target; + continue; + } + } + else { + result /= pe; + } + } + work = result; + } while (redo); + ec.clear(); + return result; +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE void copy(const path& from, const path& to) +{ + copy(from, to, copy_options::none); +} +#endif + +GHC_INLINE void copy(const path& from, const path& to, std::error_code& ec) noexcept +{ + copy(from, to, copy_options::none, ec); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE void copy(const path& from, const path& to, copy_options options) +{ + std::error_code ec; + copy(from, to, options, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), from, to, ec); + } +} +#endif + +GHC_INLINE void copy(const path& from, const path& to, copy_options options, std::error_code& ec) noexcept +{ + std::error_code tec; + file_status fs_from, fs_to; + ec.clear(); + if ((options & (copy_options::skip_symlinks | copy_options::copy_symlinks | copy_options::create_symlinks)) != copy_options::none) { + fs_from = symlink_status(from, ec); + } + else { + fs_from = status(from, ec); + } + if (!exists(fs_from)) { + if (!ec) { + ec = detail::make_error_code(detail::portable_error::not_found); + } + return; + } + if ((options & (copy_options::skip_symlinks | copy_options::create_symlinks)) != copy_options::none) { + fs_to = symlink_status(to, tec); + } + else { + fs_to = status(to, tec); + } + if (is_other(fs_from) || is_other(fs_to) || (is_directory(fs_from) && is_regular_file(fs_to)) || (exists(fs_to) && equivalent(from, to, ec))) { + ec = detail::make_error_code(detail::portable_error::invalid_argument); + } + else if (is_symlink(fs_from)) { + if ((options & copy_options::skip_symlinks) == copy_options::none) { + if (!exists(fs_to) && (options & copy_options::copy_symlinks) != copy_options::none) { + copy_symlink(from, to, ec); + } + else { + ec = detail::make_error_code(detail::portable_error::invalid_argument); + } + } + } + else if (is_regular_file(fs_from)) { + if ((options & copy_options::directories_only) == copy_options::none) { + if ((options & copy_options::create_symlinks) != copy_options::none) { + create_symlink(from.is_absolute() ? from : canonical(from, ec), to, ec); + } +#ifndef GHC_OS_WEB + else if ((options & copy_options::create_hard_links) != copy_options::none) { + create_hard_link(from, to, ec); + } +#endif + else if (is_directory(fs_to)) { + copy_file(from, to / from.filename(), options, ec); + } + else { + copy_file(from, to, options, ec); + } + } + } +#ifdef LWG_2682_BEHAVIOUR + else if (is_directory(fs_from) && (options & copy_options::create_symlinks) != copy_options::none) { + ec = detail::make_error_code(detail::portable_error::is_a_directory); + } +#endif + else if (is_directory(fs_from) && (options == copy_options::none || (options & copy_options::recursive) != copy_options::none)) { + if (!exists(fs_to)) { + create_directory(to, from, ec); + if (ec) { + return; + } + } + for (auto iter = directory_iterator(from, ec); iter != directory_iterator(); iter.increment(ec)) { + if (!ec) { + copy(iter->path(), to / iter->path().filename(), options | static_cast(0x8000), ec); + } + if (ec) { + return; + } + } + } + return; +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool copy_file(const path& from, const path& to) +{ + return copy_file(from, to, copy_options::none); +} +#endif + +GHC_INLINE bool copy_file(const path& from, const path& to, std::error_code& ec) noexcept +{ + return copy_file(from, to, copy_options::none, ec); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool copy_file(const path& from, const path& to, copy_options option) +{ + std::error_code ec; + auto result = copy_file(from, to, option, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), from, to, ec); + } + return result; +} +#endif + +GHC_INLINE bool copy_file(const path& from, const path& to, copy_options options, std::error_code& ec) noexcept +{ + std::error_code tecf, tect; + auto sf = status(from, tecf); + auto st = status(to, tect); + bool overwrite = false; + ec.clear(); + if (!is_regular_file(sf)) { + ec = tecf; + return false; + } + if (exists(st) && (!is_regular_file(st) || equivalent(from, to, ec) || (options & (copy_options::skip_existing | copy_options::overwrite_existing | copy_options::update_existing)) == copy_options::none)) { + ec = tect ? tect : detail::make_error_code(detail::portable_error::exists); + return false; + } + if (exists(st)) { + if ((options & copy_options::update_existing) == copy_options::update_existing) { + auto from_time = last_write_time(from, ec); + if (ec) { + ec = detail::make_system_error(); + return false; + } + auto to_time = last_write_time(to, ec); + if (ec) { + ec = detail::make_system_error(); + return false; + } + if (from_time <= to_time) { + return false; + } + } + overwrite = true; + } +#ifdef GHC_OS_WINDOWS + if (!::CopyFileW(detail::fromUtf8(from.u8string()).c_str(), detail::fromUtf8(to.u8string()).c_str(), !overwrite)) { + ec = detail::make_system_error(); + return false; + } + return true; +#else + std::vector buffer(16384, '\0'); + int in = -1, out = -1; + if ((in = ::open(from.c_str(), O_RDONLY)) < 0) { + ec = detail::make_system_error(); + return false; + } + int mode = O_CREAT | O_WRONLY | O_TRUNC; + if (!overwrite) { + mode |= O_EXCL; + } + if ((out = ::open(to.c_str(), mode, static_cast(sf.permissions() & perms::all))) < 0) { + ec = detail::make_system_error(); + ::close(in); + return false; + } + ssize_t br, bw; + while ((br = ::read(in, buffer.data(), buffer.size())) > 0) { + ssize_t offset = 0; + do { + if ((bw = ::write(out, buffer.data() + offset, static_cast(br))) > 0) { + br -= bw; + offset += bw; + } + else if (bw < 0) { + ec = detail::make_system_error(); + ::close(in); + ::close(out); + return false; + } + } while (br); + } + ::close(in); + ::close(out); + return true; +#endif +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE void copy_symlink(const path& existing_symlink, const path& new_symlink) +{ + std::error_code ec; + copy_symlink(existing_symlink, new_symlink, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), existing_symlink, new_symlink, ec); + } +} +#endif + +GHC_INLINE void copy_symlink(const path& existing_symlink, const path& new_symlink, std::error_code& ec) noexcept +{ + ec.clear(); + auto to = read_symlink(existing_symlink, ec); + if (!ec) { + if (exists(to, ec) && is_directory(to, ec)) { + create_directory_symlink(to, new_symlink, ec); + } + else { + create_symlink(to, new_symlink, ec); + } + } +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool create_directories(const path& p) +{ + std::error_code ec; + auto result = create_directories(p, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), p, ec); + } + return result; +} +#endif + +GHC_INLINE bool create_directories(const path& p, std::error_code& ec) noexcept +{ + path current; + ec.clear(); + bool didCreate = false; + for (path::string_type part : p) { + current /= part; + if (current != p.root_name() && current != p.root_path()) { + std::error_code tec; + auto fs = status(current, tec); + if (tec && fs.type() != file_type::not_found) { + ec = tec; + return false; + } + if (!exists(fs)) { + create_directory(current, ec); + if (ec) { + std::error_code tmp_ec; + if (is_directory(current, tmp_ec)) { + ec.clear(); + } else { + return false; + } + } + didCreate = true; + } +#ifndef LWG_2935_BEHAVIOUR + else if (!is_directory(fs)) { + ec = detail::make_error_code(detail::portable_error::exists); + return false; + } +#endif + } + } + return didCreate; +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool create_directory(const path& p) +{ + std::error_code ec; + auto result = create_directory(p, path(), ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), p, ec); + } + return result; +} +#endif + +GHC_INLINE bool create_directory(const path& p, std::error_code& ec) noexcept +{ + return create_directory(p, path(), ec); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool create_directory(const path& p, const path& attributes) +{ + std::error_code ec; + auto result = create_directory(p, attributes, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), p, ec); + } + return result; +} +#endif + +GHC_INLINE bool create_directory(const path& p, const path& attributes, std::error_code& ec) noexcept +{ + std::error_code tec; + ec.clear(); + auto fs = status(p, tec); +#ifdef LWG_2935_BEHAVIOUR + if (status_known(fs) && exists(fs)) { + return false; + } +#else + if (status_known(fs) && exists(fs) && is_directory(fs)) { + return false; + } +#endif +#ifdef GHC_OS_WINDOWS + if (!attributes.empty()) { + if (!::CreateDirectoryExW(detail::fromUtf8(attributes.u8string()).c_str(), detail::fromUtf8(p.u8string()).c_str(), NULL)) { + ec = detail::make_system_error(); + return false; + } + } + else if (!::CreateDirectoryW(detail::fromUtf8(p.u8string()).c_str(), NULL)) { + ec = detail::make_system_error(); + return false; + } +#else + ::mode_t attribs = static_cast(perms::all); + if (!attributes.empty()) { + struct ::stat fileStat; + if (::stat(attributes.c_str(), &fileStat) != 0) { + ec = detail::make_system_error(); + return false; + } + attribs = fileStat.st_mode; + } + if (::mkdir(p.c_str(), attribs) != 0) { + ec = detail::make_system_error(); + return false; + } +#endif + return true; +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE void create_directory_symlink(const path& to, const path& new_symlink) +{ + std::error_code ec; + create_directory_symlink(to, new_symlink, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), to, new_symlink, ec); + } +} +#endif + +GHC_INLINE void create_directory_symlink(const path& to, const path& new_symlink, std::error_code& ec) noexcept +{ + detail::create_symlink(to, new_symlink, true, ec); +} + +#ifndef GHC_OS_WEB +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE void create_hard_link(const path& to, const path& new_hard_link) +{ + std::error_code ec; + create_hard_link(to, new_hard_link, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), to, new_hard_link, ec); + } +} +#endif + +GHC_INLINE void create_hard_link(const path& to, const path& new_hard_link, std::error_code& ec) noexcept +{ + detail::create_hardlink(to, new_hard_link, ec); +} +#endif + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE void create_symlink(const path& to, const path& new_symlink) +{ + std::error_code ec; + create_symlink(to, new_symlink, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), to, new_symlink, ec); + } +} +#endif + +GHC_INLINE void create_symlink(const path& to, const path& new_symlink, std::error_code& ec) noexcept +{ + detail::create_symlink(to, new_symlink, false, ec); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE path current_path() +{ + std::error_code ec; + auto result = current_path(ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), ec); + } + return result; +} +#endif + +GHC_INLINE path current_path(std::error_code& ec) +{ + ec.clear(); +#ifdef GHC_OS_WINDOWS + DWORD pathlen = ::GetCurrentDirectoryW(0, 0); + std::unique_ptr buffer(new wchar_t[size_t(pathlen) + 1]); + if (::GetCurrentDirectoryW(pathlen, buffer.get()) == 0) { + ec = detail::make_system_error(); + return path(); + } + return path(std::wstring(buffer.get()), path::native_format); +#else + size_t pathlen = static_cast(std::max(int(::pathconf(".", _PC_PATH_MAX)), int(PATH_MAX))); + std::unique_ptr buffer(new char[pathlen + 1]); + if (::getcwd(buffer.get(), pathlen) == nullptr) { + ec = detail::make_system_error(); + return path(); + } + return path(buffer.get()); +#endif +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE void current_path(const path& p) +{ + std::error_code ec; + current_path(p, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), p, ec); + } +} +#endif + +GHC_INLINE void current_path(const path& p, std::error_code& ec) noexcept +{ + ec.clear(); +#ifdef GHC_OS_WINDOWS + if (!::SetCurrentDirectoryW(detail::fromUtf8(p.u8string()).c_str())) { + ec = detail::make_system_error(); + } +#else + if (::chdir(p.string().c_str()) == -1) { + ec = detail::make_system_error(); + } +#endif +} + +GHC_INLINE bool exists(file_status s) noexcept +{ + return status_known(s) && s.type() != file_type::not_found; +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool exists(const path& p) +{ + return exists(status(p)); +} +#endif + +GHC_INLINE bool exists(const path& p, std::error_code& ec) noexcept +{ + file_status s = status(p, ec); + if (status_known(s)) { + ec.clear(); + } + return exists(s); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool equivalent(const path& p1, const path& p2) +{ + std::error_code ec; + bool result = equivalent(p1, p2, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), p1, p2, ec); + } + return result; +} +#endif + +GHC_INLINE bool equivalent(const path& p1, const path& p2, std::error_code& ec) noexcept +{ + ec.clear(); +#ifdef GHC_OS_WINDOWS + std::shared_ptr file1(::CreateFileW(p1.wstring().c_str(), 0, FILE_SHARE_READ | FILE_SHARE_WRITE | FILE_SHARE_DELETE, 0, OPEN_EXISTING, FILE_FLAG_BACKUP_SEMANTICS, 0), CloseHandle); + auto e1 = ::GetLastError(); + std::shared_ptr file2(::CreateFileW(p2.wstring().c_str(), 0, FILE_SHARE_READ | FILE_SHARE_WRITE | FILE_SHARE_DELETE, 0, OPEN_EXISTING, FILE_FLAG_BACKUP_SEMANTICS, 0), CloseHandle); + if (file1.get() == INVALID_HANDLE_VALUE || file2.get() == INVALID_HANDLE_VALUE) { +#ifdef LWG_2937_BEHAVIOUR + ec = detail::make_system_error(e1 ? e1 : ::GetLastError()); +#else + if (file1 == file2) { + ec = detail::make_system_error(e1 ? e1 : ::GetLastError()); + } +#endif + return false; + } + BY_HANDLE_FILE_INFORMATION inf1, inf2; + if (!::GetFileInformationByHandle(file1.get(), &inf1)) { + ec = detail::make_system_error(); + return false; + } + if (!::GetFileInformationByHandle(file2.get(), &inf2)) { + ec = detail::make_system_error(); + return false; + } + return inf1.ftLastWriteTime.dwLowDateTime == inf2.ftLastWriteTime.dwLowDateTime && inf1.ftLastWriteTime.dwHighDateTime == inf2.ftLastWriteTime.dwHighDateTime && inf1.nFileIndexHigh == inf2.nFileIndexHigh && inf1.nFileIndexLow == inf2.nFileIndexLow && + inf1.nFileSizeHigh == inf2.nFileSizeHigh && inf1.nFileSizeLow == inf2.nFileSizeLow && inf1.dwVolumeSerialNumber == inf2.dwVolumeSerialNumber; +#else + struct ::stat s1, s2; + auto rc1 = ::stat(p1.c_str(), &s1); + auto e1 = errno; + auto rc2 = ::stat(p2.c_str(), &s2); + if (rc1 || rc2) { +#ifdef LWG_2937_BEHAVIOUR + ec = detail::make_system_error(e1 ? e1 : errno); +#else + if (rc1 && rc2) { + ec = detail::make_system_error(e1 ? e1 : errno); + } +#endif + return false; + } + return s1.st_dev == s2.st_dev && s1.st_ino == s2.st_ino && s1.st_size == s2.st_size && s1.st_mtime == s2.st_mtime; +#endif +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE uintmax_t file_size(const path& p) +{ + std::error_code ec; + auto result = file_size(p, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), p, ec); + } + return result; +} +#endif + +GHC_INLINE uintmax_t file_size(const path& p, std::error_code& ec) noexcept +{ + ec.clear(); +#ifdef GHC_OS_WINDOWS + WIN32_FILE_ATTRIBUTE_DATA attr; + if (!GetFileAttributesExW(detail::fromUtf8(p.u8string()).c_str(), GetFileExInfoStandard, &attr)) { + ec = detail::make_system_error(); + return static_cast(-1); + } + return static_cast(attr.nFileSizeHigh) << (sizeof(attr.nFileSizeHigh) * 8) | attr.nFileSizeLow; +#else + struct ::stat fileStat; + if (::stat(p.c_str(), &fileStat) == -1) { + ec = detail::make_system_error(); + return static_cast(-1); + } + return static_cast(fileStat.st_size); +#endif +} + +#ifndef GHC_OS_WEB +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE uintmax_t hard_link_count(const path& p) +{ + std::error_code ec; + auto result = hard_link_count(p, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), p, ec); + } + return result; +} +#endif + +GHC_INLINE uintmax_t hard_link_count(const path& p, std::error_code& ec) noexcept +{ + ec.clear(); +#ifdef GHC_OS_WINDOWS + uintmax_t result = static_cast(-1); + std::shared_ptr file(::CreateFileW(p.wstring().c_str(), 0, FILE_SHARE_READ | FILE_SHARE_WRITE | FILE_SHARE_DELETE, 0, OPEN_EXISTING, FILE_FLAG_BACKUP_SEMANTICS, 0), CloseHandle); + BY_HANDLE_FILE_INFORMATION inf; + if (file.get() == INVALID_HANDLE_VALUE) { + ec = detail::make_system_error(); + } + else { + if (!::GetFileInformationByHandle(file.get(), &inf)) { + ec = detail::make_system_error(); + } + else { + result = inf.nNumberOfLinks; + } + } + return result; +#else + uintmax_t result = 0; + file_status fs = detail::status_ex(p, ec, nullptr, nullptr, &result, nullptr); + if (fs.type() == file_type::not_found) { + ec = detail::make_error_code(detail::portable_error::not_found); + } + return ec ? static_cast(-1) : result; +#endif +} +#endif + +GHC_INLINE bool is_block_file(file_status s) noexcept +{ + return s.type() == file_type::block; +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool is_block_file(const path& p) +{ + return is_block_file(status(p)); +} +#endif + +GHC_INLINE bool is_block_file(const path& p, std::error_code& ec) noexcept +{ + return is_block_file(status(p, ec)); +} + +GHC_INLINE bool is_character_file(file_status s) noexcept +{ + return s.type() == file_type::character; +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool is_character_file(const path& p) +{ + return is_character_file(status(p)); +} +#endif + +GHC_INLINE bool is_character_file(const path& p, std::error_code& ec) noexcept +{ + return is_character_file(status(p, ec)); +} + +GHC_INLINE bool is_directory(file_status s) noexcept +{ + return s.type() == file_type::directory; +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool is_directory(const path& p) +{ + return is_directory(status(p)); +} +#endif + +GHC_INLINE bool is_directory(const path& p, std::error_code& ec) noexcept +{ + return is_directory(status(p, ec)); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool is_empty(const path& p) +{ + if (is_directory(p)) { + return directory_iterator(p) == directory_iterator(); + } + else { + return file_size(p) == 0; + } +} +#endif + +GHC_INLINE bool is_empty(const path& p, std::error_code& ec) noexcept +{ + auto fs = status(p, ec); + if (ec) { + return false; + } + if (is_directory(fs)) { + directory_iterator iter(p, ec); + if (ec) { + return false; + } + return iter == directory_iterator(); + } + else { + auto sz = file_size(p, ec); + if (ec) { + return false; + } + return sz == 0; + } +} + +GHC_INLINE bool is_fifo(file_status s) noexcept +{ + return s.type() == file_type::fifo; +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool is_fifo(const path& p) +{ + return is_fifo(status(p)); +} +#endif + +GHC_INLINE bool is_fifo(const path& p, std::error_code& ec) noexcept +{ + return is_fifo(status(p, ec)); +} + +GHC_INLINE bool is_other(file_status s) noexcept +{ + return exists(s) && !is_regular_file(s) && !is_directory(s) && !is_symlink(s); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool is_other(const path& p) +{ + return is_other(status(p)); +} +#endif + +GHC_INLINE bool is_other(const path& p, std::error_code& ec) noexcept +{ + return is_other(status(p, ec)); +} + +GHC_INLINE bool is_regular_file(file_status s) noexcept +{ + return s.type() == file_type::regular; +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool is_regular_file(const path& p) +{ + return is_regular_file(status(p)); +} +#endif + +GHC_INLINE bool is_regular_file(const path& p, std::error_code& ec) noexcept +{ + return is_regular_file(status(p, ec)); +} + +GHC_INLINE bool is_socket(file_status s) noexcept +{ + return s.type() == file_type::socket; +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool is_socket(const path& p) +{ + return is_socket(status(p)); +} +#endif + +GHC_INLINE bool is_socket(const path& p, std::error_code& ec) noexcept +{ + return is_socket(status(p, ec)); +} + +GHC_INLINE bool is_symlink(file_status s) noexcept +{ + return s.type() == file_type::symlink; +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool is_symlink(const path& p) +{ + return is_symlink(symlink_status(p)); +} +#endif + +GHC_INLINE bool is_symlink(const path& p, std::error_code& ec) noexcept +{ + return is_symlink(symlink_status(p, ec)); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE file_time_type last_write_time(const path& p) +{ + std::error_code ec; + auto result = last_write_time(p, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), p, ec); + } + return result; +} +#endif + +GHC_INLINE file_time_type last_write_time(const path& p, std::error_code& ec) noexcept +{ + time_t result = 0; + ec.clear(); + file_status fs = detail::status_ex(p, ec, nullptr, nullptr, nullptr, &result); + return ec ? (file_time_type::min)() : std::chrono::system_clock::from_time_t(result); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE void last_write_time(const path& p, file_time_type new_time) +{ + std::error_code ec; + last_write_time(p, new_time, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), p, ec); + } +} +#endif + +GHC_INLINE void last_write_time(const path& p, file_time_type new_time, std::error_code& ec) noexcept +{ + ec.clear(); + auto d = new_time.time_since_epoch(); +#ifdef GHC_OS_WINDOWS + std::shared_ptr file(::CreateFileW(p.wstring().c_str(), FILE_WRITE_ATTRIBUTES, FILE_SHARE_DELETE | FILE_SHARE_READ | FILE_SHARE_WRITE, NULL, OPEN_EXISTING, FILE_FLAG_BACKUP_SEMANTICS, NULL), ::CloseHandle); + FILETIME ft; + auto tt = std::chrono::duration_cast(d).count() * 10 + 116444736000000000; + ft.dwLowDateTime = static_cast(tt); + ft.dwHighDateTime = static_cast(tt >> 32); + if (!::SetFileTime(file.get(), 0, 0, &ft)) { + ec = detail::make_system_error(); + } +#elif defined(GHC_OS_MACOS) +#ifdef __MAC_OS_X_VERSION_MIN_REQUIRED +#if __MAC_OS_X_VERSION_MIN_REQUIRED < 101300 + struct ::stat fs; + if (::stat(p.c_str(), &fs) == 0) { + struct ::timeval tv[2]; + tv[0].tv_sec = fs.st_atimespec.tv_sec; + tv[0].tv_usec = static_cast(fs.st_atimespec.tv_nsec / 1000); + tv[1].tv_sec = std::chrono::duration_cast(d).count(); + tv[1].tv_usec = static_cast(std::chrono::duration_cast(d).count() % 1000000); + if (::utimes(p.c_str(), tv) == 0) { + return; + } + } + ec = detail::make_system_error(); + return; +#else + struct ::timespec times[2]; + times[0].tv_sec = 0; + times[0].tv_nsec = UTIME_OMIT; + times[1].tv_sec = std::chrono::duration_cast(d).count(); + times[1].tv_nsec = 0; //std::chrono::duration_cast(d).count() % 1000000000; + if (::utimensat(AT_FDCWD, p.c_str(), times, AT_SYMLINK_NOFOLLOW) != 0) { + ec = detail::make_system_error(); + } + return; +#endif +#endif +#else +#ifndef UTIME_OMIT +#define UTIME_OMIT ((1l << 30) - 2l) +#endif + struct ::timespec times[2]; + times[0].tv_sec = 0; + times[0].tv_nsec = UTIME_OMIT; + times[1].tv_sec = static_cast(std::chrono::duration_cast(d).count()); + times[1].tv_nsec = static_cast(std::chrono::duration_cast(d).count() % 1000000000); +#if defined(__ANDROID_API__) && __ANDROID_API__ < 12 + if (syscall(__NR_utimensat, AT_FDCWD, p.c_str(), times, AT_SYMLINK_NOFOLLOW) != 0) { +#else + if (::utimensat(AT_FDCWD, p.c_str(), times, AT_SYMLINK_NOFOLLOW) != 0) { +#endif + ec = detail::make_system_error(); + } + return; +#endif +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE void permissions(const path& p, perms prms, perm_options opts) +{ + std::error_code ec; + permissions(p, prms, opts, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), p, ec); + } +} +#endif + +GHC_INLINE void permissions(const path& p, perms prms, std::error_code& ec) noexcept +{ + permissions(p, prms, perm_options::replace, ec); +} + +GHC_INLINE void permissions(const path& p, perms prms, perm_options opts, std::error_code& ec) +{ + if (static_cast(opts & (perm_options::replace | perm_options::add | perm_options::remove)) == 0) { + ec = detail::make_error_code(detail::portable_error::invalid_argument); + return; + } + auto fs = symlink_status(p, ec); + if ((opts & perm_options::replace) != perm_options::replace) { + if ((opts & perm_options::add) == perm_options::add) { + prms = fs.permissions() | prms; + } + else { + prms = fs.permissions() & ~prms; + } + } +#ifdef GHC_OS_WINDOWS +#ifdef __GNUC__ + auto oldAttr = GetFileAttributesW(p.wstring().c_str()); + if (oldAttr != INVALID_FILE_ATTRIBUTES) { + DWORD newAttr = ((prms & perms::owner_write) == perms::owner_write) ? oldAttr & ~(static_cast(FILE_ATTRIBUTE_READONLY)) : oldAttr | FILE_ATTRIBUTE_READONLY; + if (oldAttr == newAttr || SetFileAttributesW(p.wstring().c_str(), newAttr)) { + return; + } + } + ec = detail::make_system_error(); +#else + int mode = 0; + if ((prms & perms::owner_read) == perms::owner_read) { + mode |= _S_IREAD; + } + if ((prms & perms::owner_write) == perms::owner_write) { + mode |= _S_IWRITE; + } + if (::_wchmod(p.wstring().c_str(), mode) != 0) { + ec = detail::make_system_error(); + } +#endif +#else + if ((opts & perm_options::nofollow) != perm_options::nofollow) { + if (::chmod(p.c_str(), static_cast(prms)) != 0) { + ec = detail::make_system_error(); + } + } +#endif +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE path proximate(const path& p, std::error_code& ec) +{ + return proximate(p, current_path(), ec); +} +#endif + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE path proximate(const path& p, const path& base) +{ + return weakly_canonical(p).lexically_proximate(weakly_canonical(base)); +} +#endif + +GHC_INLINE path proximate(const path& p, const path& base, std::error_code& ec) +{ + return weakly_canonical(p, ec).lexically_proximate(weakly_canonical(base, ec)); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE path read_symlink(const path& p) +{ + std::error_code ec; + auto result = read_symlink(p, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), p, ec); + } + return result; +} +#endif + +GHC_INLINE path read_symlink(const path& p, std::error_code& ec) +{ + file_status fs = symlink_status(p, ec); + if (fs.type() != file_type::symlink) { + ec = detail::make_error_code(detail::portable_error::invalid_argument); + return path(); + } + auto result = detail::resolveSymlink(p, ec); + return ec ? path() : result; +} + +GHC_INLINE path relative(const path& p, std::error_code& ec) +{ + return relative(p, current_path(ec), ec); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE path relative(const path& p, const path& base) +{ + return weakly_canonical(p).lexically_relative(weakly_canonical(base)); +} +#endif + +GHC_INLINE path relative(const path& p, const path& base, std::error_code& ec) +{ + return weakly_canonical(p, ec).lexically_relative(weakly_canonical(base, ec)); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool remove(const path& p) +{ + std::error_code ec; + auto result = remove(p, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), p, ec); + } + return result; +} +#endif + +GHC_INLINE bool remove(const path& p, std::error_code& ec) noexcept +{ + ec.clear(); +#ifdef GHC_OS_WINDOWS + std::wstring np = detail::fromUtf8(p.u8string()); + DWORD attr = GetFileAttributesW(np.c_str()); + if (attr == INVALID_FILE_ATTRIBUTES) { + auto error = ::GetLastError(); + if (error == ERROR_FILE_NOT_FOUND || error == ERROR_PATH_NOT_FOUND) { + return false; + } + ec = detail::make_system_error(error); + } + if (!ec) { + if (attr & FILE_ATTRIBUTE_DIRECTORY) { + if (!RemoveDirectoryW(np.c_str())) { + ec = detail::make_system_error(); + } + } + else { + if (!DeleteFileW(np.c_str())) { + ec = detail::make_system_error(); + } + } + } +#else + if (::remove(p.c_str()) == -1) { + auto error = errno; + if (error == ENOENT) { + return false; + } + ec = detail::make_system_error(); + } +#endif + return ec ? false : true; +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE uintmax_t remove_all(const path& p) +{ + std::error_code ec; + auto result = remove_all(p, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), p, ec); + } + return result; +} +#endif + +GHC_INLINE uintmax_t remove_all(const path& p, std::error_code& ec) noexcept +{ + ec.clear(); + uintmax_t count = 0; + if (p == "/") { + ec = detail::make_error_code(detail::portable_error::not_supported); + return static_cast(-1); + } + std::error_code tec; + auto fs = status(p, tec); + if (exists(fs) && is_directory(fs)) { + for (auto iter = directory_iterator(p, ec); iter != directory_iterator(); iter.increment(ec)) { + if (ec) { + break; + } + bool is_symlink_result = iter->is_symlink(ec); + if (ec) return static_cast(-1); + bool is_directory_result = iter->is_directory(ec); + if (ec) return static_cast(-1); + if (!is_symlink_result && is_directory_result) { + count += remove_all(iter->path(), ec); + if (ec) { + return static_cast(-1); + } + } + else { + remove(iter->path(), ec); + if (ec) { + return static_cast(-1); + } + ++count; + } + } + } + if (!ec) { + if (remove(p, ec)) { + ++count; + } + } + if (ec) { + return static_cast(-1); + } + return count; +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE void rename(const path& from, const path& to) +{ + std::error_code ec; + rename(from, to, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), from, to, ec); + } +} +#endif + +GHC_INLINE void rename(const path& from, const path& to, std::error_code& ec) noexcept +{ + ec.clear(); +#ifdef GHC_OS_WINDOWS + if (from != to) { + if (!MoveFileExW(detail::fromUtf8(from.u8string()).c_str(), detail::fromUtf8(to.u8string()).c_str(), (DWORD)MOVEFILE_REPLACE_EXISTING)) { + ec = detail::make_system_error(); + } + } +#else + if (from != to) { + if (::rename(from.c_str(), to.c_str()) != 0) { + ec = detail::make_system_error(); + } + } +#endif +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE void resize_file(const path& p, uintmax_t size) +{ + std::error_code ec; + resize_file(p, size, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), p, ec); + } +} +#endif + +GHC_INLINE void resize_file(const path& p, uintmax_t size, std::error_code& ec) noexcept +{ + ec.clear(); +#ifdef GHC_OS_WINDOWS + LARGE_INTEGER lisize; + lisize.QuadPart = static_cast(size); + if(lisize.QuadPart < 0) { +#ifdef ERROR_FILE_TOO_LARGE + ec = detail::make_system_error(ERROR_FILE_TOO_LARGE); +#else + ec = detail::make_system_error(223); +#endif + return; + } + std::shared_ptr file(CreateFileW(detail::fromUtf8(p.u8string()).c_str(), GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL), CloseHandle); + if (file.get() == INVALID_HANDLE_VALUE) { + ec = detail::make_system_error(); + } + else if (SetFilePointerEx(file.get(), lisize, NULL, FILE_BEGIN) == 0 || SetEndOfFile(file.get()) == 0) { + ec = detail::make_system_error(); + } +#else + if (::truncate(p.c_str(), static_cast(size)) != 0) { + ec = detail::make_system_error(); + } +#endif +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE space_info space(const path& p) +{ + std::error_code ec; + auto result = space(p, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), p, ec); + } + return result; +} +#endif + +GHC_INLINE space_info space(const path& p, std::error_code& ec) noexcept +{ + ec.clear(); +#ifdef GHC_OS_WINDOWS + ULARGE_INTEGER freeBytesAvailableToCaller = {{0, 0}}; + ULARGE_INTEGER totalNumberOfBytes = {{0, 0}}; + ULARGE_INTEGER totalNumberOfFreeBytes = {{0, 0}}; + if (!GetDiskFreeSpaceExW(detail::fromUtf8(p.u8string()).c_str(), &freeBytesAvailableToCaller, &totalNumberOfBytes, &totalNumberOfFreeBytes)) { + ec = detail::make_system_error(); + return {static_cast(-1), static_cast(-1), static_cast(-1)}; + } + return {static_cast(totalNumberOfBytes.QuadPart), static_cast(totalNumberOfFreeBytes.QuadPart), static_cast(freeBytesAvailableToCaller.QuadPart)}; +#else + struct ::statvfs sfs; + if (::statvfs(p.c_str(), &sfs) != 0) { + ec = detail::make_system_error(); + return {static_cast(-1), static_cast(-1), static_cast(-1)}; + } + return {static_cast(sfs.f_blocks * sfs.f_frsize), static_cast(sfs.f_bfree * sfs.f_frsize), static_cast(sfs.f_bavail * sfs.f_frsize)}; +#endif +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE file_status status(const path& p) +{ + std::error_code ec; + auto result = status(p, ec); + if (result.type() == file_type::none) { + throw filesystem_error(detail::systemErrorText(ec.value()), p, ec); + } + return result; +} +#endif + +GHC_INLINE file_status status(const path& p, std::error_code& ec) noexcept +{ + return detail::status_ex(p, ec); +} + +GHC_INLINE bool status_known(file_status s) noexcept +{ + return s.type() != file_type::none; +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE file_status symlink_status(const path& p) +{ + std::error_code ec; + auto result = symlink_status(p, ec); + if (result.type() == file_type::none) { + throw filesystem_error(detail::systemErrorText(ec.value()), ec); + } + return result; +} +#endif + +GHC_INLINE file_status symlink_status(const path& p, std::error_code& ec) noexcept +{ + return detail::symlink_status_ex(p, ec); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE path temp_directory_path() +{ + std::error_code ec; + path result = temp_directory_path(ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), ec); + } + return result; +} +#endif + +GHC_INLINE path temp_directory_path(std::error_code& ec) noexcept +{ + ec.clear(); +#ifdef GHC_OS_WINDOWS + wchar_t buffer[512]; + auto rc = GetTempPathW(511, buffer); + if (!rc || rc > 511) { + ec = detail::make_system_error(); + return path(); + } + return path(std::wstring(buffer)); +#else + static const char* temp_vars[] = {"TMPDIR", "TMP", "TEMP", "TEMPDIR", nullptr}; + const char* temp_path = nullptr; + for (auto temp_name = temp_vars; *temp_name != nullptr; ++temp_name) { + temp_path = std::getenv(*temp_name); + if (temp_path) { + return path(temp_path); + } + } + return path("/tmp"); +#endif +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE path weakly_canonical(const path& p) +{ + std::error_code ec; + auto result = weakly_canonical(p, ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), p, ec); + } + return result; +} +#endif + +GHC_INLINE path weakly_canonical(const path& p, std::error_code& ec) noexcept +{ + path result; + ec.clear(); + bool scan = true; + for (auto pe : p) { + if (scan) { + std::error_code tec; + if (exists(result / pe, tec)) { + result /= pe; + } + else { + if (ec) { + return path(); + } + scan = false; + if (!result.empty()) { + result = canonical(result, ec) / pe; + if (ec) { + break; + } + } + else { + result /= pe; + } + } + } + else { + result /= pe; + } + } + if (scan) { + if (!result.empty()) { + result = canonical(result, ec); + } + } + return ec ? path() : result.lexically_normal(); +} + +//----------------------------------------------------------------------------- +// 30.10.11 class file_status +// 30.10.11.1 constructors and destructor +GHC_INLINE file_status::file_status() noexcept + : file_status(file_type::none) +{ +} + +GHC_INLINE file_status::file_status(file_type ft, perms prms) noexcept + : _type(ft) + , _perms(prms) +{ +} + +GHC_INLINE file_status::file_status(const file_status& other) noexcept + : _type(other._type) + , _perms(other._perms) +{ +} + +GHC_INLINE file_status::file_status(file_status&& other) noexcept + : _type(other._type) + , _perms(other._perms) +{ +} + +GHC_INLINE file_status::~file_status() {} + +// assignments: +GHC_INLINE file_status& file_status::operator=(const file_status& rhs) noexcept +{ + _type = rhs._type; + _perms = rhs._perms; + return *this; +} + +GHC_INLINE file_status& file_status::operator=(file_status&& rhs) noexcept +{ + _type = rhs._type; + _perms = rhs._perms; + return *this; +} + +// 30.10.11.3 modifiers +GHC_INLINE void file_status::type(file_type ft) noexcept +{ + _type = ft; +} + +GHC_INLINE void file_status::permissions(perms prms) noexcept +{ + _perms = prms; +} + +// 30.10.11.2 observers +GHC_INLINE file_type file_status::type() const noexcept +{ + return _type; +} + +GHC_INLINE perms file_status::permissions() const noexcept +{ + return _perms; +} + +//----------------------------------------------------------------------------- +// 30.10.12 class directory_entry +// 30.10.12.1 constructors and destructor +// directory_entry::directory_entry() noexcept = default; +// directory_entry::directory_entry(const directory_entry&) = default; +// directory_entry::directory_entry(directory_entry&&) noexcept = default; +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE directory_entry::directory_entry(const filesystem::path& p) + : _path(p) + , _file_size(0) +#ifndef GHC_OS_WINDOWS + , _hard_link_count(0) +#endif + , _last_write_time(0) +{ + refresh(); +} +#endif + +GHC_INLINE directory_entry::directory_entry(const filesystem::path& p, std::error_code& ec) + : _path(p) + , _file_size(0) +#ifndef GHC_OS_WINDOWS + , _hard_link_count(0) +#endif + , _last_write_time(0) +{ + refresh(ec); +} + +GHC_INLINE directory_entry::~directory_entry() {} + +// assignments: +// directory_entry& directory_entry::operator=(const directory_entry&) = default; +// directory_entry& directory_entry::operator=(directory_entry&&) noexcept = default; + +// 30.10.12.2 directory_entry modifiers +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE void directory_entry::assign(const filesystem::path& p) +{ + _path = p; + refresh(); +} +#endif + +GHC_INLINE void directory_entry::assign(const filesystem::path& p, std::error_code& ec) +{ + _path = p; + refresh(ec); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE void directory_entry::replace_filename(const filesystem::path& p) +{ + _path.replace_filename(p); + refresh(); +} +#endif + +GHC_INLINE void directory_entry::replace_filename(const filesystem::path& p, std::error_code& ec) +{ + _path.replace_filename(p); + refresh(ec); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE void directory_entry::refresh() +{ + std::error_code ec; + refresh(ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), _path, ec); + } +} +#endif + +GHC_INLINE void directory_entry::refresh(std::error_code& ec) noexcept +{ +#ifdef GHC_OS_WINDOWS + _status = detail::status_ex(_path, ec, &_symlink_status, &_file_size, nullptr, &_last_write_time); +#else + _status = detail::status_ex(_path, ec, &_symlink_status, &_file_size, &_hard_link_count, &_last_write_time); +#endif +} + +// 30.10.12.3 directory_entry observers +GHC_INLINE const filesystem::path& directory_entry::path() const noexcept +{ + return _path; +} + +GHC_INLINE directory_entry::operator const filesystem::path&() const noexcept +{ + return _path; +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool directory_entry::exists() const +{ + return filesystem::exists(status()); +} +#endif + +GHC_INLINE bool directory_entry::exists(std::error_code& ec) const noexcept +{ + return filesystem::exists(status(ec)); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool directory_entry::is_block_file() const +{ + return filesystem::is_block_file(status()); +} +#endif +GHC_INLINE bool directory_entry::is_block_file(std::error_code& ec) const noexcept +{ + return filesystem::is_block_file(status(ec)); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool directory_entry::is_character_file() const +{ + return filesystem::is_character_file(status()); +} +#endif + +GHC_INLINE bool directory_entry::is_character_file(std::error_code& ec) const noexcept +{ + return filesystem::is_character_file(status(ec)); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool directory_entry::is_directory() const +{ + return filesystem::is_directory(status()); +} +#endif + +GHC_INLINE bool directory_entry::is_directory(std::error_code& ec) const noexcept +{ + return filesystem::is_directory(status(ec)); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool directory_entry::is_fifo() const +{ + return filesystem::is_fifo(status()); +} +#endif + +GHC_INLINE bool directory_entry::is_fifo(std::error_code& ec) const noexcept +{ + return filesystem::is_fifo(status(ec)); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool directory_entry::is_other() const +{ + return filesystem::is_other(status()); +} +#endif + +GHC_INLINE bool directory_entry::is_other(std::error_code& ec) const noexcept +{ + return filesystem::is_other(status(ec)); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool directory_entry::is_regular_file() const +{ + return filesystem::is_regular_file(status()); +} +#endif + +GHC_INLINE bool directory_entry::is_regular_file(std::error_code& ec) const noexcept +{ + return filesystem::is_regular_file(status(ec)); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool directory_entry::is_socket() const +{ + return filesystem::is_socket(status()); +} +#endif + +GHC_INLINE bool directory_entry::is_socket(std::error_code& ec) const noexcept +{ + return filesystem::is_socket(status(ec)); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE bool directory_entry::is_symlink() const +{ + return filesystem::is_symlink(symlink_status()); +} +#endif + +GHC_INLINE bool directory_entry::is_symlink(std::error_code& ec) const noexcept +{ + return filesystem::is_symlink(symlink_status(ec)); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE uintmax_t directory_entry::file_size() const +{ + if (_status.type() != file_type::none) { + return _file_size; + } + return filesystem::file_size(path()); +} +#endif + +GHC_INLINE uintmax_t directory_entry::file_size(std::error_code& ec) const noexcept +{ + if (_status.type() != file_type::none) { + ec.clear(); + return _file_size; + } + return filesystem::file_size(path(), ec); +} + +#ifndef GHC_OS_WEB +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE uintmax_t directory_entry::hard_link_count() const +{ +#ifndef GHC_OS_WINDOWS + if (_status.type() != file_type::none) { + return _hard_link_count; + } +#endif + return filesystem::hard_link_count(path()); +} +#endif + +GHC_INLINE uintmax_t directory_entry::hard_link_count(std::error_code& ec) const noexcept +{ +#ifndef GHC_OS_WINDOWS + if (_status.type() != file_type::none) { + ec.clear(); + return _hard_link_count; + } +#endif + return filesystem::hard_link_count(path(), ec); +} +#endif + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE file_time_type directory_entry::last_write_time() const +{ + if (_status.type() != file_type::none) { + return std::chrono::system_clock::from_time_t(_last_write_time); + } + return filesystem::last_write_time(path()); +} +#endif + +GHC_INLINE file_time_type directory_entry::last_write_time(std::error_code& ec) const noexcept +{ + if (_status.type() != file_type::none) { + ec.clear(); + return std::chrono::system_clock::from_time_t(_last_write_time); + } + return filesystem::last_write_time(path(), ec); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE file_status directory_entry::status() const +{ + if (_status.type() != file_type::none) { + return _status; + } + return filesystem::status(path()); +} +#endif + +GHC_INLINE file_status directory_entry::status(std::error_code& ec) const noexcept +{ + if (_status.type() != file_type::none) { + ec.clear(); + return _status; + } + return filesystem::status(path(), ec); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE file_status directory_entry::symlink_status() const +{ + if (_symlink_status.type() != file_type::none) { + return _symlink_status; + } + return filesystem::symlink_status(path()); +} +#endif + +GHC_INLINE file_status directory_entry::symlink_status(std::error_code& ec) const noexcept +{ + if (_symlink_status.type() != file_type::none) { + ec.clear(); + return _symlink_status; + } + return filesystem::symlink_status(path(), ec); +} + +GHC_INLINE bool directory_entry::operator<(const directory_entry& rhs) const noexcept +{ + return _path < rhs._path; +} + +GHC_INLINE bool directory_entry::operator==(const directory_entry& rhs) const noexcept +{ + return _path == rhs._path; +} + +GHC_INLINE bool directory_entry::operator!=(const directory_entry& rhs) const noexcept +{ + return _path != rhs._path; +} + +GHC_INLINE bool directory_entry::operator<=(const directory_entry& rhs) const noexcept +{ + return _path <= rhs._path; +} + +GHC_INLINE bool directory_entry::operator>(const directory_entry& rhs) const noexcept +{ + return _path > rhs._path; +} + +GHC_INLINE bool directory_entry::operator>=(const directory_entry& rhs) const noexcept +{ + return _path >= rhs._path; +} + +//----------------------------------------------------------------------------- +// 30.10.13 class directory_iterator + +#ifdef GHC_OS_WINDOWS +class directory_iterator::impl +{ +public: + impl(const path& p, directory_options options) + : _base(p) + , _options(options) + , _dirHandle(INVALID_HANDLE_VALUE) + { + if (!_base.empty()) { + ZeroMemory(&_findData, sizeof(WIN32_FIND_DATAW)); + if ((_dirHandle = FindFirstFileW(detail::fromUtf8((_base / "*").u8string()).c_str(), &_findData)) != INVALID_HANDLE_VALUE) { + if (std::wstring(_findData.cFileName) == L"." || std::wstring(_findData.cFileName) == L"..") { + increment(_ec); + } + else { + _current = _base / std::wstring(_findData.cFileName); + copyToDirEntry(_ec); + } + } + else { + auto error = ::GetLastError(); + _base = filesystem::path(); + if (error != ERROR_ACCESS_DENIED || (options & directory_options::skip_permission_denied) == directory_options::none) { + _ec = detail::make_system_error(); + } + } + } + } + impl(const impl& other) = delete; + ~impl() + { + if (_dirHandle != INVALID_HANDLE_VALUE) { + FindClose(_dirHandle); + _dirHandle = INVALID_HANDLE_VALUE; + } + } + void increment(std::error_code& ec) + { + if (_dirHandle != INVALID_HANDLE_VALUE) { + do { + if (FindNextFileW(_dirHandle, &_findData)) { + _current = _base; +#ifdef GHC_RAISE_UNICODE_ERRORS + try { + _current.append_name(detail::toUtf8(_findData.cFileName).c_str()); + } + catch(filesystem_error& fe) { + ec = fe.code(); + return; + } +#else + _current.append_name(detail::toUtf8(_findData.cFileName).c_str()); +#endif + copyToDirEntry(ec); + } + else { + auto err = ::GetLastError(); + if(err != ERROR_NO_MORE_FILES) { + _ec = ec = detail::make_system_error(err); + } + FindClose(_dirHandle); + _dirHandle = INVALID_HANDLE_VALUE; + _current = filesystem::path(); + break; + } + } while (std::wstring(_findData.cFileName) == L"." || std::wstring(_findData.cFileName) == L".."); + } + else { + ec = _ec; + } + } + void copyToDirEntry(std::error_code& ec) + { + _dir_entry._path = _current; + if (_findData.dwFileAttributes & FILE_ATTRIBUTE_REPARSE_POINT) { + _dir_entry._status = detail::status_ex(_current, ec, &_dir_entry._symlink_status, &_dir_entry._file_size, nullptr, &_dir_entry._last_write_time); + } + else { + _dir_entry._status = detail::status_from_INFO(_current, &_findData, ec, &_dir_entry._file_size, &_dir_entry._last_write_time); + _dir_entry._symlink_status = _dir_entry._status; + } + if (ec) { + if (_dir_entry._status.type() != file_type::none && _dir_entry._symlink_status.type() != file_type::none) { + ec.clear(); + } + else { + _dir_entry._file_size = static_cast(-1); + _dir_entry._last_write_time = 0; + } + } + } + path _base; + directory_options _options; + WIN32_FIND_DATAW _findData; + HANDLE _dirHandle; + path _current; + directory_entry _dir_entry; + std::error_code _ec; +}; +#else +// POSIX implementation +class directory_iterator::impl +{ +public: + impl(const path& path, directory_options options) + : _base(path) + , _options(options) + , _dir(nullptr) + , _entry(nullptr) + { + if (!path.empty()) { + _dir = ::opendir(path.native().c_str()); + } + if (!path.empty()) { + if (!_dir) { + auto error = errno; + _base = filesystem::path(); + if (error != EACCES || (options & directory_options::skip_permission_denied) == directory_options::none) { + _ec = detail::make_system_error(); + } + } + else { + increment(_ec); + } + } + } + impl(const impl& other) = delete; + ~impl() + { + if (_dir) { + ::closedir(_dir); + } + } + void increment(std::error_code& ec) + { + if (_dir) { + bool skip; + do { + skip = false; + errno = 0; + _entry = ::readdir(_dir); + if (_entry) { + _current = _base; + _current.append_name(_entry->d_name); + _dir_entry = directory_entry(_current, ec); + if(ec && (ec.value() == EACCES || ec.value() == EPERM) && (_options & directory_options::skip_permission_denied) == directory_options::skip_permission_denied) { + ec.clear(); + skip = true; + } + } + else { + ::closedir(_dir); + _dir = nullptr; + _current = path(); + if (errno) { + ec = detail::make_system_error(); + } + break; + } + } while (skip || std::strcmp(_entry->d_name, ".") == 0 || std::strcmp(_entry->d_name, "..") == 0); + } + } + path _base; + directory_options _options; + path _current; + DIR* _dir; + struct ::dirent* _entry; + directory_entry _dir_entry; + std::error_code _ec; +}; +#endif + +// 30.10.13.1 member functions +GHC_INLINE directory_iterator::directory_iterator() noexcept + : _impl(new impl(path(), directory_options::none)) +{ +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE directory_iterator::directory_iterator(const path& p) + : _impl(new impl(p, directory_options::none)) +{ + if (_impl->_ec) { + throw filesystem_error(detail::systemErrorText(_impl->_ec.value()), p, _impl->_ec); + } + _impl->_ec.clear(); +} + +GHC_INLINE directory_iterator::directory_iterator(const path& p, directory_options options) + : _impl(new impl(p, options)) +{ + if (_impl->_ec) { + throw filesystem_error(detail::systemErrorText(_impl->_ec.value()), p, _impl->_ec); + } +} +#endif + +GHC_INLINE directory_iterator::directory_iterator(const path& p, std::error_code& ec) noexcept + : _impl(new impl(p, directory_options::none)) +{ + if (_impl->_ec) { + ec = _impl->_ec; + } +} + +GHC_INLINE directory_iterator::directory_iterator(const path& p, directory_options options, std::error_code& ec) noexcept + : _impl(new impl(p, options)) +{ + if (_impl->_ec) { + ec = _impl->_ec; + } +} + +GHC_INLINE directory_iterator::directory_iterator(const directory_iterator& rhs) + : _impl(rhs._impl) +{ +} + +GHC_INLINE directory_iterator::directory_iterator(directory_iterator&& rhs) noexcept + : _impl(std::move(rhs._impl)) +{ +} + +GHC_INLINE directory_iterator::~directory_iterator() {} + +GHC_INLINE directory_iterator& directory_iterator::operator=(const directory_iterator& rhs) +{ + _impl = rhs._impl; + return *this; +} + +GHC_INLINE directory_iterator& directory_iterator::operator=(directory_iterator&& rhs) noexcept +{ + _impl = std::move(rhs._impl); + return *this; +} + +GHC_INLINE const directory_entry& directory_iterator::operator*() const +{ + return _impl->_dir_entry; +} + +GHC_INLINE const directory_entry* directory_iterator::operator->() const +{ + return &_impl->_dir_entry; +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE directory_iterator& directory_iterator::operator++() +{ + std::error_code ec; + _impl->increment(ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), _impl->_current, ec); + } + return *this; +} +#endif + +GHC_INLINE directory_iterator& directory_iterator::increment(std::error_code& ec) noexcept +{ + _impl->increment(ec); + return *this; +} + +GHC_INLINE bool directory_iterator::operator==(const directory_iterator& rhs) const +{ + return _impl->_current == rhs._impl->_current; +} + +GHC_INLINE bool directory_iterator::operator!=(const directory_iterator& rhs) const +{ + return _impl->_current != rhs._impl->_current; +} + +// 30.10.13.2 directory_iterator non-member functions + +GHC_INLINE directory_iterator begin(directory_iterator iter) noexcept +{ + return iter; +} + +GHC_INLINE directory_iterator end(const directory_iterator&) noexcept +{ + return directory_iterator(); +} + +//----------------------------------------------------------------------------- +// 30.10.14 class recursive_directory_iterator + +GHC_INLINE recursive_directory_iterator::recursive_directory_iterator() noexcept + : _impl(new recursive_directory_iterator_impl(directory_options::none, true)) +{ + _impl->_dir_iter_stack.push(directory_iterator()); +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE recursive_directory_iterator::recursive_directory_iterator(const path& p) + : _impl(new recursive_directory_iterator_impl(directory_options::none, true)) +{ + _impl->_dir_iter_stack.push(directory_iterator(p)); +} + +GHC_INLINE recursive_directory_iterator::recursive_directory_iterator(const path& p, directory_options options) + : _impl(new recursive_directory_iterator_impl(options, true)) +{ + _impl->_dir_iter_stack.push(directory_iterator(p, options)); +} +#endif + +GHC_INLINE recursive_directory_iterator::recursive_directory_iterator(const path& p, directory_options options, std::error_code& ec) noexcept + : _impl(new recursive_directory_iterator_impl(options, true)) +{ + _impl->_dir_iter_stack.push(directory_iterator(p, options, ec)); +} + +GHC_INLINE recursive_directory_iterator::recursive_directory_iterator(const path& p, std::error_code& ec) noexcept + : _impl(new recursive_directory_iterator_impl(directory_options::none, true)) +{ + _impl->_dir_iter_stack.push(directory_iterator(p, ec)); +} + +GHC_INLINE recursive_directory_iterator::recursive_directory_iterator(const recursive_directory_iterator& rhs) + : _impl(rhs._impl) +{ +} + +GHC_INLINE recursive_directory_iterator::recursive_directory_iterator(recursive_directory_iterator&& rhs) noexcept + : _impl(std::move(rhs._impl)) +{ +} + +GHC_INLINE recursive_directory_iterator::~recursive_directory_iterator() {} + +// 30.10.14.1 observers +GHC_INLINE directory_options recursive_directory_iterator::options() const +{ + return _impl->_options; +} + +GHC_INLINE int recursive_directory_iterator::depth() const +{ + return static_cast(_impl->_dir_iter_stack.size() - 1); +} + +GHC_INLINE bool recursive_directory_iterator::recursion_pending() const +{ + return _impl->_recursion_pending; +} + +GHC_INLINE const directory_entry& recursive_directory_iterator::operator*() const +{ + return *(_impl->_dir_iter_stack.top()); +} + +GHC_INLINE const directory_entry* recursive_directory_iterator::operator->() const +{ + return &(*(_impl->_dir_iter_stack.top())); +} + +// 30.10.14.1 modifiers recursive_directory_iterator& +GHC_INLINE recursive_directory_iterator& recursive_directory_iterator::operator=(const recursive_directory_iterator& rhs) +{ + _impl = rhs._impl; + return *this; +} + +GHC_INLINE recursive_directory_iterator& recursive_directory_iterator::operator=(recursive_directory_iterator&& rhs) noexcept +{ + _impl = std::move(rhs._impl); + return *this; +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE recursive_directory_iterator& recursive_directory_iterator::operator++() +{ + std::error_code ec; + increment(ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), _impl->_dir_iter_stack.empty() ? path() : _impl->_dir_iter_stack.top()->path(), ec); + } + return *this; +} +#endif + +GHC_INLINE recursive_directory_iterator& recursive_directory_iterator::increment(std::error_code& ec) noexcept +{ + auto status = (*this)->status(ec); + if (ec) return *this; + auto symlink_status = (*this)->symlink_status(ec); + if (ec) return *this; + if (recursion_pending() && is_directory(status) && (!is_symlink(symlink_status) || (options() & directory_options::follow_directory_symlink) != directory_options::none)) { + _impl->_dir_iter_stack.push(directory_iterator((*this)->path(), _impl->_options, ec)); + } + else { + _impl->_dir_iter_stack.top().increment(ec); + } + if (!ec) { + while (depth() && _impl->_dir_iter_stack.top() == directory_iterator()) { + _impl->_dir_iter_stack.pop(); + _impl->_dir_iter_stack.top().increment(ec); + } + } + else if (!_impl->_dir_iter_stack.empty()) { + _impl->_dir_iter_stack.pop(); + } + _impl->_recursion_pending = true; + return *this; +} + +#ifdef GHC_WITH_EXCEPTIONS +GHC_INLINE void recursive_directory_iterator::pop() +{ + std::error_code ec; + pop(ec); + if (ec) { + throw filesystem_error(detail::systemErrorText(ec.value()), _impl->_dir_iter_stack.empty() ? path() : _impl->_dir_iter_stack.top()->path(), ec); + } +} +#endif + +GHC_INLINE void recursive_directory_iterator::pop(std::error_code& ec) +{ + if (depth() == 0) { + *this = recursive_directory_iterator(); + } + else { + do { + _impl->_dir_iter_stack.pop(); + _impl->_dir_iter_stack.top().increment(ec); + } while (depth() && _impl->_dir_iter_stack.top() == directory_iterator()); + } +} + +GHC_INLINE void recursive_directory_iterator::disable_recursion_pending() +{ + _impl->_recursion_pending = false; +} + +// other members as required by 27.2.3, input iterators +GHC_INLINE bool recursive_directory_iterator::operator==(const recursive_directory_iterator& rhs) const +{ + return _impl->_dir_iter_stack.top() == rhs._impl->_dir_iter_stack.top(); +} + +GHC_INLINE bool recursive_directory_iterator::operator!=(const recursive_directory_iterator& rhs) const +{ + return _impl->_dir_iter_stack.top() != rhs._impl->_dir_iter_stack.top(); +} + +// 30.10.14.2 directory_iterator non-member functions +GHC_INLINE recursive_directory_iterator begin(recursive_directory_iterator iter) noexcept +{ + return iter; +} + +GHC_INLINE recursive_directory_iterator end(const recursive_directory_iterator&) noexcept +{ + return recursive_directory_iterator(); +} + +#endif // GHC_EXPAND_IMPL + +} // namespace filesystem +} // namespace ghc + +// cleanup some macros +#undef GHC_INLINE +#undef GHC_EXPAND_IMPL + +#endif // GHC_FILESYSTEM_H diff --git a/common/autoware_auto_common/include/common/type_traits.hpp b/common/autoware_auto_common/include/common/type_traits.hpp new file mode 100644 index 0000000000000..ed0ab1709d92c --- /dev/null +++ b/common/autoware_auto_common/include/common/type_traits.hpp @@ -0,0 +1,206 @@ +// Copyright 2021 Apex.AI, 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. +// +// Developed by Apex.AI, Inc. + +#include +#include + +#include +#include +#include + +#ifndef COMMON__TYPE_TRAITS_HPP_ +#define COMMON__TYPE_TRAITS_HPP_ + +namespace autoware +{ +namespace common +{ +namespace type_traits +{ + +/// +/// @brief A helper function to be used in static_assert to indicate an impossible branch. +/// +/// @details Typically used when a static_assert is used to guard a certain default +/// implementation to never be executed and to show a helpful message to the user. +/// +/// @tparam T Any type needed to delay the compilation of this function until it is used. +/// +/// @return A boolean that should be false for any type passed into this function. +/// +template +constexpr inline autoware::common::types::bool8_t COMMON_PUBLIC impossible_branch() noexcept +{ + return sizeof(T) == 0; +} + +/// Find an index of a type in a tuple +template +struct COMMON_PUBLIC index +{ + static_assert(!std::is_same>::value, "Could not find QueryT in given tuple"); +}; + +/// Specialization for a tuple that starts with the HeadT type. End of recursion. +template +struct COMMON_PUBLIC index> + : std::integral_constant {}; + +/// Specialization for a tuple with a type different to QueryT that calls the recursive step. +template +struct COMMON_PUBLIC index> + : std::integral_constant>::value> {}; + +/// +/// @brief Visit every element in a tuple. +/// +/// This specialization indicates the end of the recursive tuple traversal. +/// +/// @tparam I Current index. +/// @tparam Callable Callable type, usually a lambda with one auto input parameter. +/// @tparam TypesT Types in the tuple. +/// +/// @return Does not return anything. Capture variables in a lambda to return any values. +/// +template +COMMON_PUBLIC inline constexpr typename std::enable_if_t +visit(std::tuple &, Callable) noexcept {} +/// @brief Same as the previous specialization but for const tuple. +template +COMMON_PUBLIC inline constexpr typename std::enable_if_t +visit(const std::tuple &, Callable) noexcept {} + +/// +/// @brief Visit every element in a tuple. +/// +/// This specialization is used to apply the callable to an element of a tuple and +/// recursively call this function on the next one. +/// +/// @param tuple The tuple instance +/// @param[in] callable A callable, usually a lambda with one auto input parameter. +/// +/// @tparam I Current index. +/// @tparam Callable Callable type, usually a lambda with one auto input parameter. +/// @tparam TypesT Types in the tuple. +/// +/// @return Does not return anything. Capture variables in a lambda to return any values. +/// +template +COMMON_PUBLIC inline constexpr typename std::enable_if_t +visit(std::tuple & tuple, Callable callable) noexcept +{ + callable(std::get(tuple)); + visit(tuple, callable); +} +/// @brief Same as the previous specialization but for const tuple. +template +COMMON_PUBLIC inline constexpr typename std::enable_if_t +visit(const std::tuple & tuple, Callable callable) noexcept +{ + callable(std::get(tuple)); + visit(tuple, callable); +} + +/// @brief A class to compute a conjunction over given traits. +template +struct COMMON_PUBLIC conjunction : std::true_type {}; +/// @brief A conjunction of another type shall derive from that type. +template +struct COMMON_PUBLIC conjunction: TraitT {}; +template +struct COMMON_PUBLIC conjunction + : std::conditional_t(TraitT::value), conjunction, TraitT> {}; + + +/// +/// @brief A trait to check if a tuple has a type. +/// +/// @details Taken from https://stackoverflow.com/a/25958302/678093 +/// +/// @tparam QueryT A query type. +/// @tparam TupleT A tuple to search the type in. +/// +template +struct has_type; + +/// +/// @brief An overload of the general trait that signifies that nothing can be found in an +/// empty tuple. +/// +/// @tparam QueryT Any type. +/// +template +struct has_type>: std::false_type {}; + +/// +/// @brief Recursive override of the main trait. +/// +/// @tparam QueryT Query type. +/// @tparam HeadT Head type in the tuple. +/// @tparam TailTs Rest of the tuple types. +/// +template +struct has_type>: has_type> {}; + +/// +/// @brief End of recursion for the main `has_type` trait. Becomes a `true_type` when the first +/// type in the tuple matches the query type. +/// +/// @tparam QueryT Query type. +/// @tparam TailTs Other types in the tuple. +/// +template +struct has_type>: std::true_type {}; + + +/// +/// @brief A trait used to intersect types stored in tuples at compile time. The resulting +/// typedef `type` will hold a tuple with the intersection of the types provided in the +/// input tuples. +/// +/// @details Taken from https://stackoverflow.com/a/41200732/1763680 +/// +/// @tparam TupleT1 Tuple 1 +/// @tparam TupleT2 Tuple 2 +/// +template +struct intersect +{ + /// + /// @brief Intersect the types. + /// + /// @details This function "iterates" over the types in TupleT1 and checks if those are in + /// TupleT2. If this is true, these types are concatenated into a new tuple. + /// + template + static constexpr auto make_intersection(std::index_sequence) + { + return std::tuple_cat( + std::conditional_t< + has_type, TupleT2>::value, + std::tuple>, + std::tuple<>>{} ...); + } + /// The resulting tuple type. + using type = + decltype(make_intersection(std::make_index_sequence::value> {})); +}; + +} // namespace type_traits +} // namespace common +} // namespace autoware + +#endif // COMMON__TYPE_TRAITS_HPP_ diff --git a/common/autoware_auto_common/include/common/types.hpp b/common/autoware_auto_common/include/common/types.hpp new file mode 100644 index 0000000000000..f3d49fcea04ff --- /dev/null +++ b/common/autoware_auto_common/include/common/types.hpp @@ -0,0 +1,136 @@ +// Copyright 2017-2020 the Autoware Foundation, Arm Limited +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. +/// \file +/// \brief This file includes common type definition + +#ifndef COMMON__TYPES_HPP_ +#define COMMON__TYPES_HPP_ + +#include +#include +#include + +#include "helper_functions/float_comparisons.hpp" +#include "common/visibility_control.hpp" + +namespace autoware +{ +namespace common +{ +namespace types +{ +// Aliases to conform to MISRA C++ Rule 3-9-2 (Directive 4.6 in MISRA C). +// Similarly, the stdint typedefs should be used instead of plain int, long etc. types. +// We don't currently require code to comply to MISRA, but we should try to where it is +// easily possible. +using bool8_t = bool; +using char8_t = char; +using uchar8_t = unsigned char; +// If we ever compile on a platform where this is not true, float32_t and float64_t definitions +// need to be adjusted. +static_assert(sizeof(float) == 4, "float is assumed to be 32-bit"); +using float32_t = float; +static_assert(sizeof(double) == 8, "double is assumed to be 64-bit"); +using float64_t = double; + +/// pi = tau / 2 +constexpr float32_t PI = 3.14159265359F; +/// pi/2 +constexpr float32_t PI_2 = 1.5707963267948966F; +/// tau = 2 pi +constexpr float32_t TAU = 6.283185307179586476925286766559F; + +struct COMMON_PUBLIC PointXYZIF +{ + float32_t x{0}; + float32_t y{0}; + float32_t z{0}; + float32_t intensity{0}; + uint16_t id{0}; + static constexpr uint16_t END_OF_SCAN_ID = 65535u; + friend bool operator==( + const PointXYZIF & p1, + const PointXYZIF & p2) noexcept + { + using autoware::common::helper_functions::comparisons::rel_eq; + const auto epsilon = std::numeric_limits::epsilon(); + return rel_eq(p1.x, p2.x, epsilon) && + rel_eq(p1.y, p2.y, epsilon) && + rel_eq(p1.z, p2.z, epsilon) && + rel_eq(p1.intensity, p2.intensity, epsilon) && + (p1.id == p2.id); + } +}; + +struct COMMON_PUBLIC PointXYZF +{ + float32_t x{0}; + float32_t y{0}; + float32_t z{0}; + uint16_t id{0}; + static constexpr uint16_t END_OF_SCAN_ID = 65535u; + friend bool operator==( + const PointXYZF & p1, + const PointXYZF & p2) noexcept + { + using autoware::common::helper_functions::comparisons::rel_eq; + const auto epsilon = std::numeric_limits::epsilon(); + return rel_eq(p1.x, p2.x, epsilon) && + rel_eq(p1.y, p2.y, epsilon) && + rel_eq(p1.z, p2.z, epsilon) && + (p1.id == p2.id); + } +}; + +struct COMMON_PUBLIC PointXYZI +{ + float32_t x{0.0F}; + float32_t y{0.0F}; + float32_t z{0.0F}; + float32_t intensity{0.0F}; + friend bool operator==( + const PointXYZI & p1, + const PointXYZI & p2) noexcept + { + return + helper_functions::comparisons::rel_eq( + p1.x, p2.x, std::numeric_limits::epsilon()) && + + helper_functions::comparisons::rel_eq( + p1.y, p2.y, std::numeric_limits::epsilon()) && + + helper_functions::comparisons::rel_eq( + p1.z, p2.z, std::numeric_limits::epsilon()) && + + helper_functions::comparisons::rel_eq( + p1.intensity, p2.intensity, std::numeric_limits::epsilon()); + } +}; + +using PointBlock = std::vector; +using PointPtrBlock = std::vector; +/// \brief Stores basic configuration information, does some simple validity checking +static constexpr uint16_t POINT_BLOCK_CAPACITY = 512U; + +// TODO(yunus.caliskan): switch to std::void_t when C++17 is available +/// \brief `std::void_t<> implementation +template +using void_t = void; +} // namespace types +} // namespace common +} // namespace autoware + +#endif // COMMON__TYPES_HPP_ diff --git a/common/autoware_auto_common/include/common/visibility_control.hpp b/common/autoware_auto_common/include/common/visibility_control.hpp new file mode 100644 index 0000000000000..d2d3d89148d03 --- /dev/null +++ b/common/autoware_auto_common/include/common/visibility_control.hpp @@ -0,0 +1,38 @@ +// Copyright 2017-2019 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#ifndef COMMON__VISIBILITY_CONTROL_HPP_ +#define COMMON__VISIBILITY_CONTROL_HPP_ + +#if defined(_MSC_VER) && defined(_WIN64) + #if defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS) + #define COMMON_PUBLIC __declspec(dllexport) + #define COMMON_LOCAL + #else // defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS) + #define COMMON_PUBLIC __declspec(dllimport) + #define COMMON_LOCAL + #endif // defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS) +#elif defined(__GNUC__) && defined(__linux__) + #define COMMON_PUBLIC __attribute__((visibility("default"))) + #define COMMON_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__GNUC__) && defined(__APPLE__) + #define COMMON_PUBLIC __attribute__((visibility("default"))) + #define COMMON_LOCAL __attribute__((visibility("hidden"))) +#else // !(defined(__GNUC__) && defined(__APPLE__)) + #error "Unsupported Build Configuration" +#endif // _MSC_VER + +#endif // COMMON__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/angle_utils.hpp b/common/autoware_auto_common/include/helper_functions/angle_utils.hpp new file mode 100644 index 0000000000000..39ee0bf657748 --- /dev/null +++ b/common/autoware_auto_common/include/helper_functions/angle_utils.hpp @@ -0,0 +1,62 @@ +// Copyright 2020 Apex.AI, 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#ifndef HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ +#define HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ + +#include +#include + +namespace autoware +{ +namespace common +{ +namespace helper_functions +{ + +namespace detail +{ +constexpr auto kDoublePi = 2.0 * M_PI; +} // namespace detail + +/// +/// @brief Wrap angle to the [-pi, pi] range. +/// +/// @details This method uses the formula suggested in the paper [On wrapping the Kalman filter +/// and estimating with the SO(2) group](https://arxiv.org/pdf/1708.05551.pdf) and +/// implements the following formula: +/// \f$\mathrm{mod}(\alpha + \pi, 2 \pi) - \pi\f$. +/// +/// @param[in] angle The input angle +/// +/// @tparam T Type of scalar +/// +/// @return Angle wrapped to the chosen range. +/// +template +constexpr T wrap_angle(T angle) noexcept +{ + auto help_angle = angle + T(M_PI); + while (help_angle < T{}) {help_angle += T(detail::kDoublePi);} + while (help_angle >= T(detail::kDoublePi)) {help_angle -= T(detail::kDoublePi);} + return help_angle - T(M_PI); +} + +} // namespace helper_functions +} // namespace common +} // namespace autoware + +#endif // HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp b/common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp new file mode 100644 index 0000000000000..dda752bf2f2b5 --- /dev/null +++ b/common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp @@ -0,0 +1,50 @@ +// Copyright 2020 Mapless AI, Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#ifndef HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ +#define HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ + +#include "common/types.hpp" + +namespace autoware +{ +namespace common +{ +namespace helper_functions +{ +namespace comparisons +{ + +/** + * @brief Convenience method for performing logical exclusive or ops. + * @return True iff exactly one of 'a' and 'b' is true. + */ +template +types::bool8_t exclusive_or(const T & a, const T & b) +{ + return static_cast(a) != static_cast(b); +} + +} // namespace comparisons +} // namespace helper_functions +} // namespace common +} // namespace autoware + +#endif // HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/byte_reader.hpp b/common/autoware_auto_common/include/helper_functions/byte_reader.hpp new file mode 100644 index 0000000000000..aa05e5c6a7be4 --- /dev/null +++ b/common/autoware_auto_common/include/helper_functions/byte_reader.hpp @@ -0,0 +1,77 @@ +// Copyright 2017-2019 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. +/// \file +/// \brief This file includes common helper functions + +#ifndef HELPER_FUNCTIONS__BYTE_READER_HPP_ +#define HELPER_FUNCTIONS__BYTE_READER_HPP_ + +#include +#include +#include + +namespace autoware +{ +namespace common +{ +namespace helper_functions +{ +/// \brief A utility class to read byte vectors in big-endian order +class ByteReader +{ +private: + const std::vector & byte_vector_; + std::size_t index_; + +public: + /// \brief Default constructor, byte reader class + /// \param[in] byte_vector A vector to read bytes from + explicit ByteReader(const std::vector & byte_vector) + : byte_vector_(byte_vector), + index_(0U) + { + } + + // brief Read bytes and store it in the argument passed in big-endian order + /// \param[inout] value Read and store the bytes from the vector matching the size of the argument + template + void read(T & value) + { + constexpr std::size_t kTypeSize = sizeof(T); + union { + T value; + uint8_t byte_vector[kTypeSize]; + } tmp; + + for (std::size_t i = 0; i < kTypeSize; ++i) { + tmp.byte_vector[i] = byte_vector_[index_ + kTypeSize - 1 - i]; + } + + value = tmp.value; + + index_ += kTypeSize; + } + + void skip(std::size_t count) + { + index_ += count; + } +}; +} // namespace helper_functions +} // namespace common +} // namespace autoware + +#endif // HELPER_FUNCTIONS__BYTE_READER_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/crtp.hpp b/common/autoware_auto_common/include/helper_functions/crtp.hpp new file mode 100644 index 0000000000000..7cba8d0350829 --- /dev/null +++ b/common/autoware_auto_common/include/helper_functions/crtp.hpp @@ -0,0 +1,52 @@ +// Copyright 2017-2019 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. +/// \file +/// \brief This file includes common helper functions + +#ifndef HELPER_FUNCTIONS__CRTP_HPP_ +#define HELPER_FUNCTIONS__CRTP_HPP_ + +namespace autoware +{ +namespace common +{ +namespace helper_functions +{ +template +class crtp +{ +protected: + const Derived & impl() const + { + // This is the CRTP pattern for static polymorphism: this is related, static_cast is the only + // way to do this + //lint -e{9005, 9176, 1939} NOLINT + return *static_cast(this); + } + + Derived & impl() + { + // This is the CRTP pattern for static polymorphism: this is related, static_cast is the only + // way to do this + //lint -e{9005, 9176, 1939} NOLINT + return *static_cast(this); + } +}; +} // namespace helper_functions +} // namespace common +} // namespace autoware + +#endif // HELPER_FUNCTIONS__CRTP_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/float_comparisons.hpp b/common/autoware_auto_common/include/helper_functions/float_comparisons.hpp new file mode 100644 index 0000000000000..fcf1b90c59cf2 --- /dev/null +++ b/common/autoware_auto_common/include/helper_functions/float_comparisons.hpp @@ -0,0 +1,151 @@ +// Copyright 2020 Mapless AI, Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#ifndef HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ +#define HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ + +#include +#include +#include + +namespace autoware +{ +namespace common +{ +namespace helper_functions +{ +namespace comparisons +{ + +/** + * @brief Check for approximate equality in absolute terms. + * @pre eps >= 0 + * @return True iff 'a' and 'b' are within 'eps' of each other. + */ +template +bool abs_eq(const T & a, const T & b, const T & eps) +{ + static_assert( + std::is_floating_point::value, + "Float comparisons only support floating point types."); + + return std::abs(a - b) <= eps; +} + +/** + * @brief Check for approximate less than in absolute terms. + * @pre eps >= 0 + * @return True iff 'a' is less than 'b' minus 'eps'. + */ +template +bool abs_lt(const T & a, const T & b, const T & eps) +{ + return !abs_eq(a, b, eps) && (a < b); +} + +/** + * @brief Check for approximate less than or equal in absolute terms. + * @pre eps >= 0 + * @return True iff 'a' is less than or equal to 'b' plus 'eps'. + */ +template +bool abs_lte(const T & a, const T & b, const T & eps) +{ + return abs_eq(a, b, eps) || (a < b); +} + +/** + * @brief Check for approximate greater than or equal in absolute terms. + * @pre eps >= 0 + * @return True iff 'a' is greater than or equal to 'b' minus 'eps'. + */ +template +bool abs_gte(const T & a, const T & b, const T & eps) +{ + return !abs_lt(a, b, eps); +} + +/** + * @brief Check for approximate greater than in absolute terms. + * @pre eps >= 0 + * @return True iff 'a' is greater than 'b' minus 'eps'. + */ +template +bool abs_gt(const T & a, const T & b, const T & eps) +{ + return !abs_lte(a, b, eps); +} + +/** + * @brief Check whether a value is within epsilon of zero. + * @pre eps >= 0 + * @return True iff 'a' is within 'eps' of zero. + */ +template +bool abs_eq_zero(const T & a, const T & eps) +{ + return abs_eq(a, static_cast(0), eps); +} + +/** + * @brief + * https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/ + * @pre rel_eps >= 0 + * @return True iff 'a' and 'b' are within relative 'rel_eps' of each other. + */ +template +bool rel_eq(const T & a, const T & b, const T & rel_eps) +{ + static_assert( + std::is_floating_point::value, + "Float comparisons only support floating point types."); + + const auto delta = std::abs(a - b); + const auto larger = std::max(std::abs(a), std::abs(b)); + const auto max_rel_delta = (larger * rel_eps); + return delta <= max_rel_delta; +} + +// TODO(jeff): As needed, add relative variants of <, <=, >, >= + +/** + * @brief Check for approximate equality in absolute and relative terms. + * + * @note This method should be used only if an explicit relative or absolute + * comparison is not appropriate for the particular use case. + * + * @pre abs_eps >= 0 + * @pre rel_eps >= 0 + * @return True iff 'a' and 'b' are within 'eps' or 'rel_eps' of each other + */ +template +bool approx_eq(const T & a, const T & b, const T & abs_eps, const T & rel_eps) +{ + const auto are_absolute_eq = abs_eq(a, b, abs_eps); + const auto are_relative_eq = rel_eq(a, b, rel_eps); + return are_absolute_eq || are_relative_eq; +} + +} // namespace comparisons +} // namespace helper_functions +} // namespace common +} // namespace autoware + +#endif // HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp b/common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp new file mode 100644 index 0000000000000..fde5efca90a23 --- /dev/null +++ b/common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp @@ -0,0 +1,75 @@ +// Copyright 2021 Apex.AI, 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#ifndef HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ +#define HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ + +#include + +namespace autoware +{ +namespace common +{ +namespace helper_functions +{ +/// \brief Calculate square of mahalanobis distance +/// \tparam T Type of elements in the matrix +/// \tparam kNumOfStates Number of states +/// \param sample Single column matrix containing sample whose distance needs to be computed +/// \param mean Single column matrix containing mean of samples received so far +/// \param covariance_factor Covariance matrix +/// \return Square of mahalanobis distance +template +types::float32_t calculate_squared_mahalanobis_distance( + const Eigen::Matrix & sample, + const Eigen::Matrix & mean, + const Eigen::Matrix & covariance_factor) +{ + using Vector = Eigen::Matrix; + // This is equivalent to the squared Mahalanobis distance of the form: diff.T * C.inv() * diff + // Instead of the covariance matrix C we have its lower-triangular factor L, such that C = L * L.T + // squared_mahalanobis_distance = diff.T * C.inv() * diff + // = diff.T * (L * L.T).inv() * diff + // = diff.T * L.T.inv() * L.inv() * diff + // = (L.inv() * diff).T * (L.inv() * diff) + // this allows us to efficiently find the squared Mahalanobis distance using (L.inv() * diff), + // which can be found as a solution to: L * x = diff. + const Vector diff = sample - mean; + const Vector x = covariance_factor.ldlt().solve(diff); + return x.transpose() * x; +} + +/// \brief Calculate mahalanobis distance +/// \tparam T Type of elements in the matrix +/// \tparam kNumOfStates Number of states +/// \param sample Single column matrix containing sample whose distance needs to be computed +/// \param mean Single column matrix containing mean of samples received so far +/// \param covariance_factor Covariance matrix +/// \return Mahalanobis distance +template +types::float32_t calculate_mahalanobis_distance( + const Eigen::Matrix & sample, + const Eigen::Matrix & mean, + const Eigen::Matrix & covariance_factor +) +{ + return sqrtf(calculate_squared_mahalanobis_distance(sample, mean, covariance_factor)); +} +} // namespace helper_functions +} // namespace common +} // namespace autoware + +#endif // HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/message_adapters.hpp b/common/autoware_auto_common/include/helper_functions/message_adapters.hpp new file mode 100644 index 0000000000000..90156b152fdd1 --- /dev/null +++ b/common/autoware_auto_common/include/helper_functions/message_adapters.hpp @@ -0,0 +1,111 @@ +// Copyright 2017-2019 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. +/// \file +/// \brief This file includes common helper functions + +#ifndef HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ +#define HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ + +#include +#include + +namespace autoware +{ +namespace common +{ +namespace helper_functions +{ +namespace message_field_adapters +{ +/// Using alias for Time message +using TimeStamp = builtin_interfaces::msg::Time; + +/// \brief Helper class to check existance of header file in compile time: +/// https://stackoverflow.com/a/16000226/2325407 +template +struct HasHeader : std::false_type {}; + +template +struct HasHeader: std::true_type {}; + +/////////// Template declarations + +/// Get frame id from message. nullptr_t is used to prevent template ambiguity on +/// SFINAE specializations. Provide a default value on specializations for a friendly API. +/// \tparam T Message type. +/// \param msg Message. +/// \return Frame id of the message. +template +const std::string & get_frame_id(const T & msg) noexcept; + +/// Get a reference to the frame id from message. nullptr_t is used to prevent +/// template ambiguity on SFINAE specializations. Provide a default value on +/// specializations for a friendly API. +/// \tparam T Message type. +/// \param msg Message. +/// \return Frame id of the message. +template +std::string & get_frame_id(T & msg) noexcept; + +/// Get stamp from message. nullptr_t is used to prevent template ambiguity on +/// SFINAE specializations. Provide a default value on specializations for a friendly API. +/// \tparam T Message type. +/// \param msg Message. +/// \return Frame id of the message. +template +const TimeStamp & get_stamp(const T & msg) noexcept; + +/// Get a reference to the stamp from message. nullptr_t is used to prevent +/// template ambiguity on SFINAE specializations. Provide a default value on +/// specializations for a friendly API. +/// \tparam T Message type. +/// \param msg Message. +/// \return Frame id of the message. +template +TimeStamp & get_stamp(T & msg) noexcept; + + +/////////////// Default specializations for message types that contain a header. +template::value, nullptr_t>::type = nullptr> +const std::string & get_frame_id(const T & msg) noexcept +{ + return msg.header.frame_id; +} + +template::value, nullptr_t>::type = nullptr> +std::string & get_frame_id(T & msg) noexcept +{ + return msg.header.frame_id; +} + +template::value, nullptr_t>::type = nullptr> +TimeStamp & get_stamp(T & msg) noexcept +{ + return msg.header.stamp; +} + +template::value, nullptr_t>::type = nullptr> +TimeStamp get_stamp(const T & msg) noexcept +{ + return msg.header.stamp; +} + +} // namespace message_field_adapters +} // namespace helper_functions +} // namespace common +} // namespace autoware + +#endif // HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/template_utils.hpp b/common/autoware_auto_common/include/helper_functions/template_utils.hpp new file mode 100644 index 0000000000000..b52be9eae0c59 --- /dev/null +++ b/common/autoware_auto_common/include/helper_functions/template_utils.hpp @@ -0,0 +1,67 @@ +// Copyright 2021 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#ifndef HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ +#define HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ + +#include +#include + +namespace autoware +{ +namespace common +{ +namespace helper_functions +{ +/// This struct is `std::true_type` if the expression is valid for a given template and +/// `std::false_type` otherwise. +/// \tparam ExpressionTemplate Expression to be checked in compile time +/// \tparam T Template parameter to instantiate the expression. +template class ExpressionTemplate, typename T, typename = void> +struct expression_valid : std::false_type {}; + +/// This struct is `std::true_type` if the expression is valid for a given template and +/// `std::false_type` otherwise. +/// \tparam ExpressionTemplate Expression to be checked in compile time +/// \tparam T Template parameter to instantiate the expression. +template class ExpressionTemplate, typename T> +struct expression_valid>>: std::true_type {}; + +/// This struct is `std::true_type` if the expression is valid for a given template +/// type with the specified return type and `std::false_type` otherwise. +/// \tparam ExpressionTemplate Expression to be checked in compile time +/// \tparam T Template parameter to instantiate the expression. +/// \tparam ReturnT Return type of the expression. +template class ExpressionTemplate, typename T, typename ReturnT, + typename = void> +struct expression_valid_with_return : std::false_type {}; + +/// This struct is `std::true_type` if the expression is valid for a given template +/// type with the specified return type and `std::false_type` otherwise. +/// \tparam ExpressionTemplate Expression to be checked in compile time +/// \tparam T Template parameter to instantiate the expression. +/// \tparam ReturnT Return type of the expression. +template class ExpressionTemplate, typename T, typename ReturnT> +struct expression_valid_with_return>::value>>: std::true_type {}; + + +} // namespace helper_functions +} // namespace common +} // namespace autoware +#endif // HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/type_name.hpp b/common/autoware_auto_common/include/helper_functions/type_name.hpp new file mode 100644 index 0000000000000..1ca75ab86bd6e --- /dev/null +++ b/common/autoware_auto_common/include/helper_functions/type_name.hpp @@ -0,0 +1,56 @@ +// Copyright 2021 Apex.AI, 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#ifndef HELPER_FUNCTIONS__TYPE_NAME_HPP_ +#define HELPER_FUNCTIONS__TYPE_NAME_HPP_ + +#include + +#include +#include + +#if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__) +#include +#endif + +namespace autoware +{ +namespace helper_functions +{ + +/// @brief Get a demangled name of a type. +template +COMMON_PUBLIC std::string get_type_name() +{ + #if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__) + return abi::__cxa_demangle(typeid(T).name(), NULL, NULL, 0); + #else + // For unsupported compilers return a mangled name. + return typeid(T).name(); + #endif +} + +/// @brief Get a demangled name of a type given its instance. +template +COMMON_PUBLIC std::string get_type_name(const T &) +{ + return get_type_name(); +} + +} // namespace helper_functions +} // namespace autoware + +#endif // HELPER_FUNCTIONS__TYPE_NAME_HPP_ diff --git a/common/autoware_auto_common/package.xml b/common/autoware_auto_common/package.xml new file mode 100644 index 0000000000000..b79ac8cbe2291 --- /dev/null +++ b/common/autoware_auto_common/package.xml @@ -0,0 +1,21 @@ + + + + autoware_auto_common + 1.0.0 + Miscelaneous helper functions + Apex.AI, Inc. + Apache 2 + + ament_cmake_auto + autoware_auto_cmake + + builtin_interfaces + eigen + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + ament_cmake + diff --git a/common/autoware_auto_common/test/gtest_main.cpp b/common/autoware_auto_common/test/gtest_main.cpp new file mode 100644 index 0000000000000..6f8f95a74361d --- /dev/null +++ b/common/autoware_auto_common/test/gtest_main.cpp @@ -0,0 +1,23 @@ +// Copyright 2018 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#include "gtest/gtest.h" + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/autoware_auto_common/test/test_angle_utils.cpp b/common/autoware_auto_common/test/test_angle_utils.cpp new file mode 100644 index 0000000000000..dda28d7d62588 --- /dev/null +++ b/common/autoware_auto_common/test/test_angle_utils.cpp @@ -0,0 +1,37 @@ +// Copyright 2021 Apex.AI, 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. +// +// Developed by Apex.AI, Inc. + +#include +#include + +#include + +namespace +{ +using autoware::common::types::float32_t; +using autoware::common::types::float64_t; +using autoware::common::helper_functions::wrap_angle; + +} // namespace + +/// @test Wrap an angle. +TEST(TestAngleUtils, WrapAngle) { + EXPECT_DOUBLE_EQ(wrap_angle(-5.0 * M_PI_2), -M_PI_2); + EXPECT_DOUBLE_EQ(wrap_angle(5.0 * M_PI_2), M_PI_2); + EXPECT_DOUBLE_EQ(wrap_angle(M_PI), -M_PI); + EXPECT_DOUBLE_EQ(wrap_angle(-M_PI), -M_PI); + EXPECT_DOUBLE_EQ(wrap_angle(0.0), 0.0); +} diff --git a/common/autoware_auto_common/test/test_bool_comparisons.cpp b/common/autoware_auto_common/test/test_bool_comparisons.cpp new file mode 100644 index 0000000000000..6dd15f6dfb597 --- /dev/null +++ b/common/autoware_auto_common/test/test_bool_comparisons.cpp @@ -0,0 +1,44 @@ +// Copyright 2020 Mapless AI, Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include + +#include "helper_functions/bool_comparisons.hpp" + +// cppcheck does not like gtest macros inside of namespaces: +// https://sourceforge.net/p/cppcheck/discussion/general/thread/e68df47b/ +// use a namespace alias instead of putting macros into the namespace +namespace comp = autoware::common::helper_functions::comparisons; + +//------------------------------------------------------------------------------ + +TEST(HelperFunctionsComparisons, ExclusiveOr) { + EXPECT_TRUE(comp::exclusive_or(0, 1)); + EXPECT_TRUE(comp::exclusive_or(1, 0)); + EXPECT_FALSE(comp::exclusive_or(0, 0)); + EXPECT_FALSE(comp::exclusive_or(1, 1)); + + EXPECT_TRUE(comp::exclusive_or(false, true)); + EXPECT_TRUE(comp::exclusive_or(true, false)); + EXPECT_FALSE(comp::exclusive_or(false, false)); + EXPECT_FALSE(comp::exclusive_or(true, true)); +} + +//------------------------------------------------------------------------------ diff --git a/common/autoware_auto_common/test/test_byte_reader.cpp b/common/autoware_auto_common/test/test_byte_reader.cpp new file mode 100644 index 0000000000000..8425cda73a1e6 --- /dev/null +++ b/common/autoware_auto_common/test/test_byte_reader.cpp @@ -0,0 +1,54 @@ +// Copyright 2019 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#include +#include "gtest/gtest.h" +#include "helper_functions/byte_reader.hpp" +#include "common/types.hpp" + +using autoware::common::types::float64_t; + +namespace +{ +class ByteReader : public ::testing::Test +{ +}; +} // namespace + + +// tests serial_driver_node's get_packet function which receives serial packages +TEST_F(ByteReader, Basic) +{ + std::vector data = { + 0x00, 0x00, 0x00, 0x17, 0x40, 0x28, 0xAE, 0x14, 0x7A, 0xE1, 0x47, 0xAE, 0x00, 0x00, 0x08 + }; + + autoware::common::helper_functions::ByteReader byte_reader(data); + + uint32_t a = 0; + byte_reader.read(a); + ASSERT_EQ(a, 23U); + + float64_t b = 0; + byte_reader.read(b); + ASSERT_EQ(b, 12.34); + + byte_reader.skip(1); + + int16_t c = 0; + byte_reader.read(c); + ASSERT_EQ(c, 8); +} diff --git a/common/autoware_auto_common/test/test_float_comparisons.cpp b/common/autoware_auto_common/test/test_float_comparisons.cpp new file mode 100644 index 0000000000000..cb44c1bf4d038 --- /dev/null +++ b/common/autoware_auto_common/test/test_float_comparisons.cpp @@ -0,0 +1,151 @@ +// Copyright 2020 Mapless AI, Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include + +#include "helper_functions/float_comparisons.hpp" + +// cppcheck does not like gtest macros inside of namespaces: +// https://sourceforge.net/p/cppcheck/discussion/general/thread/e68df47b/ +// use a namespace alias instead of putting macros into the namespace +namespace comp = autoware::common::helper_functions::comparisons; + +namespace +{ +const auto a = 1.317; +const auto b = 2.0; +const auto c = -5.2747; +const auto d = 0.0; +const auto e = -5.2747177; +const auto f = -5.2749; +const auto epsilon = 0.0001; +} // namespace + +//------------------------------------------------------------------------------ + +TEST(HelperFunctionsComparisons, AbsEqZero) { + EXPECT_TRUE(comp::abs_eq_zero(d, epsilon)); + EXPECT_TRUE(comp::abs_eq_zero(d + epsilon * epsilon, epsilon)); + EXPECT_FALSE(comp::abs_eq_zero(d + 2.0 * epsilon, epsilon)); + EXPECT_FALSE(comp::abs_eq_zero(1.0, epsilon)); + EXPECT_TRUE(comp::abs_eq_zero(0.0, 0.0)); +} + +//------------------------------------------------------------------------------ + +TEST(HelperFunctionsComparisons, AbsEq) { + EXPECT_TRUE(comp::abs_eq(c, e, epsilon)); + EXPECT_TRUE(comp::abs_eq(e, c, epsilon)); + EXPECT_FALSE(comp::abs_eq(c, e, 0.0)); + EXPECT_FALSE(comp::abs_eq(e, c, 0.0)); + EXPECT_FALSE(comp::abs_eq(a, b, epsilon)); + EXPECT_FALSE(comp::abs_eq(b, a, epsilon)); + EXPECT_TRUE(comp::abs_eq(a, a, epsilon)); + EXPECT_TRUE(comp::abs_eq(a, a, 0.0)); +} + +//------------------------------------------------------------------------------ + +TEST(HelperFunctionsComparisons, AbsLt) { + EXPECT_TRUE(comp::abs_lt(f, c, 0.0)); + EXPECT_TRUE(comp::abs_lt(f, c, epsilon)); + EXPECT_FALSE(comp::abs_lt(c, f, epsilon)); + EXPECT_FALSE(comp::abs_lt(d, d, epsilon)); + EXPECT_FALSE(comp::abs_lt(d, d, 0.0)); +} + +//------------------------------------------------------------------------------ + +TEST(HelperFunctionsComparisons, AbsLte) { + EXPECT_TRUE(comp::abs_lte(c, e, epsilon)); + EXPECT_TRUE(comp::abs_lte(e, c, epsilon)); + EXPECT_FALSE(comp::abs_lte(c, e, 0.0)); + EXPECT_TRUE(comp::abs_lte(e, c, 0.0)); + EXPECT_TRUE(comp::abs_lte(c, e, epsilon)); + EXPECT_TRUE(comp::abs_lte(e, c, epsilon)); + EXPECT_TRUE(comp::abs_lte(a, b, epsilon)); + EXPECT_FALSE(comp::abs_lte(b, a, epsilon)); + EXPECT_TRUE(comp::abs_lte(d, d, epsilon)); + EXPECT_TRUE(comp::abs_lte(d, d, 0.0)); +} + +//------------------------------------------------------------------------------ + +TEST(HelperFunctionsComparisons, AbsGt) { + EXPECT_TRUE(comp::abs_gt(c, e, 0.0)); + EXPECT_FALSE(comp::abs_gt(c, e, epsilon)); + EXPECT_FALSE(comp::abs_gt(f, c, epsilon)); + EXPECT_TRUE(comp::abs_gt(c, f, epsilon)); + EXPECT_FALSE(comp::abs_gt(d, d, epsilon)); + EXPECT_FALSE(comp::abs_gt(d, d, 0.0)); +} + +//------------------------------------------------------------------------------ + +TEST(HelperFunctionsComparisons, AbsGte) { + EXPECT_TRUE(comp::abs_gte(c, e, 0.0)); + EXPECT_FALSE(comp::abs_gte(e, c, 0.0)); + EXPECT_TRUE(comp::abs_gte(c, e, epsilon)); + EXPECT_TRUE(comp::abs_gte(e, c, epsilon)); + EXPECT_FALSE(comp::abs_gte(f, c, epsilon)); + EXPECT_TRUE(comp::abs_gte(c, f, epsilon)); + EXPECT_TRUE(comp::abs_gte(d, d, epsilon)); + EXPECT_TRUE(comp::abs_gte(d, d, 0.0)); +} + +//------------------------------------------------------------------------------ + +TEST(HelperFunctionsComparisons, RelEq) { + EXPECT_FALSE(comp::rel_eq(c, e, 0.0)); + EXPECT_FALSE(comp::rel_eq(e, c, 0.0)); + EXPECT_TRUE(comp::rel_eq(a, a, 0.0)); + + EXPECT_TRUE(comp::rel_eq(c, e, 1.0)); + EXPECT_TRUE(comp::rel_eq(e, c, 1.0)); + EXPECT_TRUE(comp::rel_eq(a, b, 1.0)); + EXPECT_TRUE(comp::rel_eq(b, a, 1.0)); + + EXPECT_FALSE(comp::rel_eq(1.0, 1.1, 0.01)); + EXPECT_TRUE(comp::rel_eq(10000.0, 10010.0, 0.01)); +} + +//------------------------------------------------------------------------------ + +TEST(HelperFunctionsComparisons, ApproxEq) { + EXPECT_TRUE(comp::approx_eq(c, e, epsilon, 0.0)); + EXPECT_TRUE(comp::approx_eq(e, c, epsilon, 0.0)); + EXPECT_TRUE(comp::approx_eq(a, a, epsilon, 0.0)); + EXPECT_TRUE(comp::approx_eq(a, a, 0.0, 0.0)); + + EXPECT_FALSE(comp::approx_eq(c, e, 0.0, 0.0)); + EXPECT_FALSE(comp::approx_eq(e, c, 0.0, 0.0)); + EXPECT_FALSE(comp::approx_eq(a, b, epsilon, 0.0)); + EXPECT_FALSE(comp::approx_eq(b, a, epsilon, 0.0)); + + EXPECT_TRUE(comp::approx_eq(c, e, 0.0, 1.0)); + EXPECT_TRUE(comp::approx_eq(e, c, 0.0, 1.0)); + EXPECT_TRUE(comp::approx_eq(a, b, epsilon, 1.0)); + EXPECT_TRUE(comp::approx_eq(b, a, epsilon, 1.0)); + + EXPECT_TRUE(comp::approx_eq(1.0, 1.1, 0.2, 0.01)); + EXPECT_TRUE(comp::approx_eq(10000.0, 10010.0, 0.2, 0.01)); +} + +//------------------------------------------------------------------------------ diff --git a/common/autoware_auto_common/test/test_mahalanobis_distance.cpp b/common/autoware_auto_common/test/test_mahalanobis_distance.cpp new file mode 100644 index 0000000000000..878893fda161e --- /dev/null +++ b/common/autoware_auto_common/test/test_mahalanobis_distance.cpp @@ -0,0 +1,41 @@ +// Copyright 2021 Apex.AI, 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. +#include + +#include +#include + +TEST(MahalanobisDistanceTest, BasicTest) +{ + Eigen::Matrix mean; + mean << 2.F, 2.F; + Eigen::Matrix sample; + sample << 2.F, 3.F; + Eigen::Matrix cov; + cov << 0.1F, 0.0F, 0.0F, 0.6F; + + // the two states are independent and one has more variance than the other. With samples + // equidistant from mean but on two different axes will have vastly different + // mahalanobis distance values + EXPECT_FLOAT_EQ( + autoware::common::helper_functions::calculate_mahalanobis_distance( + sample, mean, cov), 1.666666666F); + + sample << 3.F, 2.F; + EXPECT_FLOAT_EQ( + autoware::common::helper_functions::calculate_mahalanobis_distance( + sample, mean, cov), 10.0F); +} diff --git a/common/autoware_auto_common/test/test_message_field_adapters.cpp b/common/autoware_auto_common/test/test_message_field_adapters.cpp new file mode 100644 index 0000000000000..aafddd8f320de --- /dev/null +++ b/common/autoware_auto_common/test/test_message_field_adapters.cpp @@ -0,0 +1,77 @@ +// Copyright 2017-2020 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#include + +#include +#include +#include +#include + +using autoware::common::helper_functions::message_field_adapters::get_stamp; +using autoware::common::helper_functions::message_field_adapters::get_frame_id; + +namespace +{ +builtin_interfaces::msg::Time get_stamp_msg(int t) +{ + builtin_interfaces::msg::Time stamp; + stamp.sec = 0; + stamp.nanosec = t; + return stamp; +} +} // namespace + +TEST(MessageFieldAdapterTest, ConstHeaderTests) { + using Message = geometry_msgs::msg::TransformStamped; + + const auto stamp = get_stamp_msg(0); + const auto frame_id = "MessageFieldAdapterTest_frame"; + + std_msgs::msg::Header header; + header.stamp = stamp; + header.frame_id = frame_id; + const Message msg{Message{}.set__header(header)}; + + EXPECT_EQ(stamp, get_stamp(msg)); + EXPECT_EQ(frame_id, get_frame_id(msg)); +} + +TEST(MessageFieldAdapterTest, NonconstHeaderTests) { + using Message = geometry_msgs::msg::TransformStamped; + + const auto stamp = get_stamp_msg(0); + const auto frame_id = "MessageFieldAdapterTest_frame"; + const auto stamp2 = get_stamp_msg(500); + const auto frame_id2 = "MessageFieldAdapterTest_frame2"; + + ASSERT_NE(stamp, stamp2); + ASSERT_NE(frame_id, frame_id2); + + std_msgs::msg::Header header; + header.stamp = stamp; + header.frame_id = frame_id; + Message msg{Message{}.set__header(header)}; + + EXPECT_EQ(stamp, get_stamp(msg)); + EXPECT_EQ(frame_id, get_frame_id(msg)); + + get_stamp(msg) = stamp2; + get_frame_id(msg) = frame_id2; + + EXPECT_EQ(stamp2, get_stamp(msg)); + EXPECT_EQ(frame_id2, get_frame_id(msg)); +} diff --git a/common/autoware_auto_common/test/test_template_utils.cpp b/common/autoware_auto_common/test/test_template_utils.cpp new file mode 100644 index 0000000000000..876119d1e6776 --- /dev/null +++ b/common/autoware_auto_common/test/test_template_utils.cpp @@ -0,0 +1,134 @@ +// Copyright 2021 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#include +#include + +struct CorrectType {}; +struct FalseType {}; + +struct Foo +{ + CorrectType bar(CorrectType, const CorrectType &, CorrectType *) + { + return CorrectType{}; + } +}; + +template class Expression, typename ... Ts> +using expression_valid_with_return = + ::autoware::common::helper_functions::expression_valid_with_return; +template class Expression, typename ... Ts> +using expression_valid = ::autoware::common::helper_functions::expression_valid; + +// Types are defined here and not in the header because these definitions are basically the test +// code themselves. + +// Correct way to call Foo::bar(...) +template +using call_bar_expression = decltype(std::declval().bar( + std::declval(), + std::declval(), + std::declval() +)); + +// Another correct way to call Foo::bar(...) since a temporary can bind to the const lvalue +// reference +template +using call_bar_expression2 = decltype(std::declval().bar( + std::declval(), + std::declval(), + std::declval() +)); + +// Signature mismatch: +template +using false_bar_expression1 = decltype(std::declval().bar( + std::declval(), + std::declval(), + std::declval() +)); + +// Signature mismatch: +template +using false_bar_expression2 = decltype(std::declval().bar( + std::declval(), + std::declval() +)); + +// Signature mismatch: +template +using false_bar_expression3 = decltype(std::declval().asdasd( + std::declval(), + std::declval(), + std::declval() +)); + +// Correct signature, correct types: +template +using correct_expression1 = call_bar_expression; +template +using correct_expression2 = call_bar_expression2; + +// Correct signature, false types: +template +using false_expression1 = call_bar_expression; +template +using false_expression2 = call_bar_expression; +template +using false_expression3 = call_bar_expression; + +// False signature, correct types: +template +using false_expression4 = false_bar_expression1; +template +using false_expression5 = false_bar_expression3; + +// False signature, false types: +template +using false_expression6 = false_bar_expression1; +template +using false_expression7 = false_bar_expression2; + + +TEST(TestTemplateUtils, ExpressionValid) { + EXPECT_TRUE((expression_valid::value)); + EXPECT_TRUE((expression_valid::value)); + EXPECT_FALSE((expression_valid::value)); + EXPECT_FALSE((expression_valid::value)); + EXPECT_FALSE((expression_valid::value)); + EXPECT_FALSE((expression_valid::value)); + EXPECT_FALSE((expression_valid::value)); + EXPECT_FALSE((expression_valid::value)); + EXPECT_FALSE((expression_valid::value)); +} + +TEST(TestTemplateUtils, ExpressionReturnValid) { + EXPECT_TRUE((expression_valid_with_return::value)); + EXPECT_FALSE((expression_valid_with_return::value)); + + EXPECT_TRUE((expression_valid_with_return::value)); + EXPECT_FALSE((expression_valid_with_return::value)); + + // If an expression is not valid, returning the right type will not be enough. + EXPECT_FALSE((expression_valid_with_return::value)); + EXPECT_FALSE((expression_valid_with_return::value)); + EXPECT_FALSE((expression_valid_with_return::value)); + EXPECT_FALSE((expression_valid_with_return::value)); + EXPECT_FALSE((expression_valid_with_return::value)); + EXPECT_FALSE((expression_valid_with_return::value)); + EXPECT_FALSE((expression_valid_with_return::value)); +} diff --git a/common/autoware_auto_common/test/test_type_name.cpp b/common/autoware_auto_common/test/test_type_name.cpp new file mode 100644 index 0000000000000..9a93c2625daeb --- /dev/null +++ b/common/autoware_auto_common/test/test_type_name.cpp @@ -0,0 +1,37 @@ +// Copyright 2021 Apex.AI, 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. +// +// Developed by Apex.AI, Inc. + +#include +#include + +#include + +namespace +{ +using autoware::common::types::float32_t; +using autoware::common::types::float64_t; +using autoware::helper_functions::get_type_name; + +struct SomeStruct {}; +} // namespace + +/// @test Test that type names can be demangled. +TEST(TestTypeDemangling, Demangle) { + EXPECT_EQ(get_type_name(), "float"); + const float64_t val{42.0}; + EXPECT_EQ(get_type_name(val), "double"); + EXPECT_EQ(get_type_name(), "(anonymous namespace)::SomeStruct"); +} diff --git a/common/autoware_auto_common/test/test_type_traits.cpp b/common/autoware_auto_common/test/test_type_traits.cpp new file mode 100644 index 0000000000000..a4fbdf53652f1 --- /dev/null +++ b/common/autoware_auto_common/test/test_type_traits.cpp @@ -0,0 +1,95 @@ +// Copyright 2021 Apex.AI, 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. +// +// Developed by Apex.AI, Inc. + +#include +#include + +#include + +#include + +namespace +{ +/// @brief A simple testing function to check if all types are arithmetic. +/// +/// Trait "is_arithmetic" is picked at random and any other trait could have been used +/// instead. +template +bool all_are_arithmetic() +{ + // This is just a random function that we use with conjunction. + return autoware::common::type_traits::conjunction...>::value; +} + +using autoware::common::types::float32_t; +using autoware::common::types::float64_t; + +} // namespace + +/// @test Test that index of a type can be computed. +TEST(TestCommonTypeTraits, Index) { + using T = std::tuple; + EXPECT_EQ(0, (autoware::common::type_traits::index::value)); + EXPECT_EQ(1, (autoware::common::type_traits::index::value)); +} + +TEST(TestCommonTypeTraits, Conjunction) { + EXPECT_TRUE((all_are_arithmetic())); + EXPECT_FALSE( + (all_are_arithmetic>())); +} + +TEST(TestCommonTypeTraits, Visit) { + const std::tuple t; + std::int32_t counter{}; + autoware::common::type_traits::visit(t, [&counter](const auto &) {counter++;}); + EXPECT_EQ(2, counter); + float64_t sum{}; + autoware::common::type_traits::visit( + std::make_tuple(2, 42.0F, 23.0), [&sum](const auto & element) { + sum += static_cast(element); + }); + EXPECT_DOUBLE_EQ(67.0, sum); +} + +TEST(TestCommonTypeTraits, HasType) { + struct T1 {}; + struct T2 {}; + struct T3 {}; + EXPECT_TRUE( + (autoware::common::type_traits::has_type>::value)); + EXPECT_FALSE( + (autoware::common::type_traits::has_type>::value)); + EXPECT_FALSE( + (autoware::common::type_traits::has_type>::value)); +} + +TEST(TestCommonTypeTraits, TypeIntersection) { + struct T1 {}; + struct T2 {}; + struct T3 {}; + using A = std::tuple; + using B = std::tuple; + using C = std::tuple; + EXPECT_TRUE( + (std::is_same, + autoware::common::type_traits::intersect::type>::value)); + EXPECT_TRUE( + (std::is_same::type>::value)); + EXPECT_TRUE( + (std::is_same, + autoware::common::type_traits::intersect::type>::value)); +} diff --git a/common/autoware_auto_geometry/CMakeLists.txt b/common/autoware_auto_geometry/CMakeLists.txt new file mode 100644 index 0000000000000..bb613373f4f46 --- /dev/null +++ b/common/autoware_auto_geometry/CMakeLists.txt @@ -0,0 +1,64 @@ +# Copyright 2019 the Autoware Foundation +# +# 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. +# +# Co-developed by Tier IV, Inc. and Apex.AI, Inc. +cmake_minimum_required(VERSION 3.5) + +### Export headers +project(autoware_auto_geometry) + +## dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +# includes +ament_auto_add_library(${PROJECT_NAME} SHARED + include/geometry/spatial_hash.hpp + include/geometry/intersection.hpp + include/geometry/spatial_hash_config.hpp + src/spatial_hash.cpp + src/bounding_box.cpp) +autoware_set_compile_options(${PROJECT_NAME}) + +if(BUILD_TESTING) + # run linters + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + # gtest + set(GEOMETRY_GTEST geometry_gtest) + set(GEOMETRY_SRC test/src/test_geometry.cpp + test/src/test_convex_hull.cpp + test/src/test_hull_pockets.cpp + test/src/test_interval.cpp + test/src/lookup_table.cpp + test/src/test_area.cpp + test/src/test_common_2d.cpp + test/src/test_intersection.cpp + ) + ament_add_gtest(${GEOMETRY_GTEST} ${GEOMETRY_SRC}) + autoware_set_compile_options(${GEOMETRY_GTEST}) + target_compile_options(${GEOMETRY_GTEST} PRIVATE -Wno-conversion -Wno-sign-conversion) + target_include_directories(${GEOMETRY_GTEST} PRIVATE "test/include" "include") + ament_target_dependencies(${GEOMETRY_GTEST} + "autoware_auto_common" + "autoware_auto_geometry_msgs" + "autoware_auto_planning_msgs" + "autoware_auto_vehicle_msgs" + "geometry_msgs" + "osrf_testing_tools_cpp") + target_link_libraries(${GEOMETRY_GTEST} ${PROJECT_NAME}) +endif() + +# Ament Exporting +ament_auto_package() diff --git a/common/autoware_auto_geometry/design/interval.md b/common/autoware_auto_geometry/design/interval.md new file mode 100644 index 0000000000000..d41e2846715c0 --- /dev/null +++ b/common/autoware_auto_geometry/design/interval.md @@ -0,0 +1,112 @@ +Interval {#geometry-interval} +======== + +The interval is a standard 1D real-valued interval. +The class implements a representation and operations on the interval type and guarantees interval validity on construction. +Basic operations and accessors are implemented, as well as other common operations. +See 'Example Usage' below. + +# Target use cases + +* Range or containment checks. +The interval class simplifies code that involves checking membership of a value to a range, or intersecting two ranges. +It also provides consistent behavior and consistent handling of edge cases. + +# Properties + +* **empty**: An empty interval is equivalent to an empty set. +It contains no elements. +It is a valid interval, but because it is empty, the notion of measure (length) is undefined; the measure of an empty interval is *not* zero. +The implementation represents the measure of an empty interval with `NaN`. +* **zero measure**: An interval with zero measure is an interval whose bounds are exactly equal. +The measure is zero because the interval contains only a single point, and points have zero measure. +However, because it does contain a single element, the interval is *not* empty. +* **valid**: A valid interval is either empty or has min/max bounds such that (min <= max). On construction, interval objects are guaranteed to be valid. +An attempt to construct an invalid interval results in a runtime_error exception being thrown. +* **pseudo-immutable**: Once constructed the only way to change the value of an interval is to overwrite it with a new one; an existing object cannot be modified. + +# Conventions + +* All operations on interval objects are defined as static class methods on the interval class. +This is a functional-style of programming that basically turns the class into a namespace that grants functions access to private member variables of the object they operate on. + +# Assumptions + +* The interval is only intended for floating point types. +This is enforced via static assertion. +* The constructor for non-empty intervals takes two arguments 'min' and 'max', and they must be ordered (i.e., min <= max). +If this assumption is violated, an exception is emitted and construction fails. + +# Example Usage + +```c++ +#include "geometry/interval.hpp" + +#include + +// using-directive is just for illustration; don't do this in practice +using namespace autoware::common::geometry; + +// bounds for example interval +constexpr auto MIN = 0.0; +constexpr auto MAX = 1.0; + +// +// Try to construct an invalid interval. This will give the following error: +// 'Attempted to construct an invalid interval: {"min": 1.0, "max": 0.0}' +// + +try { + const auto i = Interval_d(MAX, MIN); +} catch (const std::runtime_error& e) { + std::cerr << e.what(); +} + +// +// Construct a double precision interval from 0 to 1 +// + +const auto i = Interval_d(MIN, MAX); + +// +// Test accessors and properties +// + +std::cout << Interval_d::min(i) << " " << Interval_d::max(i) << "\n"; +// Prints: 0.0 1.0 + +std::cout << Interval_d::empty(i) << " " << Interval_d::length(i) << "\n"; +// Prints: false 1.0 + +std::cout << Interval_d::contains(i, 0.3) << "\n"; +// Prints: true + +std::cout << Interval_d::is_subset_eq(Interval_d(0.2, 0.4), i) << "\n"; +// Prints: true + +// +// Test operations. +// + +std::cout << Interval_d::intersect(i, Interval(-1.0, 0.3)) << "\n"; +// Prints: {"min": 0.0, "max": 0.3} + +std::cout << Interval_d::project_to_interval(i, 0.5) << " " + << Interval_d::project_to_interval(i, -1.3) << "\n"; +// Prints: 0.5 0.0 + +// +// Distinguish empty/zero measure +// + +const auto i_empty = Interval(); +const auto i_zero_length = Interval(0.0, 0.0); + +std::cout << Interval_d::empty(i_empty) << " " + << Interval_d::empty(i_zero_length) << "\n"; +// Prints: true false + +std::cout << Interval_d::zero_measure(i_empty) << " " + << Interval_d::zero_measure(i_zero_length) << "\n"; +// Prints: false false +``` diff --git a/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md b/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md new file mode 100644 index 0000000000000..d2a943b0e6e30 --- /dev/null +++ b/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md @@ -0,0 +1,50 @@ +2D Convex Polygon Intersection {#convex-polygon-intersection-2d} +============ + +Two convex polygon's intersection can be visualized on the image below as the blue area: + + + +# Purpose / Use cases + +Computing the intersection between two polygons can be useful in many applications of scene +understanding. It can be used to estimate collision detection, shape alignment, shape +association and in any application that deals with the objects around the perceiving agent. + +# Design + +[\(Livermore, Calif, 1977\)](https://www.osti.gov/servlets/purl/7309916/) mention the following +observations about convex polygon intersection: + +* Intersection of two convex polygons is a convex polygon +* A vertex from a polygon that is contained in the other polygon is a vertex of the intersection +shape. (Vertices A, C, D in the shape above) +* An edge from a polygon that is contained in the other polygon is an edge in the intersection +shape. (edge C-D in the shape above) +* Edge intersections between two polygons are vertices in the intersection shape. (Vertices B, +E in the shape above.) + +## Inner-workings / Algorithms + +With the observation mentioned above, the current algorithm operates in the following way: + +* Compute and find the vertices from each polygon that is contained in the other polygon +(Vertices A, C, D) +* Compute and find the intersection points between each polygon (Verties B, E) +* Compute the convex hull shaped by these vertices by ordering them CCW. + +## Inputs / Outputs / API + +Inputs: +* Two iterables that contain vertices of the convex polygons ordered in the CCW direction. + +Outputs: +* A list of vertices of the intersection shape ordered in the CCW direction. + +## Future Work + +- #1230: Applying efficient algorithms. + +## Related issues + +- #983: Integrate vision detections in object tracker diff --git a/common/autoware_auto_geometry/design/spatial-hash-design.md b/common/autoware_auto_geometry/design/spatial-hash-design.md new file mode 100644 index 0000000000000..2b8f5449a0da4 --- /dev/null +++ b/common/autoware_auto_geometry/design/spatial-hash-design.md @@ -0,0 +1,102 @@ +Spatial Hash {#geometry-spatial-hash} +============ + +The spatial hash is a data structure designed for efficient fixed-radius near-neighbor queries in +low dimensions. + +The fixed-radius near-neighbors problem is defined as follows: + +`For point p, find all points p' s.t. d(p, p') < r` + +Where in this case `d(p, p')` is euclidean distance, and `r` is the fixed +radius. + +For `n` points with an average of `k` neighbors each, this data structure can +perform `m` near-neighbor queries (to generate lists of near-neighbors for `m` + different points) in `O(mk)` time. + +By contrast, using a k-d tree for successive nearest-neighbor queries results in +a running time of `O(m log n)`. + +The spatial hash works as follows: + +- Each point is assigned to a bin in the predefined bounding area defined by +`x_min/x_max` and `y_min/y_max` +- This can be done by converting x and y position into x and y index +respectively + - For example with the bin containing `x_min` and `y_min` as index `(0, 0)` + - The two (or more) indices can then be converted into a single index +- Once every point of interest has been inserted into the hash, near-neighbor +queries can begin: + - The bin of the reference point is first computed + - For each point in each adjacent bin, perform an explicit distance computation + between said point and the reference point. If the distance is below the given + radius, said point is considered to be a near-neighbor + +Under the hood, an `std::unordered_multimap` is used, where the key is a bin/voxel index. +The bin size was computed to be the same as the lookup distance. + +In addition, this data structure can support 2D or 3D queries. This is determined during +configuration, and baked into the data structure via the configuration class. The purpose of +this was to avoid if statements in tight loops. The configuration class specializations themself +use CRTP (Curiously Recurring Template Patterns) to do "static polymorphism", and avoid +a dispatching call. + +# Performance characterization + +## Time + +Insertion is `O(n)` because lookup time for the underlying hashmap is `O(n)` for +hashmaps. In practice, lookup time for hashmaps and thus insertion time should +be `O(1)`. + +Removing a point is `O(1)` because the current API only supports removal via +direct reference to a node. + +Finding `k` near-neighbors is worst case `O(n)` in the case of an adversarial +example, but in practice `O(k)`. + + +## Space + +The module consists of the following components: + +- The internal hashmap is `O(n + n + A * n)`, where `A` is an arbitrary +constant (load factor) +- The other components of the spatial hash are `O(n + n)` + +This results in `O(n)` space complexity. + + +# States + +The spatial hash's state is dictated by the status of the underlying unordered_multimap. + + +The data structure is wholly configured by a +[config](@ref autoware::common::geometry::spatial_hash::Config) class. The constructor +of the class determines in the data structure accepts strictly 2D or strictly 3D queries. + +# Inputs + +The primary method of introducing data into the data structure is via the +[insert](@ref autoware::common::geometry::spatial_hash::SpatialHashBase::insert) method. + +# Outputs + +The primary method of retrieving data from the data structure is via the +[near](@ref autoware::common::geometry::spatial_hash::SpatialHash::near)\(2D +configuration\) +or [near](@ref autoware::common::geometry::spatial_hash::SpatialHash::near) +\(3D configuration\) method. + +The whole data structure can also be traversed using standard constant iterators. + + +## Future Work + +- Performance tuning and optimization + +## Related issues + +- #28: Port to autoware.Auto diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/bounding_box_common.hpp b/common/autoware_auto_geometry/include/geometry/bounding_box/bounding_box_common.hpp new file mode 100644 index 0000000000000..a95344cbdece1 --- /dev/null +++ b/common/autoware_auto_geometry/include/geometry/bounding_box/bounding_box_common.hpp @@ -0,0 +1,188 @@ +// Copyright 2017-2019 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +/// \file +/// \brief Common functionality for bounding box computation algorithms + +#ifndef GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ +#define GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware +{ +namespace common +{ +namespace geometry +{ +/// \brief Functions and types for generating enclosing bounding boxes around a set of points +namespace bounding_box +{ +using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox; + +/// \brief Computes height of bounding box given a full list of points +/// \param[in] begin The start of the list of points +/// \param[in] end One past the end of the list of points +/// \param[out] box A box for which the z component of centroid, corners, and size gets filled +/// \tparam IT An iterator type, must dereference into a point type with float member z, or +/// appropriate point adapter defined +template +void compute_height(const IT begin, const IT end, BoundingBox & box) +{ + float32_t max_z = -std::numeric_limits::max(); + float32_t min_z = std::numeric_limits::max(); + for (auto it = begin; it != end; ++it) { + const float32_t z = point_adapter::z_(*it); + if (z <= min_z) { + min_z = z; + } + if (z >= max_z) { + max_z = z; + } + } + box.centroid.z = (max_z + min_z) * 0.5F; + for (auto & corner : box.corners) { + corner.z = box.centroid.z; + } + box.size.z = (max_z - min_z) * 0.5F; +} + +/// \brief Computes height of bounding box given a full list of points +/// \param[in] begin Iterator pointing to the start of the range of points +/// \param[in] end Iterator pointing the the end of the range of points +/// \param[out] shape A shape in which vertices z values and height field will be set +/// \tparam IT An iterator type, must dereference into a point type with float member z, or +/// appropriate point adapter defined +template +void compute_height(const IT begin, const IT end, autoware_auto_perception_msgs::msg::Shape & shape) +{ + float32_t max_z = -std::numeric_limits::max(); + float32_t min_z = std::numeric_limits::max(); + for (auto it = begin; it != end; ++it) { + const float32_t z = point_adapter::z_(*it); + if (z <= min_z) { + min_z = z; + } + if (z >= max_z) { + max_z = z; + } + } + for (auto & corner : shape.footprint.points) { + corner.z = min_z; + } + shape.dimensions.z = max_z - min_z; +} + +namespace details +{ +/// Templated alias for getting a type without CV or reference qualification +template +using base_type = typename std::remove_cv::type>::type; + +/// Templated alias for an array of 4 objects of type PointT +template +using Point4 = std::array; + +/// \brief Helper struct for compile time introspection of array size from +/// stackoverflow.com/questions/16866033/getting-the-number-of-elements-in-stdarray-at-compile-time +template +struct array_size; +template +struct array_size> +{ + static std::size_t const size = N; +}; +static_assert( + array_size>::size == 4U, + "Bounding box does not have the right number of corners"); + +/// \brief Compute length, width, area of bounding box +/// \param[in] corners Corners of the current bounding box +/// \param[out] ret A point struct used to hold size of box defined by corners +void GEOMETRY_PUBLIC +size_2d(const decltype(BoundingBox::corners) & corners, geometry_msgs::msg::Point32 & ret); +/// \brief Computes centroid and orientation of a box given corners +/// \param[in] corners Array of final corners of bounding box +/// \param[out] box Message to have corners, orientation, and centroid updated +void GEOMETRY_PUBLIC +finalize_box(const decltype(BoundingBox::corners) & corners, BoundingBox & box); + +/// \brief given support points and two orthogonal directions, compute corners of bounding box +/// \tparam PointT Type of a point, must have float members x and y` +/// \tparam IT An iterator dereferencable into PointT +/// \param[out] corners Gets filled with corner points of bounding box +/// \param[in] supports Iterators referring to support points of current bounding box +/// e.g. bounding box is touching these points +/// \param[in] directions Directions of each edge of the bounding box from each support point +template +void compute_corners( + decltype(BoundingBox::corners) & corners, const Point4 & supports, + const Point4 & directions) +{ + for (uint32_t idx = 0U; idx < corners.size(); ++idx) { + const uint32_t jdx = (idx != 3U) ? (idx + 1U) : 0U; + const auto pt = + intersection_2d(*supports[idx], directions[idx], *supports[jdx], directions[jdx]); + // TODO(c.ho) handle error + point_adapter::xr_(corners[idx]) = point_adapter::x_(pt); + point_adapter::yr_(corners[idx]) = point_adapter::y_(pt); + } +} +// TODO(c.ho) type trait enum base + +/// \brief Copy vertices of the given box into a Shape type +/// \param box Box to be converted +/// \return Shape type filled with box vertices +autoware_auto_perception_msgs::msg::Shape GEOMETRY_PUBLIC make_shape(const BoundingBox & box); + +/// \brief Copy centroid and orientation info of the box into Pose type +/// \param box BoundingBox to be converted +/// \return Pose type filled with centroid and orientation from box +geometry_msgs::msg::Pose GEOMETRY_PUBLIC make_pose(const BoundingBox & box); + +/// \brief Fill DetectedObject type with contents from a BoundingBox type +/// \param box BoundingBox to be converted +/// \return Filled DetectedObject type +autoware_auto_perception_msgs::msg::DetectedObject GEOMETRY_PUBLIC +make_detected_object(const BoundingBox & box); + +/// \brief Transform corners from object-local coordinates using the given centroid and orientation +/// \param shape_msg Msg containing corners in object-local coordinates +/// \param centroid Centroid of the polygon whose corners are defined in shape_msg +/// \param orientation Orientation of the polygon +/// \return corners transformed such that their centroid and orientation correspond to the +/// given inputs +std::vector GEOMETRY_PUBLIC get_transformed_corners( + const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation); + +} // namespace details +} // namespace bounding_box +} // namespace geometry +} // namespace common +} // namespace autoware + +#endif // GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp b/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp new file mode 100644 index 0000000000000..9425c451cf3f9 --- /dev/null +++ b/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp @@ -0,0 +1,254 @@ +// Copyright 2017-2019 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +/// \file +/// \brief This file implements 2D pca on a linked list of points to estimate an oriented +/// bounding box + +#ifndef GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ +#define GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ + +#include +#include +#include + +namespace autoware +{ +namespace common +{ +namespace geometry +{ +namespace bounding_box +{ +namespace details +{ + +/// \brief Simplified 2d covariance matrix +struct Covariance2d +{ + /// \brief Variance in the x direction + float32_t xx; + /// \brief Variance in the y direction + float32_t yy; + /// \brief x-y covariance + float32_t xy; + /// \brief Number of points + std::size_t num_points; +}; // struct Covariance2d + +/// \brief Compute 2d covariance matrix of a list of points using Welford's online algorithm +/// \param[in] begin An iterator pointing to the first point in a point list +/// \param[in] end An iterator pointing to one past the last point in the point list +/// \tparam IT An iterator type dereferencable into a point with float members x and y +/// \return A 2d covariance matrix for all points inthe list +template +Covariance2d covariance_2d(const IT begin, const IT end) +{ + Covariance2d ret{0.0F, 0.0F, 0.0F, 0U}; + float32_t ux = 0.0F; + float32_t uy = 0.0F; + float32_t num_points = 0.0F; + using point_adapter::x_; + using point_adapter::y_; + for (auto it = begin; it != end; ++it) { + ++ret.num_points; + num_points = static_cast(ret.num_points); + const auto & pt = *it; + // update mean x + const float32_t dx = x_(pt) - ux; + ux = ux + (dx / num_points); + // update cov + const float32_t dy = y_(pt) - uy; + ret.xy += (x_(pt) - ux) * (dy); + // update mean y + uy = uy + (dy / num_points); + // update M2 + ret.xx += dx * (x_(pt) - ux); + ret.yy += dy * (y_(pt) - uy); + } + // finalize sample (co-)variance + if (ret.num_points > 1U) { + num_points = num_points - 1.0F; + } + ret.xx /= num_points; + ret.yy /= num_points; + ret.xy /= num_points; + + return ret; +} + +/// \brief Compute eigenvectors and eigenvalues +/// \param[in] cov 2d Covariance matrix +/// \param[out] eigvec1 First eigenvector +/// \param[out] eigvec2 Second eigenvector +/// \tparam PointT Point type that has at least float members x and y +/// \return A pairt of eigenvalues: The first is the larger eigenvalue +/// \throw std::runtime error if you would get degenerate covariance +template +std::pair eig_2d( + const Covariance2d & cov, PointT & eigvec1, PointT & eigvec2) +{ + const float32_t tr_2 = (cov.xx + cov.yy) * 0.5F; + const float32_t det = (cov.xx * cov.yy) - (cov.xy * cov.xy); + // Add a small fudge to alleviate floating point errors + float32_t disc = ((tr_2 * tr_2) - det) + std::numeric_limits::epsilon(); + if (disc < 0.0F) { + throw std::runtime_error( + "pca_2d: negative discriminant! Should never happen for well formed " + "covariance matrix"); + } + disc = sqrtf(disc); + // compute eigenvalues + const auto ret = std::make_pair(tr_2 + disc, tr_2 - disc); + // compute eigenvectors + using point_adapter::xr_; + using point_adapter::yr_; + // We compare squared value against floating epsilon to make sure that eigen vectors + // are persistent against further calculations. + // (e.g. taking cross product of two eigen vectors) + if (fabsf(cov.xy * cov.xy) > std::numeric_limits::epsilon()) { + xr_(eigvec1) = cov.xy; + yr_(eigvec1) = ret.first - cov.xx; + xr_(eigvec2) = cov.xy; + yr_(eigvec2) = ret.second - cov.xx; + } else { + if (cov.xx > cov.yy) { + xr_(eigvec1) = 1.0F; + yr_(eigvec1) = 0.0F; + xr_(eigvec2) = 0.0F; + yr_(eigvec2) = 1.0F; + } else { + xr_(eigvec1) = 0.0F; + yr_(eigvec1) = 1.0F; + xr_(eigvec2) = 1.0F; + yr_(eigvec2) = 0.0F; + } + } + return ret; +} + + +/// \brief Given eigenvectors, compute support (furthest) point in each direction +/// \tparam IT An iterator type dereferencable into a point with float members x and y +/// \tparam PointT type of a point with float members x and y +/// \param[in] begin An iterator pointing to the first point in a point list +/// \param[in] end An iterator pointing to one past the last point in the point list +/// \param[in] eig1 First principal component of cluster +/// \param[in] eig2 Second principal component of cluster +/// \param[out] support Array to get filled with extreme points in each of the principal +/// components +/// \return whether or not eig2 is ccw wrt eig1 +template +bool8_t compute_supports( + const IT begin, + const IT end, + const PointT & eig1, + const PointT & eig2, + Point4 & support) +{ + const bool8_t ret = cross_2d(eig1, eig2) >= 0.0F; + std::array metrics{{ + -std::numeric_limits::max(), + -std::numeric_limits::max(), + std::numeric_limits::max(), + std::numeric_limits::max() + }}; + for (auto it = begin; it != end; ++it) { + const PointT & pt = *it; + float32_t val = dot_2d(ret ? eig1 : eig2, pt); + if (val > metrics[0U]) { + metrics[0U] = val; + support[0U] = it; + } + if (val < metrics[2U]) { + metrics[2U] = val; + support[2U] = it; + } + val = dot_2d(ret ? eig2 : eig1, pt); + if (val > metrics[1U]) { + metrics[1U] = val; + support[1U] = it; + } + if (val < metrics[3U]) { + metrics[3U] = val; + support[3U] = it; + } + } + return ret; +} + +/// \brief Compute bounding box given a pair of basis directions +/// \tparam IT An iterator type dereferencable into a point with float members x and y +/// \tparam PointT Point type of the lists, must have float members x and y +/// \param[in] ax1 First basis direction, assumed to be normal to ax2 +/// \param[in] ax2 Second basis direction, assumed to be normal to ax1, assumed to be ccw wrt ax1 +/// \param[in] supports Array of iterators referring to points that are most extreme in each basis +/// direction. Should be result of compute_supports function +/// \return A bounding box based on the basis axes and the support points +template +BoundingBox compute_bounding_box( + const PointT & ax1, + const PointT & ax2, + const Point4 & supports) +{ + decltype(BoundingBox::corners) corners; + const Point4 directions{{ax1, ax2, minus_2d(ax1), minus_2d(ax2)}}; + compute_corners(corners, supports, directions); + + // build box + BoundingBox bbox; + finalize_box(corners, bbox); + size_2d(corners, bbox.size); + return bbox; +} +} // namespace details + +/// \brief Compute oriented bounding box using PCA. This uses all points in a list, and does not +/// modify the list. The resulting bounding box is not necessarily minimum in any way +/// \param[in] begin An iterator pointing to the first point in a point list +/// \param[in] end An iterator pointing to one past the last point in the point list +/// \tparam IT An iterator type dereferencable into a point with float members x and y +/// \return An oriented bounding box in x-y. This bounding box has no height information +template +BoundingBox eigenbox_2d(const IT begin, const IT end) +{ + // compute covariance + const details::Covariance2d cov = details::covariance_2d(begin, end); + + // compute eigenvectors + using PointT = details::base_type; + PointT eig1; + PointT eig2; + const auto eigv = details::eig_2d(cov, eig1, eig2); + + // find extreme points + details::Point4 supports; + const bool8_t is_ccw = details::compute_supports(begin, end, eig1, eig2, supports); + // build box + if (is_ccw) { + std::swap(eig1, eig2); + } + BoundingBox bbox = details::compute_bounding_box(eig1, eig2, supports); + bbox.value = eigv.first; + + return bbox; +} +} // namespace bounding_box +} // namespace geometry +} // namespace common +} // namespace autoware + +#endif // GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/lfit.hpp b/common/autoware_auto_geometry/include/geometry/bounding_box/lfit.hpp new file mode 100644 index 0000000000000..ac2417204664c --- /dev/null +++ b/common/autoware_auto_geometry/include/geometry/bounding_box/lfit.hpp @@ -0,0 +1,284 @@ +// Copyright 2017-2019 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +/// \file +/// \brief This file implements 2D pca on a linked list of points to estimate an oriented +/// bounding box + +#ifndef GEOMETRY__BOUNDING_BOX__LFIT_HPP_ +#define GEOMETRY__BOUNDING_BOX__LFIT_HPP_ + +#include +#include +#include + +namespace autoware +{ +namespace common +{ +namespace geometry +{ +namespace bounding_box +{ +namespace details +{ + +/// \brief A representation of the M matrix for the L-fit algorithm +struct LFitWs +{ + /// \brief Number of points in the first partition + std::size_t p; + /// \brief Number of points in the second partition + std::size_t q; + // assume matrix of form: [a b; c d] + /// \brief Sum of x values in first partition + float32_t m12a; + /// \brief Sum of y values in first partition + float32_t m12b; + /// \brief Sum of y values in second partition + float32_t m12c; + /// \brief Negative sum of x values in second partition + float32_t m12d; + // m22 is a symmetric matrix + /// \brief Sum_p x_2 + sum_q y_2 + float32_t m22a; + /// \brief Sum_p x*y - sum_q x*y + float32_t m22b; + /// \brief Sum_p y_2 + sum_x y_2 + float32_t m22d; +}; // struct LFitWs + +/// \brief Initialize M matrix for L-fit algorithm +/// \param[in] begin An iterator pointing to the first point in a point list +/// \param[in] end An iterator one past the last point in the point list +/// \param[in] size The number of points in the point list +/// \param[out] ws A representation of the M matrix to get initialized +/// \tparam IT The iterator type, should dereference to a point type with float members x and y +template +void init_lfit_ws(const IT begin, const IT end, const std::size_t size, LFitWs & ws) +{ + ws.p = 1UL; + ws.q = size - 1UL; + // init P terms (first partition) + using point_adapter::x_; + using point_adapter::y_; + const auto & pt = *begin; + const float32_t px = x_(pt); + const float32_t py = y_(pt); + // assume matrix of form: [a b; c d] + ws.m12a = px; + ws.m12b = py; + ws.m12c = 0.0F; + ws.m12d = 0.0F; + // m22 is a symmetric matrix + ws.m22a = px * px; + ws.m22b = px * py; + ws.m22d = py * py; + auto it = begin; + ++it; + for (; it != end; ++it) { + const auto & qt = *it; + const float32_t qx = x_(qt); + const float32_t qy = y_(qt); + ws.m12c += qy; + ws.m12d -= qx; + ws.m22a += qy * qy; + ws.m22b -= qx * qy; + ws.m22d += qx * qx; + } +} + +/// \brief Solves the L fit problem for a given M matrix +/// \tparam PointT The point type of the cluster being L-fitted +/// \param[in] ws A representation of the M Matrix +/// \param[out] dir The best fit direction for this partition/M matrix +/// \return The L2 residual for this fit (the score, lower is better) +template +float32_t solve_lfit(const LFitWs & ws, PointT & dir) +{ + const float32_t pi = 1.0F / static_cast(ws.p); + const float32_t qi = 1.0F / static_cast(ws.q); + const Covariance2d M{ // matrix of form [x, z; z y] + ws.m22a - (((ws.m12a * ws.m12a) * pi) + ((ws.m12c * ws.m12c) * qi)), + ws.m22d - (((ws.m12b * ws.m12b) * pi) + ((ws.m12d * ws.m12d) * qi)), + ws.m22b - (((ws.m12a * ws.m12b) * pi) + ((ws.m12c * ws.m12d) * qi)), + 0UL + }; + PointT eig1; + const auto eigv = eig_2d(M, eig1, dir); + return eigv.second; +} + +/// \brief Increments L fit M matrix with information in the point +/// \tparam PointT The point type +/// \param[in] pt The point to increment the M matrix with +/// \param[inout] ws A representation of the M matrix +template +void increment_lfit_ws(const PointT & pt, LFitWs & ws) +{ + const float32_t px = point_adapter::x_(pt); + const float32_t py = point_adapter::y_(pt); + ws.m12a += px; + ws.m12b += py; + ws.m12c -= py; + ws.m12d += px; + ws.m22b += 2.0F * px * py; + const float32_t px2y2 = (px - py) * (px + py); + ws.m22a += px2y2; + ws.m22d -= px2y2; +} + +/// \tparam IT An iterator type that should dereference into a point type with float members x and y +template +class LFitCompare +{ +public: + /// \brief Constructor, initializes normal direction + /// \param[in] hint A 2d vector with the direction that will be used to order the + /// point list + explicit LFitCompare(const PointT & hint) + : m_nx(point_adapter::x_(hint)), + m_ny(point_adapter::y_(hint)) + { + } + + /// \brief Comparator operation, returns true if the projection of a the tangent line + /// is smaller than the projection of b + /// \param[in] p The first point for comparison + /// \param[in] q The second point for comparison + /// \return True if a has a smaller projection than b on the tangent line + bool8_t operator()(const PointT & p, const PointT & q) const + { + using point_adapter::x_; + using point_adapter::y_; + return ((x_(p) * m_nx) + (y_(p) * m_ny)) < ((x_(q) * m_nx) + (y_(q) * m_ny)); + } + +private: + const float32_t m_nx; + const float32_t m_ny; +}; // class LFitCompare + +/// \brief The main implementation of L-fitting a bounding box to a list of points. +/// Assumes sufficiently valid, large enough, and appropriately ordered point list +/// \param[in] begin An iterator pointing to the first point in a point list +/// \param[in] end An iterator pointing to one past the last point in the point list +/// \param[in] size The number of points in the point list +/// \return A bounding box that minimizes the LFit residual +/// \tparam IT An iterator type dereferencable into a point with float members x and y +template +BoundingBox lfit_bounding_box_2d_impl(const IT begin, const IT end, const std::size_t size) +{ + // initialize M + LFitWs ws{}; + init_lfit_ws(begin, end, size, ws); + // solve initial problem + details::base_type best_normal; + float32_t min_eig = solve_lfit(ws, best_normal); + // solve subsequent problems + auto it = begin; + ++it; + for (; it != end; ++it) { + // update M + ws.p += 1U; + ws.q -= 1U; + if (ws.q == 0U) { // checks for q = 0 case + break; + } + increment_lfit_ws(*it, ws); + // solve incremented problem + decltype(best_normal) dir; + const float32_t score = solve_lfit(ws, dir); + // update optima + if (score < min_eig) { + min_eig = score; + best_normal = dir; + } + } + // can recover best corner point, but don't care, need to cover all points + const auto inorm = 1.0F / norm_2d(best_normal); + if (!std::isnormal(inorm)) { + throw std::runtime_error{"LFit: Abnormal norm"}; + } + best_normal = times_2d(best_normal, inorm); + auto best_tangent = get_normal(best_normal); + // find extreme points + Point4 supports; + const bool8_t is_ccw = details::compute_supports(begin, end, best_normal, best_tangent, supports); + if (is_ccw) { + std::swap(best_normal, best_tangent); + } + BoundingBox bbox = details::compute_bounding_box(best_normal, best_tangent, supports); + bbox.value = min_eig; + + return bbox; +} +} // namespace details + +/// \brief Compute bounding box which best fits an L-shaped cluster. Uses the method proposed +/// in "Efficient L-shape fitting of laser scanner data for vehicle pose estimation" +/// \return An oriented bounding box in x-y. This bounding box has no height information +/// \param[in] begin An iterator pointing to the first point in a point list +/// \param[in] end An iterator pointing to one past the last point in the point list +/// \param[in] size The number of points in the point list +/// \param[in] hint An iterator pointing to the point whose normal will be used to sort the list +/// \return A pair where the first element is an iterator pointing to the nearest point, and the +/// second element is the size of the list +/// \tparam IT An iterator type dereferencable into a point with float members x and y +/// \throw std::domain_error If the number of points is too few +template +BoundingBox lfit_bounding_box_2d( + const IT begin, + const IT end, + const PointT hint, + const std::size_t size) +{ + if (size <= 1U) { + throw std::domain_error("LFit requires >= 2 points!"); + } + // NOTE: assumes points are in base_link/sensor frame! + // sort along tangent wrt sensor origin + //lint -e522 NOLINT Not a pure function: data structure iterators are pointing to is modified + std::partial_sort(begin, end, end, details::LFitCompare{hint}); + + return details::lfit_bounding_box_2d_impl(begin, end, size); +} + +/// \brief Compute bounding box which best fits an L-shaped cluster. Uses the method proposed +/// in "Efficient L-shape fitting of laser scanner data for vehicle pose estimation". +/// This implementation sorts the list using std::sort +/// \return An oriented bounding box in x-y. This bounding box has no height information +/// \param[in] begin An iterator pointing to the first point in a point list +/// \param[in] end An iterator pointing to one past the last point in the point list +/// \tparam IT An iterator type dereferencable into a point with float members x and y +template +BoundingBox lfit_bounding_box_2d(const IT begin, const IT end) +{ + // use the principal component as the sorting direction + const auto cov = details::covariance_2d(begin, end); + using PointT = details::base_type; + PointT eig1; + PointT eig2; + (void)details::eig_2d(cov, eig1, eig2); + (void)eig2; + return lfit_bounding_box_2d(begin, end, eig1, cov.num_points); +} +} // namespace bounding_box +} // namespace geometry +} // namespace common +} // namespace autoware + +#endif // GEOMETRY__BOUNDING_BOX__LFIT_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/rotating_calipers.hpp b/common/autoware_auto_geometry/include/geometry/bounding_box/rotating_calipers.hpp new file mode 100644 index 0000000000000..f1a2123e25c9b --- /dev/null +++ b/common/autoware_auto_geometry/include/geometry/bounding_box/rotating_calipers.hpp @@ -0,0 +1,290 @@ +// Copyright 2017-2019 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +/// \file +/// \brief This file implements the rotating calipers algorithm for minimum oriented bounding boxes + +#ifndef GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ +#define GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware +{ +namespace common +{ +namespace geometry +{ +namespace bounding_box +{ +namespace details +{ +/// \brief Find which (next) edge has smallest angle delta to directions, rotate directions +/// \param[inout] edges Array of edges on polygon after each anchor point (in ccw order). +/// E.g. if anchor point = p_i, edge = P[i+1] - P[i] +/// \param[inout] directions Array of directions of current bounding box (in ccw order) +/// \tparam PointT Point type of the lists, must have float members x and y +/// \return index of array to advance, e.g. the one with the smallest angle between edge and dir +template +uint32_t update_angles(const Point4 & edges, Point4 & directions) +{ + // find smallest angle to next + float32_t best_cos_th = -std::numeric_limits::max(); + float32_t best_edge_dir_mag = 0.0F; + uint32_t advance_idx = 0U; + for (uint32_t idx = 0U; idx < edges.size(); ++idx) { + const float32_t edge_dir_mag = + std::max( + norm_2d(edges[idx]) * norm_2d( + directions[idx]), std::numeric_limits::epsilon()); + const float32_t cos_th = dot_2d(edges[idx], directions[idx]) / edge_dir_mag; + if (cos_th > best_cos_th) { + best_cos_th = cos_th; + best_edge_dir_mag = edge_dir_mag; + advance_idx = idx; + } + } + + // update directions by smallest angle + const float32_t sin_th = + cross_2d(directions[advance_idx], edges[advance_idx]) / best_edge_dir_mag; + for (uint32_t idx = 0U; idx < edges.size(); ++idx) { + rotate_2d(directions[idx], best_cos_th, sin_th); + } + + return advance_idx; +} + +/// \brief Given support points i, find direction of edge: e = P[i+1] - P[i] +/// \tparam PointT Point type of the lists, must have float members x and y +/// \tparam IT The iterator type, should dereference to a point type with float members x and y +/// \param[in] begin An iterator pointing to the first point in a point list +/// \param[in] end An iterator pointing to one past the last point in the point list +/// \param[in] support Array of points that are most extreme in 4 directions for current OBB +/// \param[out] edges An array to be filled with the polygon edges next from support points +template +void init_edges( + const IT begin, + const IT end, + const Point4 & support, + Point4 & edges) +{ + for (uint32_t idx = 0U; idx < support.size(); ++idx) { + auto tmp_it = support[idx]; + ++tmp_it; + const PointT & q = (tmp_it == end) ? *begin : *tmp_it; + edges[idx] = minus_2d(q, *support[idx]); + } +} + +/// \brief Scan through list to find support points for bounding box oriented in direction of +/// u = P[1] - P[0] +/// \tparam IT The iterator type, should dereference to a point type with float members x and y +/// \param[in] begin An iterator pointing to the first point in a point list +/// \param[in] end An iterator pointing to one past the last point in the point list +/// \param[out] support Array that gets filled with pointers to points that are most extreme in +/// each direction aligned with and orthogonal to the first polygon edge. +/// Most extreme = max/min wrt u = P[1]-P[0] (in the dot/cross product sense) +template +void init_bbox(const IT begin, const IT end, Point4 & support) +{ + // compute initial orientation using first two points + auto pt_it = begin; + ++pt_it; + const auto u = minus_2d(*pt_it, *begin); + support[0U] = begin; + std::array metric{{ + -std::numeric_limits::max(), + -std::numeric_limits::max(), + std::numeric_limits::max() + }}; + // track u * p, fabsf(u x p), and -u * p + for (pt_it = begin; pt_it != end; ++pt_it) { + // check points against orientation for run_ptr + const auto & pt = *pt_it; + // u * p + const float32_t up = dot_2d(u, pt); + if (up > metric[0U]) { + metric[0U] = up; + support[1U] = pt_it; + } + // -u * p + if (up < metric[2U]) { + metric[2U] = up; + support[3U] = pt_it; + } + // u x p + const float32_t uxp = cross_2d(u, pt); + if (uxp > metric[1U]) { + metric[1U] = uxp; + support[2U] = pt_it; + } + } +} +/// \brief Compute the minimum bounding box for a convex hull using the rotating calipers method. +/// This function may possibly also be used for computing the width of a convex hull, as it uses the +/// external supports of a single convex hull. +/// \param[in] begin An iterator to the first point on a convex hull +/// \param[in] end An iterator to one past the last point on a convex hull +/// \param[in] metric_fn A functor determining what measure the bounding box is minimum with respect +/// to +/// \tparam IT An iterator type dereferencable into a point type with float members x and y +/// \tparam MetricF A functor that computes a float measure from the x and y size (width and length) +/// of a bounding box, assumed to be packed in a Point32 message. +/// \throw std::domain_error if the list of points is too small to reasonable generate a bounding +/// box +template +BoundingBox rotating_calipers_impl(const IT begin, const IT end, const MetricF metric_fn) +{ + using PointT = base_type; + // sanity check TODO(c.ho) more checks (up to n = 2?) + if (begin == end) { + throw std::domain_error("Malformed list, size = " + std::to_string(std::distance(begin, end))); + } + // initial scan to get anchor points + Point4 support; + init_bbox(begin, end, support); + // initialize directions accordingly + Point4 directions; + { // I just don't want the temporary variable floating around + auto tmp = support[0U]; + ++tmp; + directions[0U] = minus_2d(*tmp, *support[0U]); + directions[1U] = get_normal(directions[0U]); + directions[2U] = minus_2d(directions[0U]); + directions[3U] = minus_2d(directions[1U]); + } + // initialize edges + Point4 edges; + init_edges(begin, end, support, edges); + // size of initial guess + BoundingBox bbox; + decltype(BoundingBox::corners) best_corners; + compute_corners(best_corners, support, directions); + size_2d(best_corners, bbox.size); + bbox.value = metric_fn(bbox.size); + // rotating calipers step: incrementally advance, update angles, points, compute area + for (auto it = begin; it != end; ++it) { + // find smallest angle to next, update directions + const uint32_t advance_idx = update_angles(edges, directions); + // recompute area + decltype(BoundingBox::corners) corners; + compute_corners(corners, support, directions); + decltype(BoundingBox::size) tmp_size; + size_2d(corners, tmp_size); + const float32_t tmp_value = metric_fn(tmp_size); + // update best if necessary + if (tmp_value < bbox.value) { + // corners: memcpy is fine since I know corners and best_corners are distinct + (void)std::memcpy(&best_corners[0U], &corners[0U], sizeof(corners)); + // size + bbox.size = tmp_size; + bbox.value = tmp_value; + } + // Step to next iteration of calipers + { + // update smallest support + auto next_it = support[advance_idx]; + ++next_it; + const auto run_it = (end == next_it) ? begin : next_it; + support[advance_idx] = run_it; + // update edges + next_it = run_it; + ++next_it; + const PointT & next = (end == next_it) ? (*begin) : (*next_it); + edges[advance_idx] = minus_2d(next, *run_it); + } + } + + finalize_box(best_corners, bbox); + + // TODO(christopher.ho) check if one of the sizes is too small, fuzz corner 1 and 2 + // This doesn't cause any issues now, it shouldn't happen in practice, and even if it did, + // it would probably get smoothed out by prediction. But, this could cause issues with the + // collision detection algorithms (i.e GJK), but probably not. + + return bbox; +} +} // namespace details + +/// \brief Compute the minimum area bounding box given a convex hull of points. +/// This function is exposed in case a user might already have a convex hull via other methods. +/// \param[in] begin An iterator to the first point on a convex hull +/// \param[in] end An iterator to one past the last point on a convex hull +/// \return A minimum area bounding box, value field is the area +/// \tparam IT An iterator type dereferencable into a point type with float members x and y +template +BoundingBox minimum_area_bounding_box(const IT begin, const IT end) +{ + const auto metric_fn = [](const decltype(BoundingBox::size) & pt) -> float32_t + { + return pt.x * pt.y; + }; + return details::rotating_calipers_impl(begin, end, metric_fn); +} + +/// \brief Compute the minimum perimeter bounding box given a convex hull of points +/// This function is exposed in case a user might already have a convex hull via other methods. +/// \param[in] begin An iterator to the first point on a convex hull +/// \param[in] end An iterator to one past the last point on a convex hull +/// \return A minimum perimeter bounding box, value field is half the perimeter +/// \tparam IT An iterator type dereferencable into a point type with float members x and y +template +BoundingBox minimum_perimeter_bounding_box(const IT begin, const IT end) +{ + const auto metric_fn = [](const decltype(BoundingBox::size) & pt) -> float32_t + { + return pt.x + pt.y; + }; + return details::rotating_calipers_impl(begin, end, metric_fn); +} + +/// \brief Compute the minimum area bounding box given an unstructured list of points. +/// Only a list is supported as it enables the convex hull to be formed in O(n log n) time and +/// without memory allocation. +/// \param[inout] list A list of points to form a hull around, gets reordered +/// \return A minimum area bounding box, value field is the area +/// \tparam PointT Point type of the lists, must have float members x and y +template +BoundingBox minimum_area_bounding_box(std::list & list) +{ + const auto last = convex_hull(list); + return minimum_area_bounding_box(list.cbegin(), last); +} + +/// \brief Compute the minimum perimeter bounding box given an unstructured list of points +/// Only a list is supported as it enables the convex hull to be formed in O(n log n) time and +/// without memory allocation. +/// \param[inout] list A list of points to form a hull around, gets reordered +/// \return A minimum perimeter bounding box, value field is half the perimeter +/// \tparam PointT Point type of the lists, must have float members x and y +template +BoundingBox minimum_perimeter_bounding_box(std::list & list) +{ + const auto last = convex_hull(list); + return minimum_perimeter_bounding_box(list.cbegin(), last); +} +} // namespace bounding_box +} // namespace geometry +} // namespace common +} // namespace autoware +#endif // GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box_2d.hpp b/common/autoware_auto_geometry/include/geometry/bounding_box_2d.hpp new file mode 100644 index 0000000000000..c072ebb2b7a99 --- /dev/null +++ b/common/autoware_auto_geometry/include/geometry/bounding_box_2d.hpp @@ -0,0 +1,34 @@ +// Copyright 2017-2020 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. +/// \file +/// \brief Main header for user-facing bounding box algorithms: functions and types +#ifndef GEOMETRY__BOUNDING_BOX_2D_HPP_ +#define GEOMETRY__BOUNDING_BOX_2D_HPP_ + +#include +#include +#include + +namespace autoware +{ +namespace common +{ +namespace geometry +{ +} // namespace geometry +} // namespace common +} // namespace autoware +#endif // GEOMETRY__BOUNDING_BOX_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/common_2d.hpp b/common/autoware_auto_geometry/include/geometry/common_2d.hpp new file mode 100644 index 0000000000000..9b2a4e6ad9058 --- /dev/null +++ b/common/autoware_auto_geometry/include/geometry/common_2d.hpp @@ -0,0 +1,595 @@ +// Copyright 2017-2021 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. +/// \file +/// \brief This file includes common functionality for 2D geometry, such as dot products + +#ifndef GEOMETRY__COMMON_2D_HPP_ +#define GEOMETRY__COMMON_2D_HPP_ + +#include +#include +#include +#include + +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "geometry/interval.hpp" + +using autoware::common::types::float32_t; +using autoware::common::types::bool8_t; + +namespace comparison = autoware::common::helper_functions::comparisons; + +namespace autoware +{ +namespace common +{ +namespace geometry +{ + +/// \brief Temporary namespace for point adapter methods, for use with nonstandard point types +namespace point_adapter +{ +/// \brief Gets the x value for a point +/// \return The x value of the point +/// \param[in] pt The point +/// \tparam PointT The point type +template +inline auto x_(const PointT & pt) +{ + return pt.x; +} +/// \brief Gets the x value for a TrajectoryPoint message +/// \return The x value of the point +/// \param[in] pt The point +inline auto x_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) +{ + return pt.pose.position.x; +} +/// \brief Gets the y value for a point +/// \return The y value of the point +/// \param[in] pt The point +/// \tparam PointT The point type +template +inline auto y_(const PointT & pt) +{ + return pt.y; +} +/// \brief Gets the y value for a TrajectoryPoint message +/// \return The y value of the point +/// \param[in] pt The point +inline auto y_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) +{ + return pt.pose.position.y; +} +/// \brief Gets the z value for a point +/// \return The z value of the point +/// \param[in] pt The point +/// \tparam PointT The point type +template +inline auto z_(const PointT & pt) +{ + return pt.z; +} +/// \brief Gets the z value for a TrajectoryPoint message +/// \return The z value of the point +/// \param[in] pt The point +inline auto z_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) +{ + return pt.pose.position.z; +} +/// \brief Gets a reference to the x value for a point +/// \return A reference to the x value of the point +/// \param[in] pt The point +/// \tparam PointT The point type +template +inline auto & xr_(PointT & pt) +{ + return pt.x; +} +/// \brief Gets a reference to the x value for a TrajectoryPoint +/// \return A reference to the x value of the TrajectoryPoint +/// \param[in] pt The TrajectoryPoint +inline auto & xr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) +{ + return pt.pose.position.x; +} +/// \brief Gets a reference to the y value for a point +/// \return A reference to The y value of the point +/// \param[in] pt The point +/// \tparam PointT The point type +template +inline auto & yr_(PointT & pt) +{ + return pt.y; +} +/// \brief Gets a reference to the y value for a TrajectoryPoint +/// \return A reference to the y value of the TrajectoryPoint +/// \param[in] pt The TrajectoryPoint +inline auto & yr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) +{ + return pt.pose.position.y; +} +/// \brief Gets a reference to the z value for a point +/// \return A reference to the z value of the point +/// \param[in] pt The point +/// \tparam PointT The point type +template +inline auto & zr_(PointT & pt) +{ + return pt.z; +} +/// \brief Gets a reference to the z value for a TrajectoryPoint +/// \return A reference to the z value of the TrajectoryPoint +/// \param[in] pt The TrajectoryPoint +inline auto & zr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) +{ + return pt.pose.position.z; +} +} // namespace point_adapter + +namespace details +{ +// Return the next iterator, cycling back to the beginning of the list if you hit the end +template +IT circular_next(const IT begin, const IT end, const IT current) noexcept +{ + auto next = std::next(current); + if (end == next) { + next = begin; + } + return next; +} + +} // namespace details + + +/// \tparam T1, T2, T3 point type. Must have point adapters defined or have float members x and y +/// \brief compute whether line segment rp is counter clockwise relative to line segment qp +/// \param[in] pt shared point for both line segments +/// \param[in] r point to check if it forms a ccw angle +/// \param[in] q reference point +/// \return whether angle formed is ccw. Three collinear points is considered ccw +template +inline auto ccw(const T1 & pt, const T2 & q, const T3 & r) +{ + using point_adapter::x_; + using point_adapter::y_; + return (((x_(q) - x_(pt)) * (y_(r) - y_(pt))) - ((y_(q) - y_(pt)) * (x_(r) - x_(pt)))) <= 0.0F; +} + +/// \tparam T1, T2 point type. Must have point adapters defined or have float members x and y +/// \brief compute p x q = p1 * q2 - p2 * q1 +/// \param[in] pt first point +/// \param[in] q second point +/// \return 2d cross product +template +inline auto cross_2d(const T1 & pt, const T2 & q) +{ + using point_adapter::x_; + using point_adapter::y_; + return (x_(pt) * y_(q)) - (y_(pt) * x_(q)); +} + +/// \tparam T1, T2 point type. Must have point adapters defined or have float members x and y +/// \brief compute p * q = p1 * q1 + p2 * q2 +/// \param[in] pt first point +/// \param[in] q second point +/// \return 2d scalar dot product +template +inline auto dot_2d(const T1 & pt, const T2 & q) +{ + using point_adapter::x_; + using point_adapter::y_; + return (x_(pt) * x_(q)) + (y_(pt) * y_(q)); +} + +/// \tparam T point type. Must have point adapters defined or have float members x and y +/// \brief Compute the 2d difference between two points, p - q +/// \param[in] p The left hand side +/// \param[in] q The right hand side +/// \return A point with the difference in the x and y fields, all other fields are default +/// initialized +template +T minus_2d(const T & p, const T & q) +{ + T r; + using point_adapter::x_; + using point_adapter::y_; + point_adapter::xr_(r) = x_(p) - x_(q); + point_adapter::yr_(r) = y_(p) - y_(q); + return r; +} + +/// \tparam T point type. Must have point adapters defined or have float members x and y +/// \brief The unary minus or negation operator applied to a single point's 2d fields +/// \param[in] p The left hand side +/// \return A point with the negation in the x and y fields, all other fields are default +/// initialized +template +T minus_2d(const T & p) +{ + T r; + point_adapter::xr_(r) = -point_adapter::x_(p); + point_adapter::yr_(r) = -point_adapter::y_(p); + return r; +} +/// \tparam T point type. Must have point adapters defined or have float members x and y +/// \brief The 2d addition operation, p + q +/// \param[in] p The left hand side +/// \param[in] q The right hand side +/// \return A point with the sum in the x and y fields, all other fields are default +/// initialized +template +T plus_2d(const T & p, const T & q) +{ + T r; + using point_adapter::x_; + using point_adapter::y_; + point_adapter::xr_(r) = x_(p) + x_(q); + point_adapter::yr_(r) = y_(p) + y_(q); + return r; +} + +/// \tparam T point type. Must have point adapters defined or have float members x and y +/// \brief The scalar multiplication operation, p * a +/// \param[in] p The point value +/// \param[in] a The scalar value +/// \return A point with the scaled x and y fields, all other fields are default +/// initialized +template +T times_2d(const T & p, const float32_t a) +{ + T r; + point_adapter::xr_(r) = static_cast(point_adapter::x_(p)) * a; + point_adapter::yr_(r) = static_cast(point_adapter::y_(p)) * a; + return r; +} + +/// \tparam T point type. Must have point adapters defined or have float members x and y +/// \brief solve p + t * u = q + s * v +/// Ref: https://stackoverflow.com/questions/563198/ +/// whats-the-most-efficent-way-to-calculate-where-two-line-segments-intersect +/// \param[in] pt anchor point of first line +/// \param[in] u direction of first line +/// \param[in] q anchor point of second line +/// \param[in] v direction of second line +/// \return intersection point +/// \throw std::runtime_error if lines are (nearly) collinear or parallel +template +inline T intersection_2d(const T & pt, const T & u, const T & q, const T & v) +{ + const float32_t num = cross_2d(minus_2d(pt, q), u); + float32_t den = cross_2d(v, u); + constexpr auto FEPS = std::numeric_limits::epsilon(); + if (fabsf(den) < FEPS) { + if (fabsf(num) < FEPS) { + // collinear case, anything is ok + den = 1.0F; + } else { + // parallel case, no valid output + throw std::runtime_error( + "intersection_2d: no unique solution (either collinear or parallel)"); + } + } + return plus_2d(q, times_2d(v, num / den)); +} + + +/// \tparam T point type. Must have point adapters defined or have float members x and y +/// \brief rotate point given precomputed sin and cos +/// \param[inout] pt point to rotate +/// \param[in] cos_th precomputed cosine value +/// \param[in] sin_th precompined sine value +template +inline void rotate_2d(T & pt, const float32_t cos_th, const float32_t sin_th) +{ + const float32_t x = point_adapter::x_(pt); + const float32_t y = point_adapter::y_(pt); + point_adapter::xr_(pt) = (cos_th * x) - (sin_th * y); + point_adapter::yr_(pt) = (sin_th * x) + (cos_th * y); +} + +/// \tparam T point type. Must have point adapters defined or have float members x and y +/// \brief rotate by radian angle th in z direction with ccw positive +/// \param[in] pt reference point to rotate +/// \param[in] th_rad angle by which to rotate point +/// \return rotated point +template +inline T rotate_2d(const T & pt, const float32_t th_rad) +{ + T q(pt); // It's reasonable to expect a copy constructor + const float32_t s = sinf(th_rad); + const float32_t c = cosf(th_rad); + rotate_2d(q, c, s); + return q; +} + +/// \tparam T point type. Must have point adapters defined or have float members x and y +/// \brief compute q s.t. p T q, or p * q = 0 +/// This is the equivalent of a 90 degree ccw rotation +/// \param[in] pt point to get normal point of +/// \return point normal to p (unnormalized) +template +inline T get_normal(const T & pt) +{ + T q(pt); + point_adapter::xr_(q) = -point_adapter::y_(pt); + point_adapter::yr_(q) = point_adapter::x_(pt); + return q; +} + +/// \tparam T point type. Must have point adapters defined or have float members x and y +/// \brief get magnitude of x and y components: +/// \param[in] pt point to get magnitude of +/// \return magitude of x and y components together +template +inline auto norm_2d(const T & pt) +{ + return sqrtf(static_cast(dot_2d(pt, pt))); +} + +/// \tparam T point type. Must have point adapters defined or have float members x and y +/// \brief Compute the closest point on line segment p-q to point r +/// Based on equations from https://stackoverflow.com/a/1501725 and +/// http://paulbourke.net/geometry/pointlineplane/ +/// \param[in] p First point defining the line segment +/// \param[in] q Second point defining the line segment +/// \param[in] r Reference point to find the closest point to +/// \return Closest point on line segment p-q to point r +template +inline T closest_segment_point_2d(const T & p, const T & q, const T & r) +{ + const T qp = minus_2d(q, p); + const float32_t len2 = static_cast(dot_2d(qp, qp)); + T ret = p; + if (len2 > std::numeric_limits::epsilon()) { + const Interval_f unit_interval(0.0f, 1.0f); + const float32_t val = static_cast(dot_2d(minus_2d(r, p), qp)) / len2; + const float32_t t = Interval_f::clamp_to(unit_interval, val); + ret = plus_2d(p, times_2d(qp, t)); + } + return ret; +} +// +/// \tparam T point type. Must have point adapters defined or have float members x and y +/// \brief Compute the closest point on the line going through p-q to point r +// Obtained by simplifying closest_segment_point_2d. +/// \param[in] p First point defining the line +/// \param[in] q Second point defining the line +/// \param[in] r Reference point to find the closest point to +/// \return Closest point on line p-q to point r +/// \throw std::runtime_error if the two points coincide and hence don't uniquely +// define a line +template +inline T closest_line_point_2d(const T & p, const T & q, const T & r) +{ + const T qp = minus_2d(q, p); + const float32_t len2 = dot_2d(qp, qp); + T ret = p; + if (len2 > std::numeric_limits::epsilon()) { + const float32_t t = dot_2d(minus_2d(r, p), qp) / len2; + ret = plus_2d(p, times_2d(qp, t)); + } else { + throw std::runtime_error( + "closet_line_point_2d: line ill-defined because given points coincide"); + } + return ret; +} + +/// \tparam T point type. Must have point adapters defined or have float members x and y +/// \brief Compute the distance from line segment p-q to point r +/// \param[in] p First point defining the line segment +/// \param[in] q Second point defining the line segment +/// \param[in] r Reference point to find the distance from the line segment to +/// \return Distance from point r to line segment p-q +template +inline auto point_line_segment_distance_2d(const T & p, const T & q, const T & r) +{ + const T pq_r = minus_2d(closest_segment_point_2d(p, q, r), r); + return norm_2d(pq_r); +} + +/// \brief Make a 2D unit vector given an angle. +/// \tparam T Point type. Must have point adapters defined or have float members x and y +/// \param th Angle in radians +/// \return Unit vector in the direction of the given angle. +template +inline T make_unit_vector2d(float th) +{ + T r; + point_adapter::xr_(r) = std::cos(th); + point_adapter::yr_(r) = std::sin(th); + return r; +} + +/// \brief Compute squared euclidean distance between two points +/// \tparam OUT return type. Type of the returned distance. +/// \tparam T1 point type. Must have point adapters defined or have float members x and y +/// \tparam T2 point type. Must have point adapters defined or have float members x and y +/// \param a point 1 +/// \param b point 2 +/// \return squared euclidean distance +template +inline OUT squared_distance_2d(const T1 & a, const T2 & b) +{ + const auto x = static_cast(point_adapter::x_(a)) - static_cast(point_adapter::x_(b)); + const auto y = static_cast(point_adapter::y_(a)) - static_cast(point_adapter::y_(b)); + return (x * x) + (y * y); +} + +/// \brief Compute euclidean distance between two points +/// \tparam OUT return type. Type of the returned distance. +/// \tparam T1 point type. Must have point adapters defined or have float members x and y +/// \tparam T2 point type. Must have point adapters defined or have float members x and y +/// \param a point 1 +/// \param b point 2 +/// \return euclidean distance +template +inline OUT distance_2d(const T1 & a, const T2 & b) +{ + return std::sqrt(squared_distance_2d(a, b)); +} + +/// \brief Check the given point's position relative the infinite line passing +/// from p1 to p2. Logic based on http://geomalgorithms.com/a01-_area.html#isLeft() +/// \tparam T T point type. Must have point adapters defined or have float members x and y +/// \param p1 point 1 laying on the infinite line +/// \param p2 point 2 laying on the infinite line +/// \param q point to be checked against the line +/// \return > 0 for point q left of the line through p1 to p2 +/// = 0 for point q on the line through p1 to p2 +/// < 0 for point q right of the line through p1 to p2 +template +inline auto check_point_position_to_line_2d(const T & p1, const T & p2, const T & q) +{ + return cross_2d(minus_2d(p2, p1), minus_2d(q, p1)); +} + +/// Check if all points are ordered in x-y plane (in either clockwise or counter-clockwise +/// direction): This function does not check for convexity +/// \tparam IT Iterator type pointing to a point containing float x and float y +/// \param[in] begin Beginning of point sequence +/// \param[in] end One past the last of the point sequence +/// \return Whether or not all point triples p_i, p_{i+1}, p_{i+2} are in a particular order. +/// Returns true for collinear points as well +template +bool all_ordered(const IT begin, const IT end) noexcept +{ + // Short circuit: a line or point is always CCW or otherwise ill-defined + if (std::distance(begin, end) <= 2U) { + return true; + } + bool is_first_point_cw = false; + // Can't use std::all_of because that works directly on the values + for (auto line_start = begin; line_start != end; ++line_start) { + const auto line_end = details::circular_next(begin, end, line_start); + const auto query_point = details::circular_next(begin, end, line_end); + // Check if 3 points starting from current point are in clockwise direction + const bool is_cw = comparison::abs_lte( + check_point_position_to_line_2d(*line_start, *line_end, *query_point), + 0.0F, 1e-3F + ); + if (is_cw) { + if (line_start == begin) { + is_first_point_cw = true; + } else { + if (!is_first_point_cw) { + return false; + } + } + } else if (is_first_point_cw) { + return false; + } + } + return true; +} + +/// Compute the area of a convex hull, points are assumed to be ordered (in either CW or CCW) +/// \tparam IT Iterator type pointing to a point containing float x and float y +/// \param[in] begin Iterator pointing to the beginning of the polygon points +/// \param[in] end Iterator pointing to one past the last of the polygon points +/// \return The area of the polygon, in squared of whatever units your points are in +template +auto area_2d(const IT begin, const IT end) noexcept +{ + using point_adapter::x_; + using point_adapter::y_; + using T = decltype(x_(*begin)); + auto area = T{}; // zero initialization + // use the approach of https://www.mathwords.com/a/area_convex_polygon.htm + for (auto it = begin; it != end; ++it) { + const auto next = details::circular_next(begin, end, it); + area += x_(*it) * y_(*next); + area -= x_(*next) * y_(*it); + } + return std::abs(T{0.5} *area); +} + +/// Compute area of convex hull, throw if points are not ordered (convexity check is not +/// implemented) +/// \throw std::domain_error if points are not ordered either CW or CCW +/// \tparam IT Iterator type pointing to a point containing float x and float y +/// \param[in] begin Iterator pointing to the beginning of the polygon points +/// \param[in] end Iterator pointing to one past the last of the polygon points +/// \return The area of the polygon, in squared of whatever units your points are in +template +auto area_checked_2d(const IT begin, const IT end) +{ + if (!all_ordered(begin, end)) { + throw std::domain_error{"Cannot compute area: points are not ordered"}; + } + return area_2d(begin, end); +} + +/// \brief Check if the given point is inside or on the edge of the given polygon +/// \tparam IteratorType iterator type. The value pointed to by this must have point adapters +/// defined or have float members x and y +/// \tparam PointType point type. Must have point adapters defined or have float members x and y +/// \param start_it iterator pointing to the first vertex of the polygon +/// \param end_it iterator pointing to the last vertex of the polygon. The vertices should be in +/// order. +/// \param p point to be searched +/// \return True if the point is inside or on the edge of the polygon. False otherwise +template +bool is_point_inside_polygon_2d( + const IteratorType & start_it, const IteratorType & end_it, const PointType & p) +{ + std::int32_t winding_num = 0; + + for (IteratorType it = start_it; it != end_it; ++it) { + auto next_it = std::next(it); + if (next_it == end_it) { + next_it = start_it; + } + if (point_adapter::y_(*it) <= point_adapter::y_(p)) { + // Upward crossing edge + if (point_adapter::y_(*next_it) >= point_adapter::y_(p)) { + if (comparison::abs_gt(check_point_position_to_line_2d(*it, *next_it, p), 0.0F, 1e-3F)) { + ++winding_num; + } else { + if (comparison::abs_eq_zero( + check_point_position_to_line_2d(*it, *next_it, p), + 1e-3F)) + { + return true; + } + } + } + } else { + // Downward crossing edge + if (point_adapter::y_(*next_it) <= point_adapter::y_(p)) { + if (comparison::abs_lt(check_point_position_to_line_2d(*it, *next_it, p), 0.0F, 1e-3F)) { + --winding_num; + } else { + if (comparison::abs_eq_zero( + check_point_position_to_line_2d(*it, *next_it, p), + 1e-3F)) + { + return true; + } + } + } + } + } + return winding_num != 0; +} + + +} // namespace geometry +} // namespace common +} // namespace autoware + +#endif // GEOMETRY__COMMON_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/common_3d.hpp b/common/autoware_auto_geometry/include/geometry/common_3d.hpp new file mode 100644 index 0000000000000..9ba839f11d352 --- /dev/null +++ b/common/autoware_auto_geometry/include/geometry/common_3d.hpp @@ -0,0 +1,77 @@ +// Copyright 2017-2019 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. +/// \file +/// \brief This file includes common functionality for 3D geometry, such as dot products + +#ifndef GEOMETRY__COMMON_3D_HPP_ +#define GEOMETRY__COMMON_3D_HPP_ + +#include + +namespace autoware +{ +namespace common +{ +namespace geometry +{ + +/// \tparam T1, T2 point type. Must have point adapters defined or have float members x, y and z +/// \brief compute p * q = p1 * q1 + p2 * q2 + p3 * 13 +/// \param[in] pt first point +/// \param[in] q second point +/// \return 3d scalar dot product +template +inline auto dot_3d(const T1 & pt, const T2 & q) +{ + using point_adapter::x_; + using point_adapter::y_; + using point_adapter::z_; + return (x_(pt) * x_(q)) + (y_(pt) * y_(q) + z_(pt) * z_(q)); +} + +/// \brief Compute 3D squared euclidean distance between two points +/// \tparam OUT return type. Type of the returned distance. +/// \tparam T1 point type. Must have point adapters defined or have float members x y and z +/// \tparam T2 point type. Must have point adapters defined or have float members x y and z +/// \param a point 1 +/// \param b point 2 +/// \return squared 3D euclidean distance +template +inline OUT squared_distance_3d(const T1 & a, const T2 & b) +{ + const auto x = static_cast(point_adapter::x_(a)) - static_cast(point_adapter::x_(b)); + const auto y = static_cast(point_adapter::y_(a)) - static_cast(point_adapter::y_(b)); + const auto z = static_cast(point_adapter::z_(a)) - static_cast(point_adapter::z_(b)); + return (x * x) + (y * y) + (z * z); +} + +/// \brief Compute 3D euclidean distance between two points +/// \tparam T1 point type. Must have point adapters defined or have float members x y and z +/// \tparam T2 point type. Must have point adapters defined or have float members x y and z +/// \param a point 1 +/// \param b point 2 +/// \return 3D euclidean distance +template +inline OUT distance_3d(const T1 & a, const T2 & b) +{ + return std::sqrt(squared_distance_3d(a, b)); +} + +} // namespace geometry +} // namespace common +} // namespace autoware + +#endif // GEOMETRY__COMMON_3D_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/convex_hull.hpp b/common/autoware_auto_geometry/include/geometry/convex_hull.hpp new file mode 100644 index 0000000000000..e780c2a23ee49 --- /dev/null +++ b/common/autoware_auto_geometry/include/geometry/convex_hull.hpp @@ -0,0 +1,194 @@ +// Copyright 2017-2019 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +/// \file +/// \brief This file implements the monotone chain algorithm to compute 2D convex hulls on linked +/// lists of points + +#ifndef GEOMETRY__CONVEX_HULL_HPP_ +#define GEOMETRY__CONVEX_HULL_HPP_ + +#include +#include + +//lint -e537 NOLINT pclint vs cpplint +#include +//lint -e537 NOLINT pclint vs cpplint +#include +#include +#include + +using autoware::common::types::float32_t; + +namespace autoware +{ +namespace common +{ +namespace geometry +{ +/// \brief Contains computation geometry functions not intended for the end user to directly use +namespace details +{ + +/// \brief Moves points comprising the lower convex hull from points to hull. +/// \param[inout] points A list of points, assumed to be sorted in lexical order +/// \param[inout] hull An empty list of points, assumed to have same allocator as points +/// (for splice) +/// \tparam PointT The point type for the points list +/// \tparam HullT the point type for the hull list +template +void form_lower_hull(std::list & points, std::list & hull) +{ + auto hull_it = hull.cbegin(); + auto point_it = points.cbegin(); + // This ensures that the points we splice to back to the end of list are not processed + const auto iters = points.size(); + for (auto idx = decltype(iters) {0}; idx < iters; ++idx) { + // splice points from tail of hull to tail of list until point from head of list satisfies ccw + bool8_t is_ccw = true; + while ((hull.cbegin() != hull_it) && is_ccw) { + const auto current_hull_it = hull_it; + --hull_it; + is_ccw = ccw(*hull_it, *current_hull_it, *point_it); + if (!is_ccw) { + hull_it = current_hull_it; + break; + } + // return this node to list for consideration in upper hull + points.splice(points.end(), hull, current_hull_it); + } + const auto last_point_it = point_it; + ++point_it; + // Splice head of list to tail of (lower) hull + hull.splice(hull.end(), points, last_point_it); + // point_it has been advanced, hull_it has been advanced (to where point_it was previously) + hull_it = last_point_it; + } + // loop is guaranteed not to underflow because a node can be removed from points AT MOST once + // per loop iteration. The loop is upper bounded to the prior size of the point list +} +/// \brief Moves points comprising the lower convex hull from points to hull. +/// \param[inout] points A list of points, assumed to be sorted in lexical order, and to contain +/// the leftmost point +/// \param[inout] hull A list of points, assumed to have same allocator as points (for splice), +/// and to contain the lower hull (minus the left-most point) +/// \tparam PointT The point type for the points list +/// \tparam HullT the point type for the hull list +template +void form_upper_hull(std::list & points, std::list & hull) +{ + // TODO(c.ho) consider reverse iterators, not sure if they work with splice() + auto hull_it = hull.cend(); + --hull_it; + const auto lower_hull_end = hull_it; + auto point_it = points.cend(); + --point_it; + // This ensures that the points we splice to back to the head of list are not processed + const auto iters = points.size(); + for (auto idx = decltype(iters) {0}; idx < iters; ++idx) { + // splice points from tail of hull to head of list until ccw is satisfied with tail of list + bool8_t is_ccw = true; + while ((lower_hull_end != hull_it) && is_ccw) { + const auto current_hull_it = hull_it; + --hull_it; + is_ccw = ccw(*hull_it, *current_hull_it, *point_it); + if (!is_ccw) { + hull_it = current_hull_it; + break; + } + points.splice(points.begin(), hull, current_hull_it); + } + const auto last_point_it = point_it; + --point_it; + // Splice points from tail of lexically ordered point list to tail of hull + hull.splice(hull.end(), points, last_point_it); + hull_it = last_point_it; + } + // loop is guaranteed not to underflow because a node can be removed from points AT MOST once + // per loop iteration. The loop is upper bounded to the prior size of the point list +} + +/// \brief A static memory implementation of convex hull computation. Shuffles points around the +/// deque such that the points of the convex hull of the deque of points are first in the +/// deque, with the internal points following in an unspecified order. +/// The head of the deque will be the point with the smallest x value, with the other +/// points following in a counter-clockwise manner (from a top down view/facing -z direction) +/// \param[inout] list A list of nodes that will be pruned down and reordered into a ccw convex hull +/// \return An iterator pointing to one after the last point contained in the hull +/// \tparam PointT Type of a point, must have x and y float members +template +typename std::list::const_iterator convex_hull_impl(std::list & list) +{ + // Functor that return whether a <= b in the lexical sense (a.x < b.x), sort by y if tied + const auto lexical_comparator = [](const PointT & a, const PointT & b) -> bool8_t + { + using point_adapter::x_; + using point_adapter::y_; + constexpr auto FEPS = std::numeric_limits::epsilon(); + return (fabsf(x_(a) - x_(b)) > FEPS) ? + (x_(a) < x_(b)) : (y_(a) < y_(b)); + }; + list.sort(lexical_comparator); + + // Temporary list to store points + std::list tmp_hull_list{list.get_allocator()}; + + // Shuffle lower hull points over to tmp_hull_list + form_lower_hull(list, tmp_hull_list); + + // Resort list since we can't guarantee the order TODO(c.ho) is this true? + list.sort(lexical_comparator); + // splice first hull point to beginning of list to ensure upper hull is properly closed + // This is done after sorting because we know exactly where it should go, and keeping it out of + // sorting reduces complexity somewhat + list.splice(list.begin(), tmp_hull_list, tmp_hull_list.begin()); + + // build upper hull + form_upper_hull(list, tmp_hull_list); + // Move hull to beginning of list, return iterator pointing to one after the convex hull + const auto ret = list.begin(); + // first move left-most point to ensure it is first + auto tmp_it = tmp_hull_list.end(); + --tmp_it; + list.splice(list.begin(), tmp_hull_list, tmp_it); + // Then move remainder of hull points + list.splice(ret, tmp_hull_list); + return ret; +} +} // namespace details + +/// \brief A static memory implementation of convex hull computation. Shuffles points around the +/// deque such that the points of the convex hull of the deque of points are first in the +/// deque, with the internal points following in an unspecified order. +/// +/// The head of the deque will be the point with the smallest x value, with the other +/// points following in a counter-clockwise manner (from a top down view/facing -z +/// direction). If the point list is 3 or smaller, nothing is done (e.g. the ordering result +/// as previously stated does not hold). +/// \param[inout] list A list of nodes that will be reordered into a ccw convex hull +/// \return An iterator pointing to one after the last point contained in the hull +/// \tparam PointT Type of a point, must have x and y float members +template +typename std::list::const_iterator convex_hull(std::list & list) +{ + return (list.size() <= 3U) ? list.end() : details::convex_hull_impl(list); +} + +} // namespace geometry +} // namespace common +} // namespace autoware + +#endif // GEOMETRY__CONVEX_HULL_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/hull_pockets.hpp b/common/autoware_auto_geometry/include/geometry/hull_pockets.hpp new file mode 100644 index 0000000000000..39aad00798229 --- /dev/null +++ b/common/autoware_auto_geometry/include/geometry/hull_pockets.hpp @@ -0,0 +1,118 @@ +// Copyright 2020 Embotech AG, Zurich, Switzerland. All rights reserved. +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +/// \file +/// \brief This file implements an algorithm for getting a list of "pockets" in the convex +/// hull of a non-convex simple polygon. + +#ifndef GEOMETRY__HULL_POCKETS_HPP_ +#define GEOMETRY__HULL_POCKETS_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +using autoware::common::types::float32_t; + +namespace autoware +{ +namespace common +{ +namespace geometry +{ + + +/// \brief Compute a list of "pockets" of a simple polygon +/// (https://en.wikipedia.org/wiki/Simple_polygon), that is, the areas that make +/// up the difference between the polygon and its convex hull. +/// This currently has a bug: +// * "Rollover" is not properly handled - if a pocket contains the segment from +// the last point in the list to the first point (which is still part of the +// polygon), that does not get added +/// +/// \param[in] polygon_start Start iterator for the simple polygon (has to be on convex hull) +/// \param[in] polygon_end End iterator for the simple polygon +/// \param[in] convex_hull_start Start iterator for the convex hull of the simple polygon +/// \param[in] convex_hull_end End iterator for the convex hull of the simple polygon +/// \return A vector of vectors of the iterator value type. Each inner vector contains the points +/// for one pocket. We return by value instead of as iterator pairs, because it is possible +/// that the edge connecting the final point in the list and the first point in the list is +/// part of a pocket as well, and this is awkward to represent using iterators into the +/// original collection. +/// +/// \tparam Iter1 Iterator to a point type +/// \tparam Iter2 Iterator to a point type (can be the same as Iter1, but does not need to be) +template +typename std::vector::value_type>> hull_pockets( + const Iter1 polygon_start, + const Iter1 polygon_end, + const Iter2 convex_hull_start, + const Iter2 convex_hull_end +) +{ + auto pockets = std::vector::value_type>>{}; + if (std::distance(polygon_start, polygon_end) <= 3) { + return pockets; + } + + // Function to check whether a point is in the convex hull. + const auto in_convex_hull = [convex_hull_start, convex_hull_end](Iter1 p) { + return std::any_of( + convex_hull_start, convex_hull_end, [p](auto hull_entry) + { + return norm_2d( + minus_2d( + hull_entry, + *p)) < std::numeric_limits::epsilon(); + }); + }; + + // We go through the points of the polygon only once, adding pockets to the list of pockets + // as we detect them. + std::vector::value_type> current_pocket{}; + for (auto it = polygon_start; it != polygon_end; it = std::next(it)) { + if (in_convex_hull(it)) { + if (current_pocket.size() > 1) { + current_pocket.emplace_back(*it); + pockets.push_back(current_pocket); + } + current_pocket.clear(); + current_pocket.emplace_back(*it); + } else { + current_pocket.emplace_back(*it); + } + } + + // At this point, we have reached the end of the polygon, but have not considered the connection + // of the final point back to the first. In case the first point is part of a pocket as well, we + // have some points left in current_pocket, and we add one final pocket that is made up by those + // points plus the first point in the collection. + if (current_pocket.size() > 1) { + current_pocket.push_back(*polygon_start); + pockets.push_back(current_pocket); + } + + return pockets; +} +} // namespace geometry +} // namespace common +} // namespace autoware + +#endif // GEOMETRY__HULL_POCKETS_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/intersection.hpp b/common/autoware_auto_geometry/include/geometry/intersection.hpp new file mode 100644 index 0000000000000..5c172a08ccfdb --- /dev/null +++ b/common/autoware_auto_geometry/include/geometry/intersection.hpp @@ -0,0 +1,324 @@ +// Copyright 2020 Embotech AG, Zurich, Switzerland +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#ifndef GEOMETRY__INTERSECTION_HPP_ +#define GEOMETRY__INTERSECTION_HPP_ + + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware +{ +namespace common +{ +namespace geometry +{ +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_auto_perception_msgs::msg::BoundingBox; +using autoware::common::geometry::convex_hull; +using autoware::common::geometry::get_normal; +using autoware::common::geometry::dot_2d; +using autoware::common::geometry::minus_2d; +using autoware::common::geometry::times_2d; +using autoware::common::geometry::norm_2d; +using autoware::common::geometry::closest_line_point_2d; + +using Point = geometry_msgs::msg::Point32; + +namespace details +{ + +/// Alias for a std::pair of two points +using Line = std::pair; + +/// \tparam Iter1 Iterator over point-types that must have point adapters +// defined or have float members x and y +/// \brief Compute a sorted list of faces of a polyhedron given a list of points +/// \param[in] start Start iterator of the list of points +/// \param[in] end End iterator of the list of points +/// \return The list of faces +template +std::vector get_sorted_face_list(const Iter start, const Iter end) +{ + // First get a sorted list of points - convex_hull does that by modifying its argument + auto corner_list = std::list(start, end); + const auto first_interior_point = convex_hull(corner_list); + + std::vector face_list{}; + auto itLast = corner_list.begin(); + auto itNext = std::next(itLast, 1); + do { + face_list.emplace_back(Line{*itLast, *itNext}); + itLast = itNext; + itNext = std::next(itNext, 1); + } while ((itNext != first_interior_point) && (itNext != corner_list.end())); + + face_list.emplace_back(Line{*itLast, corner_list.front()}); + + return face_list; +} + +/// \brief Append points of the polygon `internal` that are contained in the polygon `exernal`. +template class Iterable1T, template class Iterable2T, + typename PointT> +void append_contained_points( + const Iterable1T & external, + const Iterable2T & internal, + std::list & result) +{ + std::copy_if( + internal.begin(), internal.end(), std::back_inserter(result), + [&external](const auto & pt) { + return common::geometry::is_point_inside_polygon_2d(external.begin(), external.end(), pt); + }); +} + +/// \brief Append the intersecting points between two polygons into the output list. +template class Iterable1T, template class Iterable2T, + typename PointT> +void append_intersection_points( + const Iterable1T & polygon1, + const Iterable2T & polygon2, + std::list & result) +{ + using FloatT = decltype(point_adapter::x_(std::declval())); + using Interval = common::geometry::Interval; + + auto get_edge = [](const auto & list, const auto & iterator) { + const auto next_it = std::next(iterator); + const auto & next_pt = (next_it != list.end()) ? *next_it : list.front(); + return std::make_pair(*iterator, next_pt); + }; + + // Get the max absolute value out of two intervals and a scalar. + auto compute_eps_scale = [](const auto & i1, const auto & i2, const auto val) { + auto get_abs_max = [](const auto & interval) { + return std::max(std::fabs(Interval::min(interval)), std::fabs(Interval::max(interval))); + }; + return std::max(std::fabs(val), std::max(get_abs_max(i1), get_abs_max(i2))); + }; + + // Compare each edge from polygon1 to each edge from polygon2 + for (auto corner1_it = polygon1.begin(); corner1_it != polygon1.end(); ++corner1_it) { + const auto & edge1 = get_edge(polygon1, corner1_it); + + Interval edge1_x_interval{ + std::min(point_adapter::x_(edge1.first), point_adapter::x_(edge1.second)), + std::max(point_adapter::x_(edge1.first), point_adapter::x_(edge1.second))}; + + Interval edge1_y_interval{ + std::min(point_adapter::y_(edge1.first), point_adapter::y_(edge1.second)), + std::max(point_adapter::y_(edge1.first), point_adapter::y_(edge1.second))}; + + for (auto corner2_it = polygon2.begin(); corner2_it != polygon2.end(); ++corner2_it) { + try { + const auto & edge2 = get_edge(polygon2, corner2_it); + const auto & intersection = + common::geometry::intersection_2d( + edge1.first, minus_2d(edge1.second, edge1.first), + edge2.first, minus_2d(edge2.second, edge2.first)); + + Interval edge2_x_interval{ + std::min(point_adapter::x_(edge2.first), point_adapter::x_(edge2.second)), + std::max(point_adapter::x_(edge2.first), point_adapter::x_(edge2.second))}; + + Interval edge2_y_interval{ + std::min(point_adapter::y_(edge2.first), point_adapter::y_(edge2.second)), + std::max(point_adapter::y_(edge2.first), point_adapter::y_(edge2.second))}; + + // The accumulated floating point error depends on the magnitudes of each end of the + // intervals. Hence the upper bound of the absolute magnitude should be taken into account + // while computing the epsilon. + const auto max_feps_scale = std::max( + compute_eps_scale(edge1_x_interval, edge2_x_interval, point_adapter::x_(intersection)), + compute_eps_scale(edge1_y_interval, edge2_y_interval, point_adapter::y_(intersection)) + ); + const auto feps = max_feps_scale * std::numeric_limits::epsilon(); + // Only accept intersections that lie on both of the line segments (edges) + if (Interval::contains(edge1_x_interval, point_adapter::x_(intersection), feps) && + Interval::contains(edge2_x_interval, point_adapter::x_(intersection), feps) && + Interval::contains(edge1_y_interval, point_adapter::y_(intersection), feps) && + Interval::contains(edge2_y_interval, point_adapter::y_(intersection), feps)) + { + result.push_back(intersection); + } + } catch (const std::runtime_error &) { + // Parallel lines. TODO(yunus.caliskan): #1229 + continue; + } + } + } +} + + +} // namespace details + +// TODO(s.me) implement GJK(+EPA) algorithm as well as per Chris Ho's suggestion +/// \tparam Iter Iterator over point-types that must have point adapters +// defined or have float members x and y +/// \brief Check if polyhedra defined by two given sets of points intersect +// This uses SAT for doing the actual checking +// (https://en.wikipedia.org/wiki/Hyperplane_separation_theorem#Use_in_collision_detection) +/// \param[in] begin1 Start iterator to first list of point types +/// \param[in] end1 End iterator to first list of point types +/// \param[in] begin2 Start iterator to first list of point types +/// \param[in] end2 End iterator to first list of point types +/// \return true if the boxes collide, false otherwise. +template +bool intersect(const Iter begin1, const Iter end1, const Iter begin2, const Iter end2) +{ + // Obtain sorted lists of faces of both boxes, merge them into one big list of faces + auto faces = details::get_sorted_face_list(begin1, end1); + const auto faces_2 = details::get_sorted_face_list(begin2, end2); + faces.reserve(faces.size() + faces_2.size()); + faces.insert(faces.end(), faces_2.begin(), faces_2.end() ); + + // Also look at last line + for (const auto & face : faces) { + // Compute normal vector to the face and define a closure to get progress along it + const auto normal = get_normal(minus_2d(face.second, face.first)); + auto get_position_along_line = [&normal](auto point) + { + return dot_2d(normal, minus_2d(point, Point{}) ); + }; + + // Define a function to get the minimum and maximum projected position of the corners + // of a given bounding box along the normal line of the face + auto get_projected_min_max = [&get_position_along_line, &normal](Iter begin, Iter end) + { + const auto zero_point = Point{}; + auto min_corners = + get_position_along_line(closest_line_point_2d(normal, zero_point, *begin)); + auto max_corners = min_corners; + + for (auto & point = begin; point != end; ++point) { + const auto point_projected = closest_line_point_2d(normal, zero_point, *point); + const auto position_along_line = get_position_along_line(point_projected); + min_corners = std::min(min_corners, position_along_line); + max_corners = std::max(max_corners, position_along_line); + } + return std::pair{min_corners, max_corners}; + }; + + // Perform the actual computations for the extent computation + auto minmax_1 = get_projected_min_max(begin1, end1); + auto minmax_2 = get_projected_min_max(begin2, end2); + + // Check for any intersections + const auto eps = std::numeric_limits::epsilon(); + if (minmax_1.first > minmax_2.second + eps || minmax_2.first > minmax_1.second + eps) { + // Found separating hyperplane, stop + return false; + } + } + + // No separating hyperplane found, boxes collide + return true; +} + +/// \brief Get the intersection between two polygons. The polygons should be provided in an +/// identical format to the output of `convex_hull` function as in the corners should be ordered +/// in a CCW fashion. +/// The computation is done by: +/// * Find the corners of each polygon that are contained by the other polygon. +/// * Find the intersection points between two polygons +/// * Combine these points and order CCW to get the final polygon. +/// The criteria for intersection is better explained in: +/// "Area of intersection of arbitrary polygons" (Livermore, Calif, 1977) +/// See https://www.osti.gov/servlets/purl/7309916/, chapter II - B +/// TODO(yunus.caliskan): This is a naive implementation. We should scan for a more efficient +/// algorithm: #1230 +/// \tparam Iterable1T A container class that has stl style iterators defined. +/// \tparam Iterable2T A container class that has stl style iterators defined. +/// \tparam PointT Point type that have the adapters for the x and y fields. +/// set to `Point1T` +/// \param polygon1 A convex polygon +/// \param polygon2 A convex polygon +/// \return The resulting conv +template class Iterable1T, template class Iterable2T, + typename PointT> +std::list convex_polygon_intersection2d( + const Iterable1T & polygon1, + const Iterable2T & polygon2) +{ + std::list result; + details::append_contained_points(polygon1, polygon2, result); + details::append_contained_points(polygon2, polygon1, result); + details::append_intersection_points(polygon1, polygon2, result); + const auto end_it = common::geometry::convex_hull(result); + result.resize(static_cast(std::distance(result.cbegin(), end_it))); + return result; +} + + +/// \brief Compute the intersection over union of two 2d convex polygons. If any of the polygons +/// span a zero area, the result is 0.0. +/// \tparam Iterable1T A container class that has stl style iterators defined. +/// \tparam Iterable2T A container class that has stl style iterators defined. +/// \tparam Point1T Point type that have the adapters for the x and y fields. +/// \tparam Point2T Point type that have the adapters for the x and y fields. +/// \param polygon1 A convex polygon +/// \param polygon2 A convex polygon +/// \return (Intersection / Union) between two given polygons. +/// \throws std::domain_error If there is any inconsistency on the undderlying geometrical +/// computation. +template class Iterable1T, template class Iterable2T, + typename PointT> +common::types::float32_t convex_intersection_over_union_2d( + const Iterable1T & polygon1, + const Iterable2T & polygon2 +) +{ + constexpr auto eps = std::numeric_limits::epsilon(); + const auto intersection = convex_polygon_intersection2d(polygon1, polygon2); + + const auto intersection_area = + common::geometry::area_2d(intersection.begin(), intersection.end()); + + if (intersection_area < eps) { + return 0.0F; // There's either no intersection or the points are collinear + } + + const auto polygon1_area = + common::geometry::area_2d(polygon1.begin(), polygon1.end()); + const auto polygon2_area = + common::geometry::area_2d(polygon2.begin(), polygon2.end()); + + const auto union_area = polygon1_area + polygon2_area - intersection_area; + if (union_area < eps) { + throw std::domain_error("IoU is undefined for polygons with a zero union area"); + } + + return intersection_area / union_area; +} + +} // namespace geometry +} // namespace common +} // namespace autoware + +#endif // GEOMETRY__INTERSECTION_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/interval.hpp b/common/autoware_auto_geometry/include/geometry/interval.hpp new file mode 100644 index 0000000000000..a15fb52b42612 --- /dev/null +++ b/common/autoware_auto_geometry/include/geometry/interval.hpp @@ -0,0 +1,365 @@ +// Copyright 2020 Mapless AI, Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#ifndef GEOMETRY__INTERVAL_HPP_ +#define GEOMETRY__INTERVAL_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/types.hpp" +#include "helper_functions/float_comparisons.hpp" + +namespace autoware +{ +namespace common +{ +namespace geometry +{ + +/** + * @brief Data structure to contain scalar interval bounds. + * + * @post The interval is guaranteed to be valid upon successful construction. An + * interval [min, max] is "valid" if it is empty (min/max = NaN) or its bounds + * are ordered (min <= max). + */ +template +class Interval +{ + static_assert( + std::is_floating_point::value, + "Intervals only support floating point types."); + +public: + /** + * @brief Compute equality. + * + * Two intervals compare equal iff they are both valid and they are both + * either empty or have equal bounds. + * + * @note This is defined inline because the class is templated. + * + * @return True iff the intervals compare equal. + */ + friend bool operator==(const Interval & i1, const Interval & i2) + { + const auto min_eq = (Interval::min(i1) == Interval::min(i2)); + const auto max_eq = (Interval::max(i1) == Interval::max(i2)); + const auto bounds_equal = (min_eq && max_eq); + const auto both_empty = (Interval::empty(i1) && Interval::empty(i2)); + return both_empty || bounds_equal; + } + + /** + * @brief Compute inequality and the logical negation of equality. + * @note This is defined inline because the class is templated. + */ + friend bool operator!=(const Interval & i1, const Interval & i2) + { + return !(i1 == i2); + } + + /** + * @brief Stream overload for Interval types. + * + * @note Output precision is fixed inside the function definition, and the + * serialization is JSON compatible. + * + * @note The serialization is lossy. It is used for debugging and for + * generating exception strings. + */ + friend std::ostream & operator<<(std::ostream & os, const Interval & i) + { + constexpr auto PRECISION = 5; + + // Internal helper to format the output. + const auto readable_extremum = [](const T & val) { + if (val == std::numeric_limits::lowest()) { + return std::string("REAL_MIN"); + } + if (val == std::numeric_limits::max()) { + return std::string("REAL_MAX"); + } + + std::stringstream ss; + ss.setf(std::ios::fixed, std::ios::floatfield); + ss.precision(PRECISION); + ss << val; + return ss.str(); + }; + + // Do stream output. + if (Interval::empty(i)) { + return os << "{}"; + } + return os << "{\"min\": " << readable_extremum(Interval::min(i)) << + ", \"max\": " << readable_extremum(Interval::max(i)) << "}"; + } + + /** + * @brief Test whether the two intervals have bounds within epsilon of each + * other. + * + * @note If both intervals are empty, this returns true. If only one is empty, + * this returns false. + */ + static bool abs_eq(const Interval & i1, const Interval & i2, const T & eps); + + /** @brief The minimum bound of the interval. */ + static T min(const Interval & i) {return i.min_;} + + /** @brief The maximum bound of the interval. */ + static T max(const Interval & i) {return i.max_;} + + /** + * @brief Return the measure (length) of the interval. + * + * @note For empty or invalid intervals, NaN is returned. See Interval::empty + * for note on distinction between measure zero and the emptiness property. + */ + static T measure(const Interval & i); + + /** + * @brief Utility for checking whether an interval has zero measure. + * + * @note For empty or invalid intervals, false is returned. See + * Interval::empty for note on distinction between measure zero and the + * emptiness property. + * + * @return True iff the interval has zero measure. + */ + static bool zero_measure(const Interval & i); + + /** + * @brief Whether the interval is empty or not. + * + * @note Emptiness refers to whether the interval contains any points and is + * thus a distinct property from measure: an interval is non-empty if contains + * only a single point even though its measure in that case is zero. + * + * @return True iff the interval is empty. + */ + static bool empty(const Interval & i); + + /** + * @brief Test for whether a given interval contains a given value within the given epsilon + * @return True iff 'interval' contains 'value'. + */ + static bool contains( + const Interval & i, const T & value, + const T & eps = std::numeric_limits::epsilon()); + + /** + * @brief Test for whether 'i1' subseteq 'i2' + * @return True iff i1 subseteq i2. + */ + static bool is_subset_eq(const Interval & i1, const Interval & i2); + + /** + * @brief Compute the intersection of two intervals as a new interval. + */ + static Interval intersect(const Interval & i1, const Interval & i2); + + /** + * @brief Clamp a scalar 'val' onto 'interval'. + * @return If 'val' in 'interval', return 'val'; otherwise return the nearer + * interval bound. + */ + static T clamp_to(const Interval & i, T val); + + /** + * @brief Constructor: initialize an empty interval with members set to NaN. + */ + Interval(); + + /** + * @brief Constructor: specify exact interval bounds. + * + * @note An empty interval is specified by setting both bounds to NaN. + * @note An exception is thrown if the specified bounds are invalid. + * + * @post Construction is successful iff the interval is valid. + */ + Interval(const T & min, const T & max); + +private: + static constexpr T NaN = std::numeric_limits::quiet_NaN(); + + T min_; + T max_; + + /** + * @brief Verify that the bounds are valid in an interval. + * @note This method is private because it can only be used in the + * constructor. Once an interval has been constructed, its bounds are + * guaranteed to be valid. + */ + static bool bounds_valid(const Interval & i); +}; // class Interval + +//------------------------------------------------------------------------------ + +typedef Interval Interval_d; +typedef Interval Interval_f; + +//------------------------------------------------------------------------------ + +template +constexpr T Interval::NaN; + +//------------------------------------------------------------------------------ + +template +Interval::Interval() +: min_(Interval::NaN), max_(Interval::NaN) {} + +//------------------------------------------------------------------------------ + +template +Interval::Interval(const T & min, const T & max) +: min_(min), max_(max) +{ + if (!Interval::bounds_valid(*this)) { + std::stringstream ss; + ss << "Attempted to construct an invalid interval: " << *this; + throw std::runtime_error(ss.str()); + } +} + +//------------------------------------------------------------------------------ + +template +bool Interval::abs_eq( + const Interval & i1, const Interval & i2, const T & eps) +{ + namespace comp = helper_functions::comparisons; + + const auto both_empty = Interval::empty(i1) && Interval::empty(i2); + const auto both_non_empty = !Interval::empty(i1) && !Interval::empty(i2); + + const auto mins_equal = comp::abs_eq(Interval::min(i1), Interval::min(i2), eps); + const auto maxs_equal = comp::abs_eq(Interval::max(i1), Interval::max(i2), eps); + const auto both_non_empty_equal = both_non_empty && mins_equal && maxs_equal; + + return both_empty || both_non_empty_equal; +} + +//------------------------------------------------------------------------------ + +template +bool Interval::zero_measure(const Interval & i) +{ + // An empty interval will return false because (NaN == NaN) is false. + return Interval::min(i) == Interval::max(i); +} + +//------------------------------------------------------------------------------ + +template +bool Interval::empty(const Interval & i) +{ + return std::isnan(Interval::min(i)) && std::isnan(Interval::max(i)); +} + +//------------------------------------------------------------------------------ + +template +bool Interval::bounds_valid(const Interval & i) +{ + const auto is_ordered = (Interval::min(i) <= Interval::max(i)); + + // Check for emptiness expicitly because it requires both bounds to be NaN + return Interval::empty(i) || is_ordered; +} + +//------------------------------------------------------------------------------ + +template +bool Interval::is_subset_eq(const Interval & i1, const Interval & i2) +{ + const auto lower_contained = (Interval::min(i1) >= Interval::min(i2)); + const auto upper_contained = (Interval::max(i1) <= Interval::max(i2)); + return lower_contained && upper_contained; +} + +//------------------------------------------------------------------------------ + +template +bool Interval::contains(const Interval & i, const T & value, const T & eps) +{ + return common::helper_functions::comparisons::abs_gte(value, Interval::min(i), eps) && + common::helper_functions::comparisons::abs_lte(value, Interval::max(i), eps); +} + +//------------------------------------------------------------------------------ + +template +T Interval::measure(const Interval & i) +{ + return Interval::max(i) - Interval::min(i); +} + +//------------------------------------------------------------------------------ + +template +Interval Interval::intersect(const Interval & i1, const Interval & i2) +{ + // Construct intersection assuming an intersection exists. + try { + const auto either_empty = Interval::empty(i1) || Interval::empty(i2); + const auto min = std::max(Interval::min(i1), Interval::min(i2)); + const auto max = std::min(Interval::max(i1), Interval::max(i2)); + return either_empty ? Interval() : Interval(min, max); + } catch (...) { + } + + // Otherwise, the intersection is empty. + return Interval(); +} + +//------------------------------------------------------------------------------ + +template +T Interval::clamp_to(const Interval & i, T val) +{ + // clamp val to min + val = std::max(val, Interval::min(i)); + + // clamp val to max + val = std::min(val, Interval::max(i)); + + return Interval::empty(i) ? Interval::NaN : val; +} + +//------------------------------------------------------------------------------ + +} // namespace geometry +} // namespace common +} // namespace autoware + +#endif // GEOMETRY__INTERVAL_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/lookup_table.hpp b/common/autoware_auto_geometry/include/geometry/lookup_table.hpp new file mode 100644 index 0000000000000..6d97f392a14c9 --- /dev/null +++ b/common/autoware_auto_geometry/include/geometry/lookup_table.hpp @@ -0,0 +1,184 @@ +// Copyright 2017-2020 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +/// \file +/// \brief This file contains a 1D linear lookup table implementation + +#ifndef GEOMETRY__LOOKUP_TABLE_HPP_ +#define GEOMETRY__LOOKUP_TABLE_HPP_ + +#include +#include +#include + +#include "common/types.hpp" +#include "geometry/interval.hpp" + +namespace autoware +{ +namespace common +{ +namespace helper_functions +{ + +namespace +{ + +/** + * @brief Compute a scaling between 'a' and 'b' + * @note scaling is clamped to be in the interval [0, 1] + */ +template +T interpolate(const T & a, const T & b, U scaling) +{ + static const geometry::Interval UNIT_INTERVAL(static_cast(0), static_cast(1)); + scaling = geometry::Interval::clamp_to(UNIT_INTERVAL, scaling); + + const auto m = (b - a); + const auto offset = static_cast(m) * scaling; + return a + static_cast(offset); +} + +// TODO(c.ho) support more forms of interpolation as template functor +// Actual lookup logic, assuming all invariants hold: +// Throw if value is not finite +template +T lookup_impl_1d(const std::vector & domain, const std::vector & range, const T value) +{ + if (!std::isfinite(value)) { + throw std::domain_error{"Query value is not finite (NAN or INF)"}; + } + if (value <= domain.front()) { + return range.front(); + } else if (value >= domain.back()) { + return range.back(); + } else { + // Fall through to normal case + } + + auto second_idx{0U}; + for (auto idx = 1U; idx < domain.size(); ++idx) { + if (value < domain[idx]) { + second_idx = idx; + break; + } + } + // T must be a floating point between 0 and 1 + const auto num = static_cast(value - domain[second_idx - 1U]); + const auto den = static_cast(domain[second_idx] - domain[second_idx - 1U]); + const auto t = num / den; + const auto val = interpolate(range[second_idx - 1U], range[second_idx], t); + return static_cast(val); +} + +// Check invariants for table lookup: +template +void check_table_lookup_invariants(const std::vector & domain, const std::vector & range) +{ + if (domain.size() != range.size()) { + throw std::domain_error{"Domain's size does not match range's"}; + } + if (domain.empty()) { + throw std::domain_error{"Empty domain or range"}; + } + // Check if sorted: Can start at 1 because not empty + for (auto idx = 1U; idx < domain.size(); ++idx) { + if (domain[idx] <= domain[idx - 1U]) { // This is safe because indexing starts at 1 + throw std::domain_error{"Domain is not sorted"}; + } + } +} +} // namespace + +/// Do a 1D table lookup: Does some semi-expensive O(N) error checking first. +/// If query value fall out of the domain, then the value at the corresponding edge of the domain is +/// returned. +/// \param[in] domain The domain, or set of x values +/// \param[in] range The range, or set of y values +/// \param[in] value The point in the domain to query, x +/// \return A linearly interpolated value y, corresponding to the query, x +/// \throw std::domain_error If domain or range is empty +/// \throw std::domain_error If range is not the same size as domain +/// \throw std::domain_error If domain is not sorted +/// \throw std::domain_error If value is not finite (NAN or INF) +/// \tparam T The type of the function, must be interpolatable +template +T lookup_1d(const std::vector & domain, const std::vector & range, const T value) +{ + check_table_lookup_invariants(domain, range); + + return lookup_impl_1d(domain, range, value); +} + +/// A class wrapping a 1D lookup table. Intended for more frequent lookups. Error checking is pushed +/// into the constructor and not done in the lookup function call +/// \tparam T The type of the function, must be interpolatable +template +class LookupTable1D +{ +public: + /// Constructor + /// \param[in] domain The domain, or set of x values + /// \param[in] range The range, or set of y values + /// \throw std::domain_error If domain or range is empty + /// \throw std::domain_error If range is not the same size as domain + /// \throw std::domain_error If domain is not sorted + LookupTable1D(const std::vector & domain, const std::vector & range) + : m_domain{domain}, + m_range{range} + { + check_table_lookup_invariants(m_domain, m_range); + } + + /// Move constructor + /// \param[in] domain The domain, or set of x values + /// \param[in] range The range, or set of y values + /// \throw std::domain_error If domain or range is empty + /// \throw std::domain_error If range is not the same size as domain + /// \throw std::domain_error If domain is not sorted + LookupTable1D(std::vector && domain, std::vector && range) + : m_domain{domain}, + m_range{range} + { + check_table_lookup_invariants(m_domain, m_range); + } + + /// Do a 1D table lookup + /// If query value fall out of the domain, then the value at the corresponding edge of the domain + /// is returned. + /// \param[in] value The point in the domain to query, x + /// \return A linearly interpolated value y, corresponding to the query, x + /// \throw std::domain_error If value is not finite + T lookup(const T value) const + { + return lookup_impl_1d(m_domain, m_range, value); + } + /// Get the domain table + const std::vector & domain() const noexcept {return m_domain;} + /// Get the range table + const std::vector & range() const noexcept {return m_range;} + +private: + std::vector m_domain; + std::vector m_range; +}; // class LookupTable1D + +} // namespace helper_functions +} // namespace common +} // namespace autoware + + +#endif // GEOMETRY__LOOKUP_TABLE_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/spatial_hash.hpp b/common/autoware_auto_geometry/include/geometry/spatial_hash.hpp new file mode 100644 index 0000000000000..7a77f02738732 --- /dev/null +++ b/common/autoware_auto_geometry/include/geometry/spatial_hash.hpp @@ -0,0 +1,390 @@ +// Copyright 2019 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. +/// \file +/// \brief This file implements a spatial hash for efficient fixed-radius near neighbor queries in +/// 2D + +#ifndef GEOMETRY__SPATIAL_HASH_HPP_ +#define GEOMETRY__SPATIAL_HASH_HPP_ + +#include +#include +#include +#include +#include +#include + +using autoware::common::types::float32_t; +using autoware::common::types::bool8_t; + +namespace autoware +{ +namespace common +{ +namespace geometry +{ +/// \brief All objects related to the spatial hash data structure for efficient near neighbor lookup +namespace spatial_hash +{ + +/// \brief An implementation of the spatial hash or integer lattice data structure for efficient +/// (O(1)) near neighbor queries. +/// \tparam PointT The point type stored in this data structure. Must have float members x, y, and z +/// +/// This implementation can support both 2D and 3D queries +/// (though only one type per data structure), and can support queries of varying radius. This data +/// structure cannot do near neighbor lookups for euclidean distance in arbitrary dimensions. +template +class GEOMETRY_PUBLIC SpatialHashBase +{ + using Index3 = details::Index3; + //lint -e{9131} NOLINT There's no other way to make this work in a static assert + static_assert( + std::is_same::value || std::is_same::value, + "SpatialHash only works with Config2d or Config3d"); + +public: + using Hash = std::unordered_multimap; + using IT = typename Hash::const_iterator; + /// \brief Wrapper around an iterator and a distance (from some query point) + class Output + { +public: + /// \brief Constructor + /// \param[in] iterator An iterator pointing to some point + /// \param[in] distance The euclidean distance (2d or 3d) to a reference point + Output(const IT iterator, const float32_t distance) + : m_iterator(iterator), + m_distance(distance) + { + } + /// \brief Get stored point + /// \return A const reference to the stored point + const PointT & get_point() const + { + return m_iterator->second; + } + /// \brief Get underlying iterator + /// \return A copy of the underlying iterator + IT get_iterator() const + { + return m_iterator; + } + /// \brief Convert to underlying point + /// \return A reference to the underlying point + operator const PointT &() const + { + return get_point(); + } + /// \brief Convert to underlying iterator + /// \return A copy of the iterator + operator IT() const + { + return get_iterator(); + } + /// \brief Get distance to reference point + /// \return The distance + float32_t get_distance() const + { + return m_distance; + } + +private: + IT m_iterator; + float32_t m_distance; + }; // class Output + using OutputVector = typename std::vector; + + /// \brief Constructor + /// \param[in] cfg The configuration object for this class + explicit SpatialHashBase(const ConfigT & cfg) + : m_config{cfg}, + m_hash(), + m_neighbors{}, // TODO(c.ho) reserve, but there's no default constructor for output + m_bins_hit{}, // zero initialization (and below) + m_neighbors_found{} + { + } + + /// \brief Inserts point + /// \param[in] pt The Point to insert + /// \return Iterator pointing to the inserted point + /// \throw std::length_error If the data structure is at capacity + IT insert(const PointT & pt) + { + if (size() >= capacity()) { + throw std::length_error{"SpatialHash: Cannot insert past capacity"}; + } + return insert_impl(pt); + } + + /// \brief Inserts a range of points + /// \param[in] begin The start of the range of points to insert + /// \param[in] end The end of the range of points to insert + /// \tparam IteratorT The iterator type + /// \throw std::length_error If the range of points to insert exceeds the data structure's + /// capacity + template + void insert(IteratorT begin, IteratorT end) + { + // This check is here for strong exception safety + if ((size() + std::distance(begin, end)) > capacity()) { + throw std::length_error{"SpatialHash: Cannot multi-insert past capacity"}; + } + for (IteratorT it = begin; it != end; ++it) { + (void)insert_impl(*it); + } + } + + /// \brief Removes the specified element from the data structure + /// \param[in] point An iterator pointing to a point to be removed + /// \return An iterator pointing to the element after the erased element + /// \throw std::domain_error If pt is invalid or does not belong to this data structure + /// + /// \note There is no reliable way to check if an iterator is invalid. The checks here are + /// based on a heuristic and is not guaranteed to find all invalid iterators. This method + /// should be used with care and only on valid iterators + IT erase(const IT point) + { + if (end() == m_hash.find(point->first)) { + throw std::domain_error{"SpatialHash: Attempting to erase invalid iterator"}; + } + return m_hash.erase(point); + } + + /// \brief Reset the state of the data structure + void clear() + { + m_hash.clear(); + } + /// \brief Get current number of element stored in this data structure + /// \return Number of stored elements + Index size() const + { + return m_hash.size(); + } + /// \brief Get the maximum capacity of the data structure + /// \return The capacity of the data structure + Index capacity() const + { + return m_config.get_capacity(); + } + /// \brief Whether the hash is empty + /// \return True if data structure is empty + bool8_t empty() const + { + return m_hash.empty(); + } + /// \brief Get iterator to beginning of data structure + /// \return Iterator + IT begin() const + { + return m_hash.begin(); + } + /// \brief Get iterator to end of data structure + /// \return Iterator + IT end() const + { + return m_hash.end(); + } + /// \brief Get iterator to beginning of data structure + /// \return Iterator + IT cbegin() const + { + return begin(); + } + /// \brief Get iterator to end of data structure + /// \return Iterator + IT cend() const + { + return end(); + } + + /// \brief Get the number of bins touched during the lifetime of this object, for debugging and + /// size tuning + /// \return The total number of bins touched during near() queries + Index bins_hit() const + { + return m_bins_hit; + } + + /// \brief Get number of near neighbors found during the lifetime of this object, for debugging + /// and size tuning + /// \return The total number of neighbors found during near() queries + Index neighbors_found() const + { + return m_neighbors_found; + } + +protected: + /// \brief Finds all points within a fixed radius of a reference point + /// \param[in] x The x component of the reference point + /// \param[in] y The y component of the reference point + /// \param[in] z The z component of the reference point, respected only if the spatial hash is not + /// 2D. + /// \param[in] radius The radius within which to find all near points + /// \return A const reference to a vector containing iterators pointing to + /// all points within the radius, and the actual distance to the reference point + const OutputVector & near_impl( + const float32_t x, + const float32_t y, + const float32_t z, + const float32_t radius) + { + // reset output + m_neighbors.clear(); + // Compute bin, bin range + const Index3 ref_idx = m_config.index3(x, y, z); + const float32_t radius2 = radius * radius; + const details::BinRange idx_range = m_config.bin_range(ref_idx, radius); + Index3 idx = idx_range.first; + // For bins in radius + do { // guaranteed to have at least the bin ref_idx is in + // update book-keeping + ++m_bins_hit; + // Iterating in a square/cube pattern is easier than constructing sphere pattern + if (m_config.is_candidate_bin(ref_idx, idx, radius2)) { + // For point in bin + const Index jdx = m_config.index(idx); + const auto range = m_hash.equal_range(jdx); + for (auto it = range.first; it != range.second; ++it) { + const auto & pt = it->second; + const float32_t dist2 = m_config.distance_squared(x, y, z, pt); + if (dist2 <= radius2) { + // Only compute true distance if necessary + m_neighbors.emplace_back(it, sqrtf(dist2)); + } + } + } + } while (m_config.next_bin(idx_range, idx)); + // update book-keeping + m_neighbors_found += m_neighbors.size(); + return m_neighbors; + } + +private: + /// \brief Internal insert method with no error checking + /// \param[in] pt The Point to insert + GEOMETRY_LOCAL IT insert_impl(const PointT & pt) + { + // Compute bin + const Index idx = + m_config.bin(point_adapter::x_(pt), point_adapter::y_(pt), point_adapter::z_(pt)); + // Insert into bin + return m_hash.insert(std::make_pair(idx, pt)); + } + + const ConfigT m_config; + Hash m_hash; + OutputVector m_neighbors; + Index m_bins_hit; + Index m_neighbors_found; +}; // class SpatialHashBase + +/// \brief The class to be used for specializing on +/// apex_app::common::geometry::spatial_hash::SpatialHashBase to provide different function +/// signatures on 2D and 3D configurations +/// \tparam PointT The point type stored in this data structure. Must have float members x, y and z +template +class GEOMETRY_PUBLIC SpatialHash; + +/// \brief Explicit specialization of SpatialHash for 2D configuration +/// \tparam PointT The point type stored in this data structure. +template +class GEOMETRY_PUBLIC SpatialHash: public SpatialHashBase +{ +public: + using OutputVector = typename SpatialHashBase::OutputVector; + + explicit SpatialHash(const Config2d & cfg) + : SpatialHashBase(cfg) {} + + /// \brief Finds all points within a fixed radius of a reference point + /// \param[in] x The x component of the reference point + /// \param[in] y The y component of the reference point + /// \param[in] radius The radius within which to find all near points + /// \return A const reference to a vector containing iterators pointing to + /// all points within the radius, and the actual distance to the reference point + const OutputVector & near( + const float32_t x, + const float32_t y, + const float32_t radius) + { + return this->near_impl(x, y, 0.0F, radius); + } + + /// \brief Finds all points within a fixed radius of a reference point + /// \param[in] pt The reference point. Only the x and y members are respected. + /// \param[in] radius The radius within which to find all near points + /// \return A const reference to a vector containing iterators pointing to + /// all points within the radius, and the actual distance to the reference point + const OutputVector & near(const PointT & pt, const float32_t radius) + { + return near(point_adapter::x_(pt), point_adapter::y_(pt), radius); + } +}; + +/// \brief Explicit specialization of SpatialHash for 3D configuration +/// \tparam PointT The point type stored in this data structure. Must have float members x, y and z +template +class GEOMETRY_PUBLIC SpatialHash: public SpatialHashBase +{ +public: + using OutputVector = typename SpatialHashBase::OutputVector; + + explicit SpatialHash(const Config3d & cfg) + : SpatialHashBase(cfg) {} + + /// \brief Finds all points within a fixed radius of a reference point + /// \param[in] x The x component of the reference point + /// \param[in] y The y component of the reference point + /// \param[in] z The z component of the reference point, respected only if the spatial hash is not + /// 2D. + /// \param[in] radius The radius within which to find all near points + /// \return A const reference to a vector containing iterators pointing to + /// all points within the radius, and the actual distance to the reference point + const OutputVector & near( + const float32_t x, + const float32_t y, + const float32_t z, + const float32_t radius) + { + return this->near_impl(x, y, z, radius); + } + + /// \brief Finds all points within a fixed radius of a reference point + /// \param[in] pt The reference point. + /// \param[in] radius The radius within which to find all near points + /// \return A const reference to a vector containing iterators pointing to + /// all points within the radius, and the actual distance to the reference point + const OutputVector & near(const PointT & pt, const float32_t radius) + { + return near( + point_adapter::x_(pt), point_adapter::y_(pt), point_adapter::z_(pt), + radius); + } +}; + +template +using SpatialHash2d = SpatialHash; +template +using SpatialHash3d = SpatialHash; +} // namespace spatial_hash +} // namespace geometry +} // namespace common +} // namespace autoware + +#endif // GEOMETRY__SPATIAL_HASH_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/spatial_hash_config.hpp b/common/autoware_auto_geometry/include/geometry/spatial_hash_config.hpp new file mode 100644 index 0000000000000..76ebb8e2da79d --- /dev/null +++ b/common/autoware_auto_geometry/include/geometry/spatial_hash_config.hpp @@ -0,0 +1,494 @@ +// Copyright 2019 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. +/// \file +/// \brief This file implements a spatial hash for efficient fixed-radius near neighbor queries in +/// 2D + +#ifndef GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ +#define GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "helper_functions/crtp.hpp" + +using autoware::common::types::float64_t; +using autoware::common::types::float32_t; +using autoware::common::types::bool8_t; + +namespace autoware +{ +namespace common +{ +namespace geometry +{ +/// \brief All objects related to the spatial hash data structure for efficient near neighbor lookup +namespace spatial_hash +{ +/// \brief Index type for identifying bins of the hash/lattice +using Index = std::size_t; +/// \brief Spatial hash functionality not intended to be used by an external user +namespace details +{ +/// \brief Internal struct for packing three indices together +/// +/// The use of this struct publically is a violation of our coding standards, but I claim it's +/// fine because (a) it's details, (b) it is literally three unrelated members packaged together. +/// This type is needed for conceptual convenience so I don't have massive function parameter +/// lists +struct GEOMETRY_PUBLIC Index3 +{ + Index x; + Index y; + Index z; +}; // struct Index3 + +using BinRange = std::pair; +} // namespace details + +/// \brief The base class for the configuration object for the SpatialHash class +/// \tparam Derived The type of the derived class to support static polymorphism/CRTP +template +class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp +{ +public: + /// \brief Constructor for spatial hash + /// \param[in] min_x The minimum x value for the spatial hash + /// \param[in] max_x The maximum x value for the spatial hash + /// \param[in] min_y The minimum y value for the spatial hash + /// \param[in] max_y The maximum y value for the spatial hash + /// \param[in] min_z The minimum z value for the spatial hash + /// \param[in] max_z The maximum z value for the spatial hash + /// \param[in] radius The look up radius + /// \param[in] capacity The maximum number of points the spatial hash can store + Config( + const float32_t min_x, + const float32_t max_x, + const float32_t min_y, + const float32_t max_y, + const float32_t min_z, + const float32_t max_z, + const float32_t radius, + const Index capacity) + : m_min_x{min_x}, + m_min_y{min_y}, + m_min_z{min_z}, + m_max_x{max_x}, + m_max_y{max_y}, + m_max_z{max_z}, + m_side_length{radius}, + m_side_length2{radius * radius}, + m_side_length_inv{1.0F / radius}, + m_capacity{capacity}, + m_max_x_idx{check_basis_direction(min_x, max_x)}, + m_max_y_idx{check_basis_direction(min_y, max_y)}, + m_max_z_idx{check_basis_direction(min_z, max_z)}, + m_y_stride{m_max_x_idx + 1U}, + m_z_stride{m_y_stride * (m_max_y_idx + 1U)} + { + if (radius <= 0.0F) { + throw std::domain_error("Error constructing SpatialHash: must have positive side length"); + } + + if ((m_max_y_idx + m_y_stride) > std::numeric_limits::max() / 2U) { + // TODO(c.ho) properly check for multiplication overflow + throw std::domain_error("SpatialHash::Config: voxel index may overflow!"); + } + // small fudging to prevent weird boundary effects + // (e.g (x=xmax, y) rolls index over to (x=0, y+1) + constexpr auto FEPS = std::numeric_limits::epsilon(); + //lint -e{1938} read only access is fine NOLINT + m_max_x -= FEPS; + m_max_y -= FEPS; + m_max_z -= FEPS; + if ((m_z_stride + m_max_z_idx) > std::numeric_limits::max() / 2U) { + // TODO(c.ho) properly check for multiplication overflow + throw std::domain_error("SpatialHash::Config: voxel index may overflow!"); + } + // I don't know if this is even possible given other checks + if (std::numeric_limits::max() == m_max_z_idx) { + throw std::domain_error("SpatialHash::Config: max z index exceeds reasonable value"); + } + } + + /// \brief Given a reference index triple, compute the first and last bin + /// \param[in] ref The reference index triple + /// \param[in] radius The allowable radius for any point in the reference bin to any point in the + /// range + /// \return A pair where the first element is an index triple of the first bin, and the second + /// element is an index triple for the last bin + details::BinRange bin_range(const details::Index3 & ref, const float radius) const + { + // Compute distance in units of voxels + const Index iradius = static_cast(std::ceil(radius / m_side_length)); + // Dumb ternary because potentially unsigned Index type + const Index xmin = (ref.x > iradius) ? (ref.x - iradius) : 0U; + const Index ymin = (ref.y > iradius) ? (ref.y - iradius) : 0U; + const Index zmin = (ref.z > iradius) ? (ref.z - iradius) : 0U; + // In 2D mode, m_max_z should be 0, same with ref.z + const Index xmax = std::min(ref.x + iradius, m_max_x_idx); + const Index ymax = std::min(ref.y + iradius, m_max_y_idx); + const Index zmax = std::min(ref.z + iradius, m_max_z_idx); + // return bottom-left portion of cube and top-right portion of cube + return {{xmin, ymin, zmin}, {xmax, ymax, zmax}}; + } + + /// \brief Get next index within a given range + /// \return True if idx is valid and still in range + /// \param[in] range The max and min bin indices + /// \param[inout] idx The index to be incremented, updated even if a negative result occurs + bool8_t next_bin(const details::BinRange & range, details::Index3 & idx) const + { + // TODO(c.ho) is there any way to make this neater without triple nested if? + bool8_t ret = true; + ++idx.x; + if (idx.x > range.second.x) { + idx.x = range.first.x; + ++idx.y; + if (idx.y > range.second.y) { + idx.y = range.first.y; + ++idx.z; + if (idx.z > range.second.z) { + ret = false; + } + } + } + return ret; + } + /// \brief Get the maximum capacity of the spatial hash + /// \return The capacity + Index get_capacity() const + { + return m_capacity; + } + + /// \brief Getter for the side length, equivalently the lookup radius + float32_t radius2() const + { + return m_side_length2; + } + + //////////////////////////////////////////////////////////////////////////////////////////// + // "Polymorphic" API + /// \brief Compute the single index given a point + /// \param[in] x The x component of the point + /// \param[in] y The y component of the point + /// \param[in] z The z component of the point + /// \return The combined index of the bin for a given point + Index bin(const float32_t x, const float32_t y, const float32_t z) const + { + return this->impl().bin_(x, y, z); + } + /// \brief Compute whether the query bin and reference bin could possibly contain a pair of points + /// such that their distance is within a certain threshold + /// \param[in] ref The index triple of the bin containing the reference point + /// \param[in] query The index triple of the bin being queried + /// \param[in] ref_distance2 The squared threshold distance + /// \return True if query and ref could possibly hold points within reference distance to one + /// another + bool is_candidate_bin( + const details::Index3 & ref, + const details::Index3 & query, + const float ref_distance2) const + { + return this->impl().valid(ref, query, ref_distance2); + } + /// \brief Compute the decomposed index given a point + /// \param[in] x The x component of the point + /// \param[in] y The y component of the point + /// \param[in] z The z component of the point + /// \return The decomposed index triple of the bin for the given point + details::Index3 index3(const float32_t x, const float32_t y, const float32_t z) const + { + return this->impl().index3_(x, y, z); + } + /// \brief Compute the composed single index given a decomposed index + /// \param[in] idx A decomposed index triple for a bin + /// \return The composed bin index + Index index(const details::Index3 & idx) const + { + return this->impl().index_(idx); + } + /// \brief Compute the squared distance between the two points + /// \tparam PointT A point type with float members x, y and z, or point adapters defined + /// \param[in] x The x component of the first point + /// \param[in] y The y component of the first point + /// \param[in] z The z component of the first point + /// \param[in] pt The other point being compared + /// \return The squared distance between the points (2d or 3d) + template + float32_t distance_squared( + const float32_t x, + const float32_t y, + const float32_t z, + const PointT & pt) const + { + return this->impl().distance_squared_(x, y, z, pt); + } + +protected: + /// \brief Computes the index in the x basis direction + /// \param[in] x The x value of a point + /// \return The x offset of the index + Index x_index(const float32_t x) const + { + return static_cast( + std::floor((std::min(std::max(x, m_min_x), m_max_x) - m_min_x) * m_side_length_inv)); + } + /// \brief Computes the index in the y basis direction + /// \param[in] y The y value of a point + /// \return The x offset of the index + Index y_index(const float32_t y) const + { + return static_cast( + std::floor((std::min(std::max(y, m_min_y), m_max_y) - m_min_y) * m_side_length_inv)); + } + /// \brief Computes the index in the z basis direction + /// \param[in] z The z value of a point + /// \return The x offset of the index + Index z_index(const float32_t z) const + { + return static_cast( + std::floor((std::min(std::max(z, m_min_z), m_max_z) - m_min_z) * m_side_length_inv)); + } + /// \brief Compose the provided index offsets + Index bin_impl(const Index xdx, const Index ydx) const + { + return xdx + (ydx * m_y_stride); + } + /// \brief Compose the provided index offsets + Index bin_impl(const Index xdx, const Index ydx, const Index zdx) const + { + return bin_impl(xdx, ydx) + (zdx * m_z_stride); + } + + /// \brief The index offset of a point given it's x and y values + /// \param[in] x The x value of a point + /// \param[in] y the y value of a point + /// \return The index of the point in the 2D case, or the offset within a z-slice of the hash + Index bin_impl(const float32_t x, const float32_t y) const + { + return bin_impl(x_index(x), y_index(y)); + } + /// \brief The index of a point given it's x, y and z values + /// \param[in] x The x value of a point + /// \param[in] y the y value of a point + /// \param[in] z the z value of a point + /// \return The index of the bin for the specified point + Index bin_impl(const float32_t x, const float32_t y, const float32_t z) const + { + return bin_impl(x, y) + (z_index(z) * m_z_stride); + } + /// \brief The distance between two indices as a float, where adjacent indices have zero + /// distance (e.g. dist(0, 1) = 0) + float32_t idx_distance(const Index ref_idx, const Index query_idx) const + { + /// Not using fabs because Index is (possibly) unsigned + const Index idist = (ref_idx >= query_idx) ? (ref_idx - query_idx) : (query_idx - ref_idx); + float32_t dist = static_cast(idist) - 1.0F; + return std::max(dist, 0.0F); + } + + /// \brief Get side length squared + float side_length2() const + { + return m_side_length2; + } + +private: + /// \brief Sanity check a range in a basis direction + /// \return The number of voxels in the given basis direction + Index check_basis_direction(const float32_t min, const float32_t max) const + { + if (min >= max) { + throw std::domain_error("SpatialHash::Config: must have min < max"); + } + // This family of checks is to ensure that you don't get weird casting effects due to huge + // floating point values + const float64_t dmax = static_cast(max); + const float64_t dmin = static_cast(min); + const float64_t width = (dmax - dmin) * static_cast(m_side_length_inv); + constexpr float64_t fltmax = static_cast(std::numeric_limits::max()); + if (fltmax <= width) { + throw std::domain_error("SpatialHash::Config: voxel size approaching floating point limit"); + } + return static_cast(width); + } + float32_t m_min_x; + float32_t m_min_y; + float32_t m_min_z; + float32_t m_max_x; + float32_t m_max_y; + float32_t m_max_z; + float32_t m_side_length; + float32_t m_side_length2; + float32_t m_side_length_inv; + Index m_capacity; + Index m_max_x_idx; + Index m_max_y_idx; + Index m_max_z_idx; + Index m_y_stride; + Index m_z_stride; +}; // class Config + +/// \brief Configuration class for a 2d spatial hash +class GEOMETRY_PUBLIC Config2d : public Config +{ +public: + /// \brief Config constructor for 2D spatial hash + /// \param[in] min_x The minimum x value for the spatial hash + /// \param[in] max_x The maximum x value for the spatial hash + /// \param[in] min_y The minimum y value for the spatial hash + /// \param[in] max_y The maximum y value for the spatial hash + /// \param[in] radius The lookup distance + /// \param[in] capacity The maximum number of points the spatial hash can store + Config2d( + const float32_t min_x, + const float32_t max_x, + const float32_t min_y, + const float32_t max_y, + const float32_t radius, + const Index capacity); + /// \brief The index of a point given it's x, y and z values, 2d implementation + /// \param[in] x The x value of a point + /// \param[in] y the y value of a point + /// \param[in] z the z value of a point + /// \return The index of the bin for the specified point + Index bin_(const float32_t x, const float32_t y, const float32_t z) const; + /// \brief Determine if a bin could possibly hold a point within a distance to any point in a + /// reference bin for the 2D case + /// \param[in] ref The decomposed index triple of the reference bin + /// \param[in] query The decomposed index triple of the bin being queried + /// \param[in] ref_distance2 The squared threshold distance + /// \return True if the reference bin and query bin could possibly hold a point within the + /// reference distance + bool valid( + const details::Index3 & ref, + const details::Index3 & query, + const float ref_distance2) const; + /// \brief Compute the decomposed index given a point, 2d implementation + /// \param[in] x The x component of the point + /// \param[in] y The y component of the point + /// \param[in] z The z component of the point + /// \return The decomposed index triple of the bin for the given point + details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const; + /// \brief Compute the composed single index given a decomposed index, 2d implementation + /// \param[in] idx A decomposed index triple for a bin + /// \return The composed bin index + Index index_(const details::Index3 & idx) const; + /// \brief Compute the squared distance between the two points, 2d implementation + /// \tparam PointT A point type with float members x, y and z, or point adapters defined + /// \param[in] x The x component of the first point + /// \param[in] y The y component of the first point + /// \param[in] z The z component of the first point + /// \param[in] pt The other point being compared + /// \return The squared distance between the points (2d) + template + float32_t distance_squared_( + const float32_t x, + const float32_t y, + const float32_t z, + const PointT & pt) const + { + (void)z; + const float32_t dx = x - point_adapter::x_(pt); + const float32_t dy = y - point_adapter::y_(pt); + return (dx * dx) + (dy * dy); + } +}; // class Config2d + +/// \brief Configuration class for a 3d spatial hash +class GEOMETRY_PUBLIC Config3d : public Config +{ +public: + /// \brief Config constructor for a 3d spatial hash + /// \param[in] min_x The minimum x value for the spatial hash + /// \param[in] max_x The maximum x value for the spatial hash + /// \param[in] min_y The minimum y value for the spatial hash + /// \param[in] max_y The maximum y value for the spatial hash + /// \param[in] min_z The minimum z value for the spatial hash + /// \param[in] max_z The maximum z value for the spatial hash + /// \param[in] radius The lookup distance + /// \param[in] capacity The maximum number of points the spatial hash can store + Config3d( + const float32_t min_x, + const float32_t max_x, + const float32_t min_y, + const float32_t max_y, + const float32_t min_z, + const float32_t max_z, + const float32_t radius, + const Index capacity); + /// \brief The index of a point given it's x, y and z values, 3d implementation + /// \param[in] x The x value of a point + /// \param[in] y the y value of a point + /// \param[in] z the z value of a point + /// \return The index of the bin for the specified point + Index bin_(const float32_t x, const float32_t y, const float32_t z) const; + /// \brief Determine if a bin could possibly hold a point within a distance to any point in a + /// reference bin for the 3D case + /// \param[in] ref The decomposed index triple of the reference bin + /// \param[in] query The decomposed index triple of the bin being queried + /// \param[in] ref_distance2 The squared threshold distance + /// \return True if the reference bin and query bin could possibly hold a point within the + /// reference distance + bool valid( + const details::Index3 & ref, + const details::Index3 & query, + const float ref_distance2) const; + /// \brief Compute the decomposed index given a point, 3d implementation + /// \param[in] x The x component of the point + /// \param[in] y The y component of the point + /// \param[in] z The z component of the point + /// \return The decomposed index triple of the bin for the given point + details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const; + /// \brief Compute the composed single index given a decomposed index, 3d implementation + /// \param[in] idx A decomposed index triple for a bin + /// \return The composed bin index + Index index_(const details::Index3 & idx) const; + /// \brief Compute the squared distance between the two points, 3d implementation + /// \tparam PointT A point type with float members x, y and z, or point adapters defined + /// \param[in] x The x component of the first point + /// \param[in] y The y component of the first point + /// \param[in] z The z component of the first point + /// \param[in] pt The other point being compared + /// \return The squared distance between the points (3d) + template + float32_t distance_squared_( + const float32_t x, + const float32_t y, + const float32_t z, + const PointT & pt) const + { + const float32_t dx = x - point_adapter::x_(pt); + const float32_t dy = y - point_adapter::y_(pt); + const float32_t dz = z - point_adapter::z_(pt); + return (dx * dx) + (dy * dy) + (dz * dz); + } +}; // class Config3d +} // namespace spatial_hash +} // namespace geometry +} // namespace common +} // namespace autoware + +#endif // GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/visibility_control.hpp b/common/autoware_auto_geometry/include/geometry/visibility_control.hpp new file mode 100644 index 0000000000000..d7da65dd7bec8 --- /dev/null +++ b/common/autoware_auto_geometry/include/geometry/visibility_control.hpp @@ -0,0 +1,42 @@ +// Copyright 2017-2019 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#ifndef GEOMETRY__VISIBILITY_CONTROL_HPP_ +#define GEOMETRY__VISIBILITY_CONTROL_HPP_ + + +//////////////////////////////////////////////////////////////////////////////// +#if defined(__WIN32) + #if defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) + #define GEOMETRY_PUBLIC __declspec(dllexport) + #define GEOMETRY_LOCAL + #else // defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) + #define GEOMETRY_PUBLIC __declspec(dllimport) + #define GEOMETRY_LOCAL + #endif // defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) +#elif defined(__linux__) + #define GEOMETRY_PUBLIC __attribute__((visibility("default"))) + #define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) + #define GEOMETRY_PUBLIC __attribute__((visibility("default"))) + #define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) +#elif defined(QNX) + #define GEOMETRY_PUBLIC __attribute__((visibility("default"))) + #define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) +#else // defined(__linux__) + #error "Unsupported Build Configuration" +#endif // defined(__WIN32) +#endif // GEOMETRY__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_geometry/package.xml b/common/autoware_auto_geometry/package.xml new file mode 100644 index 0000000000000..db117a380e325 --- /dev/null +++ b/common/autoware_auto_geometry/package.xml @@ -0,0 +1,26 @@ + + + + autoware_auto_geometry + 1.0.0 + Geometry related algorithms + Apex.AI, Inc. + Apache 2 + + ament_cmake_auto + autoware_auto_cmake + + autoware_auto_common + autoware_auto_geometry_msgs + autoware_auto_planning_msgs + autoware_auto_vehicle_msgs + autoware_auto_tf2 + geometry_msgs + + ament_cmake_gtest + + + osrf_testing_tools_cpp + + ament_cmake + diff --git a/common/autoware_auto_geometry/src/bounding_box.cpp b/common/autoware_auto_geometry/src/bounding_box.cpp new file mode 100644 index 0000000000000..fa47f33b7c24a --- /dev/null +++ b/common/autoware_auto_geometry/src/bounding_box.cpp @@ -0,0 +1,154 @@ +// Copyright 2017-2019 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace autoware +{ +namespace common +{ +namespace geometry +{ +namespace bounding_box +{ +namespace details +{ +//////////////////////////////////////////////////////////////////////////////// +void size_2d(const decltype(BoundingBox::corners) & corners, geometry_msgs::msg::Point32 & ret) +{ + ret.x = std::max( + norm_2d(minus_2d(corners[1U], corners[0U])), std::numeric_limits::epsilon()); + ret.y = std::max( + norm_2d(minus_2d(corners[2U], corners[1U])), std::numeric_limits::epsilon()); +} +//////////////////////////////////////////////////////////////////////////////// +void finalize_box(const decltype(BoundingBox::corners) & corners, BoundingBox & box) +{ + (void)std::copy(&corners[0U], &corners[corners.size()], &box.corners[0U]); + // orientation: this is robust to lines + const float32_t yaw_rad_2 = + atan2f(box.corners[2U].y - box.corners[1U].y, box.corners[2U].x - box.corners[1U].x) * 0.5F; + box.orientation.w = cosf(yaw_rad_2); + box.orientation.z = sinf(yaw_rad_2); + // centroid + box.centroid = times_2d(plus_2d(corners[0U], corners[2U]), 0.5F); +} + +autoware_auto_perception_msgs::msg::Shape make_shape(const BoundingBox & box) +{ + autoware_auto_perception_msgs::msg::Shape retval; + // Polygon is 2D rectangle + geometry_msgs::msg::Polygon polygon; + auto & points = polygon.points; + points.resize(4); + + // Note that the x and y of size field in BoundingBox should be swapped when being used to + // compute corners. + // origin of reference system: centroid of bbox + points[0].x = -0.5F * box.size.y; + points[0].y = -0.5F * box.size.x; + points[0].z = -box.size.z; + + points[1] = points[0]; + points[1].y += box.size.x; + + points[2] = points[1]; + points[2].x += box.size.y; + + points[3] = points[2]; + points[3].y -= box.size.x; + + retval.footprint = polygon; + retval.dimensions.z = 2 * box.size.z; + + return retval; +} + +autoware_auto_perception_msgs::msg::DetectedObject make_detected_object(const BoundingBox & box) +{ + autoware_auto_perception_msgs::msg::DetectedObject ret; + + ret.kinematics.pose_with_covariance.pose.position.x = static_cast(box.centroid.x); + ret.kinematics.pose_with_covariance.pose.position.y = static_cast(box.centroid.y); + ret.kinematics.pose_with_covariance.pose.position.z = static_cast(box.centroid.z); + ret.kinematics.pose_with_covariance.pose.orientation.x = static_cast(box.orientation.x); + ret.kinematics.pose_with_covariance.pose.orientation.y = static_cast(box.orientation.y); + ret.kinematics.pose_with_covariance.pose.orientation.z = static_cast(box.orientation.z); + ret.kinematics.pose_with_covariance.pose.orientation.w = static_cast(box.orientation.w); + ret.kinematics.orientation_availability = + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; + + ret.shape = make_shape(box); + + ret.existence_probability = 1.0F; + + autoware_auto_perception_msgs::msg::ObjectClassification label; + label.label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + label.probability = 1.0F; + ret.classification.emplace_back(std::move(label)); + + return ret; +} + +std::vector GEOMETRY_PUBLIC get_transformed_corners( + const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation) +{ + std::vector retval(shape_msg.footprint.points.size()); + geometry_msgs::msg::TransformStamped tf; + tf.transform.rotation = orientation; + tf.transform.translation.x = centroid.x; + tf.transform.translation.y = centroid.y; + tf.transform.translation.z = centroid.z; + + for (size_t i = 0U; i < shape_msg.footprint.points.size(); ++i) { + tf2::doTransform(shape_msg.footprint.points[i], retval[i], tf); + } + return retval; +} + +} // namespace details +/////////////////////////////////////////////////////////////////////////////// +// precompilation +using autoware::common::types::PointXYZIF; +template BoundingBox minimum_area_bounding_box(std::list & list); +template BoundingBox minimum_perimeter_bounding_box(std::list & list); +using PointXYZIFVIT = std::vector::iterator; +template BoundingBox eigenbox_2d(const PointXYZIFVIT begin, const PointXYZIFVIT end); +template BoundingBox lfit_bounding_box_2d( + const PointXYZIFVIT begin, const PointXYZIFVIT end); +using geometry_msgs::msg::Point32; +template BoundingBox minimum_area_bounding_box(std::list & list); +template BoundingBox minimum_perimeter_bounding_box(std::list & list); +using Point32VIT = std::vector::iterator; +template BoundingBox eigenbox_2d(const Point32VIT begin, const Point32VIT end); +template BoundingBox lfit_bounding_box_2d(const Point32VIT begin, const Point32VIT end); +} // namespace bounding_box +} // namespace geometry +} // namespace common +} // namespace autoware diff --git a/common/autoware_auto_geometry/src/spatial_hash.cpp b/common/autoware_auto_geometry/src/spatial_hash.cpp new file mode 100644 index 0000000000000..93cba784d0e0e --- /dev/null +++ b/common/autoware_auto_geometry/src/spatial_hash.cpp @@ -0,0 +1,118 @@ +// Copyright 2019 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#include +#include +//lint -e537 NOLINT repeated include file due to cpplint rule +#include +//lint -e537 NOLINT repeated include file due to cpplint rule +#include + +namespace autoware +{ +namespace common +{ +namespace geometry +{ +namespace spatial_hash +{ +//////////////////////////////////////////////////////////////////////////////// +Config2d::Config2d( + const float32_t min_x, + const float32_t max_x, + const float32_t min_y, + const float32_t max_y, + const float32_t radius, + const Index capacity) +: Config(min_x, max_x, min_y, max_y, {}, std::numeric_limits::min(), + radius, capacity) +{ +} +//////////////////////////////////////////////////////////////////////////////// +Index Config2d::bin_(const float32_t x, const float32_t y, const float32_t z) const +{ + (void)z; + return bin_impl(x, y); +} +//////////////////////////////////////////////////////////////////////////////// +bool Config2d::valid( + const details::Index3 & ref, + const details::Index3 & query, + const float ref_distance2) const +{ + const float dx = idx_distance(ref.x, query.x); + const float dy = idx_distance(ref.y, query.y); + // minor algebraic manipulation happening below + return (((dx * dx) + (dy * dy)) * side_length2()) <= ref_distance2; +} +//////////////////////////////////////////////////////////////////////////////// +details::Index3 Config2d::index3_(const float32_t x, const float32_t y, const float32_t z) const +{ + (void)z; + return {x_index(x), y_index(y), Index{}}; // zero initialization +} +//////////////////////////////////////////////////////////////////////////////// +Index Config2d::index_(const details::Index3 & idx) const +{ + return bin_impl(idx.x, idx.y); +} +//////////////////////////////////////////////////////////////////////////////// +Config3d::Config3d( + const float32_t min_x, + const float32_t max_x, + const float32_t min_y, + const float32_t max_y, + const float32_t min_z, + const float32_t max_z, + const float32_t radius, + const Index capacity) +: Config(min_x, max_x, min_y, max_y, min_z, max_z, radius, capacity) +{ +} +//////////////////////////////////////////////////////////////////////////////// +Index Config3d::bin_(const float32_t x, const float32_t y, const float32_t z) const +{ + return bin_impl(x, y, z); +} +//////////////////////////////////////////////////////////////////////////////// +bool Config3d::valid( + const details::Index3 & ref, + const details::Index3 & query, + const float ref_distance2) const +{ + const float dx = idx_distance(ref.x, query.x); + const float dy = idx_distance(ref.y, query.y); + const float dz = idx_distance(ref.z, query.z); + // minor algebraic manipulation happening below + return (((dx * dx) + (dy * dy) + (dz * dz)) * side_length2()) <= ref_distance2; +} +//////////////////////////////////////////////////////////////////////////////// +details::Index3 Config3d::index3_(const float32_t x, const float32_t y, const float32_t z) const +{ + return {x_index(x), y_index(y), z_index(z)}; // zero initialization +} +//////////////////////////////////////////////////////////////////////////////// +Index Config3d::index_(const details::Index3 & idx) const +{ + return bin_impl(idx.x, idx.y, idx.z); +} +//////////////////////////////////////////////////////////////////////////////// +template class SpatialHash; +template class SpatialHash; +} // namespace spatial_hash +} // namespace geometry +} // namespace common +} // namespace autoware diff --git a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp b/common/autoware_auto_geometry/test/include/test_bounding_box.hpp new file mode 100644 index 0000000000000..f112517fe0a8f --- /dev/null +++ b/common/autoware_auto_geometry/test/include/test_bounding_box.hpp @@ -0,0 +1,669 @@ +// Copyright 2017-2019 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#ifndef TEST_BOUNDING_BOX_HPP_ +#define TEST_BOUNDING_BOX_HPP_ + +#include + +#include +#include +#include + +#include "geometry/bounding_box/rotating_calipers.hpp" +#include "geometry/bounding_box/lfit.hpp" + +using autoware_auto_perception_msgs::msg::BoundingBox; +using autoware::common::geometry::point_adapter::x_; +using autoware::common::geometry::point_adapter::y_; +using autoware::common::geometry::point_adapter::xr_; +using autoware::common::geometry::point_adapter::yr_; + +template +class BoxTest : public ::testing::Test +{ +protected: + std::list points; + BoundingBox box; + + void minimum_area_bounding_box() + { + // apex_test_tools::memory_test::start(); + box = autoware::common::geometry::bounding_box::minimum_area_bounding_box(points); + // apex_test_tools::memory_test::stop(); + } + + void minimum_perimeter_bounding_box() + { + // apex_test_tools::memory_test::start(); + box = autoware::common::geometry::bounding_box::minimum_perimeter_bounding_box(points); + // apex_test_tools::memory_test::stop(); + } + template + void eigenbox(const IT begin, const IT end) + { + // apex_test_tools::memory_test::start(); + box = autoware::common::geometry::bounding_box::eigenbox_2d(begin, end); + // apex_test_tools::memory_test::stop(); + } + template + void lfit_bounding_box_2d(const IT begin, const IT end) + { + // apex_test_tools::memory_test::start(); + box = autoware::common::geometry::bounding_box::lfit_bounding_box_2d(begin, end); + // apex_test_tools::memory_test::stop(); + } + + PointT make(const float x, const float y) + { + PointT ret; + xr_(ret) = x; + yr_(ret) = y; + return ret; + } + + void check(const float cx, const float cy, const float sx, const float sy, const float val) const + { + constexpr float32_t TOL = 1.0E-4F; + ASSERT_LT(fabsf(box.size.x - sx), TOL); + ASSERT_LT(fabsf(box.size.y - sy), TOL); + ASSERT_LT(fabsf(box.value - val), TOL) << box.value; + + ASSERT_LT(fabsf(box.centroid.x - cx), TOL); + ASSERT_LT(fabsf(box.centroid.y - cy), TOL); + } + + void test_corners( + const std::vector & expect, + const float TOL = 1.0E-6F) const + { + for (uint32_t idx = 0U; idx < 4U; ++idx) { + bool found = false; + for (auto & p : expect) { + if (fabsf(x_(p) - box.corners[idx].x) < TOL && fabsf(y_(p) - box.corners[idx].y) < TOL) { + found = true; + break; + } + } + ASSERT_TRUE(found) << idx << ": " << box.corners[idx].x << ", " << box.corners[idx].y; + } + } + + /// \brief th_deg - phi_deg, normalized to +/- 180 deg + float32_t angle_distance_deg(const float th_deg, const float phi_deg) const + { + return fmodf((th_deg - phi_deg) + 540.0F, 360.0F) - 180.0F; + } + + /// \brief converts a radian value to a degree value + float32_t rad2deg(const float rad_val) const + { + return rad_val * 57.2958F; + } + + void test_orientation( + const float ref_angle_deg, + const float TOL = 1.0E-4F) const + { + bool found = false; + const float angle_deg = rad2deg(2.0F * asinf(box.orientation.z)); + found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg)) < TOL; + found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg + 90.0F)) < TOL; + found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg + 180.0F)) < TOL; + found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg + 270.0F)) < TOL; + ASSERT_TRUE(found) << angle_deg; + } +}; // BoxTest + +// Instantiate tests for given types, add more types here as they are used +using PointTypesBoundingBox = + ::testing::Types; +// cppcheck-suppress syntaxError +TYPED_TEST_SUITE(BoxTest, PointTypesBoundingBox, ); +/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE + +// TODO(c.ho) consider typed and paremterized tests: +// https://stackoverflow.com/questions/3258230/passing-a-typename-and-string-to-parameterized-test-using-google-test + +/////////////////////////////////////////// +TYPED_TEST(BoxTest, PointSegmentDistance) +{ + using autoware::common::geometry::closest_segment_point_2d; + using autoware::common::geometry::point_line_segment_distance_2d; + // normal case + TypeParam p = this->make(-1.0F, 0.0F); + TypeParam q = this->make(-1.0F, 2.0F); + TypeParam r = this->make(1.0F, 1.0F); + TypeParam t = closest_segment_point_2d(p, q, r); + ASSERT_FLOAT_EQ(x_(t), -1.0F); + ASSERT_FLOAT_EQ(y_(t), 1.0F); + ASSERT_FLOAT_EQ(point_line_segment_distance_2d(p, q, r), 2.0F); + // boundary case + p = this->make(1.0F, 0.0F); + q = this->make(-2.0F, 0.0F); + r = this->make(-5.0F, 0.0F); + t = closest_segment_point_2d(p, q, r); + ASSERT_FLOAT_EQ(x_(t), -2.0F); + ASSERT_NEAR(y_(t), 0.0F, std::numeric_limits::epsilon()); + ASSERT_FLOAT_EQ(point_line_segment_distance_2d(p, q, r), 3.0F); + // singular case + p = this->make(1.0F, 5.0F); + q = this->make(1.0F, 5.0F); + r = this->make(1.0F, 1.0F); + t = closest_segment_point_2d(p, q, r); + ASSERT_FLOAT_EQ(x_(t), 1.0F); + ASSERT_FLOAT_EQ(y_(t), 5.0F); + ASSERT_FLOAT_EQ(point_line_segment_distance_2d(p, q, r), 4.0F); +} + +TYPED_TEST(BoxTest, ClosestPointOnLine) +{ + using autoware::common::geometry::closest_line_point_2d; + // normal case + TypeParam p = this->make(-1.0F, 0.0F); + TypeParam q = this->make(-1.0F, 2.0F); + TypeParam r = this->make(1.0F, 1.0F); + TypeParam t = closest_line_point_2d(p, q, r); + ASSERT_FLOAT_EQ(x_(t), -1.0F); + ASSERT_FLOAT_EQ(y_(t), 1.0F); + // out-of-boundary case + p = this->make(1.0F, 0.0F); + q = this->make(-2.0F, 0.0F); + r = this->make(-5.0F, 0.0F); + t = closest_line_point_2d(p, q, r); + ASSERT_FLOAT_EQ(x_(t), -5.0F); + ASSERT_NEAR(y_(t), 0.0F, std::numeric_limits::epsilon()); + // singular case + p = this->make(1.0F, 5.0F); + q = this->make(1.0F, 5.0F); + r = this->make(1.0F, 1.0F); + EXPECT_THROW(t = closest_line_point_2d(p, q, r), std::runtime_error); +} + + +TYPED_TEST(BoxTest, Basic) +{ + const std::vector data{ + {this->make(0, 0), this->make(1, 0), this->make(1, 1), this->make(0, 1)}}; + this->points.insert(this->points.begin(), data.begin(), data.end()); + + this->minimum_area_bounding_box(); + + this->check(0.5F, 0.5F, 1.0F, 1.0F, 1.0F); + this->test_corners(data); + this->test_orientation(0.0F); + + this->minimum_perimeter_bounding_box(); + + this->check(0.5F, 0.5F, 1.0F, 1.0F, 2.0F); + this->test_corners(data); + this->test_orientation(0.0F); +} + +// +TYPED_TEST(BoxTest, OrientedTriangle) +{ + this->points.insert( + this->points.begin(), + {this->make(5, 5), this->make(7, 7), this->make(5, 7)}); + + this->minimum_area_bounding_box(); + + this->check(5.5F, 6.5F, sqrtf(2.0F), 2.0F * sqrtf(2.0F), 4.0F); + this->test_corners({this->make(5, 5), this->make(7, 7), this->make(4, 6), this->make(6, 8)}); + this->test_orientation(45.0F); + + this->minimum_perimeter_bounding_box(); + + this->check(6.0F, 6.0F, 2.0F, 2.0F, 4.0F); + this->test_corners({this->make(5, 5), this->make(7, 7), this->make(5, 7), this->make(7, 5)}); + this->test_orientation(0.0F); +} +// +TYPED_TEST(BoxTest, Hull) +{ + const uint32_t FUZZ_SIZE = 1024U; + const float dx = 9.0F; + const float dy = 15.0F; + const float rx = 10.0F; + const float ry = 5.0F; + const float dth = 0.0F; + + ASSERT_EQ(FUZZ_SIZE % 4U, 0U); + + // fuzz part 1 + for (uint32_t idx = 0U; idx < FUZZ_SIZE; ++idx) { + const float th = ((idx * autoware::common::types::TAU) / FUZZ_SIZE) + dth; + this->points.push_back(this->make(rx * cosf(th) + dx, ry * sinf(th) + dy)); + } + + this->minimum_area_bounding_box(); + + // allow 10cm = 2% of size for tolerance + const float TOL_M = 0.1F; + ASSERT_LT(fabsf(this->box.centroid.x - dx), TOL_M); + ASSERT_LT(fabsf(this->box.centroid.y - dy), TOL_M); + + this->test_corners( + {this->make(dx + rx, dy + ry), + this->make(dx - rx, dy + ry), + this->make(dx - rx, dy - ry), + this->make(dx + rx, dy - ry)}, + TOL_M); + + this->test_orientation(this->rad2deg(dth), 1.0F); + // allow 1 degree of tolerance + + ASSERT_LT(fabsf(this->box.size.y - 2.0F * rx), TOL_M); + ASSERT_LT(fabsf(this->box.size.x - 2.0F * ry), TOL_M); + ASSERT_FLOAT_EQ(this->box.value, this->box.size.x * this->box.size.y); +} + +// +TYPED_TEST(BoxTest, Collinear) +{ + this->points.insert( + this->points.begin(), + { + this->make(-2, -2), + this->make(-3, -2), + this->make(-4, -2), + this->make(-2, -4), + this->make(-3, -4), + this->make(-4, -4), + this->make(-2, -3), + this->make(-2, -3), + this->make(-2, -4) + }); + + this->minimum_area_bounding_box(); + + this->check(-3.0F, -3.0F, 2.0F, 2.0F, 4.0F); + this->test_corners( + {this->make(-2, -2), this->make(-2, -4), this->make(-4, -4), this->make(-4, -2)}); + this->test_orientation(0.0F); +} + + +// +TYPED_TEST(BoxTest, Line1) +{ + this->points.insert( + this->points.begin(), { + this->make(-4, 3), + this->make(-8, 6), + this->make(-12, 9), + this->make(-16, 12), + this->make(-20, 15), + this->make(-24, 18), + this->make(-28, 21), + this->make(-32, 24), + this->make(-36, 27) + }); + + this->minimum_area_bounding_box(); + + this->check(-20.0F, 15.0F, 1.0E-6F, 40.0F, 4.0E-5F); + this->test_orientation(this->rad2deg(atan2f(3, -4))); + this->test_corners( + {this->make(-4, 3), this->make(-30, 27), this->make(-4, 3), this->make( + -36, + 27)}); + + this->minimum_perimeter_bounding_box(); + + this->check(-20.0F, 15.0F, 1.0E-6F, 40.0F, 40.00001F); + this->test_orientation(this->rad2deg(atan2f(3, -4))); + this->test_corners( + {this->make(-4, 3), this->make(-30, 27), this->make(-4, 3), this->make( + -36, + 27)}); +} + +// +TYPED_TEST(BoxTest, Line2) +{ + this->points.insert( + this->points.begin(), { + this->make(4, 0), + this->make(8, 0), + this->make(12, 0), + this->make(16, 0), + this->make(20, 0), + this->make(24, 0), + this->make(28, 0), + this->make(32, 0), + this->make(36, 0) + }); + + this->minimum_area_bounding_box(); + + this->check(20.0F, 0.0F, 1.0E-6F, 32.0F, 3.2E-5F); + this->test_orientation(0.0F); + this->test_corners({this->make(4, 0), this->make(36, 0), this->make(4, 0), this->make(36, 0)}); +} + +// +TYPED_TEST(BoxTest, Line3) +{ + this->points.insert( + this->points.begin(), { + this->make(4, 3), + this->make(8, 6), + this->make(12, 9), + this->make(16, 12), + this->make(20, 15), + this->make(24, 18), + this->make(28, 21), + this->make(32, 24), + this->make(36, 27) + }); + + this->minimum_area_bounding_box(); + + this->check(20.0F, 15.0F, 1.0E-6F, 40.0F, 4.0E-5F); + this->test_orientation(this->rad2deg(atan2f(3, 4))); + this->test_corners({this->make(4, 3), this->make(36, 27), this->make(4, 3), this->make(36, 27)}); +} + +/* _ + /_/ <-- first guess is suboptimal + +*/ +TYPED_TEST(BoxTest, SuboptInit) +{ + this->points.insert( + this->points.begin(), { + this->make(8, 15), + this->make(17, 0), + this->make(0, 0), + this->make(25, 15) + }); + + this->minimum_area_bounding_box(); + + this->check(12.5F, 7.5F, 15.0F, 25.0F, 375.0F); + this->test_orientation(this->rad2deg(atan2f(15, 8))); + // these are approximate. + this->test_corners( + {this->make(0, 0), this->make(25, 15), + this->make(11.7647F, 22.0588F), this->make(13.2353F, -7.05882F)}, + 0.1F); +} + +// +TYPED_TEST(BoxTest, Centered) +{ + this->points.insert( + this->points.begin(), { + this->make(-1, 0), + this->make(1, 0), + this->make(0, -1), + this->make(0, 1) + }); + + this->minimum_area_bounding_box(); + + this->check(0.0F, 0.0F, sqrtf(2.0F), sqrtf(2.0F), 2.0F); + this->test_orientation(45.0F); + this->test_corners({this->make(-1, 0), this->make(1, 0), this->make(0, 1), this->make(0, -1)}); +} + +// convex_hull is imperfect in this case, check if this can save the day +TYPED_TEST(BoxTest, OverlappingPoints) +{ + this->points.insert( + this->points.begin(), { + this->make(0, 0), + this->make(1, 0), + this->make(1, 1), + this->make(0, 1), + this->make(0, 0), + this->make(1, 0), + this->make(1, 1), + this->make(0, 1) + }); + + this->minimum_area_bounding_box(); + + this->check(0.5F, 0.5F, 1.0F, 1.0F, 1.0F); + this->test_orientation(0.0F); + this->test_corners({this->make(0, 0), this->make(1, 0), this->make(0, 1), this->make(1, 1)}); +} + +// check that minimum perimeter box is different from minimum area box +TYPED_TEST(BoxTest, Perimeter) +{ + this->points.insert( + this->points.begin(), { + this->make(0, 0), + this->make(0, 1), + this->make(0, 2), + this->make(0, 3), + this->make(0, 4), + this->make(1, -0.1), // small fudge to force diagonal + this->make(2, 0), + this->make(3, 0) + }); + + this->minimum_area_bounding_box(); + + this->check(0.54F, 1.28F, 5.0F, 12.0F / 5.0F, 12.0F); + this->test_orientation(-53.13F, 0.001F); + this->test_corners( + {this->make(3, 0), this->make(0, 4), this->make(-1.92, 2.56), + this->make(1.08, -1.44)}); + + // eigenbox should produce AABB TODO(c.ho) + // compute minimum perimeter box + this->minimum_perimeter_bounding_box(); + this->check(1.5F, 1.95F, 4.1F, 3.0F, 7.1F); + // perimeter for first box would be 14.8 + this->test_orientation(0.0F, 0.001F); + this->test_corners( + {this->make(3, -0.1), this->make(0, 4), this->make(3, 4), + this->make(0, -0.1)}); +} + +// bounding box is diagonal on an L +TYPED_TEST(BoxTest, Eigenbox1) +{ + std::vector v{ + this->make(0, 0), + this->make(0, 1), + this->make(-1, 2), // small fudge to force diagonal + this->make(0, 3), + this->make(0, 4), + this->make(1, 0), + this->make(2, -1), // small fudge to force diagonal + this->make(3, 0), + this->make(4, 0) + }; + this->points.insert(this->points.begin(), v.begin(), v.end()); + + // rotating calipers should produce a diagonal box + this->minimum_area_bounding_box(); + const float r = 4.0F; + + this->check(1.0F, 1.0F, r / sqrtf(2.0F), sqrtf(2.0F) * r, r * r); + const std::vector + diag_corners{this->make(4, 0), this->make(0, 4), this->make(-2, 2), this->make(2, -2)}; + this->test_corners(diag_corners); + this->test_orientation(45.0F, 0.001F); + + //// perimeter should also produce diagonal //// + this->minimum_perimeter_bounding_box(); + this->check( + 1.0F, 1.0F, r / sqrtf(2.0F), sqrtf(2.0F) * r, + r * (sqrtf(2.0F) + (1.0F / sqrtf(2.0F)))); + this->test_corners(diag_corners); + this->test_orientation(45.0F, 0.001F); + //// eigenbox should also produce aabb //// + this->eigenbox(v.begin(), v.end()); + // TODO(c.ho) don't care about value.. + this->check(1.0F, 1.0F, r * sqrtf(2.0F), r / sqrtf(2.0F), 4.375F); + this->test_corners(diag_corners); + this->test_orientation(45.0F, 0.001F); + //// Lfit should produce aabb //// + this->lfit_bounding_box_2d(v.begin(), v.end()); + + this->test_corners( + {this->make(4, -1), this->make(-1, 4), this->make(4, 4), this->make(-1, -1)}, 0.25F); + this->test_orientation(0.0F, 3.0F); +} + +// same as above test, just rotated +TYPED_TEST(BoxTest, Eigenbox2) +{ + std::vector v{ + this->make(0, 0), + this->make(1, 1), + this->make(1, 2), // small fudge to force diagonal + this->make(3, 3), + this->make(4, 4), + this->make(1, -1), + this->make(1, -2), // small fudge to force diagonal + this->make(3, -3), + this->make(4, -4) + }; + this->points.insert(this->points.begin(), v.begin(), v.end()); + + const std::vector + diag_corners{this->make(4, 4), this->make(0, 4), this->make(0, -4), this->make(4, -4)}; + // rotating calipers should produce a aabb + this->minimum_area_bounding_box(); + this->check(2.0F, 0.0F, 8, 4, 32); + this->test_corners(diag_corners); + this->test_orientation(0.0F, 0.001F); + + //// perimeter should also produce aabb //// + this->minimum_perimeter_bounding_box(); + this->check(2.0F, 0.0F, 8, 4, 12); + this->test_corners(diag_corners); + + //// eigenbox should also produce obb //// + this->eigenbox(v.begin(), v.end()); + this->test_orientation(0.0F, 0.001F); + this->test_corners(diag_corners); + //// Lfit should produce obb //// + this->lfit_bounding_box_2d(v.begin(), v.end()); + this->test_corners( + {this->make(3.5, 4.5), this->make(-1, 0), this->make(3.5, -4.5), this->make(8, 0)}, 0.2F); + this->test_orientation(45.0F, 2.0F); +} +// line case for eigenbox +TYPED_TEST(BoxTest, Eigenbox3) +{ + // horizontal line with some noise + std::vector v{ + this->make(0, 0.00), + this->make(1, -0.01), + this->make(3, 0.02), + this->make(3, 0.03), + this->make(4, -0.04), + this->make(-1, 0.01), + this->make(-2, -0.02), + this->make(-3, -0.03), + this->make(-4, 0.04) + }; + this->lfit_bounding_box_2d(v.begin(), v.end()); + this->test_corners( + {this->make(-4, -0.04), this->make(-4, 0.04), this->make(4, -0.04), this->make(4, 0.04)}, 0.2F); + this->test_orientation(0.0F, 1.0F); +} + +// bad case: causes intersection2d to fail +// See https://gitlab.apex.ai/ApexAI/grand_central/issues/2862#note_156875 for more failure cases +TYPED_TEST(BoxTest, IntersectFail) +{ + std::vector vals{ + this->make(-13.9, 0.100006), + this->make(-14.1, 0.100006), + this->make(-13.9, 0.300003), + this->make(-14.1, 0.300003), + this->make(-13.9, 0.5), + this->make(-14.1, 0.5), + this->make(-13.9, 0.699997), + this->make(-14.1, 0.699997), + this->make(-14.3, 0.699997) + }; + EXPECT_NO_THROW(this->lfit_bounding_box_2d(vals.begin(), vals.end())); + vals = { + this->make(-2.1, 10.1), + this->make(-1.89999, 10.1), + this->make(-1.89999, 10.3), + this->make(-1.7, 10.3), + this->make(-1.5, 10.3), + this->make(-1.3, 10.3), + this->make(-0.5, 10.3), + this->make(-0.300003, 10.3), + this->make(-0.0999908, 10.3), + this->make(0.699997, 10.3), + this->make(0.900009, 10.3), + this->make(1.3, 10.3), + this->make(1.7, 10.3) + }; + EXPECT_NO_THROW(this->lfit_bounding_box_2d(vals.begin(), vals.end())); + vals = { + this->make(2.7, -5.5), + this->make(2.7, -5.3), + this->make(2.7, -5.1), + this->make(2.7, -4.9), + this->make(2.7, -4.7), + this->make(2.7, -4.5), + this->make(2.7, -4.3), + this->make(2.7, -4.1), + this->make(2.7, -3.9) + }; + EXPECT_NO_THROW(this->lfit_bounding_box_2d(vals.begin(), vals.end())); +} +/// Handle slight floating point underflow case +// Note: raw discriminant checks are disabled because they don't work. I suspect this is due to +// tiny differences in floating point math when using our compile flags +TYPED_TEST(BoxTest, EigUnderflow) +{ + using autoware::common::geometry::bounding_box::details::Covariance2d; + // auto discriminant = [](const Covariance2d cov) -> float32_t { + // // duplicated raw math + // const float32_t tr_2 = (cov.xx + cov.yy) * 0.5F; + // const float32_t det = (cov.xx * cov.yy) - (cov.xy * cov.xy); + // return (tr_2 * tr_2) - det; + // }; + TypeParam u, v; + const Covariance2d c1{0.0300002, 0.0300002, 5.46677e-08, 0U}; + // EXPECT_LT(discriminant(c1), 0.0F); + EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c1, u, v)); + const Covariance2d c2{0.034286, 0.0342847, 2.12874e-09, 0U}; + // EXPECT_LT(discriminant(c2), 0.0F); + EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c2, u, v)); + const Covariance2d c3{0.0300002, 0.0300002, -2.84987e-08, 0U}; + // EXPECT_LT(discriminant(c3), 0.0F); + EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c3, u, v)); + const Covariance2d c4{0.0300004, 0.0300002, 3.84156e-08, 0U}; + // EXPECT_LT(discriminant(c4), 0.0F); + EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c4, u, v)); + const Covariance2d c5{0.0300014, 0.0300014, -7.45058e-09, 0U}; + // EXPECT_LT(discriminant(c5), 0.0F); + EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c5, u, v)); + const Covariance2d c6{0.0400004, 0.0400002, 3.28503e-08, 0U}; + // EXPECT_LT(discriminant(c6), 0.0F); + EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c6, u, v)); +} + + +//////////////////////////////////////////////// + +#endif // TEST_BOUNDING_BOX_HPP_ diff --git a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp b/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp new file mode 100644 index 0000000000000..9b0563ff010f7 --- /dev/null +++ b/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp @@ -0,0 +1,266 @@ +// Copyright 2019 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#ifndef TEST_SPATIAL_HASH_HPP_ +#define TEST_SPATIAL_HASH_HPP_ + +#include +#include +#include +#include "geometry/spatial_hash.hpp" + +using autoware::common::types::float32_t; +using autoware::common::types::float64_t; +using autoware::common::types::bool8_t; +using autoware::common::geometry::spatial_hash::Config2d; +using autoware::common::geometry::spatial_hash::Config3d; +using autoware::common::geometry::spatial_hash::SpatialHash; +using autoware::common::geometry::spatial_hash::SpatialHash2d; +using autoware::common::geometry::spatial_hash::SpatialHash3d; + +template +class TypedSpatialHashTest : public ::testing::Test +{ +public: + TypedSpatialHashTest() + : ref(), + EPS(0.001F) + { + ref.x = 0.0F; + ref.y = 0.0F; + ref.z = 0.0F; + } + +protected: + template + void add_points( + SpatialHash & hash, + const uint32_t points_per_ring, + const uint32_t num_rings, + const float32_t dr, + const float32_t dx = 0.0F, + const float32_t dy = 0.0F) + { + const float32_t dth = 2.0F * 3.14159F / points_per_ring; + + // insert + float32_t r = dr; + for (uint32_t rdx = 0U; rdx < num_rings; ++rdx) { + float32_t th = 0.0F; + for (uint32_t pdx = 0U; pdx < points_per_ring; ++pdx) { + PointT pt; + pt.x = r * cosf(th) + dx; + pt.y = r * sinf(th) + dy; + pt.z = 0.0F; + hash.insert(pt); + th += dth; + } + r += dr; + } + } + PointT ref; + const float32_t EPS; +}; // SpatialHash +// test struct + +// Instantiate tests for given types, add more types here as they are used +using PointTypesSpatialHash = ::testing::Types; +// cppcheck-suppress syntaxError +TYPED_TEST_SUITE(TypedSpatialHashTest, PointTypesSpatialHash, ); +/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE + + +/////////////////////////////////////////////////////////////// +// TODO(christopher.ho) helper functions to simplify this stuff +/// all points in one bin +TYPED_TEST(TypedSpatialHashTest, OneBin) +{ + using PointT = TypeParam; + const float32_t dr = 1.0F; + Config2d cfg{-10.0F, 10.0F, -10.0F, 10.0F, 3.0F, 1024U}; + SpatialHash2d hash{cfg}; + + // build concentric rings around origin + const uint32_t PTS_PER_RING = 10U; + const uint32_t NUM_RINGS = 1U; + this->add_points(hash, PTS_PER_RING, NUM_RINGS, dr); + + // loop through all points + float r = dr - this->EPS; + for (uint32_t rdx = 0U; rdx < NUM_RINGS + 1U; ++rdx) { + const uint32_t n_pts = rdx * PTS_PER_RING; + const auto & neighbors = hash.near(this->ref, r); + uint32_t points_seen = 0U; + for (const auto & itd : neighbors) { + const PointT & pt = itd; + const float dist = sqrtf((pt.x * pt.x) + (pt.y * pt.y)); + ASSERT_LT(dist, r); + ASSERT_FLOAT_EQ(dist, itd.get_distance()); + ++points_seen; + } + ASSERT_EQ(points_seen, n_pts); + r += dr; + // Make sure statistics are consistent + EXPECT_EQ(hash.bins_hit(), 9U * (1U + rdx)); + EXPECT_EQ(hash.neighbors_found(), rdx * PTS_PER_RING); + } + // check iterators etc. + uint32_t count = 0U; + for (auto iter = hash.cbegin(); iter != hash.cend(); ++iter) { + // TODO(c.ho) check uniqueness of stuff + ++count; + } + EXPECT_EQ(PTS_PER_RING, count); + hash.clear(); + EXPECT_EQ(hash.size(), 0U); + EXPECT_TRUE(hash.empty()); + count = 0U; + for (auto it : hash) { + // TODO(c.ho) check uniqueness of stuff + (void) it; + ++count; + } + EXPECT_EQ(count, 0U); +} +/// test out of bounds points +TYPED_TEST(TypedSpatialHashTest, Oob) +{ + using PointT = TypeParam; + const float32_t dr = 20.0F; + Config2d cfg{-2.0F, 2.0F, -2.0F, 2.0F, dr + this->EPS, 1024U}; + SpatialHash2d hash{cfg}; + + // build concentric rings around origin + const uint32_t PTS_PER_RING = 12U; + this->add_points(hash, PTS_PER_RING, 1U, dr); + + // loop through all points + float32_t r = dr + this->EPS; + const uint32_t n_pts = PTS_PER_RING; + const auto & nbrs = hash.near(this->ref, r); + uint32_t points_seen = 0U; + for (const auto itd : nbrs) { + const PointT & pt = itd; + const float32_t dist = sqrtf((pt.x * pt.x) + (pt.y * pt.y)); + ASSERT_LT(dist, r); + ASSERT_GT(dist, 10.0F * sqrtf(2.0F)); + ASSERT_FLOAT_EQ(dist, itd.get_distance()); + ++points_seen; + } + + ASSERT_EQ(points_seen, n_pts); +} +// 3d test case +TYPED_TEST(TypedSpatialHashTest, 3d) +{ + using PointT = TypeParam; + Config3d cfg{-30.0F, 30.0F, -30.0F, 30.0F, -30.0F, 30.0F, 30.0F, 1024U}; + SpatialHash3d hash{cfg}; + EXPECT_TRUE(hash.empty()); + + // build concentric rings around origin + const uint32_t points_per_ring = 32U; + const uint32_t num_rings = 5U; + const float32_t dth = 2.0F * 3.14159F / points_per_ring; + std::vector pts{}; + + // insert + const float32_t r = 10.0F; + float32_t phi = 0.0f; + for (uint32_t rdx = 0U; rdx < num_rings; ++rdx) { + float32_t th = 0.0F; + for (uint32_t pdx = 0U; pdx < points_per_ring; ++pdx) { + PointT pt; + pt.x = r * cosf(th) * cosf(phi); + pt.y = r * sinf(th) * cosf(phi); + pt.z = r * sinf(phi); + pts.push_back(pt); + th += dth; + } + hash.insert(pts.begin(), pts.end()); + pts.clear(); + phi += 1.0f; + } + EXPECT_FALSE(hash.empty()); + EXPECT_EQ(hash.size(), num_rings * points_per_ring); + + // loop through all points + const uint32_t n_pts = num_rings * points_per_ring; + const auto & neighbors = hash.near(this->ref, r + this->EPS); + uint32_t points_seen = 0U; + for (const auto & itd : neighbors) { + const PointT & pt = itd; + const float32_t dist = sqrtf((pt.x * pt.x) + (pt.y * pt.y) + (pt.z * pt.z)); + ASSERT_LT(dist, r + this->EPS); + ASSERT_FLOAT_EQ(dist, itd.get_distance()); + ++points_seen; + } + + ASSERT_EQ(points_seen, n_pts); + + // check iterators etc. + uint32_t count = 0U; + for (auto iter = hash.cbegin(); iter != hash.cend(); ++iter) { + // TODO(c.ho) check uniqueness of stuff + ++count; + } + EXPECT_EQ(n_pts, count); + hash.clear(); + EXPECT_EQ(hash.size(), 0U); + EXPECT_TRUE(hash.empty()); + count = 0U; + for (auto it : hash) { + // TODO(c.ho) check uniqueness of stuff + (void) it; + ++count; + } + EXPECT_EQ(count, 0U); +} + +/// edge cases +TEST(SpatialHashConfig, BadCases) +{ + // negative side length + EXPECT_THROW(Config2d({-30.0F, 30.0F, -30.0F, 30.0F, -1.0F, 1024U}), std::domain_error); + // min_x >= max_x + EXPECT_THROW(Config2d({31.0F, 30.0F, -30.0F, 30.0F, 1.0F, 1024U}), std::domain_error); + // min_y >= max_y + EXPECT_THROW(Config2d({-30.0F, 30.0F, 30.1F, 30.0F, 1.0F, 1024U}), std::domain_error); + // min_z >= max_z + EXPECT_THROW( + Config3d({-30.0F, 30.0F, -30.0F, 30.0F, 31.0F, 30.0F, 1.0F, 1024U}), + std::domain_error); + // floating point limit + constexpr float32_t max_float = std::numeric_limits::max(); + EXPECT_THROW(Config2d({-max_float, max_float, -30.0F, 30.0F, 1.0F, 1024U}), std::domain_error); + EXPECT_THROW( + Config3d({-30.0F, 30.0F, -max_float, max_float, -30.0F, 30.0F, 1.0F, 1024U}), + std::domain_error); + EXPECT_THROW( + Config3d({-30.0F, 30.0F, -30.0F, 30.0F, -max_float, max_float, 1.0F, 1024U}), + std::domain_error); + // y would overflow + // constexpr float32_t big_float = + // static_cast(std::numeric_limits::max() / 4UL); + // EXPECT_THROW(Config({-big_float, big_float, -big_float, big_float, 0.001F, 1024U}), + // std::domain_error); + // z would overflow + // EXPECT_THROW( + // Config3d({-30.0F, 30.0F, -99999.0F, 99999.0F, -99999.0F, 99999.0F, 0.001F, 1024U}), + // std::domain_error); + // TODO(c.ho) re-enable test when we can actually check unsigned integer multiplication overflow +} +#endif // TEST_SPATIAL_HASH_HPP_ diff --git a/common/autoware_auto_geometry/test/src/lookup_table.cpp b/common/autoware_auto_geometry/test/src/lookup_table.cpp new file mode 100644 index 0000000000000..f825d9055f9b2 --- /dev/null +++ b/common/autoware_auto_geometry/test/src/lookup_table.cpp @@ -0,0 +1,164 @@ +// Copyright 2017-2020 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#include + +#include +#include + +#include +#include + +using autoware::common::helper_functions::lookup_1d; +using autoware::common::helper_functions::interpolate; +using autoware::common::helper_functions::LookupTable1D; +using autoware::common::types::float32_t; +using autoware::common::types::float64_t; + +template +class BadCase : public ::testing::Test +{ +}; + +using BadTypes = ::testing::Types; +// cppcheck-suppress syntaxError +TYPED_TEST_SUITE(BadCase, BadTypes, ); + +// Empty domain/range +TYPED_TEST(BadCase, Empty) +{ + using T = TypeParam; + EXPECT_THROW(lookup_1d({}, {}, T{}), std::domain_error); +} + +// Unequal domain/range +TYPED_TEST(BadCase, UnequalDomain) +{ + using T = TypeParam; + EXPECT_THROW(lookup_1d({T{1}, T{2}}, {T{1}}, T{}), std::domain_error); +} + +// Not sorted domain +TYPED_TEST(BadCase, DomainNotSorted) +{ + using T = TypeParam; + EXPECT_THROW(lookup_1d({T{2}, T{1}}, {T{1}, T{2}}, T{}), std::domain_error); +} + +//////////////////////////////////////////////////////////////////////////////// + +template +class SanityCheck : public ::testing::Test +{ +public: + using T = Type; + + void SetUp() override + { + domain_ = std::vector{T{1}, T{3}, T{5}}; + range_ = std::vector{T{2}, T{4}, T{0}}; + ASSERT_NO_THROW(table_ = std::make_unique>(domain_, range_)); + } + + bool not_in_domain(const T bad_value) const noexcept + { + return std::find(domain_.begin(), domain_.end(), bad_value) == domain_.end(); + } + + void check(const T expected, const T actual) const noexcept + { + if (std::is_floating_point::value) { + EXPECT_FLOAT_EQ(actual, expected); + } else { + EXPECT_EQ(actual, expected); + } + } + +protected: + std::vector domain_{}; + std::vector range_{}; + std::unique_ptr> table_{}; +}; + +using NormalTypes = ::testing::Types; +TYPED_TEST_SUITE(SanityCheck, NormalTypes, ); + +TYPED_TEST(SanityCheck, Exact) +{ + const auto x = this->domain_[1U]; + const auto result = this->table_->lookup(x); + ASSERT_FALSE(this->not_in_domain(x)); + this->check(result, this->range_[1U]); +} + +TYPED_TEST(SanityCheck, Interpolation) +{ + const auto x = TypeParam{2}; + // Asssert x is not identically in domain_ + ASSERT_TRUE(this->not_in_domain(x)); + const auto result = this->table_->lookup(x); + this->check(result, TypeParam{3}); +} + +TYPED_TEST(SanityCheck, AboveRange) +{ + const auto x = TypeParam{999999}; + ASSERT_GT(x, this->domain_.back()); // domain is known to be sorted + const auto result = this->table_->lookup(x); + this->check(result, this->range_.back()); +} + +TYPED_TEST(SanityCheck, BelowRange) +{ + const auto x = TypeParam{0}; + ASSERT_LT(x, this->domain_.front()); // domain is known to be sorted + const auto result = this->table_->lookup(x); + this->check(result, this->range_.front()); +} + +TEST(LookupTableHelpers, Interpolate) { + { + const auto scaling = 0.0f; + EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 0.0f); + EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 2.0f); + } + + { + const auto scaling = 1.0f; + EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 1.0f); + EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 3.5f); + } + + { + const auto scaling = -1.0f; + EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 0.0f); + EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 2.0f); + } + + { + const auto scaling = 2.0f; + EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 1.0f); + EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 3.5f); + } + + { + const auto scaling = 0.75f; + EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 0.75f); + EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 3.125f); + } +} + +// TODO(c.ho) check with more interesting functions diff --git a/common/autoware_auto_geometry/test/src/test_area.cpp b/common/autoware_auto_geometry/test/src/test_area.cpp new file mode 100644 index 0000000000000..c7f141a77d16f --- /dev/null +++ b/common/autoware_auto_geometry/test/src/test_area.cpp @@ -0,0 +1,137 @@ +// Copyright 2021 Apex.AI, 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#include + +#include +#include + +#include +#include +#include + +template +class AreaTest : public ::testing::Test +{ +protected: + DataStructure data_{}; + using Point = typename DataStructure::value_type; + using Real = decltype(autoware::common::geometry::point_adapter::x_(std::declval())); + + auto area() + { + return autoware::common::geometry::area_checked_2d(data_.begin(), data_.end()); + } + + void add_point(Real x, Real y) + { + namespace pa = autoware::common::geometry::point_adapter; + Point p{}; + pa::xr_(p) = x; + pa::yr_(p) = y; + (void)data_.insert(data_.end(), p); + } +}; + +// Data structures to test... +template +using TestTypes_ = ::testing::Types< + std::vector..., + std::list... +>; +// ... and point types to test +using TestTypes = TestTypes_; +// cppcheck-suppress syntaxError +TYPED_TEST_SUITE(AreaTest, TestTypes, ); + +// The empty set has zero area +TYPED_TEST(AreaTest, DegenerateZero) +{ + EXPECT_FLOAT_EQ(0.0, this->area()); +} + +// An individual point has zero area +TYPED_TEST(AreaTest, DegenerateOne) +{ + this->add_point(0.0, 0.0); + EXPECT_FLOAT_EQ(0.0, this->area()); +} + +// An line segment has zero area +TYPED_TEST(AreaTest, DegenerateTwo) +{ + this->add_point(1.0, -1.0); + this->add_point(-3.0, 2.0); + EXPECT_FLOAT_EQ(0.0, this->area()); +} + +// Simple triangle +TYPED_TEST(AreaTest, Triangle) +{ + this->add_point(1.0, 0.0); + this->add_point(3.0, 0.0); // 2.0 wide + this->add_point(2.0, 2.0); // 2.0 tall + EXPECT_FLOAT_EQ(2.0, this->area()); // A = (1/2) * b * h +} + +// Rectangle is easy to do computationall +TYPED_TEST(AreaTest, Rectangle) +{ + this->add_point(-5.0, -5.0); + this->add_point(-2.0, -5.0); // L = 3 + this->add_point(-2.0, -1.0); // H = 4 + this->add_point(-5.0, -1.0); + EXPECT_FLOAT_EQ(12.0, this->area()); // A = b * h +} + +// Parallelogram is slightly less trivial than a rectangle +TYPED_TEST(AreaTest, Parallelogram) +{ + this->add_point(-5.0, 1.0); + this->add_point(-2.0, 1.0); // L = 3 + this->add_point(-1.0, 3.0); // H = 2 + this->add_point(-4.0, 3.0); + EXPECT_FLOAT_EQ(6.0, this->area()); // A = b * h +} + +// Octagon is analytical and reasonably easy to build +TYPED_TEST(AreaTest, Octagon) +{ + const auto sq2 = std::sqrt(2.0); + const auto a = 1.0; + const auto a2 = a / 2.0; + const auto b = (a + sq2) / 2.0; + this->add_point(-a2, -b); + this->add_point(a2, -b); + this->add_point(b, -a2); + this->add_point(b, a2); + this->add_point(a2, b); + this->add_point(-a2, b); + this->add_point(-b, a2); + this->add_point(-b, -a2); + const auto expect = (2.0 * (1.0 + sq2)) * (a * a); + EXPECT_FLOAT_EQ(expect, this->area()); // A = b * h +} + +// Bad case +TYPED_TEST(AreaTest, NotCcw) +{ + this->add_point(0.0, 0.0); + this->add_point(1.0, 1.0); + this->add_point(1.0, 0.0); + this->add_point(2.0, 1.0); + EXPECT_THROW(this->area(), std::domain_error); +} diff --git a/common/autoware_auto_geometry/test/src/test_common_2d.cpp b/common/autoware_auto_geometry/test/src/test_common_2d.cpp new file mode 100644 index 0000000000000..b1abf7182f15d --- /dev/null +++ b/common/autoware_auto_geometry/test/src/test_common_2d.cpp @@ -0,0 +1,198 @@ +// Copyright 2021 The Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#include +#include +#include +#include + +#include +#include + +using autoware::common::geometry::point_adapter::xr_; +using autoware::common::geometry::point_adapter::yr_; + + +// Helper function for adding new points +template +T make_points(const float x, const float y) +{ + T ret; + xr_(ret) = x; + yr_(ret) = y; + return ret; +} + +// PointTypes to be tested +using PointTypes = ::testing::Types; + +// Wrapper function for stubbing output of +// autoware::common::geometry::check_point_position_to_line_2d +template +int point_position_checker(const T & p1, const T & p2, const T & q) +{ + auto result = autoware::common::geometry::check_point_position_to_line_2d(p1, p2, q); + if (result > 0.0F) { + return 1; + } else if (result < 0.0F) { + return -1; + } + return result; +} + +// Templated struct defining the function parameters for +// autoware::common::geometry::check_point_position_to_line_2d and input_output vector for +// assertion of return values from the function +template +struct PointPositionToLine : public ::testing::Test +{ + struct Parameters + { + T p1; + T p2; + T q; + }; + static std::vector> input_output; +}; + +TYPED_TEST_SUITE_P(PointPositionToLine); + + +template +std::vector::Parameters, int>> +PointPositionToLine::input_output{ + {{make_points(0.0F, 0.0F), make_points(-1.0F, 1.0F), make_points(1.0F, 5.0F)}, + -1}, + {{make_points(0.0F, 0.0F), make_points(-1.0F, 1.0F), make_points(-1.0F, 0.5F)}, + 1}, + // Check point on the line + {{make_points(0.0F, 0.0F), make_points(-1.0F, 1.0F), make_points(-2.0F, 2.0F)}, + 0}, +}; + +TYPED_TEST_P(PointPositionToLine, PointPositionToLineTest) { + for (size_t i = 0; i < PointPositionToLine::input_output.size(); ++i) { + const auto & input = PointPositionToLine::input_output[i].first; + EXPECT_EQ( + point_position_checker(input.p1, input.p2, input.q), + PointPositionToLine::input_output[i].second) << "Index " << i; + } +} + +REGISTER_TYPED_TEST_SUITE_P(PointPositionToLine, PointPositionToLineTest); +// cppcheck-suppress syntaxError +INSTANTIATE_TYPED_TEST_SUITE_P(Test, PointPositionToLine, PointTypes, ); + +///////////////////////////////////////////////////////////////////////////////////// + +// Templated struct defining the function parameters for +// autoware::common::geometry::is_point_inside_polygon_2d and input_output vector for +// assertion of return values from the function +template +struct InsidePolygon : public ::testing::Test +{ + struct Parameters + { + std::vector polygon; + T q; + }; + static std::vector> input_output; +}; + +TYPED_TEST_SUITE_P(InsidePolygon); + +template +std::vector::Parameters, bool>> +InsidePolygon::input_output{ + // point inside the rectangle + {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), + make_points(-0.5F, 0.5F)}, make_points(0.F, 0.5F)}, + true}, + // point below the rectangle + {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), + make_points(-0.5F, 0.5F)}, make_points(0.5F, 0.F)}, + false}, + // point above the rectangle + {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), + make_points(-0.5F, 0.5F)}, make_points(0.5F, 1.75F)}, + false}, + // point on the rectangle + {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), + make_points(-0.5F, 0.5F)}, make_points(0.5F, 0.5F)}, + true}, +}; + +TYPED_TEST_P(InsidePolygon, InsidePolygonTest) { + for (size_t i = 0; i < InsidePolygon::input_output.size(); ++i) { + const auto & input = InsidePolygon::input_output[i].first; + EXPECT_EQ( + autoware::common::geometry::is_point_inside_polygon_2d( + input.polygon.begin(), + input.polygon.end(), input.q), InsidePolygon::input_output[i].second) << + "Index " << i; + } +} + +REGISTER_TYPED_TEST_SUITE_P(InsidePolygon, InsidePolygonTest); +// cppcheck-suppress syntaxError +INSTANTIATE_TYPED_TEST_SUITE_P(Test, InsidePolygon, PointTypes, ); + +TEST(ordered_check, basic) { + // CW + std::vector points_list = { + make_points(8.0, 4.0), + make_points(9.0, 1.0), + make_points(3.0, 2.0), + make_points(2.0, 5.0) + }; + EXPECT_TRUE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); + + // CCW + points_list = { + make_points(2.0, 5.0), + make_points(3.0, 2.0), + make_points(9.0, 1.0), + make_points(8.0, 4.0) + }; + EXPECT_TRUE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); + + // Unordered + points_list = { + make_points(2.0, 5.0), + make_points(3.0, 2.0), + make_points(8.0, 4.0), + make_points(9.0, 1.0) + }; + EXPECT_FALSE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); + + // Unordered + points_list = { + make_points(0.0, 0.0), + make_points(1.0, 1.0), + make_points(1.0, 0.0), + make_points(2.0, 1.0) + }; + EXPECT_FALSE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); + + // Colinearity + points_list = { + make_points(2.0, 2.0), + make_points(4.0, 4.0), + make_points(6.0, 6.0) + }; + EXPECT_TRUE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); +} diff --git a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp b/common/autoware_auto_geometry/test/src/test_convex_hull.cpp new file mode 100644 index 0000000000000..b22c72c377ab6 --- /dev/null +++ b/common/autoware_auto_geometry/test/src/test_convex_hull.cpp @@ -0,0 +1,372 @@ +// Copyright 2017-2019 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#include +#include +#include +#include +#include "geometry/convex_hull.hpp" + +using autoware::common::types::float32_t; +using autoware::common::types::float64_t; +using autoware::common::types::bool8_t; + +template +class TypedConvexHullTest : public ::testing::Test +{ +protected: + std::list list; + + typename std::list::const_iterator convex_hull() + { + const auto ret = autoware::common::geometry::convex_hull(list); + return ret; + } + + void check_hull( + const typename std::list::const_iterator last, + const std::vector & expect, + const bool8_t strict = true) + { + uint32_t items = 0U; + for (auto & pt : expect) { + bool8_t found = false; + auto it = list.begin(); + while (it != last) { + constexpr float32_t TOL = 1.0E-6F; + if (fabsf(pt.x - it->x) <= TOL && + fabsf(pt.y - it->y) <= TOL && + (fabsf(pt.z - it->z) <= TOL || !strict)) // TODO(@estive): z if only strict + { + found = true; + break; + } + ++it; + } + ASSERT_TRUE(found) << items; + ++items; + } + if (strict) { + ASSERT_EQ(items, expect.size()); + } + } + + PointT make(const float32_t x, const float32_t y, const float32_t z) + { + PointT ret; + ret.x = x; + ret.y = y; + ret.z = z; + return ret; + } +}; // class convex_hull_test + +// Instantiate tests for given types, add more types here as they are used +using PointTypes = ::testing::Types; +// cppcheck-suppress syntaxError +TYPED_TEST_SUITE(TypedConvexHullTest, PointTypes, ); +/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE + +////////////////////////////////////////// + +/* + 3 + 2 +1 +*/ +TYPED_TEST(TypedConvexHullTest, Triangle) +{ + std::vector expect({this->make(1, 0, 0), this->make(3, 1, 0), this->make(2, 2, 0)}); + this->list.insert(this->list.begin(), expect.begin(), expect.end()); + + const auto last = this->convex_hull(); + + this->check_hull(last, expect); + ASSERT_EQ(this->list.size(), 3U); + // check order + auto it = this->list.begin(); + ASSERT_FLOAT_EQ(it->x, 1); ++it; // node 1 + ASSERT_FLOAT_EQ(it->x, 3); ++it; // node 2 + ASSERT_FLOAT_EQ(it->x, 2); ++it; // node 3 + ASSERT_EQ(it, last); +} +/* + 2 1 + +4 + 3 +*/ +// test that things get reordered to ccw +TYPED_TEST(TypedConvexHullTest, Quadrilateral) +{ + std::vector expect({ + this->make(-1, -1, 1), + this->make(-5, -1, 2), + this->make(-2, -6, 3), + this->make(-6, -5, 4)}); + this->list.insert(this->list.begin(), expect.begin(), expect.end()); + const auto last = this->convex_hull(); + + this->check_hull(last, expect); + ASSERT_EQ(this->list.size(), 4U); + + // check for order + auto it = this->list.begin(); + ASSERT_FLOAT_EQ(it->x, -6); ++it; // node 4 + ASSERT_FLOAT_EQ(it->x, -2); ++it; // node 3 + ASSERT_FLOAT_EQ(it->x, -1); ++it; // node 1 + ASSERT_FLOAT_EQ(it->x, -5); ++it; // node 2 + ASSERT_EQ(it, last); +} + +// test that things get reordered to ccw +TYPED_TEST(TypedConvexHullTest, Quadhull) +{ + std::vector data({ + this->make(1, 1, 1), + this->make(5, 1, 2), + this->make(2, 6, 3), + this->make(3, 3, 4), + this->make(6, 5, 5)}); + std::vector expect{{data[0], data[1], data[2], data[4]}}; + this->list.insert(this->list.begin(), data.begin(), data.end()); + + const auto last = this->convex_hull(); + + this->check_hull(last, expect); + ASSERT_EQ(std::distance(this->list.cbegin(), last), 4U); + + // check for order + auto it = this->list.begin(); + ASSERT_FLOAT_EQ(it->x, 1); ++it; // node 1 + ASSERT_FLOAT_EQ(it->x, 5); ++it; // node 2 + ASSERT_FLOAT_EQ(it->x, 6); ++it; // node 4 + ASSERT_FLOAT_EQ(it->x, 2); ++it; // node 3 + ASSERT_EQ(it, last); +} + + +// a ring plus a bunch of random stuff in the middle +TYPED_TEST(TypedConvexHullTest, Hull) +{ + const uint32_t HULL_SIZE = 13U; + const uint32_t FUZZ_SIZE = 50U; + const float32_t dth = 1.133729384F; // some weird irrational(ish) number + const float32_t r_hull = 20.0F; + const float32_t r_fuzz = 10.0F; + ASSERT_LT(r_fuzz, r_hull); + + std::vector hull; + + uint32_t hull_pts = 0U; + float32_t th = 0.0F; + // hull part 1 + for (uint32_t idx = 0U; idx < HULL_SIZE / 3U; ++idx) { + const auto pt = this->make(r_hull * cosf(th), r_hull * sinf(th), th); + hull.push_back(pt); + this->list.push_back(pt); + th = fmodf(th + dth, 2.0F * 3.14159F); + ++hull_pts; + } + + // fuzz part 1 + uint32_t fuzz_pts = 0U; + for (uint32_t idx = 0U; idx < FUZZ_SIZE / 2U; ++idx) { + const auto pt = this->make(r_fuzz * cosf(th), r_fuzz * sinf(th), th); + this->list.push_back(pt); + th = fmodf(th + dth, 2.0F * 3.14159F); + ++fuzz_pts; + } + + // hull part 2 + for (uint32_t idx = 0U; idx < HULL_SIZE / 3U; ++idx) { + const auto pt = this->make(r_hull * cosf(th), r_hull * sinf(th), th); + hull.push_back(pt); + this->list.push_back(pt); + th = fmodf(th + dth, 2.0F * 3.14159F); + ++hull_pts; + } + + // fuzz part 2 + for (uint32_t idx = 0U; idx < FUZZ_SIZE - fuzz_pts; ++idx) { + const auto pt = this->make(r_fuzz * cosf(th), r_fuzz * sinf(th), th); + this->list.push_back(pt); + th = fmodf(th + dth, 2.0F * 3.14159F); + } + + // hull part 3 + for (uint32_t idx = 0U; idx < HULL_SIZE - hull_pts; ++idx) { + const auto pt = this->make(r_hull * cosf(th), r_hull * sinf(th), th); + hull.push_back(pt); + this->list.push_back(pt); + th = fmodf(th + dth, 2.0F * 3.14159F); + } + + const auto last = this->convex_hull(); + + this->check_hull(last, hull); + ASSERT_EQ(std::distance(this->list.cbegin(), last), HULL_SIZE); +} + +TYPED_TEST(TypedConvexHullTest, Collinear) +{ + std::vector data({ + this->make(0, 0, 1), + this->make(1, 0, 2), + this->make(2, 0, 3), + this->make(0, 2, 4), + this->make(1, 2, 8), + this->make(2, 2, 7), + this->make(1, 0, 6), + this->make(1, 2, 5), + this->make(1, 1, 0) + }); + const std::vector expect{{data[0], data[2], data[3], data[5]}}; + this->list.insert(this->list.begin(), data.begin(), data.end()); + + const auto last = this->convex_hull(); + + this->check_hull(last, expect); + ASSERT_EQ(std::distance(this->list.cbegin(), last), 4U); + + // check for order + auto it = this->list.begin(); + ASSERT_FLOAT_EQ(it->z, 1); ++it; // node 1 + ASSERT_FLOAT_EQ(it->z, 3); ++it; // node 1 + ASSERT_FLOAT_EQ(it->z, 7); ++it; // node 2 + ASSERT_FLOAT_EQ(it->z, 4); ++it; // node 3 + ASSERT_EQ(it, last); +} + +// degenerate cases +TYPED_TEST(TypedConvexHullTest, OverlappingPoints) +{ + std::vector data({ + this->make(3, -1, 1), + this->make(4, -2, 2), + this->make(5, -7, 3), + this->make(4, -2, 4), + this->make(5, -7, 8), + this->make(3, -1, 7), + this->make(5, -7, 6), + this->make(4, -2, 5), + this->make(3, -1, 0) + }); + const std::vector expect{{data[0], data[1], data[2]}}; + this->list.insert(this->list.begin(), data.begin(), data.end()); + + const auto last = this->convex_hull(); + + ASSERT_EQ(std::distance(this->list.cbegin(), last), 3U); + this->check_hull(last, expect, false); +} + +TYPED_TEST(TypedConvexHullTest, Line) +{ + std::vector data({ + this->make(-3, 3, 1), + this->make(-2, 2, 2), + this->make(-1, 1, 3), + this->make(-8, 8, 4), + this->make(-6, 6, 8), + this->make(-4, 4, 7), + this->make(-10, 10, 6), + this->make(-12, 12, 5), + this->make(-11, 11, 0) + }); + const std::vector expect{{data[2], data[7]}}; + this->list.insert(this->list.begin(), data.begin(), data.end()); + + const auto last = this->convex_hull(); + + ASSERT_EQ(std::distance(this->list.cbegin(), last), 2U); + this->check_hull(last, expect, false); + + // check for order: this part is a little loose + auto it = this->list.begin(); + ASSERT_FLOAT_EQ(it->z, 5); ++it; // node 8 + ASSERT_FLOAT_EQ(it->z, 3); ++it; // node 3 + ASSERT_EQ(it, last); +} + +/* +1 + 4 + + 3 + 2 +*/ +TYPED_TEST(TypedConvexHullTest, LowerHull) +{ + const std::vector data({ + this->make(1, 3, 1), + this->make(2, -2, 2), + this->make(3, -1, 3), + this->make(4, 1, 4), + }); + this->list.insert(this->list.begin(), data.begin(), data.end()); + + const auto last = this->convex_hull(); + + ASSERT_EQ(std::distance(this->list.cbegin(), last), 4U); + this->check_hull(last, data); + + // check for order: this part is a little loose + auto it = this->list.begin(); + ASSERT_FLOAT_EQ(it->z, 1); ++it; + ASSERT_FLOAT_EQ(it->z, 2); ++it; + ASSERT_FLOAT_EQ(it->z, 3); ++it; + ASSERT_FLOAT_EQ(it->z, 4); ++it; + ASSERT_EQ(it, last); +} + +// Ensure the leftmost item is properly shuffled +/* + 5 +1 6 + 2 4 + 3 +*/ +TYPED_TEST(TypedConvexHullTest, Root) +{ + const std::vector data({ + this->make(0, 0, 1), + this->make(1, -1, 2), + this->make(3, -2, 3), + this->make(4, 0, 4), + this->make(3, 1, 5), + this->make(1, 0, 6), + }); + const std::vector expect{{data[0], data[1], data[2], data[3], data[4]}}; + this->list.insert(this->list.begin(), data.begin(), data.end()); + + const auto last = this->convex_hull(); + + ASSERT_EQ(std::distance(this->list.cbegin(), last), 5); + this->check_hull(last, expect); + + auto it = this->list.begin(); + ASSERT_FLOAT_EQ(it->z, 1); ++it; + ASSERT_FLOAT_EQ(it->z, 2); ++it; + ASSERT_FLOAT_EQ(it->z, 3); ++it; + ASSERT_FLOAT_EQ(it->z, 4); ++it; + ASSERT_FLOAT_EQ(it->z, 5); ++it; + ASSERT_EQ(it, last); + EXPECT_NE(last, this->list.cend()); + EXPECT_EQ(last->z, 6); +} + +// TODO(c.ho) random input, fuzzing, stress tests diff --git a/common/autoware_auto_geometry/test/src/test_geometry.cpp b/common/autoware_auto_geometry/test/src/test_geometry.cpp new file mode 100644 index 0000000000000..cbc1f06cb8bb1 --- /dev/null +++ b/common/autoware_auto_geometry/test/src/test_geometry.cpp @@ -0,0 +1,27 @@ +// Copyright 2018 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#include "gtest/gtest.h" + +#include "test_spatial_hash.hpp" +#include "test_bounding_box.hpp" + + +int32_t main(int32_t argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp b/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp new file mode 100644 index 0000000000000..a926d9e36b3fe --- /dev/null +++ b/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp @@ -0,0 +1,208 @@ +// Copyright 2020 Embotech AG, Zurich, Switzerland. All rights reserved. +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#include +#include +#include +#include +#include +#include +#include "geometry/convex_hull.hpp" +#include "geometry/hull_pockets.hpp" + +using autoware::common::types::float32_t; +using autoware::common::types::float64_t; +using autoware::common::geometry::point_adapter::x_; +using autoware::common::geometry::point_adapter::y_; + +template +class TypedHullPocketsTest : public ::testing::Test +{ +protected: + PointT make(const float32_t x, const float32_t y, const float32_t z) + { + PointT ret; + ret.x = x; + ret.y = y; + ret.z = z; + return ret; + } +}; // class TypedHullPocketsTest + +using PointTypes = ::testing::Types; +// cppcheck-suppress syntaxError +TYPED_TEST_SUITE(TypedHullPocketsTest, PointTypes, ); +/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE + +// Inner test function, shared among all the tests +template +typename std::vector::value_type>> +compute_hull_and_pockets( + const Iter polygon_start, + const Iter polygon_end) +{ + auto convex_hull_list = std::list::value_type>{ + polygon_start, polygon_end}; + const auto cvx_hull_end = + autoware::common::geometry::convex_hull(convex_hull_list); + + const typename decltype(convex_hull_list)::const_iterator list_beginning = + convex_hull_list.begin(); + const auto pockets = + autoware::common::geometry::hull_pockets( + polygon_start, polygon_end, + list_beginning, cvx_hull_end); + + return pockets; +} + + +// Test for a triangle - any triangle should really not result in any pockets. +TYPED_TEST(TypedHullPocketsTest, Triangle) +{ + const auto polygon = std::vectormake(0, 0, 0))> { + this->make(0, 0, 0), + this->make(2, 0, 0), + this->make(1, 1, 0) + }; + + const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end() ); + ASSERT_EQ(pockets.size(), 0u); +} + +// Test for the use case: +// +--------------------+ +// | | +// | | +// | | +// | | +// | | +// | | +// +--------------------+ +// This should not result in any pockets. +TYPED_TEST(TypedHullPocketsTest, Box) +{ + const auto polygon = std::vectormake(0, 0, 0))> { + this->make(0, 0, 0), + this->make(2, 0, 0), + this->make(2, 1, 0), + this->make(0, 1, 0) + }; + + const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end() ); + ASSERT_EQ(pockets.size(), 0u); +} + + +// Test for the use case: +// +-----+ +-----+ +// / | | | +// / | | | +// + | | + +// | | | | +// | | | | +// | -------------- | +// | | +// | | +// | | +// | | +// +------------------------------+ +// This should come up with a single box on the top left. +TYPED_TEST(TypedHullPocketsTest, Ushape) +{ + const auto polygon = std::vectormake(0, 0, 0))> { + this->make(0, 0, 0), + this->make(5, 0, 0), + this->make(5, 4.5, 0), + this->make(4, 5, 0), + this->make(4, 2, 0), + this->make(2, 2, 0), + this->make(2, 5, 0), + this->make(0, 4.5, 0), + }; + + const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end() ); + + ASSERT_EQ(pockets.size(), 1u); + ASSERT_EQ(pockets[0].size(), 4u); + ASSERT_FLOAT_EQ(x_(pockets[0][0]), 4.0); + ASSERT_FLOAT_EQ(y_(pockets[0][0]), 5.0); + ASSERT_FLOAT_EQ(x_(pockets[0][1]), 4.0); + ASSERT_FLOAT_EQ(y_(pockets[0][1]), 2.0); +} + +// Test for the use case: +// +------+ +// | | +// | | +// | | +// +------------------+ +------+ +// | | +// | | +// | | +// +--------------------------------+ +// +// This should come up with two pockets, a triangle on the top left and one on the +// top right. +TYPED_TEST(TypedHullPocketsTest, TypicalGap) +{ + const auto polygon = std::vectormake(0, 0, 0))> { + this->make(0, 0, 0), + this->make(10, 0, 0), + this->make(10, 2, 0), + this->make(8, 2, 0), + this->make(8, 4, 0), + this->make(6, 4, 0), + this->make(6, 2, 0), + this->make(0, 2, 0), + }; + + const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end() ); + + ASSERT_EQ(pockets.size(), 2u); + ASSERT_EQ(pockets[0].size(), 3u); + ASSERT_EQ(pockets[1].size(), 3u); + // TODO(s.me) check for correct pocket positioning +} + +// Test for the use case: +// +// +-----------------+ +// | | +// | | +// + | +// / | +// / | +// +-----------------+ +// +// This should come up with one pocket, in particular a pocket that contains +// the segment of the final to the first point. +TYPED_TEST(TypedHullPocketsTest, EndsInPocket) +{ + const auto polygon = std::vectormake(0, 0, 0))> { + this->make(0, 0, 0), + this->make(2, 0, 0), + this->make(2, 2, 0), + this->make(0, 2, 0), + this->make(0.1, 1, 0), + }; + + const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end() ); + + ASSERT_EQ(pockets.size(), 1u); + ASSERT_EQ(pockets[0].size(), 3u); + // TODO(s.me) check for correct pocket positioning +} diff --git a/common/autoware_auto_geometry/test/src/test_intersection.cpp b/common/autoware_auto_geometry/test/src/test_intersection.cpp new file mode 100644 index 0000000000000..f71bf335f4624 --- /dev/null +++ b/common/autoware_auto_geometry/test/src/test_intersection.cpp @@ -0,0 +1,178 @@ +// Copyright 2021 Apex.AI, 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. + +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#include +#include +#include +#include + +struct TestPoint +{ + autoware::common::types::float32_t x; + autoware::common::types::float32_t y; +}; + +struct IntersectionTestParams +{ + std::list polygon1_pts; + std::list polygon2_pts; + std::list expected_intersection_pts; +}; + +void order_ccw(std::list & points) +{ + const auto end_it = autoware::common::geometry::convex_hull(points); + ASSERT_EQ(end_it, points.end()); // Points should have represent a shape +} + +class IntersectionTest : public ::testing::TestWithParam +{ +}; + + +TEST_P(IntersectionTest, Basic) { + const auto get_ordered_polygon = [](auto polygon) { + order_ccw(polygon); + return polygon; + }; + + const auto polygon1 = get_ordered_polygon(GetParam().polygon1_pts); + const auto polygon2 = get_ordered_polygon(GetParam().polygon2_pts); + const auto expected_intersection = get_ordered_polygon(GetParam().expected_intersection_pts); + + const auto result = + autoware::common::geometry::convex_polygon_intersection2d(polygon1, polygon2); + + ASSERT_EQ(result.size(), expected_intersection.size()); + auto expected_shape_it = expected_intersection.begin(); + for (auto result_it = result.begin(); result_it != result.end(); ++result_it) { + EXPECT_FLOAT_EQ(result_it->x, expected_shape_it->x); + EXPECT_FLOAT_EQ(result_it->y, expected_shape_it->y); + ++expected_shape_it; + } +} + +INSTANTIATE_TEST_SUITE_P( + Basic, IntersectionTest, + ::testing::Values( + IntersectionTestParams{ + {}, + {}, + {} +}, + IntersectionTestParams{ // Partial intersection + {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, + {{5.0F, 5.0F}, {15.0F, 5.0F}, {5.0F, 15.0F}, {15.0F, 15.0F}}, + {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}} +}, + // Full intersection with overlapping edges + // TODO(yunus.caliskan): enable after #1231 +// IntersectionTestParams{ +// {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, +// {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}, +// {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}, +// }, + IntersectionTestParams{ // Fully contained + {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, + {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}, {8.0F, 8.0F}}, + {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}, {8.0F, 8.0F}}, +}, + IntersectionTestParams{ // Fully contained triangle + {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, + {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}}, + {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}}, +}, + IntersectionTestParams{ // Triangle rectangle intersection. + {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, + {{5.0F, 1.0F}, {5.0F, 9.0F}, {15.0F, 5.0F}}, + {{5.0F, 1.0F}, {5.0F, 9.0F}, {10.0F, 3.0F}, {10.0F, 7.0F}} +}, + IntersectionTestParams{ // No intersection + {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, + {{15.0F, 15.0F}, {20.0F, 15.0F}, {15.0F, 20.0F}, {20.0F, 20.0F}}, + {} +} + // cppcheck-suppress syntaxError +)); + +TEST(PolygonPointTest, Basic) { + GTEST_SKIP(); // TODO(yunus.caliskan): enable after #1231 + std::list polygon{{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}; + order_ccw(polygon); + EXPECT_FALSE( + autoware::common::geometry::is_point_inside_polygon_2d( + polygon.begin(), polygon.end(), TestPoint{0.0F, 10.0F})); +} + +// IoU of two intersecting shapes: a pentagon and a square. The test includes pen and paper +// computations for the intermediate steps as assertions. +TEST(IoUTest, PentagonRectangleIntersection) { + std::list polygon1{ + {0.0F, 3.0F}, + {3.0F, 4.0F}, + {6.0F, 3.0F}, + {4.0F, 1.0F}, + {2.0F, 1.0F} + }; + std::list polygon2{ + {3.0F, 0.0F}, + {3.0F, 2.0F}, + {5.0F, 2.0F}, + {5.0F, 0.0F} + }; + + order_ccw(polygon1); + order_ccw(polygon2); + + const auto intersection = + autoware::common::geometry::convex_polygon_intersection2d(polygon1, polygon2); + const auto expected_intersection_area = + autoware::common::geometry::area_2d(intersection.begin(), intersection.end()); + ASSERT_FLOAT_EQ(expected_intersection_area, 1.5F); // Pen & paper proof. + + const auto expected_union_area = + autoware::common::geometry::area_2d(polygon1.begin(), polygon1.end()) + + autoware::common::geometry::area_2d(polygon2.begin(), polygon2.end()) - + expected_intersection_area; + ASSERT_FLOAT_EQ(expected_union_area, (11.0F + 4.0F - 1.5F)); // Pen & paper proof. + + const auto computed_iou = + autoware::common::geometry::convex_intersection_over_union_2d(polygon1, polygon2); + + EXPECT_FLOAT_EQ(computed_iou, expected_intersection_area / expected_union_area); +} + +// IoU of two non-intersecting rectangles. +TEST(IoUTest, NoIntersection) { + std::list polygon1{ + {0.0F, 0.0F}, + {0.0F, 1.0F}, + {1.0F, 1.0F}, + {1.0F, 0.0F} + }; + std::list polygon2{ + {3.0F, 0.0F}, + {3.0F, 2.0F}, + {5.0F, 2.0F}, + {5.0F, 0.0F} + }; + + order_ccw(polygon1); + order_ccw(polygon2); + + EXPECT_FLOAT_EQ( + autoware::common::geometry::convex_intersection_over_union_2d(polygon1, polygon2), 0.0F); +} diff --git a/common/autoware_auto_geometry/test/src/test_interval.cpp b/common/autoware_auto_geometry/test/src/test_interval.cpp new file mode 100644 index 0000000000000..2314159d2ba1c --- /dev/null +++ b/common/autoware_auto_geometry/test/src/test_interval.cpp @@ -0,0 +1,263 @@ +// Copyright 2020 Mapless AI, Inc. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include + +#include +#include +#include + +#include "geometry/interval.hpp" + +using autoware::common::geometry::Interval_d; +using autoware::common::geometry::Interval_f; +using autoware::common::geometry::Interval; + +namespace +{ +const auto Inf = std::numeric_limits::infinity(); +const auto Min = std::numeric_limits::lowest(); +const auto Max = std::numeric_limits::max(); +const auto NaN = std::numeric_limits::quiet_NaN(); +const auto epsilon = 1e-5; +} // namespace + +//------------------------------------------------------------------------------ + +TEST(GeometryInterval, AbsEq) { + const auto i1 = Interval_d(-1.0, 1.0); + const auto i2 = Interval_d(-1.0 + 0.5 * epsilon, 1.0 + 0.5 * epsilon); + const auto shift = (2.0 * epsilon); + const auto i3 = Interval_d(-1.0 + shift, 1.0 + shift); + const auto i_empty = Interval_d(); + + EXPECT_TRUE(Interval_d::abs_eq(i1, i1, epsilon)); + EXPECT_TRUE(Interval_d::abs_eq(i1, i2, epsilon)); + EXPECT_TRUE(Interval_d::abs_eq(i2, i1, epsilon)); + EXPECT_FALSE(Interval_d::abs_eq(i1, i3, epsilon)); + EXPECT_FALSE(Interval_d::abs_eq(i3, i1, epsilon)); + EXPECT_FALSE(Interval_d::abs_eq(i1, i_empty, epsilon)); + EXPECT_FALSE(Interval_d::abs_eq(i_empty, i1, epsilon)); + EXPECT_TRUE(Interval_d::abs_eq(i_empty, i_empty, epsilon)); +} + +//------------------------------------------------------------------------------ + +TEST(GeometryInterval, IsSubsetEq) { + EXPECT_TRUE( + Interval_d::is_subset_eq( + Interval_d(-0.5, 0.5), + Interval_d(-1.0, 1.0))); + EXPECT_TRUE( + Interval_d::is_subset_eq( + Interval_d(3.2, 4.2), + Interval_d(3.2, 4.2))); + EXPECT_FALSE( + Interval_d::is_subset_eq( + Interval_d(-3.0, -1.0), + Interval_d(1.0, 3.0))); + EXPECT_FALSE( + Interval_d::is_subset_eq( + Interval_d(1.0, 3.0), + Interval_d(2.0, 4.0))); + EXPECT_FALSE( + Interval_d::is_subset_eq(Interval_d(), Interval_d())); +} + +//------------------------------------------------------------------------------ + +TEST(GeometryInterval, ClampTo) { + const auto i = Interval_d(-1.0, 1.0); + { + const auto val = 0.0; + const auto p = Interval_d::clamp_to(i, val); + EXPECT_EQ(p, val); + } + + { + const auto val = -3.4; + const auto p = Interval_d::clamp_to(i, val); + EXPECT_EQ(p, Interval_d::min(i)); + } + + { + const auto val = 2.7; + const auto p = Interval_d::clamp_to(i, val); + EXPECT_EQ(p, Interval_d::max(i)); + } + + const auto val = 1.0; + const auto empty_interval = Interval_d(); + const auto projected = Interval_d::clamp_to(empty_interval, val); + EXPECT_TRUE(std::isnan(projected)); +} + +//------------------------------------------------------------------------------ + +TEST(GeometryInterval, Comparisons) { + { + const auto i1 = Interval_d(0.25, 1); + const auto i2 = Interval_d(0, 1); + EXPECT_FALSE((i1 == i2)); + EXPECT_TRUE((i1 != i2)); + } + + { + const auto i1 = Interval_d(-0.25, 0.5); + const auto i2 = Interval_d(0, 1); + EXPECT_FALSE((i1 == i2)); + EXPECT_TRUE((i1 != i2)); + } + + { + const auto i1 = Interval_d(0, 0.5); + const auto i2 = Interval_d(0, 1); + EXPECT_FALSE((i1 == i2)); + EXPECT_TRUE((i1 != i2)); + } + + { + const auto i1 = Interval_d(0, 1); + const auto i2 = Interval_d(0, 1); + EXPECT_TRUE((i1 == i2)); + EXPECT_FALSE((i1 != i2)); + } + + { + const auto i1 = Interval_d(0, 1); + const auto i2 = Interval_d(); + EXPECT_FALSE((i1 == i2)); + EXPECT_TRUE((i1 != i2)); + } + + { + const auto i1 = Interval_d(); + const auto i2 = Interval_d(); + EXPECT_TRUE((i1 == i2)); + EXPECT_FALSE((i1 != i2)); + } +} + +//------------------------------------------------------------------------------ + +TEST(GeometryInterval, Contains) { + { + const auto i = Interval_d(); + EXPECT_FALSE(Interval_d::contains(i, 0.0)); + } + + { + const auto i = Interval_d(-1.0, 1.0); + EXPECT_TRUE(Interval_d::contains(i, 0.0)); + EXPECT_FALSE(Interval_d::contains(i, 2.0)); + } +} + +//------------------------------------------------------------------------------ + +TEST(GeometryInterval, Empty) { + { + const auto i1 = Interval_d(); + const auto i2 = Interval_d(); + const auto i3 = Interval_d::intersect(i1, i2); + EXPECT_TRUE(Interval_d::empty(i3)); + } + + { + const auto i1 = Interval_d(); + const auto i2 = Interval_d(0.0, 1.0); + const auto i3 = Interval_d::intersect(i1, i2); + EXPECT_TRUE(Interval_d::empty(i3)); + } +} + +//------------------------------------------------------------------------------ + +TEST(GeometryInterval, ZeroMeasure) { + { + const auto i = Interval_d(0, 1); + EXPECT_FALSE(Interval_d::zero_measure(i)); + } + + { + const auto i = Interval_d(); + EXPECT_FALSE(Interval_d::zero_measure(i)); + } + + { + const auto i = Interval_d(2, 2); + EXPECT_TRUE(Interval_d::zero_measure(i)); + } +} + +//------------------------------------------------------------------------------ + +TEST(GeometryInterval, IntersectionMeasure) { + { + const auto i1 = Interval_d(-1.0, 1.0); + const auto i2 = Interval_d(-0.5, 1.5); + const auto i = Interval_d::intersect(i1, i2); + EXPECT_EQ(Interval_d::min(i), -0.5); + EXPECT_EQ(Interval_d::max(i), 1.0); + EXPECT_NEAR(Interval_d::measure(i), 1.5, epsilon); + } + + { + const auto i1 = Interval_d(-2.0, -1.0); + const auto i2 = Interval_d(1.0, 2.0); + const auto i = Interval_d::intersect(i1, i2); + EXPECT_TRUE(Interval_d::empty(i)); + EXPECT_TRUE(std::isnan(Interval_d::min(i))); + EXPECT_TRUE(std::isnan(Interval_d::max(i))); + EXPECT_TRUE(std::isnan(Interval_d::measure(i))); + } +} + +//------------------------------------------------------------------------------ + +TEST(GeometryInterval, ConstructionMeasure) { + { + const auto i = Interval_d(); + EXPECT_TRUE(std::isnan(Interval_d::min(i))); + EXPECT_TRUE(std::isnan(Interval_d::max(i))); + EXPECT_TRUE(std::isnan(Interval_d::measure(i))); + } + + { + const auto i = Interval_d(-1.0, 1.0); + EXPECT_EQ(Interval_d::min(i), -1.0); + EXPECT_EQ(Interval_d::max(i), 1.0); + EXPECT_NEAR(Interval_d::measure(i), 2.0, epsilon); + } + + { + const auto i = Interval_d(0.0, 0.0); + EXPECT_EQ(Interval_d::min(i), 0.0); + EXPECT_EQ(Interval_d::max(i), 0.0); + EXPECT_FALSE(Interval_d::empty(i)); + EXPECT_EQ(Interval_d::measure(i), 0.0); + } + + { + EXPECT_THROW({Interval_d(1.0, -1.0);}, std::runtime_error); + } +} + +//------------------------------------------------------------------------------ diff --git a/common/autoware_auto_tf2/CMakeLists.txt b/common/autoware_auto_tf2/CMakeLists.txt new file mode 100755 index 0000000000000..edeeecd61dbe5 --- /dev/null +++ b/common/autoware_auto_tf2/CMakeLists.txt @@ -0,0 +1,46 @@ +# Copyright 2020, The Autoware Foundation. +# +# 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. +cmake_minimum_required(VERSION 3.5) + +### Export headers +project(autoware_auto_tf2) + +## dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + + +### Test +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # Linters + ament_lint_auto_find_test_dependencies() + # Unit test + ament_add_gtest(test_tf2_autoware_auto_msgs test/test_tf2_autoware_auto_msgs.cpp) + autoware_set_compile_options(test_tf2_autoware_auto_msgs) + target_include_directories(test_tf2_autoware_auto_msgs PRIVATE "include") + ament_target_dependencies(test_tf2_autoware_auto_msgs + "autoware_auto_common" + "autoware_auto_perception_msgs" + "autoware_auto_system_msgs" + "autoware_auto_geometry_msgs" + "geometry_msgs" + "orocos_kdl" + "tf2" + "tf2_ros" +) +endif() + +ament_auto_package() diff --git a/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md b/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md new file mode 100644 index 0000000000000..fac3a78b0dbc2 --- /dev/null +++ b/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md @@ -0,0 +1,175 @@ +autoware_auto_tf2 {#autoware-auto-tf2-design} +================= + +This is the design document for the `autoware_auto_tf2` package. + + +# Purpose / Use cases + +In general, users of ROS rely on tf (and its successor, tf2) for publishing and utilizing coordinate +frame transforms. This is true even to the extent that the tf2 contains the packages +`tf2_geometry_msgs` and `tf2_sensor_msgs` which allow for easy conversion to and from the message +types defined in `geometry_msgs` and `sensor_msgs`, respectively. However, AutowareAuto contains +some specialized message types which are not transformable between frames using the ROS2 library. +The `autoware_auto_tf2` package aims to provide developers with tools to transform applicable +`autoware_auto_msgs` types. In addition to this, this package also provides transform tools for +messages types in `geometry_msgs` missing in `tf2_geometry_msgs`. + + +# Design + +While writing `tf2_some_msgs` or contributing to `tf2_geometry_msgs`, compatibility and design +intent was ensured with the following files in the existing tf2 framework: + * `tf2/convert.h` + * `tf2_ros/buffer_interface.h` + +For example: +``` +void tf2::convert( const A & a,B & b) +``` + +The method `tf2::convert` is dependent on the following: +``` +template + B tf2::toMsg(const A& a); +template + void tf2::fromMsg(const A&, B& b); + +// New way to transform instead of using tf2::doTransform() directly +tf2_ros::BufferInterface::transform(...) +``` + +Which, in turn, is dependent on the following: +``` +void tf2::convert( const A & a,B & b) +const std::string& tf2::getFrameId(const T& t) +const ros::Time& tf2::getTimestamp(const T& t); +``` + +## Current Implementation of tf2_geometry_msgs + +In both ROS1 and ROS2 stamped msgs like `Vector3Stamped`, `QuaternionStamped` have associated +functions like: + * `getTimestamp` + * `getFrameId` + * `doTransform` + * `toMsg` + * `fromMsg` + +In ROS1, to support `tf2::convert` and need in `doTransform` of the stamped data, non-stamped +underlying data like `Vector3`, `Point`, have implementations of the following functions: + * `toMsg` + * `fromMsg` + +In ROS2, much of the `doTransform` method is not using `toMsg` and `fromMsg` as data types from tf2 +are not used. Instead `doTransform` is done using `KDL`, thus functions relating to underlying data +were not added; such as `Vector3`, `Point`, or ported in this commit ros/geometry2/commit/6f2a82. +The non-stamped data with `toMsg` and `fromMsg` are `Quaternion`, `Transform`. `Pose` has the +modified `toMsg` and not used by `PoseStamped`. + +## Plan for autoware_auto_perception_msgs::msg::BoundingBoxArray + +The initial rough plan was to implement some of the common tf2 functions like `toMsg`, `fromMsg`, +and `doTransform`, as needed for all the underlying data types in `BoundingBoxArray`. Examples +of the data types include: `BoundingBox`, `Quaternion32`, and `Point32`. In addition, the +implementation should be done such that upstream contributions could also be made to `geometry_msgs`. + +## Assumptions / Known limits + +Due to conflicts in a function signatures, the predefined template of `convert.h`/ +`transform_functions.h` is not followed and compatibility with `tf2::convert(..)` is broken and +`toMsg` is written differently. +``` +// Old style +geometry_msgs::Vector3 toMsg(const tf2::Vector3& in) +geometry_msgs::Point& toMsg(const tf2::Vector3& in) + +// New style +geometry_msgs::Point& toMsg(const tf2::Vector3& in, geometry_msgs::Point& out) +``` + + +## Inputs / Outputs / API + + +The library provides API `doTransform` for the following data-types that are either not available +in `tf2_geometry_msgs` or the messages types are part of `autoware_auto_msgs` and are therefore +custom and not inherently supported by any of the tf2 libraries. The following APIs are provided +for the following data types: + +* `Point32` +``` +inline void doTransform( + const geometry_msgs::msg::Point32 & t_in, + geometry_msgs::msg::Point32 & t_out, + const geometry_msgs::msg::TransformStamped & transform) +``` +* `Quarternion32` (`autoware_auto_msgs`) +``` +inline void doTransform( + const autoware_auto_geometry_msgs::msg::Quaternion32 & t_in, + autoware_auto_geometry_msgs::msg::Quaternion32 & t_out, + const geometry_msgs::msg::TransformStamped & transform) +``` +* `BoundingBox` (`autoware_auto_msgs`) +``` +inline void doTransform( + const BoundingBox & t_in, BoundingBox & t_out, + const geometry_msgs::msg::TransformStamped & transform) +``` +* `BoundingBoxArray` +``` +inline void doTransform( + const BoundingBoxArray & t_in, + BoundingBoxArray & t_out, + const geometry_msgs::msg::TransformStamped & transform) +``` + +In addition, the following helper methods are also added: +* `BoundingBoxArray` +``` +inline tf2::TimePoint getTimestamp(const BoundingBoxArray & t) + +inline std::string getFrameId(const BoundingBoxArray & t) +``` + + + + + + + + + + + + + + + + + + +# Future extensions / Unimplemented parts + +## Challenges + + * `tf2_geometry_msgs` does not implement `doTransform` for any non-stamped datatypes, but it is + possible with the same function template. It is needed when transforming sub-data, with main data + that does have a stamp and can call doTransform on the sub-data with the same transform. Is this a useful upstream contribution? + * `tf2_geometry_msgs` does not have `Point`, `Point32`, does not seem it needs one, also the + implementation of non-standard `toMsg` would not help the convert. + * `BoundingBox` uses 32-bit float like `Quaternion32` and `Point32` to save space, as they are used + repeatedly in `BoundingBoxArray`. While transforming is it better to convert to 64-bit `Quaternion`, + `Point`, or `PoseStamped`, to re-use existing implementation of `doTransform`, or does it need to be + implemented? Templatization may not be simple. + + + + diff --git a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp new file mode 100644 index 0000000000000..c8ce96b1a6eca --- /dev/null +++ b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp @@ -0,0 +1,199 @@ +// Copyright 2020, The Autoware Foundation. +// +// 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. +/// \file +/// \brief This file includes common transform functionality for autoware_auto_msgs + +#ifndef AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ +#define AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +using autoware::common::types::float32_t; +using autoware::common::types::float64_t; +using BoundingBoxArray = autoware_auto_perception_msgs::msg::BoundingBoxArray; +using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox; + +namespace tf2 +{ + + +/*************/ +/** Point32 **/ +/*************/ + +/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Point32 type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The point to transform, as a Point32 message. + * \param t_out The transformed point, as a Point32 message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template<> +inline +void doTransform( + const geometry_msgs::msg::Point32 & t_in, geometry_msgs::msg::Point32 & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + const KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.x, t_in.y, t_in.z); + t_out.x = static_cast(v_out[0]); + t_out.y = static_cast(v_out[1]); + t_out.z = static_cast(v_out[2]); +} + + +/*************/ +/** Polygon **/ +/*************/ + +/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Polygon type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The polygon to transform. + * \param t_out The transformed polygon. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template<> +inline +void doTransform( + const geometry_msgs::msg::Polygon & t_in, geometry_msgs::msg::Polygon & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + // Don't call the Point32 doTransform to avoid doing this conversion every time + const auto kdl_frame = gmTransformToKDL(transform); + // We don't use std::back_inserter to allow aliasing between t_in and t_out + t_out.points.resize(t_in.points.size()); + for (size_t i = 0; i < t_in.points.size(); ++i) { + const KDL::Vector v_out = kdl_frame * KDL::Vector( + t_in.points[i].x, t_in.points[i].y, t_in.points[i].z); + t_out.points[i].x = static_cast(v_out[0]); + t_out.points[i].y = static_cast(v_out[1]); + t_out.points[i].z = static_cast(v_out[2]); + } +} + +/******************/ +/** Quaternion32 **/ +/******************/ + +/** \brief Apply a geometry_msgs TransformStamped to an autoware_auto_msgs Quaternion32 type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The Quaternion32 message to transform. + * \param t_out The transformed Quaternion32 message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template<> +inline +void doTransform( + const autoware_auto_geometry_msgs::msg::Quaternion32 & t_in, + autoware_auto_geometry_msgs::msg::Quaternion32 & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + KDL::Rotation r_in = KDL::Rotation::Quaternion(t_in.x, t_in.y, t_in.z, t_in.w); + KDL::Rotation out = gmTransformToKDL(transform).M * r_in; + + double qx, qy, qz, qw; + out.GetQuaternion(qx, qy, qz, qw); + t_out.x = static_cast(qx); + t_out.y = static_cast(qy); + t_out.z = static_cast(qz); + t_out.w = static_cast(qw); +} + + +/******************/ +/** BoundingBox **/ +/******************/ + +/** \brief Apply a geometry_msgs TransformStamped to an autoware_auto_msgs BoundingBox type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The BoundingBox message to transform. + * \param t_out The transformed BoundingBox message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template<> +inline +void doTransform( + const BoundingBox & t_in, BoundingBox & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + t_out = t_in; + doTransform(t_in.orientation, t_out.orientation, transform); + doTransform(t_in.centroid, t_out.centroid, transform); + doTransform(t_in.corners[0], t_out.corners[0], transform); + doTransform(t_in.corners[1], t_out.corners[1], transform); + doTransform(t_in.corners[2], t_out.corners[2], transform); + doTransform(t_in.corners[3], t_out.corners[3], transform); + // TODO(jitrc): add conversion for other fields of BoundingBox, such as heading, variance, size +} + + +/**********************/ +/** BoundingBoxArray **/ +/**********************/ + +/** \brief Extract a timestamp from the header of a BoundingBoxArray message. + * This function is a specialization of the getTimestamp template defined in tf2/convert.h. + * \param t A timestamped BoundingBoxArray message to extract the timestamp from. + * \return The timestamp of the message. + */ +template<> +inline +tf2::TimePoint getTimestamp(const BoundingBoxArray & t) +{ + return tf2_ros::fromMsg(t.header.stamp); +} + +/** \brief Extract a frame ID from the header of a BoundingBoxArray message. + * This function is a specialization of the getFrameId template defined in tf2/convert.h. + * \param t A timestamped BoundingBoxArray message to extract the frame ID from. + * \return A string containing the frame ID of the message. + */ +template<> +inline +std::string getFrameId(const BoundingBoxArray & t) {return t.header.frame_id;} + +/** \brief Apply a geometry_msgs TransformStamped to an autoware_auto_msgs BoundingBoxArray type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The BoundingBoxArray to transform, as a timestamped BoundingBoxArray message. + * \param t_out The transformed BoundingBoxArray, as a timestamped BoundingBoxArray message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template<> +inline +void doTransform( + const BoundingBoxArray & t_in, + BoundingBoxArray & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + t_out = t_in; + for (auto idx = 0U; idx < t_in.boxes.size(); idx++) { + doTransform(t_out.boxes[idx], t_out.boxes[idx], transform); + } + t_out.header.stamp = transform.header.stamp; + t_out.header.frame_id = transform.header.frame_id; +} + +} // namespace tf2 + +#endif // AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ diff --git a/common/autoware_auto_tf2/package.xml b/common/autoware_auto_tf2/package.xml new file mode 100644 index 0000000000000..27f07c37912d4 --- /dev/null +++ b/common/autoware_auto_tf2/package.xml @@ -0,0 +1,27 @@ + + + + autoware_auto_tf2 + 1.0.0 + Transform related utilites for different msg types + Jit Ray Chowdhury + Apache 2 + + ament_cmake + autoware_auto_cmake + + autoware_auto_perception_msgs + autoware_auto_geometry_msgs + autoware_auto_system_msgs + autoware_auto_common + geometry_msgs + tf2 + tf2_ros + orocos_kdl + + ament_cmake_gtest + + + + ament_cmake + diff --git a/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp b/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp new file mode 100755 index 0000000000000..bf512b4405d57 --- /dev/null +++ b/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp @@ -0,0 +1,325 @@ +// Copyright 2020, The Autoware Foundation. +// +// 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. +/// \file +/// \brief This file includes common transoform functionaly for autoware_auto_msgs + + +#include +#include +#include +#include +#include +#include +#include + +std::unique_ptr tf_buffer = nullptr; +constexpr double EPS = 1e-3; + +geometry_msgs::msg::TransformStamped filled_transfom() +{ + geometry_msgs::msg::TransformStamped t; + t.transform.translation.x = 10; + t.transform.translation.y = 20; + t.transform.translation.z = 30; + t.transform.rotation.w = 0; + t.transform.rotation.x = 1; + t.transform.rotation.y = 0; + t.transform.rotation.z = 0; + t.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); + t.header.frame_id = "A"; + t.child_frame_id = "B"; + + return t; +} + + +TEST(Tf2AutowareAuto, DoTransformPoint32) +{ + const auto trans = filled_transfom(); + geometry_msgs::msg::Point32 p1; + p1.x = 1; + p1.y = 2; + p1.z = 3; + + // doTransform + geometry_msgs::msg::Point32 p_out; + tf2::doTransform(p1, p_out, trans); + + EXPECT_NEAR(p_out.x, 11, EPS); + EXPECT_NEAR(p_out.y, 18, EPS); + EXPECT_NEAR(p_out.z, 27, EPS); +} + + +TEST(Tf2AutowareAuto, DoTransformPolygon) +{ + const auto trans = filled_transfom(); + geometry_msgs::msg::Polygon poly; + geometry_msgs::msg::Point32 p1; + p1.x = 1; + p1.y = 2; + p1.z = 3; + poly.points.push_back(p1); + // doTransform + geometry_msgs::msg::Polygon poly_out; + tf2::doTransform(poly, poly_out, trans); + + EXPECT_NEAR(poly_out.points[0].x, 11, EPS); + EXPECT_NEAR(poly_out.points[0].y, 18, EPS); + EXPECT_NEAR(poly_out.points[0].z, 27, EPS); +} + + +TEST(Tf2AutowareAuto, DoTransformQuaternion32) +{ + const auto trans = filled_transfom(); + autoware_auto_geometry_msgs::msg::Quaternion32 q1; + q1.w = 0; + q1.x = 0; + q1.y = 0; + q1.z = 1; + + // doTransform + autoware_auto_geometry_msgs::msg::Quaternion32 q_out; + tf2::doTransform(q1, q_out, trans); + + EXPECT_NEAR(q_out.x, 0.0, EPS); + EXPECT_NEAR(q_out.y, 1.0, EPS); + EXPECT_NEAR(q_out.z, 0.0, EPS); + EXPECT_NEAR(q_out.w, 0.0, EPS); +} + + +TEST(Tf2AutowareAuto, DoTransformBoundingBox) +{ + const auto trans = filled_transfom(); + BoundingBox bb1; + bb1.orientation.w = 0; + bb1.orientation.x = 0; + bb1.orientation.y = 0; + bb1.orientation.z = 1; + bb1.centroid.x = 1; + bb1.centroid.y = 2; + bb1.centroid.z = 3; + bb1.corners[0].x = 4; + bb1.corners[0].y = 5; + bb1.corners[0].z = 6; + bb1.corners[1].x = 7; + bb1.corners[1].y = 8; + bb1.corners[1].z = 9; + bb1.corners[2].x = 10; + bb1.corners[2].y = 11; + bb1.corners[2].z = 12; + bb1.corners[3].x = 13; + bb1.corners[3].y = 14; + bb1.corners[3].z = 15; + + // doTransform + BoundingBox bb_out; + tf2::doTransform(bb1, bb_out, trans); + + EXPECT_NEAR(bb_out.orientation.x, 0.0, EPS); + EXPECT_NEAR(bb_out.orientation.y, 1.0, EPS); + EXPECT_NEAR(bb_out.orientation.z, 0.0, EPS); + EXPECT_NEAR(bb_out.orientation.w, 0.0, EPS); + EXPECT_NEAR(bb_out.centroid.x, 11, EPS); + EXPECT_NEAR(bb_out.centroid.y, 18, EPS); + EXPECT_NEAR(bb_out.centroid.z, 27, EPS); + EXPECT_NEAR(bb_out.corners[0].x, 14, EPS); + EXPECT_NEAR(bb_out.corners[0].y, 15, EPS); + EXPECT_NEAR(bb_out.corners[0].z, 24, EPS); + EXPECT_NEAR(bb_out.corners[1].x, 17, EPS); + EXPECT_NEAR(bb_out.corners[1].y, 12, EPS); + EXPECT_NEAR(bb_out.corners[1].z, 21, EPS); + EXPECT_NEAR(bb_out.corners[2].x, 20, EPS); + EXPECT_NEAR(bb_out.corners[2].y, 9, EPS); + EXPECT_NEAR(bb_out.corners[2].z, 18, EPS); + EXPECT_NEAR(bb_out.corners[3].x, 23, EPS); + EXPECT_NEAR(bb_out.corners[3].y, 6, EPS); + EXPECT_NEAR(bb_out.corners[3].z, 15, EPS); + + // Testing unused fields are unmodified + EXPECT_EQ(bb_out.size, bb1.size); + EXPECT_EQ(bb_out.heading, bb1.heading); + EXPECT_EQ(bb_out.heading_rate, bb1.heading_rate); + EXPECT_EQ(bb_out.variance, bb1.variance); + EXPECT_EQ(bb_out.vehicle_label, bb1.vehicle_label); + EXPECT_EQ(bb_out.signal_label, bb1.signal_label); + EXPECT_EQ(bb_out.class_likelihood, bb1.class_likelihood); +} + +TEST(Tf2AutowareAuto, TransformBoundingBoxArray) +{ + BoundingBox bb1; + bb1.orientation.w = 0; + bb1.orientation.x = 0; + bb1.orientation.y = 0; + bb1.orientation.z = 1; + bb1.centroid.x = 20; + bb1.centroid.y = 21; + bb1.centroid.z = 22; + bb1.corners[0].x = 23; + bb1.corners[0].y = 24; + bb1.corners[0].z = 25; + bb1.corners[1].x = 26; + bb1.corners[1].y = 27; + bb1.corners[1].z = 28; + bb1.corners[2].x = 29; + bb1.corners[2].y = 30; + bb1.corners[2].z = 31; + bb1.corners[3].x = 32; + bb1.corners[3].y = 33; + bb1.corners[3].z = 34; + + BoundingBox bb2; + bb2.orientation.w = 0.707f; + bb2.orientation.x = -0.706f; + bb2.orientation.y = 0; + bb2.orientation.z = 0; + bb2.centroid.x = 50; + bb2.centroid.y = 51; + bb2.centroid.z = 52; + bb2.corners[0].x = 53; + bb2.corners[0].y = 54; + bb2.corners[0].z = 55; + bb2.corners[1].x = 56; + bb2.corners[1].y = 57; + bb2.corners[1].z = 58; + bb2.corners[2].x = 59; + bb2.corners[2].y = 50; + bb2.corners[2].z = 51; + bb2.corners[3].x = 52; + bb2.corners[3].y = 53; + bb2.corners[3].z = 54; + + BoundingBoxArray bba1; + bba1.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); + bba1.header.frame_id = "A"; + bba1.boxes.push_back(bb1); + bba1.boxes.push_back(bb2); + + // simple api + const auto bba_simple = tf_buffer->transform(bba1, "B", tf2::durationFromSec(2.0)); + + + EXPECT_EQ(bba_simple.header.frame_id, "B"); + + // checking boxes[0] + EXPECT_NEAR(bba_simple.boxes[0].orientation.x, 0.0, EPS); + EXPECT_NEAR(bba_simple.boxes[0].orientation.y, 1.0, EPS); + EXPECT_NEAR(bba_simple.boxes[0].orientation.z, 0.0, EPS); + EXPECT_NEAR(bba_simple.boxes[0].orientation.w, 0.0, EPS); + EXPECT_NEAR(bba_simple.boxes[0].centroid.x, 10, EPS); + EXPECT_NEAR(bba_simple.boxes[0].centroid.y, -1, EPS); + EXPECT_NEAR(bba_simple.boxes[0].centroid.z, 8, EPS); + EXPECT_NEAR(bba_simple.boxes[0].corners[0].x, 13, EPS); + EXPECT_NEAR(bba_simple.boxes[0].corners[0].y, -4, EPS); + EXPECT_NEAR(bba_simple.boxes[0].corners[0].z, 5, EPS); + EXPECT_NEAR(bba_simple.boxes[0].corners[1].x, 16, EPS); + EXPECT_NEAR(bba_simple.boxes[0].corners[1].y, -7, EPS); + EXPECT_NEAR(bba_simple.boxes[0].corners[1].z, 2, EPS); + EXPECT_NEAR(bba_simple.boxes[0].corners[2].x, 19, EPS); + EXPECT_NEAR(bba_simple.boxes[0].corners[2].y, -10, EPS); + EXPECT_NEAR(bba_simple.boxes[0].corners[2].z, -1, EPS); + EXPECT_NEAR(bba_simple.boxes[0].corners[3].x, 22, EPS); + EXPECT_NEAR(bba_simple.boxes[0].corners[3].y, -13, EPS); + EXPECT_NEAR(bba_simple.boxes[0].corners[3].z, -4, EPS); + + // checking boxes[1] + EXPECT_NEAR(bba_simple.boxes[1].orientation.x, 0.707, EPS); + EXPECT_NEAR(bba_simple.boxes[1].orientation.y, 0.0, EPS); + EXPECT_NEAR(bba_simple.boxes[1].orientation.z, 0.0, EPS); + EXPECT_NEAR(bba_simple.boxes[1].orientation.w, 0.707, EPS); + EXPECT_NEAR(bba_simple.boxes[1].centroid.x, 40, EPS); + EXPECT_NEAR(bba_simple.boxes[1].centroid.y, -31, EPS); + EXPECT_NEAR(bba_simple.boxes[1].centroid.z, -22, EPS); + EXPECT_NEAR(bba_simple.boxes[1].corners[0].x, 43, EPS); + EXPECT_NEAR(bba_simple.boxes[1].corners[0].y, -34, EPS); + EXPECT_NEAR(bba_simple.boxes[1].corners[0].z, -25, EPS); + EXPECT_NEAR(bba_simple.boxes[1].corners[1].x, 46, EPS); + EXPECT_NEAR(bba_simple.boxes[1].corners[1].y, -37, EPS); + EXPECT_NEAR(bba_simple.boxes[1].corners[1].z, -28, EPS); + EXPECT_NEAR(bba_simple.boxes[1].corners[2].x, 49, EPS); + EXPECT_NEAR(bba_simple.boxes[1].corners[2].y, -30, EPS); + EXPECT_NEAR(bba_simple.boxes[1].corners[2].z, -21, EPS); + EXPECT_NEAR(bba_simple.boxes[1].corners[3].x, 42, EPS); + EXPECT_NEAR(bba_simple.boxes[1].corners[3].y, -33, EPS); + EXPECT_NEAR(bba_simple.boxes[1].corners[3].z, -24, EPS); + + + // advanced api + const auto bba_advanced = tf_buffer->transform( + bba1, "B", + tf2::timeFromSec(2.0), "A", tf2::durationFromSec(3.0)); + + EXPECT_EQ(bba_advanced.header.frame_id, "B"); + + // checking boxes[0] + EXPECT_NEAR(bba_advanced.boxes[0].orientation.x, 0.0, EPS); + EXPECT_NEAR(bba_advanced.boxes[0].orientation.y, 1.0, EPS); + EXPECT_NEAR(bba_advanced.boxes[0].orientation.z, 0.0, EPS); + EXPECT_NEAR(bba_advanced.boxes[0].orientation.w, 0.0, EPS); + EXPECT_NEAR(bba_advanced.boxes[0].centroid.x, 10, EPS); + EXPECT_NEAR(bba_advanced.boxes[0].centroid.y, -1, EPS); + EXPECT_NEAR(bba_advanced.boxes[0].centroid.z, 8, EPS); + EXPECT_NEAR(bba_advanced.boxes[0].corners[0].x, 13, EPS); + EXPECT_NEAR(bba_advanced.boxes[0].corners[0].y, -4, EPS); + EXPECT_NEAR(bba_advanced.boxes[0].corners[0].z, 5, EPS); + EXPECT_NEAR(bba_advanced.boxes[0].corners[1].x, 16, EPS); + EXPECT_NEAR(bba_advanced.boxes[0].corners[1].y, -7, EPS); + EXPECT_NEAR(bba_advanced.boxes[0].corners[1].z, 2, EPS); + EXPECT_NEAR(bba_advanced.boxes[0].corners[2].x, 19, EPS); + EXPECT_NEAR(bba_advanced.boxes[0].corners[2].y, -10, EPS); + EXPECT_NEAR(bba_advanced.boxes[0].corners[2].z, -1, EPS); + EXPECT_NEAR(bba_advanced.boxes[0].corners[3].x, 22, EPS); + EXPECT_NEAR(bba_advanced.boxes[0].corners[3].y, -13, EPS); + EXPECT_NEAR(bba_advanced.boxes[0].corners[3].z, -4, EPS); + + // checking boxes[1] + EXPECT_NEAR(bba_advanced.boxes[1].orientation.x, 0.707, EPS); + EXPECT_NEAR(bba_advanced.boxes[1].orientation.y, 0.0, EPS); + EXPECT_NEAR(bba_advanced.boxes[1].orientation.z, 0.0, EPS); + EXPECT_NEAR(bba_advanced.boxes[1].orientation.w, 0.707, EPS); + EXPECT_NEAR(bba_advanced.boxes[1].centroid.x, 40, EPS); + EXPECT_NEAR(bba_advanced.boxes[1].centroid.y, -31, EPS); + EXPECT_NEAR(bba_advanced.boxes[1].centroid.z, -22, EPS); + EXPECT_NEAR(bba_advanced.boxes[1].corners[0].x, 43, EPS); + EXPECT_NEAR(bba_advanced.boxes[1].corners[0].y, -34, EPS); + EXPECT_NEAR(bba_advanced.boxes[1].corners[0].z, -25, EPS); + EXPECT_NEAR(bba_advanced.boxes[1].corners[1].x, 46, EPS); + EXPECT_NEAR(bba_advanced.boxes[1].corners[1].y, -37, EPS); + EXPECT_NEAR(bba_advanced.boxes[1].corners[1].z, -28, EPS); + EXPECT_NEAR(bba_advanced.boxes[1].corners[2].x, 49, EPS); + EXPECT_NEAR(bba_advanced.boxes[1].corners[2].y, -30, EPS); + EXPECT_NEAR(bba_advanced.boxes[1].corners[2].z, -21, EPS); + EXPECT_NEAR(bba_advanced.boxes[1].corners[3].x, 42, EPS); + EXPECT_NEAR(bba_advanced.boxes[1].corners[3].y, -33, EPS); + EXPECT_NEAR(bba_advanced.boxes[1].corners[3].z, -24, EPS); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf_buffer = std::make_unique(clock); + tf_buffer->setUsingDedicatedThread(true); + + // populate buffer + const geometry_msgs::msg::TransformStamped t = filled_transfom(); + tf_buffer->setTransform(t, "test"); + + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/common/autoware_testing/CMakeLists.txt b/common/autoware_testing/CMakeLists.txt new file mode 100644 index 0000000000000..7908a469b35f3 --- /dev/null +++ b/common/autoware_testing/CMakeLists.txt @@ -0,0 +1,33 @@ +# Copyright 2021 the Autoware Foundation +# +# 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. + +cmake_minimum_required(VERSION 3.5) + +project(autoware_testing) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + + +list(APPEND ${PROJECT_NAME}_CONFIG_EXTRAS + "autoware_testing-extras.cmake" +) + +if(BUILD_TESTING) + ament_lint_cmake(${CMAKE_CURRENT_SOURCE_DIR}) +endif() + +ament_auto_package( + INSTALL_TO_SHARE cmake autoware_testing +) diff --git a/common/autoware_testing/autoware_testing-extras.cmake b/common/autoware_testing/autoware_testing-extras.cmake new file mode 100644 index 0000000000000..cf708f7255a4d --- /dev/null +++ b/common/autoware_testing/autoware_testing-extras.cmake @@ -0,0 +1,17 @@ +# Copyright 2021 The Autoware Foundation +# +# 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. + +find_package(ros_testing REQUIRED) + +include("${autoware_testing_DIR}/add_smoke_test.cmake") diff --git a/common/autoware_testing/autoware_testing/__init__.py b/common/autoware_testing/autoware_testing/__init__.py new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/common/autoware_testing/autoware_testing/smoke_test.py b/common/autoware_testing/autoware_testing/smoke_test.py new file mode 100644 index 0000000000000..89892e85dd824 --- /dev/null +++ b/common/autoware_testing/autoware_testing/smoke_test.py @@ -0,0 +1,89 @@ +# Copyright 2021 the Autoware Foundation +# +# 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. +# +# Developed by Robotec.ai. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node +import launch_testing + +import os +import pytest +import unittest +import shlex + + +def resolve_node(context, *args, **kwargs): + + smoke_test_node = Node( + package=LaunchConfiguration('arg_package'), + executable=LaunchConfiguration('arg_package_exe'), + namespace='test', + parameters=[ + os.path.join( + get_package_share_directory(LaunchConfiguration('arg_package').perform(context)), + "param", + LaunchConfiguration('arg_param_filename').perform(context) + ) + ], + arguments=shlex.split(LaunchConfiguration('arg_executable_arguments').perform(context)) + ) + return [smoke_test_node] + + +@pytest.mark.launch_test +def generate_test_description(): + + arg_package = DeclareLaunchArgument( + 'arg_package', + default_value=['default'], + description='Package containing tested executable' + ) + arg_package_exe = DeclareLaunchArgument( + 'arg_package_exe', + default_value=['default'], + description='Tested executable' + ) + arg_param_filename = DeclareLaunchArgument( + 'arg_param_filename', + default_value=['test.param.yaml'], + description='Test param file' + ) + arg_executable_arguments = DeclareLaunchArgument( + 'arg_executable_arguments', + default_value=[''], + description='Tested executable arguments' + ) + + return LaunchDescription([ + arg_package, + arg_package_exe, + arg_param_filename, + arg_executable_arguments, + OpaqueFunction(function=resolve_node), + launch_testing.actions.ReadyToTest()] + ) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + + def test_exit_code(self, proc_output, proc_info): + # Check that process exits with code -15 code: termination request, sent to the program + launch_testing.asserts.assertExitCodes(proc_info, [-15]) diff --git a/common/autoware_testing/cmake/add_smoke_test.cmake b/common/autoware_testing/cmake/add_smoke_test.cmake new file mode 100644 index 0000000000000..74a072f55b46f --- /dev/null +++ b/common/autoware_testing/cmake/add_smoke_test.cmake @@ -0,0 +1,46 @@ +# Copyright 2021 The Autoware Foundation +# +# 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. +# +# Developed by Robotec.ai + +# Add a smoke test +# :param package_name: name of the package to smoke test +# :type package_name: string +# :param package_exec: package executable to run during smoke test +# :type executable_name: string +# :param PARAM_FILENAME: yaml filename containing test parameters +# :type PARAM_FILENAME: string +# :param EXECUTABLE_ARGUMENTS: arguments passed to tested executable +# :type EXECUTABLE_ARGUMENTS: string + +function(add_smoke_test package_name executable_name) + cmake_parse_arguments(PARSE_ARGV 2 smoke_test "" "PARAM_FILENAME;EXECUTABLE_ARGUMENTS" "") + + set(ARGUMENTS "arg_package:=${package_name}" "arg_package_exe:=${executable_name}") + + if(smoke_test_PARAM_FILENAME) + list(APPEND ARGUMENTS "arg_param_filename:=${smoke_test_PARAM_FILENAME}") + endif() + + if(smoke_test_EXECUTABLE_ARGUMENTS) + list(APPEND ARGUMENTS "arg_executable_arguments:=${smoke_test_EXECUTABLE_ARGUMENTS}") + endif() + + add_ros_test( + ${autoware_testing_DIR}/../autoware_testing/smoke_test.py + TARGET "${executable_name}_smoke_test" + ARGS "${ARGUMENTS}" + TIMEOUT "30" + ) +endfunction() diff --git a/common/autoware_testing/design/autoware_testing-design.md b/common/autoware_testing/design/autoware_testing-design.md new file mode 100644 index 0000000000000..fffbb3aa1341a --- /dev/null +++ b/common/autoware_testing/design/autoware_testing-design.md @@ -0,0 +1,65 @@ +autoware_testing {#autoware_testing-package-design} +=========== + +This is the design document for the `autoware_testing` package. + +# Purpose / Use cases + +The package aims to provide a unified way to add standard testing functionality to the package, currently supporting: +- Smoke testing (`add_smoke_test`): launch a node with default configuration and ensure that it starts up and does not crash. + +# Design + +Uses `ros_testing` (which is an extension of `launch_testing`) and provides some parametrized, reusable standard tests to run. + +## Assumptions / Known limits + +Parametrization is limited to package, executable names, parameters filename and executable arguments. Test namespace is set as 'test'. +Parameters file for the package is expected to be in `param` directory inside package. + +## Inputs / Outputs / API + +To add a smoke test to your package tests, add test dependency on `autoware_testing` to `package.xml` + +```{xml} +autoware_testing +``` + +and add the following two lines to `CMakeLists.txt` in the `IF (BUILD_TESTING)` section: + +```{cmake} +find_package(autoware_testing REQUIRED) +add_smoke_test( [PARAM_FILENAME ] [EXECUTABLE_ARGUMENTS ]) +``` + +Where + +`` - [required] tested node package name. + +`` - [required] tested node executable name. + +`` - [optional] param filename. Default value is `test.param.yaml`. Required mostly in situation where there are multiple smoke tests in a package and each requires different paramteres set + +`` - [optional] arguments passed to executable. By default no arguments are passed. + +which adds `_smoke_test` test to suite. + +Example test result: + +``` +build//test_results//_smoke_test.xunit.xml: 1 test, 0 errors, 0 failures, 0 skipped + +``` + +# References / External links +- https://en.wikipedia.org/wiki/Smoke_testing_(software) +- https://github.com/ros2/ros_testing +- https://github.com/ros2/launch/blob/master/launch_testing + +# Future extensions / Unimplemented parts + +- Adding more types of standard tests. + +# Related issues +- Issue #700: add smoke test +- Issue #1224: Port other packages with smoke tests to use `autoware_testing` diff --git a/common/autoware_testing/package.xml b/common/autoware_testing/package.xml new file mode 100644 index 0000000000000..d5d7720dbbea5 --- /dev/null +++ b/common/autoware_testing/package.xml @@ -0,0 +1,22 @@ + + + + autoware_testing + 0.1.0 + Tools for handling standard tests based on ros_testing + Adam Dabrowski + Apache 2.0 + + ament_cmake_auto + ament_cmake_lint_cmake + + ros_testing + ament_copyright + ament_flake8 + ament_pep257 + ament_cmake_core + + + ament_cmake + + diff --git a/common/autoware_testing/resource/autoware_testing b/common/autoware_testing/resource/autoware_testing new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/common/autoware_testing/setup.cfg b/common/autoware_testing/setup.cfg new file mode 100644 index 0000000000000..e160d885afb8a --- /dev/null +++ b/common/autoware_testing/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/autoware_testing +[install] +install-scripts=$base/lib/autoware_testing diff --git a/common/autoware_testing/setup.py b/common/autoware_testing/setup.py new file mode 100644 index 0000000000000..16f2c1a6042a9 --- /dev/null +++ b/common/autoware_testing/setup.py @@ -0,0 +1,25 @@ +from setuptools import setup + +package_name = 'autoware_testing' + +setup( + name=package_name, + version='0.1.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Adam Dabrowski', + maintainer_email='adam.dabrowski@robotec.ai', + description='Tools for handling standard tests based on ros_testing', + license='Apache 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/common/fake_test_node/CMakeLists.txt b/common/fake_test_node/CMakeLists.txt new file mode 100644 index 0000000000000..71c5ebe547962 --- /dev/null +++ b/common/fake_test_node/CMakeLists.txt @@ -0,0 +1,41 @@ +# Copyright 2021 Apex.AI, 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. +# +# Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +cmake_minimum_required(VERSION 3.5) +project(fake_test_node) + +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +ament_auto_add_library(fake_test_node SHARED src/fake_test_node.cpp) +autoware_set_compile_options(fake_test_node) + +## Testing +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_fake_test_node + test/test_fake_test_node.cpp + ) + autoware_set_compile_options(test_fake_test_node) + add_dependencies(test_fake_test_node fake_test_node) + target_link_libraries(test_fake_test_node fake_test_node) +endif() + +ament_auto_package() diff --git a/common/fake_test_node/design/fake_test_node-design.md b/common/fake_test_node/design/fake_test_node-design.md new file mode 100644 index 0000000000000..afd4d1e6b3ae5 --- /dev/null +++ b/common/fake_test_node/design/fake_test_node-design.md @@ -0,0 +1,66 @@ +Fake Test Node {#fake-test-node-design} +===================== + +# What this package provides + +When writing an integration test for a node in C++ using GTest, there is quite some boilerplate code +that needs to be written to set up a fake node that would publish expected messages on an expected +topic and subscribes to messages on some other topic. This is usually implemented as a custom GTest +fixture. + +This package contains a library that introduces two utility classes that can be used in place of +custom fixtures described above to write integration tests for a node: +- `autoware::tools::testing::FakeTestNode` - to use as a custom test fixture with `TEST_F` tests +- `autoware::tools::testing::FakeTestNodeParametrized` - to use a custom test fixture with the + parametrized `TEST_P` tests (accepts a template parameter that gets forwarded to + `testing::TestWithParam`) + +These fixtures take care of initializing and re-initializing rclcpp as well as of checking that all +subscribers and publishers have a match, thus reducing the amount of boilerplate code that the user +needs to write. + +# How to use this library +After including the relevant header the user can use a typedef to use a custom fixture name and use +the provided classes as fixtures in `TEST_F` and `TEST_P` tests directly. + +## Example usage +Let's say there is a node `NodeUnderTest` that requires testing. It just +subscribes to `std_msgs::msg::Int32` messages and publishes a +`std_msgs::msg::Bool` to indicate that the input is positive. To test such a +node the following code can be used utilizing the +`autoware::tools::testing::FakeTestNode`: + +```cpp +using FakeNodeFixture = autoware::tools::testing::FakeTestNode; + +/// @test Test that we can use a non-parametrized test. +TEST_F(FakeNodeFixture, Test) { + Int32 msg{}; + msg.data = 15; + const auto node = std::make_shared(); + + Bool::SharedPtr last_received_msg{}; + auto fake_odom_publisher = create_publisher("/input_topic"); + auto result_odom_subscription = create_subscription("/output_topic", *node, + [&last_received_msg](const Bool::SharedPtr msg) {last_received_msg = msg;}); + + const auto dt{std::chrono::milliseconds{100LL}}; + const auto max_wait_time{std::chrono::seconds{10LL}}; + auto time_passed{std::chrono::milliseconds{0LL}}; + while (!last_received_msg) { + fake_odom_publisher->publish(msg); + rclcpp::spin_some(node); + rclcpp::spin_some(get_fake_node()); + std::this_thread::sleep_for(dt); + time_passed += dt; + if (time_passed > max_wait_time) { + FAIL() << "Did not receive a message soon enough."; + } + } + EXPECT_TRUE(last_received_msg->data); + SUCCEED(); +} +``` + +Here only the `TEST_F` example is shown but a `TEST_P` usage is very similar with a little bit more +boilerplate to set up all the parameter values, see `test_fake_test_node.cpp` for an example usage. diff --git a/common/fake_test_node/include/fake_test_node/fake_test_node.hpp b/common/fake_test_node/include/fake_test_node/fake_test_node.hpp new file mode 100644 index 0000000000000..e1346d0b55dd7 --- /dev/null +++ b/common/fake_test_node/include/fake_test_node/fake_test_node.hpp @@ -0,0 +1,253 @@ +// Copyright 2021 Apex.AI, 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +/// \copyright Copyright 2021 Apex.AI, Inc. +/// All rights reserved. + +#ifndef FAKE_TEST_NODE__FAKE_TEST_NODE_HPP_ +#define FAKE_TEST_NODE__FAKE_TEST_NODE_HPP_ + + +#include +#include + +#include + +#include + +#include +#include +#include + +namespace autoware +{ +namespace tools +{ +namespace testing +{ + +/// +/// @brief Forward definition of the parametrized fake node to be used with `TEST_P`. +/// +/// @tparam T Type of the parameter as forwarded to testing::TestWithParam. +/// +template +class FAKE_TEST_NODE_PUBLIC FakeTestNodeParametrized; + +/// +/// @brief A forward definition of the non-parametrized fake node to be used with `TEST_F`. +/// +class FAKE_TEST_NODE_PUBLIC FakeTestNode; + +namespace detail +{ + +/// +/// @brief This class defines a test fake node parametrized with the parameter type. +/// +/// @details The user must typedef this node to a new name and use that new name in their +/// `TEST_P` (with parameter type provided) or `TEST_F` (with no parameter type +/// provided) entries. It takes care of the boilerplate creation of a node and whenever +/// a publisher or a subscription are created it checks that they have a matching +/// subscription or publisher respectively on the same topic. +/// +/// @note This class should not be used by the user, but FakeTestNode and +/// FakeTestNodeParametrized should be used instead. +/// +class FAKE_TEST_NODE_PUBLIC FakeNodeCore +{ +public: + /// + /// @brief Initialize rclcpp, tf and setup the fake node. + /// + /// @param[in] test_name The name of the currently run test, usually received from the fixture. + /// + void set_up(const std::string & test_name); + + /// + /// @brief Shuts down rclcpp. + /// + void tear_down(); + + /// + /// @brief Create a publisher with a custom topic and message type. + /// + /// @details This does not do much more than create the relevant publisher and check that there + /// is a subscription with the relevant topic being requested. + /// + /// @param[in] topic The topic + /// @param[in] timeout The timeout for matching to a subscription + /// @param[in] qos The QoS profile for the publisher handler + /// + /// @tparam MsgT Type of messages to publish. + /// + /// @throws runtime_error If no matching subscriber for `topic` found within `timeout` + /// + /// @return A publisher pointer; + /// + template + typename rclcpp::Publisher::SharedPtr create_publisher( + const std::string & topic, + const std::chrono::milliseconds & timeout = std::chrono::seconds{10LL}, + const rclcpp::QoS & qos = rclcpp::QoS(rclcpp::KeepLast(10))) + { + // Set the QoS profile history depth + typename rclcpp::Publisher::SharedPtr publisher = + m_fake_node->create_publisher(topic, qos); + + std::chrono::milliseconds spent_time{0LL}; + std::chrono::milliseconds dt{100LL}; + while (m_fake_node->count_subscribers(topic) < 1) { + spent_time += dt; + if (spent_time > timeout) { + throw std::runtime_error( + std::string( + "No matching subscriber to the mock topic '") + topic + "' that we publish"); + } + std::this_thread::sleep_for(dt); + } + return publisher; + } + + /// + /// @brief Creates a subscription to a certain message type for a given node. + /// + /// @param[in] topic The topic to subscribe to + /// @param[in] publishing_node The node that publishes the data that this subscription + /// subscribes to. This function will check that the newly created + /// subscription matches a publisher in this node. + /// @param[in] callback The callback to be called by the subscription + /// @param[in] timeout The timeout for matching to a publisher + /// @param[in] qos The QoS profile for the subscription handler + /// + /// @tparam MsgT Message type to which this must subscribe + /// @tparam NodeT The type of the node under test + /// + /// @throws runtime_error If no matching publisher for `topic` is found within `timeout` + /// + /// @return Returns a subscription pointer. + /// + template + typename rclcpp::Subscription::SharedPtr create_subscription( + const std::string & topic, + const NodeT & publishing_node, + std::function callback, + const std::chrono::milliseconds & timeout = std::chrono::seconds{10LL}, + const rclcpp::QoS & qos = rclcpp::QoS(rclcpp::KeepLast(10))) + { + auto subscription = m_fake_node->create_subscription(topic, qos, callback); + std::chrono::milliseconds spent_time{0LL}; + std::chrono::milliseconds dt{100LL}; + while (publishing_node.count_publishers(topic) < 1) { + spent_time += dt; + if (spent_time > timeout) { + throw std::runtime_error( + std::string( + "The node under test '") + publishing_node.get_name() + + "' is not publishing the topic '" + topic + + "' that we listen to."); + } + std::this_thread::sleep_for(dt); + } + return subscription; + } + + /// + /// @brief Gets the mock node. + /// + /// @return The mock node. + /// + inline rclcpp::Node::SharedPtr get_fake_node() {return m_fake_node;} + + /// + /// @brief Gets the tf buffer. + /// + /// @return The tf buffer. + /// + inline tf2::BufferCore & get_tf_buffer() {return m_tf_buffer;} + +private: + std::shared_ptr m_fake_node{nullptr}; + std::shared_ptr m_tf_listener{nullptr}; + tf2::BufferCore m_tf_buffer; +}; + +/// +/// @brief A utility function that gets the full test name from the current fixture instance. +/// +/// @param[in] info The current test information +/// +/// @return The full test name. +/// +FAKE_TEST_NODE_PUBLIC std::string get_test_name(const ::testing::TestInfo * info); + +} // namespace detail + + +/// +/// @brief This class describes a fake test node that inherits from the parametrized GTest +/// fixture. +/// +/// @tparam T Type of parameter to be used in the fixture. +/// +template +class FAKE_TEST_NODE_PUBLIC FakeTestNodeParametrized + : public detail::FakeNodeCore, public ::testing::TestWithParam +{ + using FakeNodeCore::FakeNodeCore; + +public: + /// + /// @brief Override the setup function of the fixture. + /// + void SetUp() override + { + set_up(detail::get_test_name(::testing::UnitTest::GetInstance()->current_test_info())); + } + + /// + /// @brief Override the tear down function of the fixture. + /// + void TearDown() override {tear_down();} +}; + +/// +/// @brief This class describes a fake test node that inherits from the parametrized GTest +/// fixture. +/// +class FAKE_TEST_NODE_PUBLIC FakeTestNode + : public detail::FakeNodeCore, public ::testing::Test +{ + using FakeNodeCore::FakeNodeCore; + +public: + /// + /// @brief Override the setup function of the fixture. + /// + void SetUp() override; + + /// + /// @brief Override the tear down function of the fixture. + /// + void TearDown() override; +}; + + +} // namespace testing +} // namespace tools +} // namespace autoware + +#endif // FAKE_TEST_NODE__FAKE_TEST_NODE_HPP_ diff --git a/common/fake_test_node/include/fake_test_node/visibility_control.hpp b/common/fake_test_node/include/fake_test_node/visibility_control.hpp new file mode 100644 index 0000000000000..c8d60d0b17e90 --- /dev/null +++ b/common/fake_test_node/include/fake_test_node/visibility_control.hpp @@ -0,0 +1,42 @@ +// Copyright 2021 Apex.AI, 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +/// \copyright Copyright 2021 Apex.AI, Inc. +/// All rights reserved. + +#ifndef FAKE_TEST_NODE__VISIBILITY_CONTROL_HPP_ +#define FAKE_TEST_NODE__VISIBILITY_CONTROL_HPP_ + +//////////////////////////////////////////////////////////////////////////////// +#if defined(__WIN32) + #if defined(FAKE_TEST_NODE_BUILDING_DLL) || defined(FAKE_TEST_NODE_EXPORTS) + #define FAKE_TEST_NODE_PUBLIC __declspec(dllexport) + #define FAKE_TEST_NODE_LOCAL + #else // defined(FAKE_TEST_NODE_BUILDING_DLL) || defined(FAKE_TEST_NODE_EXPORTS) + #define FAKE_TEST_NODE_PUBLIC __declspec(dllimport) + #define FAKE_TEST_NODE_LOCAL + #endif // defined(FAKE_TEST_NODE_BUILDING_DLL) || defined(FAKE_TEST_NODE_EXPORTS) +#elif defined(__linux__) + #define FAKE_TEST_NODE_PUBLIC __attribute__((visibility("default"))) + #define FAKE_TEST_NODE_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) + #define FAKE_TEST_NODE_PUBLIC __attribute__((visibility("default"))) + #define FAKE_TEST_NODE_LOCAL __attribute__((visibility("hidden"))) +#else // defined(__linux__) + #error "Unsupported Build Configuration" +#endif // defined(__WIN32) + +#endif // FAKE_TEST_NODE__VISIBILITY_CONTROL_HPP_ diff --git a/common/fake_test_node/package.xml b/common/fake_test_node/package.xml new file mode 100644 index 0000000000000..4ce6fc2ddc5c0 --- /dev/null +++ b/common/fake_test_node/package.xml @@ -0,0 +1,25 @@ + + + + fake_test_node + 1.0.0 + A fake node that we can use in the integration-like cpp tests. + Apex.AI, Inc. + Apache 2.0 + + ament_cmake_auto + autoware_auto_cmake + + autoware_auto_common + rclcpp + rclcpp_components + ament_cmake_gtest + tf2 + tf2_ros + + ament_index_python + + + + ament_cmake + diff --git a/common/fake_test_node/src/fake_test_node.cpp b/common/fake_test_node/src/fake_test_node.cpp new file mode 100644 index 0000000000000..dadfb89f6033e --- /dev/null +++ b/common/fake_test_node/src/fake_test_node.cpp @@ -0,0 +1,81 @@ +// Copyright 2021 Apex.AI, 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +/// \copyright Copyright 2021 Apex.AI, Inc. +/// All rights reserved. + + +#include + +#include +#include + +namespace +{ +constexpr auto kSpinThread = false; +constexpr auto kArgc = 0; + +std::string sanitize_test_name(const std::string & name) +{ + auto sanitize_test_name = name; + std::replace(sanitize_test_name.begin(), sanitize_test_name.end(), '/', '_'); + return sanitize_test_name; +} + +} // namespace + +namespace autoware +{ +namespace tools +{ +namespace testing +{ + +void detail::FakeNodeCore::set_up(const std::string & test_name) +{ + ASSERT_FALSE(rclcpp::ok()); + rclcpp::init(kArgc, nullptr); + ASSERT_TRUE(rclcpp::ok()); + m_fake_node = std::make_shared("FakeNodeForTest_" + sanitize_test_name(test_name)); + m_tf_listener = std::make_shared( + m_tf_buffer, m_fake_node, kSpinThread); +} + +void detail::FakeNodeCore::tear_down() +{ + (void)rclcpp::shutdown(); +} + +std::string detail::get_test_name(const ::testing::TestInfo * info) +{ + if (!info) {throw std::runtime_error("No test info available.");} + return std::string{info->test_case_name()} + "_" + info->name(); +} + +void FakeTestNode::SetUp() +{ + set_up(detail::get_test_name(::testing::UnitTest::GetInstance()->current_test_info())); +} + +void FakeTestNode::TearDown() +{ + tear_down(); +} + + +} // namespace testing +} // namespace tools +} // namespace autoware diff --git a/common/fake_test_node/test/test_fake_test_node.cpp b/common/fake_test_node/test/test_fake_test_node.cpp new file mode 100644 index 0000000000000..fbacf0aa162eb --- /dev/null +++ b/common/fake_test_node/test/test_fake_test_node.cpp @@ -0,0 +1,109 @@ +// Copyright 2021 Apex.AI, 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +/// \copyright Copyright 2021 Apex.AI, Inc. +/// All rights reserved. + +#include +#include +#include +#include + +#include + +#include +#include +#include + +using autoware::common::types::bool8_t; + +using FakeNodeFixture = autoware::tools::testing::FakeTestNode; +using FakeNodeFixtureParametrized = autoware::tools::testing::FakeTestNodeParametrized; +using std_msgs::msg::Bool; +using std_msgs::msg::Int32; + +namespace +{ + +class NodeUnderTest : public rclcpp::Node +{ +public: + NodeUnderTest() + : rclcpp::Node{"is_positive_node"}, + m_pub{this->create_publisher("/output_topic", 10)}, + m_sub{this->create_subscription( + "/input_topic", 10, + [&](const Int32::SharedPtr msg) { + Bool output; + output.data = msg->data > 0; + m_pub->publish(output); + })} + { + } + +private: + rclcpp::Publisher::SharedPtr m_pub{}; + rclcpp::Subscription::SharedPtr m_sub{}; +}; + +template +void run_test(int32_t value_in_message, FixtureT * fixture) +{ + Int32 msg{}; + msg.data = value_in_message; + const auto node = std::make_shared(); + + Bool::SharedPtr last_received_msg{}; + auto fake_odom_publisher = fixture->template create_publisher("/input_topic"); + auto result_odom_subscription = fixture->template create_subscription( + "/output_topic", *node, + [&last_received_msg](const Bool::SharedPtr msg) {last_received_msg = msg;}); + + const auto dt{std::chrono::milliseconds{100LL}}; + const auto max_wait_time{std::chrono::seconds{10LL}}; + auto time_passed{std::chrono::milliseconds{0LL}}; + while (!last_received_msg) { + fake_odom_publisher->publish(msg); + rclcpp::spin_some(node); + rclcpp::spin_some(fixture->get_fake_node()); + std::this_thread::sleep_for(dt); + time_passed += dt; + if (time_passed > max_wait_time) { + FAIL() << "Did not receive a message soon enough."; + } + } + EXPECT_EQ(last_received_msg->data, value_in_message > 0); + SUCCEED(); +} + + +} // namespace + +/// @test Test that we can use a non-parametrized test. +TEST_F(FakeNodeFixture, Test) { + run_test(15, this); +} + +INSTANTIATE_TEST_SUITE_P( + FakeNodeFixtureTests, + FakeNodeFixtureParametrized, + // cppcheck-suppress syntaxError // cppcheck doesn't like the trailing comma. + ::testing::Values(-5, 0, 42)); + +/// @test Test that we can use a parametrized test. +TEST_P(FakeNodeFixtureParametrized, Test) { + run_test(GetParam(), this); +} diff --git a/common/had_map_utils/CMakeLists.txt b/common/had_map_utils/CMakeLists.txt new file mode 100644 index 0000000000000..c80443fec1377 --- /dev/null +++ b/common/had_map_utils/CMakeLists.txt @@ -0,0 +1,38 @@ +# Copyright 2020 TierIV, Inc. +# All rights reserved. +cmake_minimum_required(VERSION 3.5) + +### Export headers +project(had_map_utils) + +find_package(ament_cmake_auto REQUIRED) +find_package(CGAL REQUIRED COMPONENTS Core) +#find_package(Eigen3 REQUIRED) +ament_auto_find_build_dependencies() + +# Disable warnings due to external dependencies (Eigen) +#include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR}) + +ament_auto_add_library(${PROJECT_NAME} SHARED + include/had_map_utils/had_map_utils.hpp + include/had_map_utils/had_map_computation.hpp + include/had_map_utils/had_map_conversion.hpp + include/had_map_utils/had_map_query.hpp + include/had_map_utils/had_map_visualization.hpp + include/had_map_utils/visibility_control.hpp + src/had_map_utils.cpp + src/had_map_computation.cpp + src/had_map_conversion.cpp + src/had_map_query.cpp + src/had_map_visualization.cpp) + +set(CGAL_DO_NOT_WARN_ABOUT_CMAKE_BUILD_TYPE TRUE) +target_link_libraries(${PROJECT_NAME} CGAL CGAL::CGAL CGAL::CGAL_Core) +autoware_set_compile_options(${PROJECT_NAME}) + +# if(BUILD_TESTING) +# find_package(ament_lint_auto REQUIRED) +# ament_lint_auto_find_test_dependencies() +# endif() + +ament_auto_package() diff --git a/common/had_map_utils/include/had_map_utils/had_map_computation.hpp b/common/had_map_utils/include/had_map_utils/had_map_computation.hpp new file mode 100644 index 0000000000000..35f3f3dcf31d2 --- /dev/null +++ b/common/had_map_utils/include/had_map_utils/had_map_computation.hpp @@ -0,0 +1,38 @@ +// 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 HAD_MAP_UTILS__HAD_MAP_COMPUTATION_HPP_ +#define HAD_MAP_UTILS__HAD_MAP_COMPUTATION_HPP_ + +#include +#include + +#include "visibility_control.hpp" + +namespace autoware +{ +namespace common +{ +namespace had_map_utils +{ + +lanelet::Polygon3d HAD_MAP_UTILS_PUBLIC coalesce_drivable_areas( + const autoware_auto_planning_msgs::msg::HADMapRoute & had_map_route, + const lanelet::LaneletMapPtr & lanelet_map_ptr); + +} // namespace had_map_utils +} // namespace common +} // namespace autoware + +#endif // HAD_MAP_UTILS__HAD_MAP_COMPUTATION_HPP_ diff --git a/common/had_map_utils/include/had_map_utils/had_map_conversion.hpp b/common/had_map_utils/include/had_map_utils/had_map_conversion.hpp new file mode 100644 index 0000000000000..e0c5ca8c8c9a9 --- /dev/null +++ b/common/had_map_utils/include/had_map_utils/had_map_conversion.hpp @@ -0,0 +1,42 @@ +// Copyright 2020 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 HAD_MAP_UTILS__HAD_MAP_CONVERSION_HPP_ +#define HAD_MAP_UTILS__HAD_MAP_CONVERSION_HPP_ + +#include +#include +#include +#include "had_map_utils/visibility_control.hpp" + +namespace autoware +{ +namespace common +{ +namespace had_map_utils +{ + +void HAD_MAP_UTILS_PUBLIC toBinaryMsg( + const std::shared_ptr & map, + autoware_auto_mapping_msgs::msg::HADMapBin & msg); + +void HAD_MAP_UTILS_PUBLIC fromBinaryMsg( + const autoware_auto_mapping_msgs::msg::HADMapBin & msg, + std::shared_ptr & map); + +} // namespace had_map_utils +} // namespace common +} // namespace autoware + +#endif // HAD_MAP_UTILS__HAD_MAP_CONVERSION_HPP_ diff --git a/common/had_map_utils/include/had_map_utils/had_map_query.hpp b/common/had_map_utils/include/had_map_utils/had_map_query.hpp new file mode 100644 index 0000000000000..de51b11f5e1d2 --- /dev/null +++ b/common/had_map_utils/include/had_map_utils/had_map_query.hpp @@ -0,0 +1,61 @@ +// Copyright 2020 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 HAD_MAP_UTILS__HAD_MAP_QUERY_HPP_ +#define HAD_MAP_UTILS__HAD_MAP_QUERY_HPP_ + + +#include +#include +#include +#include +#include + +#include +#include +#include "had_map_utils/visibility_control.hpp" + +namespace autoware +{ +namespace common +{ +namespace had_map_utils +{ + +lanelet::Areas HAD_MAP_UTILS_PUBLIC getAreaLayer(const lanelet::LaneletMapPtr ll_map); + +lanelet::Areas HAD_MAP_UTILS_PUBLIC subtypeAreas( + const lanelet::Areas areas, const char subtype[]); + +lanelet::Polygons3d HAD_MAP_UTILS_PUBLIC getPolygonLayer(const lanelet::LaneletMapPtr ll_map); + +lanelet::Polygons3d HAD_MAP_UTILS_PUBLIC subtypePolygons( + const lanelet::Polygons3d polygons, const char subtype[]); + +lanelet::LineStrings3d HAD_MAP_UTILS_PUBLIC getLineStringLayer(const lanelet::LaneletMapPtr ll_map); + +lanelet::LineStrings3d HAD_MAP_UTILS_PUBLIC subtypeLineStrings( + const lanelet::LineStrings3d linestrings, const char subtype[]); + +lanelet::ConstLanelets HAD_MAP_UTILS_PUBLIC getConstLaneletLayer( + const std::shared_ptr & ll_map); + +lanelet::Lanelets HAD_MAP_UTILS_PUBLIC getLaneletLayer( + const std::shared_ptr & ll_map); + +} // namespace had_map_utils +} // namespace common +} // namespace autoware + +#endif // HAD_MAP_UTILS__HAD_MAP_QUERY_HPP_ diff --git a/common/had_map_utils/include/had_map_utils/had_map_utils.hpp b/common/had_map_utils/include/had_map_utils/had_map_utils.hpp new file mode 100644 index 0000000000000..b5b06016bdcc9 --- /dev/null +++ b/common/had_map_utils/include/had_map_utils/had_map_utils.hpp @@ -0,0 +1,45 @@ +// Copyright 2020 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 HAD_MAP_UTILS__HAD_MAP_UTILS_HPP_ +#define HAD_MAP_UTILS__HAD_MAP_UTILS_HPP_ + + +#include +#include +#include +#include + +#include +#include "had_map_utils/visibility_control.hpp" + +namespace autoware +{ +namespace common +{ +namespace had_map_utils +{ + +using autoware::common::types::float64_t; + +void HAD_MAP_UTILS_PUBLIC overwriteLaneletsCenterline( + lanelet::LaneletMapPtr lanelet_map, const autoware::common::types::bool8_t force_overwrite); +lanelet::LineString3d HAD_MAP_UTILS_PUBLIC generateFineCenterline( + const lanelet::ConstLanelet & lanelet_obj, const float64_t resolution); + +} // namespace had_map_utils +} // namespace common +} // namespace autoware + +#endif // HAD_MAP_UTILS__HAD_MAP_UTILS_HPP_ diff --git a/common/had_map_utils/include/had_map_utils/had_map_visualization.hpp b/common/had_map_utils/include/had_map_utils/had_map_visualization.hpp new file mode 100644 index 0000000000000..2952724fde4ff --- /dev/null +++ b/common/had_map_utils/include/had_map_utils/had_map_visualization.hpp @@ -0,0 +1,275 @@ +// Copyright 2020 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 HAD_MAP_UTILS__HAD_MAP_VISUALIZATION_HPP_ +#define HAD_MAP_UTILS__HAD_MAP_VISUALIZATION_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include "had_map_utils/visibility_control.hpp" + +using autoware::common::types::float32_t; +using autoware::common::types::float64_t; +using autoware::common::types::bool8_t; + +namespace autoware +{ +namespace common +{ +namespace had_map_utils +{ +/** + * \brief Set set rgba information to a Color Object + * \param[out] cl color object to be set + * \param r red value + * \param g green value + * \param b blue value + * \param a alpha value + */ +void HAD_MAP_UTILS_PUBLIC setColor( + std_msgs::msg::ColorRGBA * cl, + const float32_t & r, const float32_t & g, const float32_t & b, const float32_t & a); + +/** + * \brief Set the header information to a marker object + * \param m input marker + * \param id id of the marker + * \param t timestamp of the marker + * \param frame_id frame of the marker + * \param ns namespace of the marker + * \param c color of the marker + * \param action action used to visualize the marker + * \param type type of the marker + * \param scale scale of the marker + * \return visualization_msgs::msg::Marker + */ +void HAD_MAP_UTILS_PUBLIC setMarkerHeader( + visualization_msgs::msg::Marker * m, + const int32_t & id, const rclcpp::Time & t, + const std::string & frame_id, const std::string & ns, + const std_msgs::msg::ColorRGBA & c, const int32_t & action, const int32_t & type, + const float32_t & scale); + +/** + * \brief creates marker with type LINE_STRIP from a lanelet::LineString3d object + * \param t timestamp set to the marker + * \param ls input linestring + * \param frame_id frame id set to the marker + * \param ns namespace set to the marker + * \param c color of the marker + * \param lss linestrip scale (i.e. width) + * \return created visualization_msgs::msg::Marker + */ +visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC lineString2Marker( + const rclcpp::Time & t, + const lanelet::LineString3d & ls, + const std::string & frame_id, const std::string & ns, const std_msgs::msg::ColorRGBA & c, + const float32_t & lss); + +/** + * \brief creates marker with type LINE_STRIP from a lanelet::ConstLineString3d object + * \param t timestamp set to the marker + * \param ls input linestring + * \param frame_id frame id set to the marker + * \param ns namespace set to the marker + * \param c color of the marker + * \param lss linestrip scale (i.e. width) + * \return created visualization_msgs::msg::Marker + */ +visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC lineString2Marker( + const rclcpp::Time & t, + const lanelet::ConstLineString3d & ls, + const std::string & frame_id, const std::string & ns, const std_msgs::msg::ColorRGBA & c, + const float32_t & lss); + +/** + * \brief converts lanelet::LineString into markers with type LINE_STRIP + * \param t time set to returned marker message + * \param ns namespace set to the marker + * \param linestrings input linestring objects + * \param c color of the marker + * \return created visualization_msgs::msg::MarkerArray + */ +visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC +lineStringsAsMarkerArray( + const rclcpp::Time & t, + const std::string & ns, + const lanelet::LineStrings3d & linestrings, + const std_msgs::msg::ColorRGBA & c); + +/** + * \brief converts outer bound of lanelet::Lanelet into markers with type LINE_STRIP + * \param t time set to returned marker message + * \param lanelets input lanelet objects + * \param c color of the marker + * \param viz_centerline option to add centerline to the marker array + * \return created visualization_msgs::msg::MarkerArray + */ +visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC laneletsBoundaryAsMarkerArray( + const rclcpp::Time & t, + const lanelet::ConstLanelets & lanelets, + const std_msgs::msg::ColorRGBA & c, + const bool8_t & viz_centerline); + +/** + * \brief creates marker with type LINE_STRIP from a lanelet::BasicPolygon object + * \param t timestamp set to the marker + * \param line_id id set to the marker + * \param pg input polygon + * \param frame_id frame id set to the marker + * \param ns namespace set to the marker + * \param c color of the marker + * \param lss linestrip scale (i.e. width) + * \return created visualization_msgs::msg::Marker + */ +visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC basicPolygon2Marker( + const rclcpp::Time & t, + const int32_t & line_id, const lanelet::BasicPolygon3d & pg, + const std::string & frame_id, const std::string & ns, + const std_msgs::msg::ColorRGBA & c, const float32_t & lss); + +/** + * \brief converts outer bound of lanelet::Area into markers with type LINE_STRIP + * \param t time set to returned marker message + * \param ns namespace set to the marker + * \param areas input area objects + * \param c color of the marker + * \return created visualization_msgs::msg::MarkerArray + */ +visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC areasBoundaryAsMarkerArray( + const rclcpp::Time & t, + const std::string & ns, const lanelet::Areas & areas, + const std_msgs::msg::ColorRGBA & c); + +/** + * \brief converts outer bound of lanelet::Polygon into markers with type LINE_STRIP + * \param t Time set to returned marker message + * \param ns namespace set to the marker + * \param polygons input polygons + * \param c color of the marker + * \return created visualization_msgs::msg::MarkerArray + */ +visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC polygonsBoundaryAsMarkerArray( + const rclcpp::Time & t, + const std::string & ns, const lanelet::Polygons3d & polygons, + const std_msgs::msg::ColorRGBA & c); + +/** + * \brief creates marker with type LINE_STRIP from a bounding box + * \param t Time set to returned marker message + * \param line_id id set to marker + * \param lower lower bound of the bounding box with length 3(x,y,z) + * \param upper upper bound of the bounding box with length 3(x,y,z) + * \param frame_id frame id set to the marker + * \param ns namespace set to the marker + * \param c color of the marker + * \param lss linestrip scale (i.e. width) + * \return created visualization_msgs::msg::Marker + */ +visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC bbox2Marker( + const rclcpp::Time & t, const int32_t & line_id, + const float64_t lower[], const float64_t upper[], + const std::string & frame_id, const std::string & ns, + const std_msgs::msg::ColorRGBA & c, const float32_t & lss); + +/** + * \brief creates marker array from bounding box + * \param t Time set to returned marker message + * \param ns Namespace set to returned marker message + * \param upper upper bound of the bounding box with length 3(x,y,z) + * \param lower lower bound of the bounding box with length 3(x,y,z) + * \param c Color of the marker array + * \return created visualization_msgs::msg::MarkerArray + */ +visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC boundingBoxAsMarkerArray( + const rclcpp::Time & t, + const std::string & ns, const float64_t upper[], const float64_t lower[], + const std_msgs::msg::ColorRGBA & c); + +/** + * \brief converts area enclosed by lanelet::Lanelet into list of triangles. + * \param ll input lanelet + * \return result of triangulation + */ +std::vector HAD_MAP_UTILS_PUBLIC lanelet2Triangle( + const lanelet::ConstLanelet & ll); + +/** + * \brief converts area enclosed by geometry_msg::msg::Polygon into list of triangles. + * \param polygon input polygon + * \return result of triangulation + */ +std::vector HAD_MAP_UTILS_PUBLIC polygon2Triangle( + const geometry_msgs::msg::Polygon & polygon); + +/** + * \brief converts lanelet::Area into geometry_msgs::msg::Polygon type + * \param area input area + * \return converted geometry_msgs::msg::Polygon + */ +geometry_msgs::msg::Polygon HAD_MAP_UTILS_PUBLIC area2Polygon( + const lanelet::ConstArea & area); + +/** + * \brief converts lanelet::Lanelet into geometry_msgs::msg::Polygon type + * \param ll input lanelet + * \return converted geometry_msgs::msg::Polygon + */ +geometry_msgs::msg::Polygon HAD_MAP_UTILS_PUBLIC lanelet2Polygon( + const lanelet::ConstLanelet & ll); + +/** + * \brief converts bounded area by lanelet::Lanelet into triangle markers + * \param t Time set to returned marker message + * \param ns Namespace set to returned marker message + * \param lanelets input lanelet::Lanelet + * \param c Color of the marker array + * \return Converted triangle markers enclosed by the Lanelet + */ +visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC laneletsAsTriangleMarkerArray( + const rclcpp::Time & t, const std::string & ns, const lanelet::ConstLanelets & lanelets, + const std_msgs::msg::ColorRGBA & c); + +/** + * \brief converts bounded area by lanelet::Area into triangle markers + * \param t Time set to returned marker message + * \param ns Namespace set to returned marker message + * \param areas input lanelet::Area objects + * \param c Color of the marker array + * \return Converted triangle markers enclosed by the area + */ +visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC areasAsTriangleMarkerArray( + const rclcpp::Time & t, const std::string & ns, const lanelet::Areas & areas, + const std_msgs::msg::ColorRGBA & c); + +} // namespace had_map_utils +} // namespace common +} // namespace autoware + +#endif // HAD_MAP_UTILS__HAD_MAP_VISUALIZATION_HPP_ diff --git a/common/had_map_utils/include/had_map_utils/visibility_control.hpp b/common/had_map_utils/include/had_map_utils/visibility_control.hpp new file mode 100644 index 0000000000000..f53a7dce48093 --- /dev/null +++ b/common/had_map_utils/include/had_map_utils/visibility_control.hpp @@ -0,0 +1,38 @@ +// Copyright 2017-2019 the Autoware Foundation +// +// 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#ifndef HAD_MAP_UTILS__VISIBILITY_CONTROL_HPP_ +#define HAD_MAP_UTILS__VISIBILITY_CONTROL_HPP_ + +#if defined(_MSC_VER) && defined(_WIN64) + #if defined(HAD_MAP_UTILS_BUILDING_DLL) || defined(HAD_MAP_UTILS_EXPORTS) + #define HAD_MAP_UTILS_PUBLIC __declspec(dllexport) + #define HAD_MAP_UTILS_LOCAL + #else // defined(HAD_MAP_UTILS_BUILDING_DLL) || defined(HAD_MAP_UTILS_EXPORTS) + #define HAD_MAP_UTILS_PUBLIC __declspec(dllimport) + #define HAD_MAP_UTILS_LOCAL + #endif // defined(HAD_MAP_UTILS_BUILDING_DLL) || defined(HAD_MAP_UTILS_EXPORTS) +#elif defined(__GNUC__) && defined(__linux__) + #define HAD_MAP_UTILS_PUBLIC __attribute__((visibility("default"))) + #define HAD_MAP_UTILS_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__GNUC__) && defined(__APPLE__) + #define HAD_MAP_UTILS_PUBLIC __attribute__((visibility("default"))) + #define HAD_MAP_UTILS_LOCAL __attribute__((visibility("hidden"))) +#else // !(defined(__GNUC__) && defined(__APPLE__)) + #error "Unsupported Build Configuration" +#endif // _MSC_VER + +#endif // HAD_MAP_UTILS__VISIBILITY_CONTROL_HPP_ diff --git a/common/had_map_utils/package.xml b/common/had_map_utils/package.xml new file mode 100644 index 0000000000000..74d03b4f1e186 --- /dev/null +++ b/common/had_map_utils/package.xml @@ -0,0 +1,33 @@ + + + + had_map_utils + 1.0.0 + Common utility functions and classes for HAD maps + TierIV, Inc. + Apache 2.0 + + ament_cmake_auto + autoware_auto_cmake + rclcpp + sensor_msgs + + cgal + lanelet2_core + lanelet2_io + lanelet2 + + autoware_auto_mapping_msgs + autoware_auto_planning_msgs + autoware_auto_common + visualization_msgs + geometry_msgs + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/common/had_map_utils/src/had_map_computation.cpp b/common/had_map_utils/src/had_map_computation.cpp new file mode 100644 index 0000000000000..0a8bd8186d3fd --- /dev/null +++ b/common/had_map_utils/src/had_map_computation.cpp @@ -0,0 +1,135 @@ +// Copyright 2021 TierIV, 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 + +#include + +#include + +#include "had_map_utils/had_map_computation.hpp" +#include "had_map_utils/had_map_visualization.hpp" + +namespace autoware +{ +namespace common +{ +namespace had_map_utils +{ + +using Kernel = CGAL::Exact_predicates_exact_constructions_kernel; +using CGAL_Point = Kernel::Point_2; +using CGAL_Polygon = CGAL::Polygon_2; +using CGAL_Polygon_with_holes = CGAL::Polygon_with_holes_2; + +// TODO(s.me) this is getting a bit long, break up +lanelet::Polygon3d coalesce_drivable_areas( + const autoware_auto_planning_msgs::msg::HADMapRoute & had_map_route, + const lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + CGAL_Polygon_with_holes drivable_area; + + for (const auto & map_segment : had_map_route.segments) { + // Attempt to obtain a polygon from the primitive ID + geometry_msgs::msg::Polygon current_area_polygon{}; + const auto & lanelet_layer = lanelet_map_ptr->laneletLayer; + const auto & current_lanelet_candidate = lanelet_layer.find(map_segment.preferred_primitive_id); + if (current_lanelet_candidate != lanelet_layer.end()) { + current_area_polygon = lanelet2Polygon(*current_lanelet_candidate); + } else { + const auto & area_layer = lanelet_map_ptr->areaLayer; + const auto & current_area_candidate = area_layer.find(map_segment.preferred_primitive_id); + if (current_area_candidate != area_layer.end()) { + current_area_polygon = area2Polygon(*current_area_candidate); + } else { + // This might happen if a primitive is on the route, but outside of the bounding box that we + // query the map for. Not sure how to deal with this at this point though. + std::cerr << "Error: primitive ID " << map_segment.preferred_primitive_id << + " not found, skipping" << + std::endl; + continue; + } + } + + if (drivable_area.outer_boundary().size() > 0) { + // Convert current_area_polygon to a CGAL_Polygon and make sure the orientation is correct + CGAL_Polygon to_join{}; + CGAL_Polygon_with_holes temporary_union; + const auto first_point = current_area_polygon.points.begin(); + for (auto area_point_it = + current_area_polygon.points.begin(); + // Stop if we run out of points, or if we encounter the first point again + area_point_it < current_area_polygon.points.end() && + !(first_point != area_point_it && first_point->x == area_point_it->x && + first_point->y == area_point_it->y); + area_point_it++) + { + to_join.push_back(CGAL_Point(area_point_it->x, area_point_it->y)); + } + + if (to_join.is_clockwise_oriented() ) { + to_join.reverse_orientation(); + } + + // Merge this CGAL polygon with the growing drivable_area. We need an intermediate merge + // result because as far as I can tell from the CGAL docs, I can't "join to" a polygon + // in-place with the join() interface. + const auto polygons_overlap = CGAL::join(drivable_area, to_join, temporary_union); + if (!polygons_overlap && !drivable_area.outer_boundary().is_empty()) { + // TODO(s.me) cancel here? Right now we just ignore that polygon, if it doesn't + // overlap with the rest, there is no way to get to it anyway + std::cerr << "Error: polygons in union do not overlap!" << std::endl; + } else { + drivable_area = temporary_union; + } + } else { + // Otherwise, just set the current drivable area equal to the area to add to it, because + // CGAL seems to do "union(empty, non-empty) = empty" for some reason. + const auto first_point = current_area_polygon.points.begin(); + for (auto area_point_it = + current_area_polygon.points.begin(); + area_point_it < current_area_polygon.points.end() && + // Stop if we run out of points, or if we encounter the first point again + !(first_point != area_point_it && first_point->x == area_point_it->x && + first_point->y == area_point_it->y); + area_point_it++) + { + drivable_area.outer_boundary().push_back(CGAL_Point(area_point_it->x, area_point_it->y)); + } + if (drivable_area.outer_boundary().is_clockwise_oriented() ) { + drivable_area.outer_boundary().reverse_orientation(); + } + } + } + + // At this point, all the polygons from the route should be merged into drivable_area, + // and we now need to turn this back into a lanelet polygon. + std::vector lanelet_drivable_area_points{}; + lanelet_drivable_area_points.reserve(drivable_area.outer_boundary().size()); + for (auto p = drivable_area.outer_boundary().vertices_begin(); + p != drivable_area.outer_boundary().vertices_end(); p++) + { + lanelet_drivable_area_points.emplace_back( + lanelet::Point3d( + lanelet::utils::getId(), CGAL::to_double(p->x()), + CGAL::to_double(p->y()), 0.0)); + } + lanelet::Polygon3d lanelet_drivable_area(lanelet::utils::getId(), lanelet_drivable_area_points); + return lanelet_drivable_area; +} + + +} // namespace had_map_utils +} // namespace common +} // namespace autoware diff --git a/common/had_map_utils/src/had_map_conversion.cpp b/common/had_map_utils/src/had_map_conversion.cpp new file mode 100644 index 0000000000000..c7b2bc711270a --- /dev/null +++ b/common/had_map_utils/src/had_map_conversion.cpp @@ -0,0 +1,73 @@ +// Copyright 2020 TierIV, 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +//lint -e537 pclint vs cpplint NOLINT + +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include "had_map_utils/had_map_conversion.hpp" + + +namespace autoware +{ +namespace common +{ +namespace had_map_utils +{ + +void toBinaryMsg( + const std::shared_ptr & map, + autoware_auto_mapping_msgs::msg::HADMapBin & msg) +{ + std::stringstream ss; + boost::archive::binary_oarchive oa(ss); + oa << *map; + auto id_counter = lanelet::utils::getId(); + oa << id_counter; + + std::string tmp_str = ss.str(); + msg.data.clear(); + msg.data.resize(tmp_str.size()); + msg.data.assign(tmp_str.begin(), tmp_str.end()); +} + +void fromBinaryMsg( + const autoware_auto_mapping_msgs::msg::HADMapBin & msg, + std::shared_ptr & map) +{ + std::string data_str; + data_str.assign(msg.data.begin(), msg.data.end()); + std::stringstream ss; + ss << data_str; + boost::archive::binary_iarchive oa(ss); + oa >> *map; + lanelet::Id id_counter; + oa >> id_counter; + lanelet::utils::registerId(id_counter); +} + +} // namespace had_map_utils +} // namespace common +} // namespace autoware diff --git a/common/had_map_utils/src/had_map_query.cpp b/common/had_map_utils/src/had_map_query.cpp new file mode 100644 index 0000000000000..eeca6543b06bc --- /dev/null +++ b/common/had_map_utils/src/had_map_query.cpp @@ -0,0 +1,132 @@ +// Copyright 2020 TierIV, 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +//lint -e537 pclint vs cpplint NOLINT + +#include +#include +#include +#include +#include + +#include "had_map_utils/had_map_query.hpp" + +namespace autoware +{ +namespace common +{ +namespace had_map_utils +{ + +lanelet::Areas getAreaLayer(const lanelet::LaneletMapPtr ll_map) +{ + lanelet::Areas areas; + for (auto ai = ll_map->areaLayer.begin(); ai != ll_map->areaLayer.end(); ai++) { + areas.push_back(*ai); + } + return areas; +} + +lanelet::Areas subtypeAreas(const lanelet::Areas areas, const char subtype[]) +{ + lanelet::Areas subtype_areas; + for (auto ai = areas.begin(); ai != areas.end(); ai++) { + lanelet::Area a = *ai; + if (a.hasAttribute(lanelet::AttributeName::Subtype)) { + lanelet::Attribute attr = a.attribute(lanelet::AttributeName::Subtype); + if (attr.value() == subtype) { + subtype_areas.push_back(a); + } + } + } + return subtype_areas; +} + +lanelet::Polygons3d getPolygonLayer(const lanelet::LaneletMapPtr ll_map) +{ + lanelet::Polygons3d polygons; + for (auto ai = ll_map->polygonLayer.begin(); ai != ll_map->polygonLayer.end(); ai++) { + polygons.push_back(*ai); + } + return polygons; +} + +lanelet::Polygons3d subtypePolygons(const lanelet::Polygons3d polygons, const char subtype[]) +{ + lanelet::Polygons3d subtype_polygons; + for (auto pi = polygons.begin(); pi != polygons.end(); pi++) { + lanelet::Polygon3d p = *pi; + if (p.hasAttribute(lanelet::AttributeName::Subtype)) { + lanelet::Attribute attr = p.attribute(lanelet::AttributeName::Subtype); + if (attr.value() == subtype) { + subtype_polygons.push_back(p); + } + } + } + return subtype_polygons; +} + + +lanelet::LineStrings3d getLineStringLayer(const lanelet::LaneletMapPtr ll_map) +{ + lanelet::LineStrings3d linestrings; + for (auto lsi = ll_map->lineStringLayer.begin(); + lsi != ll_map->lineStringLayer.end(); lsi++) + { + linestrings.push_back(*lsi); + } + return linestrings; +} + +lanelet::LineStrings3d subtypeLineStrings( + const lanelet::LineStrings3d linestrings, + const char subtype[]) +{ + lanelet::LineStrings3d subtype_linestrings; + for (auto lsi = linestrings.begin(); lsi != linestrings.end(); lsi++) { + lanelet::LineString3d ls = *lsi; + if (ls.hasAttribute(lanelet::AttributeName::Subtype)) { + lanelet::Attribute attr = ls.attribute(lanelet::AttributeName::Subtype); + if (attr.value() == subtype) { + subtype_linestrings.push_back(ls); + } + } + } + return subtype_linestrings; +} + +lanelet::ConstLanelets getConstLaneletLayer(const std::shared_ptr & ll_map) +{ + lanelet::ConstLanelets lanelets; + for (auto li = ll_map->laneletLayer.begin(); li != ll_map->laneletLayer.end(); li++) { + lanelets.push_back(*li); + } + + return lanelets; +} +lanelet::Lanelets getLaneletLayer(const std::shared_ptr & ll_map) +{ + lanelet::Lanelets lanelets; + for (auto li = ll_map->laneletLayer.begin(); li != ll_map->laneletLayer.end(); li++) { + lanelets.push_back(*li); + } + + return lanelets; +} + +} // namespace had_map_utils +} // namespace common +} // namespace autoware diff --git a/common/had_map_utils/src/had_map_utils.cpp b/common/had_map_utils/src/had_map_utils.cpp new file mode 100644 index 0000000000000..ff56754268864 --- /dev/null +++ b/common/had_map_utils/src/had_map_utils.cpp @@ -0,0 +1,175 @@ +// Copyright 2020 TierIV, 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +//lint -e537 pclint vs cpplint NOLINT + + +#include "had_map_utils/had_map_utils.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +using autoware::common::types::float64_t; + +namespace autoware +{ +namespace common +{ +namespace had_map_utils +{ + +std::vector calculateSegmentDistances(const lanelet::ConstLineString3d & line_string) +{ + std::vector segment_distances; + segment_distances.reserve(line_string.size() - 1); + + for (size_t i = 1; i < line_string.size(); ++i) { + const auto distance = lanelet::geometry::distance(line_string[i], line_string[i - 1]); + segment_distances.push_back(distance); + } + + return segment_distances; +} + +std::vector calculateAccumulatedLengths(const lanelet::ConstLineString3d & line_string) +{ + const auto segment_distances = calculateSegmentDistances(line_string); + + std::vector accumulated_lengths{0}; + accumulated_lengths.reserve(segment_distances.size() + 1); + std::partial_sum( + std::begin(segment_distances), std::end(segment_distances), + std::back_inserter(accumulated_lengths)); + + return accumulated_lengths; +} + +std::pair findNearestIndexPair( + const std::vector & accumulated_lengths, const float64_t target_length) +{ + // List size + const auto N = accumulated_lengths.size(); + + // Front + if (target_length < accumulated_lengths.at(1)) { + return std::make_pair(0, 1); + } + + // Back + if (target_length > accumulated_lengths.at(N - 2)) { + return std::make_pair(N - 2, N - 1); + } + + // Middle + for (size_t i = 1; i < N; ++i) { + if ( + accumulated_lengths.at(i - 1) <= target_length && + target_length <= accumulated_lengths.at(i)) + { + return std::make_pair(i - 1, i); + } + } + + // Throw an exception because this never happens + throw std::runtime_error( + "findNearestIndexPair(): No nearest point found."); +} + +std::vector resamplePoints( + const lanelet::ConstLineString3d & line_string, const int32_t num_segments) +{ + // Calculate length + const auto line_length = lanelet::geometry::length(line_string); + + // Calculate accumulated lengths + const auto accumulated_lengths = calculateAccumulatedLengths(line_string); + + // Create each segment + std::vector resampled_points; + for (auto i = 0; i <= num_segments; ++i) { + // Find two nearest points + const float64_t target_length = (static_cast(i) / num_segments) * + static_cast(line_length); + const auto index_pair = findNearestIndexPair(accumulated_lengths, target_length); + + // Apply linear interpolation + const lanelet::BasicPoint3d back_point = line_string[index_pair.first]; + const lanelet::BasicPoint3d front_point = line_string[index_pair.second]; + const auto direction_vector = (front_point - back_point); + + const auto back_length = accumulated_lengths.at(index_pair.first); + const auto front_length = accumulated_lengths.at(index_pair.second); + const auto segment_length = front_length - back_length; + const auto target_point = + back_point + (direction_vector * (target_length - back_length) / segment_length); + + // Add to list + resampled_points.push_back(target_point); + } + + return resampled_points; +} + +lanelet::LineString3d generateFineCenterline( + const lanelet::ConstLanelet & lanelet_obj, const float64_t resolution) +{ + // Get length of longer border + const float64_t left_length = + static_cast(lanelet::geometry::length(lanelet_obj.leftBound())); + const float64_t right_length = + static_cast(lanelet::geometry::length(lanelet_obj.rightBound())); + const float64_t longer_distance = (left_length > right_length) ? left_length : right_length; + const int32_t num_segments = + std::max(static_cast(ceil(longer_distance / resolution)), 1); + + // Resample points + const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); + const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); + + // Create centerline + lanelet::LineString3d centerline(lanelet::utils::getId()); + for (size_t i = 0; i < static_cast(num_segments + 1); i++) { + // Add ID for the average point of left and right + const auto center_basic_point = (right_points.at(i) + left_points.at(i)) / 2.0; + const lanelet::Point3d center_point( + lanelet::utils::getId(), center_basic_point.x(), center_basic_point.y(), + center_basic_point.z()); + centerline.push_back(center_point); + } + return centerline; +} + +void overwriteLaneletsCenterline( + lanelet::LaneletMapPtr lanelet_map, + const autoware::common::types::bool8_t force_overwrite) +{ + for (auto & lanelet_obj : lanelet_map->laneletLayer) { + if (force_overwrite || !lanelet_obj.hasCustomCenterline()) { + const auto fine_center_line = generateFineCenterline(lanelet_obj, 2.0); + lanelet_obj.setCenterline(fine_center_line); + } + } +} + +} // namespace had_map_utils +} // namespace common +} // namespace autoware diff --git a/common/had_map_utils/src/had_map_visualization.cpp b/common/had_map_utils/src/had_map_visualization.cpp new file mode 100644 index 0000000000000..6133bc1eb22be --- /dev/null +++ b/common/had_map_utils/src/had_map_visualization.cpp @@ -0,0 +1,558 @@ +// Copyright 2020 TierIV, 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +//lint -e537 pclint vs cpplint NOLINT + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "had_map_utils/had_map_visualization.hpp" + +using autoware::common::types::float64_t; + +namespace autoware +{ +namespace common +{ +namespace had_map_utils +{ + +template +bool8_t exists(const std::unordered_set & set, const T & element) +{ + return std::find(set.begin(), set.end(), element) != set.end(); +} + +void setColor( + std_msgs::msg::ColorRGBA * cl, + const float32_t & r, const float32_t & g, const float32_t & b, const float32_t & a) +{ + cl->r = r; + cl->g = g; + cl->b = b; + cl->a = a; +} + +void setMarkerHeader( + visualization_msgs::msg::Marker * m, + const int32_t & id, + const rclcpp::Time & t, + const std::string & frame_id, + const std::string & ns, + const std_msgs::msg::ColorRGBA & c, + const int32_t & action, + const int32_t & type, + const float32_t & scale) +{ + m->header.frame_id = frame_id; + m->header.stamp = t; + m->ns = ns; + m->action = action; + m->type = type; + + m->id = id; + m->pose.position.x = 0.0; + m->pose.position.y = 0.0; + m->pose.position.z = 0.0; + m->pose.orientation.x = 0.0; + m->pose.orientation.y = 0.0; + m->pose.orientation.z = 0.0; + m->pose.orientation.w = 1.0; + m->scale.x = scale; + m->scale.y = scale; + m->scale.z = scale; + m->color = c; +} + +visualization_msgs::msg::Marker lineString2Marker( + const rclcpp::Time & t, + const lanelet::LineString3d & ls, + const std::string & frame_id, + const std::string & ns, const std_msgs::msg::ColorRGBA & c, const float32_t & lss) +{ + visualization_msgs::msg::Marker line_strip; + setMarkerHeader( + &line_strip, static_cast(ls.id()), t, frame_id, ns, c, + visualization_msgs::msg::Marker::ADD, + visualization_msgs::msg::Marker::LINE_STRIP, + lss); + + for (auto i = ls.begin(); i != ls.end(); i++) { + geometry_msgs::msg::Point p; + p.x = (*i).x(); + p.y = (*i).y(); + p.z = (*i).z(); + line_strip.points.push_back(p); + } + return line_strip; +} + +visualization_msgs::msg::Marker lineString2Marker( + const rclcpp::Time & t, + const lanelet::ConstLineString3d & ls, + const std::string & frame_id, const std::string & ns, const std_msgs::msg::ColorRGBA & c, + const float32_t & lss) +{ + visualization_msgs::msg::Marker line_strip; + setMarkerHeader( + &line_strip, static_cast(ls.id()), t, frame_id, ns, c, + visualization_msgs::msg::Marker::ADD, + visualization_msgs::msg::Marker::LINE_STRIP, + lss); + + for (auto i = ls.begin(); i != ls.end(); i++) { + geometry_msgs::msg::Point p; + p.x = (*i).x(); + p.y = (*i).y(); + p.z = (*i).z(); + line_strip.points.push_back(p); + } + return line_strip; +} + +visualization_msgs::msg::MarkerArray lineStringsAsMarkerArray( + const rclcpp::Time & t, + const std::string & ns, + const lanelet::LineStrings3d & linestrings, + const std_msgs::msg::ColorRGBA & c) +{ + float32_t lss = 0.1f; + visualization_msgs::msg::MarkerArray marker_array; + + for (auto lsi = linestrings.begin(); lsi != linestrings.end(); lsi++) { + lanelet::LineString3d ls = *lsi; + visualization_msgs::msg::Marker line_strip = lineString2Marker(t, ls, "map", ns, c, lss); + marker_array.markers.push_back(line_strip); + } + return marker_array; +} + +visualization_msgs::msg::MarkerArray laneletsBoundaryAsMarkerArray( + const rclcpp::Time & t, + const lanelet::ConstLanelets & lanelets, + const std_msgs::msg::ColorRGBA & c, + const bool8_t & viz_centerline) +{ + float32_t lss = 0.1f; + std::unordered_set added; + visualization_msgs::msg::MarkerArray marker_array; + + for (auto li = lanelets.begin(); li != lanelets.end(); li++) { + lanelet::ConstLanelet lll = *li; + lanelet::ConstLineString3d left_ls = lll.leftBound(); + lanelet::ConstLineString3d right_ls = lll.rightBound(); + lanelet::ConstLineString3d center_ls = lll.centerline(); + + visualization_msgs::msg::Marker left_line_strip, right_line_strip, center_line_strip; + if (!exists(added, left_ls.id())) { + left_line_strip = lineString2Marker(t, left_ls, "map", "left_lane_bound", c, lss); + marker_array.markers.push_back(left_line_strip); + added.insert(left_ls.id()); + } + if (!exists(added, right_ls.id())) { + right_line_strip = lineString2Marker( + t, right_ls, "map", "right_lane_bound", c, lss); + marker_array.markers.push_back(right_line_strip); + added.insert(right_ls.id()); + } + if (viz_centerline && !exists(added, center_ls.id())) { + center_line_strip = lineString2Marker( + t, center_ls, "map", "center_lane_line", + c, std::max(lss * 0.1f, 0.01f)); + marker_array.markers.push_back(center_line_strip); + added.insert(center_ls.id()); + } + } + return marker_array; +} + +visualization_msgs::msg::Marker basicPolygon2Marker( + const rclcpp::Time & t, + const int32_t & line_id, + const lanelet::BasicPolygon3d & pg, + const std::string & frame_id, + const std::string & ns, + const std_msgs::msg::ColorRGBA & c, + const float32_t & lss) +{ + visualization_msgs::msg::Marker line_strip; + setMarkerHeader( + &line_strip, line_id, t, frame_id, ns, c, + visualization_msgs::msg::Marker::ADD, + visualization_msgs::msg::Marker::LINE_STRIP, + lss); + + for (auto i = pg.begin(); i != pg.end(); i++) { + geometry_msgs::msg::Point p; + p.x = (*i).x(); + p.y = (*i).y(); + p.z = (*i).z(); + line_strip.points.push_back(p); + } + geometry_msgs::msg::Point pb; + auto i = pg.begin(); + pb.x = (*i).x(); + pb.y = (*i).y(); + pb.z = (*i).z(); + line_strip.points.push_back(pb); + return line_strip; +} + +visualization_msgs::msg::MarkerArray areasBoundaryAsMarkerArray( + const rclcpp::Time & t, + const std::string & ns, + const lanelet::Areas & areas, + const std_msgs::msg::ColorRGBA & c) +{ + int pg_count = 0; + float32_t lss = 0.1f; + visualization_msgs::msg::MarkerArray marker_array; + for (auto area : areas) { + lanelet::CompoundPolygon3d cpg = area.outerBoundPolygon(); + lanelet::BasicPolygon3d bpg = cpg.basicPolygon(); + + visualization_msgs::msg::Marker line_strip = basicPolygon2Marker( + t, pg_count, bpg, "map", ns, c, + lss); + marker_array.markers.push_back(line_strip); + pg_count++; + } + return marker_array; +} + +visualization_msgs::msg::MarkerArray polygonsBoundaryAsMarkerArray( + const rclcpp::Time & t, const std::string & ns, + const lanelet::Polygons3d & polygons, const std_msgs::msg::ColorRGBA & c) +{ + int32_t pg_count = 0; + float32_t lss = 0.1f; + visualization_msgs::msg::MarkerArray marker_array; + for (auto poly : polygons) { + lanelet::BasicPolygon3d bpg = poly.basicPolygon(); + visualization_msgs::msg::Marker line_strip = basicPolygon2Marker( + t, pg_count, bpg, "map", ns, c, + lss); + marker_array.markers.push_back(line_strip); + pg_count++; + } + return marker_array; +} + +visualization_msgs::msg::Marker bbox2Marker( + const rclcpp::Time & t, const int32_t & line_id, const float64_t lower[], const float64_t upper[], + const std::string & frame_id, const std::string & ns, + const std_msgs::msg::ColorRGBA & c, const float32_t & lss) +{ + visualization_msgs::msg::Marker line_strip; + setMarkerHeader( + &line_strip, line_id, t, frame_id, ns, c, + visualization_msgs::msg::Marker::ADD, + visualization_msgs::msg::Marker::LINE_STRIP, + lss); + + geometry_msgs::msg::Point bl, br, tl, tr; + bl.x = lower[0]; bl.y = lower[0]; bl.z = 0.0; + br.x = upper[0]; br.y = lower[0]; br.z = 0.0; + tl.x = lower[0]; tl.y = upper[0]; tl.z = 0.0; + tr.x = upper[0]; tr.y = upper[0]; tr.z = 0.0; + + line_strip.points.push_back(bl); + line_strip.points.push_back(br); + line_strip.points.push_back(tr); + line_strip.points.push_back(tl); + line_strip.points.push_back(bl); + return line_strip; +} + +visualization_msgs::msg::MarkerArray boundingBoxAsMarkerArray( + const rclcpp::Time & t, + const std::string & ns, const float64_t upper[], const float64_t lower[], + const std_msgs::msg::ColorRGBA & c) +{ + float32_t lss = 0.2f; + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker line_strip = bbox2Marker(t, 0, upper, lower, "map", ns, c, lss); + marker_array.markers.push_back(line_strip); + return marker_array; +} + +geometry_msgs::msg::Point toGeomMsgPt(const geometry_msgs::msg::Point32 & src) +{ + geometry_msgs::msg::Point dst; + dst.x = static_cast(src.x); + dst.y = static_cast(src.y); + dst.z = static_cast(src.z); + return dst; +} + +geometry_msgs::msg::Point32 toGeomMsgPt32(const lanelet::BasicPoint3d & src) +{ + geometry_msgs::msg::Point32 dst; + dst.x = static_cast(src.x()); + dst.y = static_cast(src.y()); + dst.z = static_cast(src.z()); + return dst; +} +void adjacentPoints( + const size_t i, const size_t N, + const geometry_msgs::msg::Polygon poly, + geometry_msgs::msg::Point32 * p0, + geometry_msgs::msg::Point32 * p1, + geometry_msgs::msg::Point32 * p2) +{ + *p1 = poly.points[i]; + if (i == 0) { + *p0 = poly.points[N - 1]; + } else { + *p0 = poly.points[i - 1]; + } + if (i < N - 1) { + *p2 = poly.points[i + 1]; + } else { + *p2 = poly.points[0]; + } +} + +std::vector lanelet2Triangle( + const lanelet::ConstLanelet & ll) +{ + geometry_msgs::msg::Polygon ls_poly = lanelet2Polygon(ll); + return polygon2Triangle(ls_poly); +} + +std::vector area2Triangle( + const lanelet::Area & area) +{ + geometry_msgs::msg::Polygon ls_poly = area2Polygon(area); + return polygon2Triangle(ls_poly); +} + +// Is angle AOB less than 180? +// https://qiita.com/fujii-kotaro/items/a411f2a45627ed2f156e +bool8_t isAcuteAngle( + const geometry_msgs::msg::Point32 & vertex_a, + const geometry_msgs::msg::Point32 & vertex_o, + const geometry_msgs::msg::Point32 & vertex_b) +{ + return (vertex_a.x - vertex_o.x) * (vertex_b.y - vertex_o.y) - + (vertex_a.y - vertex_o.y) * (vertex_b.x - vertex_o.x) >= 0; +} + +// https://qiita.com/fujii-kotaro/items/a411f2a45627ed2f156e +bool8_t isWithinTriangle( + const geometry_msgs::msg::Point32 & vertex_a, + const geometry_msgs::msg::Point32 & vertex_b, + const geometry_msgs::msg::Point32 & vertex_c, + const geometry_msgs::msg::Point32 & pt) +{ + float64_t c1 = (vertex_b.x - vertex_a.x) * (pt.y - vertex_b.y) - + (vertex_b.y - vertex_a.y) * (pt.x - vertex_b.x); + float64_t c2 = (vertex_c.x - vertex_b.x) * (pt.y - vertex_c.y) - + (vertex_c.y - vertex_b.y) * (pt.x - vertex_c.x); + float64_t c3 = (vertex_a.x - vertex_c.x) * (pt.y - vertex_a.y) - + (vertex_a.y - vertex_c.y) * (pt.x - vertex_a.x); + + return (c1 > 0.0 && c2 > 0.0 && c3 > 0.0) || (c1 < 0.0 && c2 < 0.0 && c3 < 0.0); +} + +std::vector polygon2Triangle( + const geometry_msgs::msg::Polygon & polygon) +{ + std::vector triangles; + geometry_msgs::msg::Polygon poly = polygon; + size_t num_vertices = poly.points.size(); + + std::vector is_acute_angle; + is_acute_angle.assign(num_vertices, false); + for (size_t i = 0; i < num_vertices; i++) { + geometry_msgs::msg::Point32 p0, p1, p2; + + adjacentPoints(i, num_vertices, poly, &p0, &p1, &p2); + is_acute_angle.at(i) = isAcuteAngle(p0, p1, p2); + } + while (num_vertices >= 3) { + size_t clipped_vertex = 0; + + for (size_t i = 0; i < num_vertices; i++) { + bool8_t theta = is_acute_angle.at(i); + if (theta == true) { + geometry_msgs::msg::Point32 p0, p1, p2; + adjacentPoints(i, num_vertices, poly, &p0, &p1, &p2); + + size_t j_begin = (i + 2) % num_vertices; + size_t j_end = (i - 1 + num_vertices) % num_vertices; + bool8_t is_ear = true; + for (size_t j = j_begin; j != j_end; j = (j + 1) % num_vertices) { + if (isWithinTriangle(p0, p1, p2, poly.points.at(j))) { + is_ear = false; + break; + } + } + + if (is_ear) { + clipped_vertex = i; + break; + } + } + } + geometry_msgs::msg::Point32 p0, p1, p2; + adjacentPoints(clipped_vertex, num_vertices, poly, &p0, &p1, &p2); + geometry_msgs::msg::Polygon triangle; + triangle.points.push_back(p0); + triangle.points.push_back(p1); + triangle.points.push_back(p2); + triangles.push_back(triangle); + auto it = poly.points.begin(); + std::advance(it, clipped_vertex); + poly.points.erase(it); + + auto it_angle = is_acute_angle.begin(); + std::advance(it_angle, clipped_vertex); + is_acute_angle.erase(it_angle); + + num_vertices = poly.points.size(); + if (clipped_vertex == num_vertices) { + clipped_vertex = 0; + } + adjacentPoints(clipped_vertex, num_vertices, poly, &p0, &p1, &p2); + is_acute_angle.at(clipped_vertex) = isAcuteAngle(p0, p1, p2); + + size_t i_prev = (clipped_vertex == 0) ? num_vertices - 1 : clipped_vertex - 1; + adjacentPoints(i_prev, num_vertices, poly, &p0, &p1, &p2); + is_acute_angle.at(i_prev) = isAcuteAngle(p0, p1, p2); + } + return triangles; +} + + +geometry_msgs::msg::Polygon area2Polygon( + const lanelet::ConstArea & area) +{ + geometry_msgs::msg::Polygon polygon; + polygon.points.clear(); + polygon.points.reserve(area.outerBoundPolygon().size()); + + std::transform( + area.outerBoundPolygon().begin(), + area.outerBoundPolygon().end(), + std::back_inserter(polygon.points), + [](lanelet::ConstPoint3d pt) {return toGeomMsgPt32(pt.basicPoint());}); + return polygon; +} + +geometry_msgs::msg::Polygon lanelet2Polygon( + const lanelet::ConstLanelet & ll) +{ + geometry_msgs::msg::Polygon polygon; + + const lanelet::CompoundPolygon3d & ll_poly = ll.polygon3d(); + polygon.points.clear(); + polygon.points.reserve(ll_poly.size()); + + std::transform( + ll_poly.begin(), + ll_poly.end(), + std::back_inserter(polygon.points), + [](lanelet::ConstPoint3d pt) {return toGeomMsgPt32(pt.basicPoint());}); + return polygon; +} + +visualization_msgs::msg::MarkerArray laneletsAsTriangleMarkerArray( + const rclcpp::Time & t, + const std::string & ns, + const lanelet::ConstLanelets & lanelets, + const std_msgs::msg::ColorRGBA & c) +{ + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker marker; + + if (lanelets.empty()) { + return marker_array; + } + + setMarkerHeader( + &marker, 0, t, "map", ns, c, + visualization_msgs::msg::Marker::ADD, + visualization_msgs::msg::Marker::TRIANGLE_LIST, + 1.0); + + for (auto ll : lanelets) { + std::vector triangles = lanelet2Triangle(ll); + + for (auto tri : triangles) { + geometry_msgs::msg::Point tri0[3]; + + for (size_t i = 0; i < 3; i++) { + tri0[i] = toGeomMsgPt(tri.points[i]); + + marker.points.push_back(tri0[i]); + marker.colors.push_back(c); + } + } + } + if (!marker.points.empty()) { + marker_array.markers.push_back(marker); + } + + return marker_array; +} + +visualization_msgs::msg::MarkerArray areasAsTriangleMarkerArray( + const rclcpp::Time & t, const std::string & ns, const lanelet::Areas & areas, + const std_msgs::msg::ColorRGBA & c) +{ + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker marker; + + if (areas.empty()) { + return marker_array; + } + + setMarkerHeader( + &marker, 0, t, "map", ns, c, + visualization_msgs::msg::Marker::ADD, + visualization_msgs::msg::Marker::TRIANGLE_LIST, + 1.0); + + for (auto area : areas) { + std::vector triangles = area2Triangle(area); + for (auto tri : triangles) { + for (size_t i = 0; i < 3; i++) { + marker.points.push_back(toGeomMsgPt(tri.points[i])); + marker.colors.push_back(c); + } + } + } + + if (!marker.points.empty()) { + marker_array.markers.push_back(marker); + } + return marker_array; +} + +} // namespace had_map_utils +} // namespace common +} // namespace autoware diff --git a/common/motion_common/CMakeLists.txt b/common/motion_common/CMakeLists.txt new file mode 100644 index 0000000000000..1080b4d7d32c1 --- /dev/null +++ b/common/motion_common/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 3.6) + +project(motion_common) + +### Dependencies +find_package(ament_cmake_auto REQUIRED) +find_package(Eigen3 REQUIRED) +ament_auto_find_build_dependencies() + +# Build library +ament_auto_add_library(${PROJECT_NAME} SHARED + src/motion_common/config.cpp + src/motion_common/motion_common.cpp + src/motion_common/trajectory_common.cpp +) +autoware_set_compile_options(${PROJECT_NAME}) + +### Test +if(BUILD_TESTING) + # Linters + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + # Unit test + #apex_test_tools_add_gtest(motion_common_unit_tests + # test/sanity_checks.cpp + # test/interpolation.cpp + # test/trajectory.cpp) + #autoware_set_compile_options(motion_common_unit_tests) + #target_compile_options(motion_common_unit_tests PRIVATE -Wno-float-conversion) + #target_link_libraries(motion_common_unit_tests ${PROJECT_NAME}) + #add_dependencies(motion_common_unit_tests ${PROJECT_NAME}) + #ament_target_dependencies(motion_common_unit_tests + # "geometry_msgs" + # "autoware_auto_geometry_msgs" + #) +endif() + +# Install snippets for code generation +install(DIRECTORY scripts/autogeneration_code_snippets DESTINATION share/) + +ament_auto_package() diff --git a/common/motion_common/include/motion_common/config.hpp b/common/motion_common/include/motion_common/config.hpp new file mode 100644 index 0000000000000..3f4cdde7f45c7 --- /dev/null +++ b/common/motion_common/include/motion_common/config.hpp @@ -0,0 +1,182 @@ +// Copyright 2019 Christopher Ho +// +// 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 MOTION_COMMON__CONFIG_HPP_ +#define MOTION_COMMON__CONFIG_HPP_ + +#include +#include + +#define MOTION_COMMON_COPY_MOVE_ASSIGNABLE(Class) \ + Class(const Class &) = default; \ + Class(Class &&) = default; \ + Class & operator=(const Class &) = default; \ + Class & operator=(Class &&) = default; \ + ~Class() = default; + +namespace motion +{ +namespace motion_common +{ +using motion_common::Real; +using motion_common::Command; +using motion_common::State; +using motion_common::Trajectory; +using motion_common::Point; +using motion_common::Heading; +using motion_common::Index; + +/// Extreme values for state/control variables +class MOTION_COMMON_PUBLIC LimitsConfig +{ +public: + /// \brief Class representing min and max values for a variable + class Extremum + { +public: + Extremum(Real min, Real max); + MOTION_COMMON_COPY_MOVE_ASSIGNABLE(Extremum) + + Real min() const noexcept; + Real max() const noexcept; + +private: + Real m_min; + Real m_max; + }; // class Extremum + + LimitsConfig( + Extremum longitudinal_velocity_mps, + Extremum lateral_velocity_mps, + Extremum acceleration_mps2, + Extremum yaw_rate_rps, + Extremum jerk_mps3, + Extremum steer_angle_rad, + Extremum steer_angle_rate_rps); + MOTION_COMMON_COPY_MOVE_ASSIGNABLE(LimitsConfig) + + Extremum longitudinal_velocity() const noexcept; + Extremum lateral_velocity() const noexcept; + Extremum acceleration() const noexcept; + Extremum jerk() const noexcept; + Extremum yaw_rate() const noexcept; + Extremum steer_angle() const noexcept; + Extremum steer_angle_rate() const noexcept; + +private: + Extremum m_longitudinal_velocity_limits_mps; + Extremum m_lateral_velocity_limits_mps; + Extremum m_acceleration_limits_mps2; + Extremum m_yaw_rate_limits_rps; + Extremum m_jerk_limits_mps3; + Extremum m_steer_angle_limits_rad; + Extremum m_steer_angle_rate_limits_rps; +}; // class LimitsConfig + +/// Vehicle parameters specifying vehicle's handling performance +class MOTION_COMMON_PUBLIC VehicleConfig +{ +public: + VehicleConfig( + Real length_cg_front_axel_m, + Real length_cg_rear_axel_m, + Real front_cornering_stiffness_N, + Real rear_cornering_stiffness_N, + Real mass_kg, + Real inertia_kgm2, + Real width_m, + Real front_overhang_m, + Real rear_overhang_m); + MOTION_COMMON_COPY_MOVE_ASSIGNABLE(VehicleConfig) + + Real length_cg_front_axel() const noexcept; + Real length_cg_rear_axel() const noexcept; + Real front_cornering_stiffness() const noexcept; + Real rear_cornering_stiffness() const noexcept; + Real mass() const noexcept; + Real inertia() const noexcept; + Real width() const noexcept; + Real front_overhang() const noexcept; + Real rear_overhang() const noexcept; + +private: + Real m_length_cg_to_front_axel_m; + Real m_length_cg_to_rear_axel_m; + Real m_front_cornering_stiffness_N; + Real m_rear_cornering_stiffness_N; + Real m_mass_kg; + Real m_inertia_kgm2; + Real m_width_m; + Real m_front_overhang_m; + Real m_rear_overhang_m; +}; // class VehicleConfig + +/// \brief Specifies the weights used for particular state weights in the least-squares objective +/// function of the mpc solver +class MOTION_COMMON_PUBLIC StateWeight +{ +public: + StateWeight( + Real pose, + Real heading, + Real longitudinal_velocity, + Real lateral_velocity, + Real yaw_rate, + Real acceleration, + Real jerk, + Real steer_angle, + Real steer_angle_rate); + MOTION_COMMON_COPY_MOVE_ASSIGNABLE(StateWeight) + + Real pose() const noexcept; + Real heading() const noexcept; + Real longitudinal_velocity() const noexcept; + Real lateral_velocity() const noexcept; + Real yaw_rate() const noexcept; + Real acceleration() const noexcept; + Real jerk() const noexcept; + Real steer_angle() const noexcept; + Real steer_angle_rate() const noexcept; + +private: + Real m_pose_weight; + Real m_heading_weight; + Real m_longitudinal_velocity_weight; + Real m_lateral_velocity_weight; + Real m_yaw_rate_weight; + Real m_acceleration_weight; + Real m_jerk_weight; + Real m_steer_angle_weight; + Real m_steer_angle_rate_weight; +}; // class StateWeight + +/// \brief Specifies various parameters specific to the optimization problem and it's behaviors +/// Depending on problem setup, some weights may be ignored +class MOTION_COMMON_PUBLIC OptimizationConfig +{ +public: + OptimizationConfig( + StateWeight nominal_weights, + StateWeight terminal_weights); + MOTION_COMMON_COPY_MOVE_ASSIGNABLE(OptimizationConfig) + + StateWeight nominal() const noexcept; + StateWeight terminal() const noexcept; + +private: + StateWeight m_nominal_weights; + StateWeight m_terminal_weights; +}; // class OptimizationConfig +} // namespace motion_common +} // namespace motion +#endif // MOTION_COMMON__CONFIG_HPP_ diff --git a/common/motion_common/include/motion_common/motion_common.hpp b/common/motion_common/include/motion_common/motion_common.hpp new file mode 100644 index 0000000000000..6856636439240 --- /dev/null +++ b/common/motion_common/include/motion_common/motion_common.hpp @@ -0,0 +1,279 @@ +// Copyright 2019-2021 the Autoware Foundation +// +// 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 MOTION_COMMON__MOTION_COMMON_HPP_ +#define MOTION_COMMON__MOTION_COMMON_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace motion +{ +namespace motion_common +{ +// Use same representation as message type +using Real = decltype(autoware_auto_planning_msgs::msg::TrajectoryPoint::heading_rate_rps); +using Command = autoware_auto_vehicle_msgs::msg::VehicleControlCommand; +using Diagnostic = autoware_auto_system_msgs::msg::ControlDiagnostic; +using State = autoware_auto_vehicle_msgs::msg::VehicleKinematicState; +using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; +using Heading = autoware_auto_geometry_msgs::msg::Complex32; +using Orientation = geometry_msgs::msg::Quaternion; +using Index = decltype(Trajectory::points)::size_type; +using Point = decltype(Trajectory::points)::value_type; + +/// Check if a state is past a given trajectory point, assuming heading is correct +MOTION_COMMON_PUBLIC bool is_past_point(const Point & state, const Point & pt) noexcept; +/// Check if a state is past a given trajectory point given the last (aka current) +/// trajectory point +MOTION_COMMON_PUBLIC bool is_past_point( + const Point & state, const Point & current_pt, const Point & next_pt) noexcept; +/// Given a normal, determine if state is past a point +MOTION_COMMON_PUBLIC +bool is_past_point(const Point & state, const Point & pt, double nx, double ny) noexcept; + +/// Advance to the first trajectory point past state according to criterion is_past_point +template +Index update_reference_index( + const Trajectory & traj, + const State & state, + Index start_idx, + IsPastPointF is_past_point) +{ + // Invariant: m_reference_trajectory.points.size > 0U + if (traj.points.empty()) { + return 0U; + } + auto next_idx = start_idx; + for (auto idx = start_idx; idx < traj.points.size() - 1U; ++idx) { + const auto current_pt = traj.points[idx]; + const auto next_pt = traj.points[idx + 1U]; + + // If I'm not past the next point, then I'm done + if (!is_past_point(state, current_pt, next_pt, traj)) { + break; + } + // Otherwise, update + next_idx = idx + 1U; + } + return next_idx; +} + +/// Check that all heading values in a trajectory are normalized 2D quaternions +MOTION_COMMON_PUBLIC bool heading_ok(const Trajectory & traj); + +//////////////////////////////////////////////////////////////////////////////// +// Operations relating to algebraic manipulation of VehicleKinematicState and TrajectoryPoint + +/// Apply transform to TrajectoryPoint +MOTION_COMMON_PUBLIC void doTransform( + const Point & t_in, + Point & t_out, + const geometry_msgs::msg::TransformStamped & transform) noexcept; +/// Apply transform to VehicleKinematicState +MOTION_COMMON_PUBLIC void doTransform( + const State & t_in, + State & t_out, + const geometry_msgs::msg::TransformStamped & transform) noexcept; + +/// Converts 2D quaternion to simple heading representation +MOTION_COMMON_PUBLIC Real to_angle(Heading heading) noexcept; +/// Converts 3D quaternion to simple heading representation +MOTION_COMMON_PUBLIC Real to_angle(Orientation orientation) noexcept; + +/// Basic conversion +template +Heading heading_from_angle(RealT angle) noexcept +{ + static_assert(std::is_floating_point::value, "angle must be floating point"); + Heading ret{}; + ret.real = static_cast(std::cos(angle * RealT{0.5})); + ret.imag = static_cast(std::sin(angle * RealT{0.5})); + return ret; +} + +template +Orientation from_angle(RealT angle) noexcept +{ + static_assert(std::is_floating_point::value, "angle must be floating point"); + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, angle); + return tf2::toMsg(quat); +} + +/// \brief Converts a quaternion-like object to a simple heading representation +/// \tparam QuatT A quaternion-like object with at least z and w members +/// \param[in] quat A quaternion-like object to be converted to a heading object +/// \returns A converted heading object +template +Heading from_quat(QuatT quat) noexcept +{ + Heading ret{}; + ret.real = quat.w; + ret.imag = quat.z; + return ret; +} + +/// Standard clamp implementation +template +T clamp(T val, T min, T max) +{ + if (max < min) { + throw std::domain_error{"max < min"}; + } + return std::min(max, std::max(min, val)); +} + +/// Standard linear interpolation +template +T interpolate(T a, T b, RealT t) +{ + static_assert(std::is_floating_point::value, "t must be floating point"); + t = clamp(t, RealT{0.0}, RealT{1.0}); + const auto del = b - a; + return static_cast(t * static_cast(del)) + a; +} + +/// Spherical linear interpolation +MOTION_COMMON_PUBLIC Orientation slerp(const Orientation & a, const Orientation & b, const Real t); + +/// Trajectory point interpolation +template +Point interpolate(Point a, Point b, Real t, SlerpF slerp_fn) +{ + Point ret{rosidl_runtime_cpp::MessageInitialization::ALL}; + { + const auto dt0 = time_utils::from_message(a.time_from_start); + const auto dt1 = time_utils::from_message(b.time_from_start); + ret.time_from_start = time_utils::to_message(time_utils::interpolate(dt0, dt1, t)); + } + ret.pose.position.x = interpolate(a.pose.position.x, b.pose.position.x, t); + ret.pose.position.y = interpolate(a.pose.position.y, b.pose.position.y, t); + ret.pose.orientation = slerp_fn(a.pose.orientation, b.pose.orientation, t); + ret.longitudinal_velocity_mps = + interpolate(a.longitudinal_velocity_mps, b.longitudinal_velocity_mps, t); + ret.lateral_velocity_mps = interpolate(a.lateral_velocity_mps, b.lateral_velocity_mps, t); + ret.acceleration_mps2 = interpolate(a.acceleration_mps2, b.acceleration_mps2, t); + ret.heading_rate_rps = interpolate(a.heading_rate_rps, b.heading_rate_rps, t); + ret.front_wheel_angle_rad = interpolate(a.front_wheel_angle_rad, b.front_wheel_angle_rad, t); + ret.rear_wheel_angle_rad = interpolate(a.rear_wheel_angle_rad, b.rear_wheel_angle_rad, t); + + return ret; +} + +/// Default point interpolation +MOTION_COMMON_PUBLIC Point interpolate(Point a, Point b, Real t); + +/// Sample a trajectory using interpolation; does not extrapolate +template +void sample( + const Trajectory & in, + Trajectory & out, + std::chrono::nanoseconds period, + SlerpF slerp_fn) +{ + out.points.clear(); + out.header = in.header; + if (in.points.empty()) { + return; + } + // Determine number of points + using time_utils::from_message; + const auto last_time = from_message(in.points.back().time_from_start); + auto count_ = last_time / period; + // should round down + using SizeT = typename decltype(in.points)::size_type; + const auto count = + static_cast(std::min(count_, static_cast(in.CAPACITY))); + out.points.reserve(count); + // Insert first point + out.points.push_back(in.points.front()); + // Add remaining points + auto ref_duration = period; + auto after_current_ref_idx = SizeT{1}; + for (auto idx = SizeT{1}; idx < count; ++idx) { + // Determine next reference index + for (auto jdx = after_current_ref_idx; jdx < in.points.size(); ++jdx) { + const auto & pt = in.points[jdx]; + if (from_message(pt.time_from_start) > ref_duration) { + after_current_ref_idx = jdx; + break; + } + } + // Interpolate + { + const auto & curr_pt = in.points[after_current_ref_idx - 1U]; + const auto & next_pt = in.points[after_current_ref_idx]; + const auto dt = std::chrono::duration_cast>( + from_message(next_pt.time_from_start) - from_message(curr_pt.time_from_start)); + const auto dt_ = std::chrono::duration_cast>( + ref_duration - from_message(curr_pt.time_from_start)); + const auto t = dt_.count() / dt.count(); + out.points.push_back(interpolate(curr_pt, next_pt, t, slerp_fn)); + } + ref_duration += period; + } +} + +/// Trajectory sampling with default interpolation +MOTION_COMMON_PUBLIC void sample( + const Trajectory & in, + Trajectory & out, + std::chrono::nanoseconds period); + +/// Diagnostic header stuff +MOTION_COMMON_PUBLIC +void error(const Point & state, const Point & ref, Diagnostic & out) noexcept; +} // namespace motion_common +} // namespace motion + +namespace autoware_auto_geometry_msgs +{ +namespace msg +{ +// TODO(c.ho) these should go into some autoware_auto_msgs package +// TODO(Maxime CLEMENT): remove is these are no longer used +/// Addition operator +MOTION_COMMON_PUBLIC Complex32 operator+(Complex32 a, Complex32 b) noexcept; +/// Unary minus +MOTION_COMMON_PUBLIC Complex32 operator-(Complex32 a) noexcept; +/// Difference operator +MOTION_COMMON_PUBLIC Complex32 operator-(Complex32 a, Complex32 b) noexcept; +} // namespace msg +} // namespace autoware_auto_msgs +namespace geometry_msgs +{ +namespace msg +{ +/// Addition operator +MOTION_COMMON_PUBLIC Quaternion operator+(Quaternion a, Quaternion b) noexcept; +/// Unary minus +MOTION_COMMON_PUBLIC Quaternion operator-(Quaternion a) noexcept; +/// Difference operator +MOTION_COMMON_PUBLIC Quaternion operator-(Quaternion a, Quaternion b) noexcept; +} // namespace msg +} // namespace geometry_msgs +#endif // MOTION_COMMON__MOTION_COMMON_HPP_ diff --git a/common/motion_common/include/motion_common/trajectory_common.hpp b/common/motion_common/include/motion_common/trajectory_common.hpp new file mode 100644 index 0000000000000..afd64496335ef --- /dev/null +++ b/common/motion_common/include/motion_common/trajectory_common.hpp @@ -0,0 +1,170 @@ +// Copyright 2021 the Autoware Foundation +// +// 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 MOTION_COMMON__TRAJECTORY_COMMON_HPP_ +#define MOTION_COMMON__TRAJECTORY_COMMON_HPP_ + +#include +#include +#include +#include + +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "common/types.hpp" +#include "eigen3/Eigen/Core" +#include "geometry/common_2d.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "helper_functions/angle_utils.hpp" +#include "motion_common/motion_common.hpp" +#include "tf2/utils.h" + +namespace autoware +{ +namespace motion +{ +namespace motion_common +{ +typedef autoware_auto_planning_msgs::msg::TrajectoryPoint Point; +typedef decltype (autoware_auto_planning_msgs::msg::Trajectory::points) Points; +using autoware::common::types::float64_t; +typedef Eigen::Matrix Vector3f; + +/** + * @brief throws an exception if the given list of points is empty + * @param [in] points list of points to check + */ +MOTION_COMMON_PUBLIC void validateNonEmpty(const Points & points); + +/** + * @brief calculate the yaw deviation between two angles + * @param [in] base_yaw base yaw angle [radians] + * @param [in] target_yaw target yaw angle [radians] + * @return normalized angle from the base to the target [radians] + */ +MOTION_COMMON_PUBLIC float64_t calcYawDeviation( + const float64_t & base_yaw, + const float64_t & target_yaw); + +/** + * @brief search first index with a velocity of zero in the given range of points + * @param [in] points list of points to check + * @param [in] src_idx starting search index + * @param [in] dst_idx ending (excluded) search index + * @param [in] epsilon optional value to use to determine zero velocities + * @return index of the first zero velocity point found + */ +MOTION_COMMON_PUBLIC std::experimental::optional searchZeroVelocityIndex( + const Points & points, const size_t src_idx, const size_t dst_idx, + const float64_t epsilon = 1e-3); + +/** + * @brief search first index with a velocity of zero in the given points + * @param [in] points list of points to check + */ +MOTION_COMMON_PUBLIC std::experimental::optional searchZeroVelocityIndex( + const Points & points); + +/** + * @brief search the index of the point nearest to the given target + * @param [in] points list of points to search + * @param [in] point target point + * @return index of the point nearest to the target + */ +MOTION_COMMON_PUBLIC size_t findNearestIndex( + const Points & points, + const geometry_msgs::msg::Point & point); + +/** + * @brief search the index of the point nearest to the given target with limits on the distance and yaw deviation + * @param [in] points list of points to search + * @param [in] pose target point + * @param [in] max_dist optional maximum distance from the pose when searching for the nearest index + * @param [in] max_yaw optional maximum deviation from the pose when searching for the nearest index + * @return index of the point nearest to the target + */ +MOTION_COMMON_PUBLIC std::experimental::optional findNearestIndex( + const Points & points, const geometry_msgs::msg::Pose & pose, + const float64_t max_dist = std::numeric_limits::max(), + const float64_t max_yaw = std::numeric_limits::max()); + +/** + * @brief calculate 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 + * @param points trajectory points + * @param seg_idx segment index of point at beginning of length + * @param p_target target point at end of length + * @return signed length + */ +MOTION_COMMON_PUBLIC float64_t calcLongitudinalOffsetToSegment( + const Points & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target); + +/** + * @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 + * @param point point to which to find nearest segment index + * @return nearest index + */ +MOTION_COMMON_PUBLIC size_t findNearestSegmentIndex( + const Points & points, + const geometry_msgs::msg::Point & point); + +/** + * @brief calculate arc length along points + * @param [in] points input points + * @param [in] src_idx source index + * @param [in] dst_idx destination index + * @return arc length distance from source to destination along the input points + */ +MOTION_COMMON_PUBLIC float64_t calcSignedArcLength( + const Points & points, const size_t src_idx, + const size_t dst_idx); + +/** + * @brief calculate arc length along points + * @param [in] points input points + * @param [in] src_point source point + * @param [in] dst_idx destination index + * @return arc length distance from source to destination along the input points + */ +MOTION_COMMON_PUBLIC float64_t calcSignedArcLength( + const Points & points, const geometry_msgs::msg::Point & src_point, const size_t & dst_idx); + +/** + * @brief calculate arc length along points + * @param [in] points input points + * @param [in] src_point source point + * @param [in] dst_point destination point + * @return arc length distance from source to destination along the input points + */ +MOTION_COMMON_PUBLIC float64_t calcSignedArcLength( + const Points & points, const geometry_msgs::msg::Point & src_point, + const geometry_msgs::msg::Point & dst_point); + +/** + * @brief calculate longitudinal deviation of a point relative to a pose + * @param [in] base_pose base from which to calculate the deviation + * @param [in] target_point point for which to calculate the deviation + * @return longitudinal distance between the base and the target + */ +MOTION_COMMON_PUBLIC float64_t calcLongitudinalDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Point & target_point); + +} // namespace motion_common +} // namespace motion +} // namespace autoware + +#endif // MOTION_COMMON__TRAJECTORY_COMMON_HPP_ diff --git a/common/motion_common/include/motion_common/visibility_control.hpp b/common/motion_common/include/motion_common/visibility_control.hpp new file mode 100644 index 0000000000000..75725a22ffa23 --- /dev/null +++ b/common/motion_common/include/motion_common/visibility_control.hpp @@ -0,0 +1,36 @@ +// Copyright 2019 Christopher Ho +// +// 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 MOTION_COMMON__VISIBILITY_CONTROL_HPP_ +#define MOTION_COMMON__VISIBILITY_CONTROL_HPP_ + +#if defined(__WIN32) + #if defined(MOTION_COMMON_BUILDING_DLL) || defined(MOTION_COMMON_EXPORTS) + #define MOTION_COMMON_PUBLIC __declspec(dllexport) + #define MOTION_COMMON_LOCAL + #else // defined(MOTION_COMMON_BUILDING_DLL) || defined(MOTION_COMMON_EXPORTS) + #define MOTION_COMMON_PUBLIC __declspec(dllimport) + #define MOTION_COMMON_LOCAL + #endif // defined(MOTION_COMMON_BUILDING_DLL) || defined(MOTION_COMMON_EXPORTS) +#elif defined(__linux__) + #define MOTION_COMMON_PUBLIC __attribute__((visibility("default"))) + #define MOTION_COMMON_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) + #define MOTION_COMMON_PUBLIC __attribute__((visibility("default"))) + #define MOTION_COMMON_LOCAL __attribute__((visibility("hidden"))) +#else // defined(_LINUX) + #error "Unsupported Build Configuration" +#endif // defined(_WINDOWS) + +#endif // MOTION_COMMON__VISIBILITY_CONTROL_HPP_ diff --git a/common/motion_common/package.xml b/common/motion_common/package.xml new file mode 100644 index 0000000000000..2fb4751dbf872 --- /dev/null +++ b/common/motion_common/package.xml @@ -0,0 +1,33 @@ + + + + motion_common + 1.0.0 + + Helper functions and base classes to aid the development of motion controllers and planners + + Christopher Ho + Apache License 2.0 + + ament_cmake_auto + autoware_auto_cmake + + autoware_auto_cmake + + autoware_auto_common + autoware_auto_geometry + autoware_auto_geometry_msgs + eigen + geometry_msgs + tf2 + tf2_geometry_msgs + time_utils + + motion_testing + + apex_test_tools + + + + ament_cmake + diff --git a/common/motion_common/scripts/autogeneration_code_snippets/kinematic_bicycle.snippet.hpp b/common/motion_common/scripts/autogeneration_code_snippets/kinematic_bicycle.snippet.hpp new file mode 100644 index 0000000000000..517b34f1df6b0 --- /dev/null +++ b/common/motion_common/scripts/autogeneration_code_snippets/kinematic_bicycle.snippet.hpp @@ -0,0 +1,50 @@ +// Copyright 2019 Christopher Ho +// +// 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 COMMON__MOTION_COMMON__SCRIPTS__AUTOGENERATION_CODE_SNIPPETS__KINEMATIC_BICYCLE_SNIPPET_HPP_ // NOLINT +#define COMMON__MOTION_COMMON__SCRIPTS__AUTOGENERATION_CODE_SNIPPETS__KINEMATIC_BICYCLE_SNIPPET_HPP_ // NOLINT + +// For help on getting random weird models to work, see: +// https://sourceforge.net/p/acado/discussion/general +// and +// https://github.com/acado/acado/issues + +// Note: Order of variable declaration specifies order in C-arrays +// +// Variables +// + +DifferentialState x, y, yaw; // pose +DifferentialState u; +Control ax; // acceleration +Control delta; // wheel angle + +// Vehicle parameters +OnlineData L_f, L_r; // front, rear wheelbase length +// +// Differential algebraic equation +// + +// Kinematic bicycle model: +// https://borrelli.me.berkeley.edu/pdfpub/IV_KinematicMPC_jason.pdf +// Intermediate variables +IntermediateState beta = atan((L_r * tan(delta)) / (L_f + L_r)); + +DifferentialEquation f; + +f << dot(x) == u * cos(yaw + beta); +f << dot(y) == u * sin(yaw + beta); +f << dot(yaw) == (u * sin(beta)) / L_r; +f << dot(u) == ax; +#endif // COMMON__MOTION_COMMON__SCRIPTS__AUTOGENERATION_CODE_SNIPPETS__KINEMATIC_BICYCLE_SNIPPET_HPP_ // NOLINT diff --git a/common/motion_common/scripts/autogeneration_code_snippets/linear_tire.snippet.hpp b/common/motion_common/scripts/autogeneration_code_snippets/linear_tire.snippet.hpp new file mode 100644 index 0000000000000..8f3aa4881c81c --- /dev/null +++ b/common/motion_common/scripts/autogeneration_code_snippets/linear_tire.snippet.hpp @@ -0,0 +1,59 @@ +// Copyright 2019 Christopher Ho +// +// 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 COMMON__MOTION_COMMON__SCRIPTS__AUTOGENERATION_CODE_SNIPPETS__LINEAR_TIRE_SNIPPET_HPP_ // NOLINT +#define COMMON__MOTION_COMMON__SCRIPTS__AUTOGENERATION_CODE_SNIPPETS__LINEAR_TIRE_SNIPPET_HPP_ // NOLINT +// For help on getting random weird models to work, see: +// https://sourceforge.net/p/acado/discussion/general +// and +// https://github.com/acado/acado/issues + +// Note: Order of variable declaration specifies order in C-arrays +// +// Variables +// + +DifferentialState x, y, yaw; // pose +DifferentialState u, v, omega; // velocities +DifferentialState delta; // wheel angle +DifferentialState ax; // acceleration +Control jx, delta_dot; + +// Vehicle parameters +OnlineData L_f, L_r; // front, rear wheelbase length +OnlineData C_f, C_r; // front, rear cornering stiffness +OnlineData m, I; // mass, moment of inertia + +// +// Differential algebraic equation +// + +// Using the linear model due to LaValle: +// http://planning.cs.uiuc.edu/node695.html +// Intermediate variables +IntermediateState F_f = C_f * (((v + (L_f * omega)) / u) + delta); +IntermediateState F_r = C_r * ((v - (L_r * omega)) / u); + +DifferentialEquation f; + +// Easy stuff +f << dot(x) == ((u * cos(yaw)) - (v * sin(yaw))); +f << dot(y) == ((u * sin(yaw)) + (v * cos(yaw))); +f << dot(yaw) == omega; +f << dot(u) == ax; +f << dot(v) == ((-u * omega) + ((F_f + F_r) / m)); +f << dot(omega) == (((L_f * F_f) - (L_r * F_r)) / I); +f << dot(delta) == delta_dot; +f << dot(ax) == jx; +#endif // COMMON__MOTION_COMMON__SCRIPTS__AUTOGENERATION_CODE_SNIPPETS__LINEAR_TIRE_SNIPPET_HPP_ // NOLINT diff --git a/common/motion_common/src/motion_common/config.cpp b/common/motion_common/src/motion_common/config.cpp new file mode 100644 index 0000000000000..c6784a0bb931e --- /dev/null +++ b/common/motion_common/src/motion_common/config.cpp @@ -0,0 +1,264 @@ +// Copyright 2019 Christopher Ho +// +// 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 "motion_common/config.hpp" + +#include +#include + +namespace motion +{ +namespace motion_common +{ + +LimitsConfig::Extremum::Extremum(Real min, Real max) +: m_min{min}, m_max{max} +{ + if (min >= max - std::numeric_limits::epsilon()) { + throw std::domain_error{"Extremum: min >= max - epsilon"}; + } +} + +Real LimitsConfig::Extremum::min() const noexcept +{ + return m_min; +} +Real LimitsConfig::Extremum::max() const noexcept +{ + return m_max; +} + +//////////////////////////////////////////////////////////////////////////////// +LimitsConfig::LimitsConfig( + Extremum longitudinal_velocity_mps, + Extremum lateral_velocity_mps, + Extremum acceleration_mps2, + Extremum yaw_rate_rps, + Extremum jerk_mps3, + Extremum steer_angle_rad, + Extremum steer_angle_rate_rps) +: m_longitudinal_velocity_limits_mps{longitudinal_velocity_mps}, + m_lateral_velocity_limits_mps{lateral_velocity_mps}, + m_acceleration_limits_mps2{acceleration_mps2}, + m_yaw_rate_limits_rps{yaw_rate_rps}, + m_jerk_limits_mps3{jerk_mps3}, + m_steer_angle_limits_rad{steer_angle_rad}, + m_steer_angle_rate_limits_rps{steer_angle_rate_rps} +{ +} + +LimitsConfig::Extremum LimitsConfig::longitudinal_velocity() const noexcept +{ + return m_longitudinal_velocity_limits_mps; +} +LimitsConfig::Extremum LimitsConfig::lateral_velocity() const noexcept +{ + return m_lateral_velocity_limits_mps; +} +LimitsConfig::Extremum LimitsConfig::acceleration() const noexcept +{ + return m_acceleration_limits_mps2; +} +LimitsConfig::Extremum LimitsConfig::jerk() const noexcept +{ + return m_jerk_limits_mps3; +} +LimitsConfig::Extremum LimitsConfig::steer_angle() const noexcept +{ + return m_steer_angle_limits_rad; +} +LimitsConfig::Extremum LimitsConfig::steer_angle_rate() const noexcept +{ + return m_steer_angle_rate_limits_rps; +} +LimitsConfig::Extremum LimitsConfig::yaw_rate() const noexcept +{ + return m_yaw_rate_limits_rps; +} + +//////////////////////////////////////////////////////////////////////////////// +VehicleConfig::VehicleConfig( + Real length_cg_front_axel_m, + Real length_cg_rear_axel_m, + Real front_cornering_stiffness_N, + Real rear_cornering_stiffness_N, + Real mass_kg, + Real inertia_kgm2, + Real width_m, + Real front_overhang_m, + Real rear_overhang_m) +: m_length_cg_to_front_axel_m{length_cg_front_axel_m}, + m_length_cg_to_rear_axel_m{length_cg_rear_axel_m}, + m_front_cornering_stiffness_N{front_cornering_stiffness_N}, + m_rear_cornering_stiffness_N{rear_cornering_stiffness_N}, + m_mass_kg{mass_kg}, + m_inertia_kgm2{inertia_kgm2}, + m_width_m{width_m}, + m_front_overhang_m{front_overhang_m}, + m_rear_overhang_m{rear_overhang_m} +{ +} + +Real VehicleConfig::length_cg_front_axel() const noexcept +{ + return m_length_cg_to_front_axel_m; +} +Real VehicleConfig::length_cg_rear_axel() const noexcept +{ + return m_length_cg_to_rear_axel_m; +} +Real VehicleConfig::front_cornering_stiffness() const noexcept +{ + return m_front_cornering_stiffness_N; +} +Real VehicleConfig::rear_cornering_stiffness() const noexcept +{ + return m_rear_cornering_stiffness_N; +} +Real VehicleConfig::mass() const noexcept +{ + return m_mass_kg; +} +Real VehicleConfig::inertia() const noexcept +{ + return m_inertia_kgm2; +} +Real VehicleConfig::width() const noexcept +{ + return m_width_m; +} +Real VehicleConfig::front_overhang() const noexcept +{ + return m_front_overhang_m; +} +Real VehicleConfig::rear_overhang() const noexcept +{ + return m_rear_overhang_m; +} + +//////////////////////////////////////////////////////////////////////////////// +StateWeight::StateWeight( + Real pose, + Real heading, + Real longitudinal_velocity, + Real lateral_velocity, + Real yaw_rate, + Real acceleration, + Real jerk, + Real steer_angle, + Real steer_angle_rate) +: m_pose_weight{pose}, + m_heading_weight{heading}, + m_longitudinal_velocity_weight{longitudinal_velocity}, + m_lateral_velocity_weight{lateral_velocity}, + m_yaw_rate_weight{yaw_rate}, + m_acceleration_weight{acceleration}, + m_jerk_weight{jerk}, + m_steer_angle_weight{steer_angle}, + m_steer_angle_rate_weight{steer_angle_rate} +{ + if (pose < Real{}) { // zero initialization + throw std::domain_error{"Pose weight is negative!"}; + } + if (heading < Real{}) { // zero initialization + throw std::domain_error{"Heading weight is negative!"}; + } + if (longitudinal_velocity < Real{}) { // zero initialization + throw std::domain_error{"Longitudinal weight is negative!"}; + } + if (lateral_velocity < Real{}) { // zero initialization + throw std::domain_error{"Lateral velocity weight is negative!"}; + } + if (yaw_rate < Real{}) { // zero initialization + throw std::domain_error{"Yaw rate weight is negative!"}; + } + if (acceleration < Real{}) { // zero initialization + throw std::domain_error{"Acceleration weight is negative!"}; + } + if (jerk < Real{}) { // zero initialization + throw std::domain_error{"Jerk weight is negative!"}; + } + if (steer_angle < Real{}) { // zero initialization + throw std::domain_error{"Steer angle weight is negative!"}; + } + if (steer_angle_rate < Real{}) { // zero initialization + throw std::domain_error{"Steer angle rate weight is negative!"}; + } +} + +Real StateWeight::pose() const noexcept +{ + return m_pose_weight; +} + +Real StateWeight::heading() const noexcept +{ + return m_heading_weight; +} + +Real StateWeight::longitudinal_velocity() const noexcept +{ + return m_longitudinal_velocity_weight; +} + +Real StateWeight::lateral_velocity() const noexcept +{ + return m_lateral_velocity_weight; +} + +Real StateWeight::yaw_rate() const noexcept +{ + return m_yaw_rate_weight; +} + +Real StateWeight::acceleration() const noexcept +{ + return m_acceleration_weight; +} + +Real StateWeight::steer_angle() const noexcept +{ + return m_steer_angle_weight; +} + +Real StateWeight::steer_angle_rate() const noexcept +{ + return m_steer_angle_rate_weight; +} + +Real StateWeight::jerk() const noexcept +{ + return m_jerk_weight; +} + +//////////////////////////////////////////////////////////////////////////////// +OptimizationConfig::OptimizationConfig( + StateWeight nominal_weights, + StateWeight terminal_weights) +: m_nominal_weights{nominal_weights}, + m_terminal_weights{terminal_weights} +{ +} + +StateWeight OptimizationConfig::nominal() const noexcept +{ + return m_nominal_weights; +} + +StateWeight OptimizationConfig::terminal() const noexcept +{ + return m_terminal_weights; +} +} // namespace motion_common +} // namespace motion diff --git a/common/motion_common/src/motion_common/motion_common.cpp b/common/motion_common/src/motion_common/motion_common.cpp new file mode 100644 index 0000000000000..d89ba33d9021e --- /dev/null +++ b/common/motion_common/src/motion_common/motion_common.cpp @@ -0,0 +1,270 @@ +// Copyright 2019-2021 the Autoware Foundation +// +// 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 "motion_common/motion_common.hpp" + +#include +#include +#include + +#include "helper_functions/angle_utils.hpp" +#include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" + +namespace motion +{ +namespace motion_common +{ +//////////////////////////////////////////////////////////////////////////////// +bool is_past_point(const Point & state, const Point & pt) noexcept +{ + const auto w = static_cast(pt.pose.orientation.w); + const auto z = static_cast(pt.pose.orientation.z); + // double angle rule + const auto c = (w + z) * (w - z); + const auto s = 2.0F * w * z; + + return is_past_point(state, pt, c, s); +} + +//////////////////////////////////////////////////////////////////////////////// +bool is_past_point( + const Point & state, + const Point & current_pt, + const Point & next_pt) noexcept +{ + const auto nx = next_pt.pose.position.x - current_pt.pose.position.x; + const auto ny = next_pt.pose.position.y - current_pt.pose.position.y; + + return is_past_point(state, next_pt, nx, ny); +} + +//////////////////////////////////////////////////////////////////////////////// +bool is_past_point( + const Point & state, + const Point & pt, + const double nx, + const double ny) noexcept +{ + const auto dx = (state.pose.position.x - pt.pose.position.x); + const auto dy = (state.pose.position.y - pt.pose.position.y); + + // Check if state is past last_pt when projected onto the ray defined by heading + return ((nx * dx) + (ny * dy)) >= -std::numeric_limits::epsilon(); +} + +//////////////////////////////////////////////////////////////////////////////// +bool is_aligned(const Heading a, const Heading b, const Real dot_threshold) +{ + if (dot_threshold < Real{}) { + throw std::domain_error{"Dot product threshold cannot be negative"}; + } + const auto dot = (a.real * b.real) + (a.imag * b.imag); + const auto amag = std::sqrt((a.real * a.real) + (a.imag * a.imag)); + const auto bmag = std::sqrt((b.real * b.real) + (b.imag * b.imag)); + const auto thresh = std::min(dot_threshold, Real{1.0}); + return (dot / (amag * bmag)) > thresh; +} + +//////////////////////////////////////////////////////////////////////////////// +bool heading_ok(const Trajectory & traj) +{ + const auto bad_heading = [](const auto & pt) -> bool { + const auto real2 = static_cast(pt.pose.orientation.w * pt.pose.orientation.w); + const auto imag2 = static_cast(pt.pose.orientation.z * pt.pose.orientation.z); + constexpr auto TOL = 1.0E-3F; + return std::fabs(1.0F - (real2 + imag2)) > TOL; + }; + const auto bad_it = std::find_if(traj.points.begin(), traj.points.end(), bad_heading); + // True if there is no bad point + return bad_it == traj.points.end(); +} + +//////////////////////////////////////////////////////////////////////////////// +void doTransform( + const Point & t_in, + Point & t_out, + const geometry_msgs::msg::TransformStamped & transform) noexcept +{ + geometry_msgs::msg::PoseStamped p_in; + p_in.pose = t_in.pose; + p_in.header.stamp = transform.header.stamp; + geometry_msgs::msg::PoseStamped p_out; + tf2::doTransform(p_in, p_out, transform); + t_out.pose = p_out.pose; +} + +//////////////////////////////////////////////////////////////////////////////// +void doTransform( + const State & t_in, + State & t_out, + const geometry_msgs::msg::TransformStamped & transform) noexcept +{ + doTransform(t_in.state, t_out.state, transform); + t_out.header.frame_id = transform.header.frame_id; +} + +//////////////////////////////////////////////////////////////////////////////// +Real to_angle(Heading heading) noexcept +{ + const auto mag2 = (heading.real * heading.real) + (heading.imag * heading.imag); + if (std::abs(mag2 - 1.0F) > std::numeric_limits::epsilon()) { + const auto imag = Real{1} / std::sqrt(mag2); + heading.real *= imag; + heading.imag *= imag; + // Don't need to touch imaginary/z part + } + // See: + // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + const auto y = Real{2.0} *heading.real * heading.imag; + const auto x = Real{1} - (Real{2.0} *heading.imag * heading.imag); + // TODO(c.ho) fast atan2 + return std::atan2(y, x); +} + +//////////////////////////////////////////////////////////////////////////////// +Real to_angle(Orientation orientation) noexcept +{ + return static_cast(tf2::getYaw(orientation)); +} + +//////////////////////////////////////////////////////////////////////////////// +Heading nlerp(Heading a, Heading b, Real t) +{ + // Could technically use none, but I get basically nothing from that + Heading ret{rosidl_runtime_cpp::MessageInitialization::ALL}; + // check dot product: if negative, reflect one quaternion (360 deg rotation) + { + const auto dot = (a.real * b.real) + (a.imag * b.imag); + if (dot < Real{}) { // zero initialization + b.real = -b.real; + b.imag = -b.imag; + } + } + // Linearly interpolate + ret.real = interpolate(a.real, b.real, t); + ret.imag = interpolate(a.imag, b.imag, t); + // Normalize + const auto s = 1.0F / std::sqrt((ret.real * ret.real) + (ret.imag * ret.imag)); + ret.real *= s; + ret.imag *= s; + return ret; +} + +//////////////////////////////////////////////////////////////////////////////// +Orientation slerp(const Orientation & a, const Orientation & b, const Real t) +{ + tf2::Quaternion quat_a; + tf2::Quaternion quat_b; + tf2::fromMsg(a, quat_a); + tf2::fromMsg(b, quat_b); + return tf2::toMsg(quat_a.slerp(quat_b, t)); +} + +//////////////////////////////////////////////////////////////////////////////// +Point interpolate(Point a, Point b, Real t) +{ + return interpolate(a, b, t, slerp); +} + +//////////////////////////////////////////////////////////////////////////////// +void sample(const Trajectory & in, Trajectory & out, std::chrono::nanoseconds period) +{ + sample(in, out, period, slerp); +} + +//////////////////////////////////////////////////////////////////////////////// +void error(const Point & state, const Point & ref, Diagnostic & out) noexcept +{ + { + // compute heading normal of reference point + const auto & q = ref.pose.orientation; + const auto nx = (q.w * q.w) - (q.z * q.z); + const auto ny = decltype(nx) {2.0} *q.w * q.z; + // project state onto reference basis + const auto dx = state.pose.position.x - ref.pose.position.x; + const auto dy = state.pose.position.y - ref.pose.position.y; + // normals rotated +90 deg + out.lateral_error_m = static_cast((dx * (-ny)) + (dy * nx)); + out.longitudinal_error_m = static_cast((dx * nx) + (dy * ny)); + } + out.velocity_error_mps = state.longitudinal_velocity_mps - ref.longitudinal_velocity_mps; + out.acceleration_error_mps2 = state.acceleration_mps2 - ref.acceleration_mps2; + + using autoware::common::helper_functions::wrap_angle; + out.yaw_error_rad = wrap_angle(to_angle(state.pose.orientation) - to_angle(ref.pose.orientation)); + out.yaw_rate_error_rps = state.heading_rate_rps - ref.heading_rate_rps; +} +} // namespace motion_common +} // namespace motion +namespace autoware_auto_geometry_msgs +{ +namespace msg +{ +Complex32 operator+(Complex32 a, Complex32 b) noexcept +{ + // Could technically use none, but I get basically nothing from that + Complex32 ret{rosidl_runtime_cpp::MessageInitialization::ALL}; + // check dot product: if negative, reflect one quaternion (360 deg rotation) + { + const auto dot = (a.real * b.real) + (a.imag * b.imag); + if (dot < decltype(b.real) {}) { // zero initialization + b.real = -b.real; + b.imag = -b.imag; + } + } + ret.real = (a.real * b.real) - (a.imag * b.imag); + ret.imag = (a.real * b.imag) + (a.imag * b.real); + return ret; +} +Complex32 operator-(Complex32 a) noexcept +{ + a.real = -a.real; + return a; +} +Complex32 operator-(Complex32 a, Complex32 b) noexcept +{ + return a + (-b); +} +} // namespace msg +} // namespace autoware_auto_msgs + +namespace geometry_msgs +{ +namespace msg +{ +Quaternion operator+(Quaternion a, Quaternion b) noexcept +{ + tf2::Quaternion quat_a; + tf2::Quaternion quat_b; + tf2::fromMsg(a, quat_a); + tf2::fromMsg(b, quat_b); + return tf2::toMsg(quat_a + quat_b); +} +Quaternion operator-(Quaternion a) noexcept +{ + tf2::Quaternion quat_a; + tf2::fromMsg(a, quat_a); + return tf2::toMsg(quat_a * -1.0); +} +Quaternion operator-(Quaternion a, Quaternion b) noexcept +{ + tf2::Quaternion quat_a; + tf2::Quaternion quat_b; + tf2::fromMsg(a, quat_a); + tf2::fromMsg(b, quat_b); + return tf2::toMsg(quat_a * quat_b.inverse()); +} +} // namespace msg +} // namespace geometry_msgs diff --git a/common/motion_common/src/motion_common/trajectory_common.cpp b/common/motion_common/src/motion_common/trajectory_common.cpp new file mode 100644 index 0000000000000..f461bc77973a7 --- /dev/null +++ b/common/motion_common/src/motion_common/trajectory_common.cpp @@ -0,0 +1,219 @@ +// Copyright 2021 the Autoware Foundation +// +// 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 "motion_common/trajectory_common.hpp" + +#include + +#include "motion_common/motion_common.hpp" + +namespace autoware +{ +namespace motion +{ +namespace motion_common +{ +using ::motion::motion_common::to_angle; + +void validateNonEmpty(const Points & points) +{ + if (points.empty()) { + throw std::invalid_argument("Empty points"); + } +} + +float64_t calcYawDeviation(const float64_t & base_yaw, const float64_t & target_yaw) +{ + return autoware::common::helper_functions::wrap_angle(target_yaw - base_yaw); +} + +std::experimental::optional searchZeroVelocityIndex( + const Points & points, const size_t src_idx, const size_t dst_idx, const float64_t epsilon) +{ + validateNonEmpty(points); + + for (size_t i = src_idx; i < dst_idx; ++i) { + if (static_cast(std::fabs(points.at(i).longitudinal_velocity_mps)) < epsilon) { + return i; + } + } + + return {}; +} + +std::experimental::optional searchZeroVelocityIndex(const Points & points) +{ + return searchZeroVelocityIndex(points, 0, points.size()); +} + +size_t findNearestIndex(const Points & points, const geometry_msgs::msg::Point & point) +{ + validateNonEmpty(points); + + float64_t min_dist = std::numeric_limits::max(); + size_t min_idx = 0; + + for (size_t i = 0; i < points.size(); ++i) { + const auto dist = autoware::common::geometry::distance_2d(points.at(i), point); + if (dist < min_dist) { + min_dist = dist; + min_idx = i; + } + } + return min_idx; +} + +std::experimental::optional findNearestIndex( + const Points & points, const geometry_msgs::msg::Pose & pose, + const float64_t max_dist, + const float64_t max_yaw) +{ + validateNonEmpty(points); + + float64_t min_dist = std::numeric_limits::max(); + bool is_nearest_found = false; + size_t min_idx = 0; + + const auto target_yaw = to_angle(pose.orientation); + for (size_t i = 0; i < points.size(); ++i) { + const auto dist = + autoware::common::geometry::distance_2d(points.at(i), pose.position); + if (dist > max_dist) { + continue; + } + + const auto base_yaw = to_angle(points.at(i).pose.orientation); + const auto yaw = calcYawDeviation(base_yaw, target_yaw); + if (std::fabs(yaw) > max_yaw) { + continue; + } + + if (dist >= min_dist) { + continue; + } + + min_dist = dist; + min_idx = i; + is_nearest_found = true; + } + return is_nearest_found ? std::experimental::optional(min_idx) : std::experimental:: + nullopt; +} + +float64_t calcLongitudinalOffsetToSegment( + const Points & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target) +{ + validateNonEmpty(points); + + const auto p_front = points.at(seg_idx); + const auto p_back = points.at(seg_idx + 1); + const auto x_front = static_cast(p_front.pose.position.x); + const auto y_front = static_cast(p_front.pose.position.y); + const auto x_back = static_cast(p_back.pose.position.x); + const auto y_back = static_cast(p_back.pose.position.y); + + const Vector3f segment_vec{x_back - x_front, y_back - y_front, 0.0}; + const Vector3f target_vec{static_cast(p_target.x) - x_front, + static_cast(p_target.y) - y_front, 0.0}; + + if (segment_vec.norm() == 0.0) { + throw std::runtime_error("Same points are given."); + } + + return segment_vec.dot(target_vec) / segment_vec.norm(); +} + +size_t findNearestSegmentIndex(const Points & points, const geometry_msgs::msg::Point & point) +{ + const size_t nearest_idx = findNearestIndex(points, point); + + if (nearest_idx == 0) { + return 0; + } else if (nearest_idx == points.size() - 1) { + return points.size() - 2; + } + + const float64_t signed_length = calcLongitudinalOffsetToSegment(points, nearest_idx, point); + + if (signed_length <= 0) { + return nearest_idx - 1; + } + + return nearest_idx; +} + +float64_t calcSignedArcLength(const Points & points, const size_t src_idx, const size_t dst_idx) +{ + validateNonEmpty(points); + + if (src_idx > dst_idx) { + return -calcSignedArcLength(points, dst_idx, src_idx); + } + + float64_t dist_sum = 0.0; + for (size_t i = src_idx; i < dst_idx; ++i) { + dist_sum += autoware::common::geometry::distance_2d(points.at(i), points.at(i + 1)); + } + return dist_sum; +} + +float64_t calcSignedArcLength( + const Points & points, const geometry_msgs::msg::Point & src_point, const size_t & dst_idx) +{ + validateNonEmpty(points); + + const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); + + const float64_t signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_idx); + const float64_t signed_length_src_offset = + calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); + + return signed_length_on_traj - signed_length_src_offset; +} + +float64_t calcSignedArcLength( + const Points & points, const geometry_msgs::msg::Point & src_point, + const geometry_msgs::msg::Point & dst_point) +{ + validateNonEmpty(points); + + const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); + const size_t dst_seg_idx = findNearestSegmentIndex(points, dst_point); + + const float64_t signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_seg_idx); + const float64_t signed_length_src_offset = + calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); + const float64_t signed_length_dst_offset = + calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point); + + return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; +} + +float64_t calcLongitudinalDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Point & target_point) +{ + const auto & base_point = base_pose.position; + + const auto yaw = to_angle(base_pose.orientation); + const Vector3f base_unit_vec{std::cos(yaw), std::sin(yaw), 0}; + + const auto dx = target_point.x - base_point.x; + const auto dy = target_point.y - base_point.y; + const Vector3f diff_vec{dx, dy, 0}; + + return base_unit_vec.dot(diff_vec); +} +} // namespace motion_common +} // namespace motion +} // namespace autoware diff --git a/common/motion_common/test/gtest_main.cpp b/common/motion_common/test/gtest_main.cpp new file mode 100644 index 0000000000000..809ccb3951198 --- /dev/null +++ b/common/motion_common/test/gtest_main.cpp @@ -0,0 +1,21 @@ +// Copyright 2019 Christopher Ho +// +// 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 + +int32_t main(int32_t argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/motion_common/test/interpolation.cpp b/common/motion_common/test/interpolation.cpp new file mode 100644 index 0000000000000..15ef68b61b302 --- /dev/null +++ b/common/motion_common/test/interpolation.cpp @@ -0,0 +1,271 @@ +// Copyright 2019-2021 Christopher Ho +// +// 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 +#include +#include + +#include +#include + +using motion::motion_common::Command; +using motion::motion_common::Point; +using motion::motion_common::State; +using motion::motion_common::Trajectory; +using motion::motion_common::from_angle; +using motion::motion_common::to_angle; +using motion::motion_testing::make_state; +using motion::motion_testing::constant_velocity_trajectory; + +using time_utils::from_message; + +using std::chrono::milliseconds; + +TEST(Interpolation, Clamp) +{ + const auto fn_floating = [](auto val, auto min, auto max, auto res, auto tol) + { + EXPECT_LT(std::fabs(motion::motion_common::clamp(val, min, max) - res), tol) << + val << ", " << min << ", " << max << ", " << res << ", " << tol; + }; + const auto fn_suite = [ = ](auto min, auto max, auto tol) + { + const auto med = (min + max) / decltype(min) {2.0}; + fn_floating(med, min, max, med, tol); + fn_floating(max, min, max, max, tol); + fn_floating(min, min, max, min, tol); + constexpr auto eps = std::numeric_limits::epsilon(); + fn_floating(min - eps, min, max, min, tol); + fn_floating(max + eps, min, max, max, tol); + constexpr auto lim = std::numeric_limits::max(); + fn_floating(min - lim, min, max, min, tol); + fn_floating(max + lim, min, max, max, tol); + }; + apex_test_tools::memory_test::start_paused(); + // Just have one of these in a compilation unit to make sure everything is hunky-dory +#ifndef __aarch64__ + ASSERT_TRUE(osrf_testing_tools_cpp::memory_tools::is_working()); +#endif + apex_test_tools::memory_test::resume(); + + constexpr auto TOL = 1.0E-5; + fn_suite(5.0, 15.0, TOL); + fn_suite(-15.0, -5.0, TOL); + fn_suite(-5.0, 5.0, TOL); + constexpr auto FTOL = 1.0E-5F; + fn_suite(5.0F, 15.0F, FTOL); + fn_suite(-15.0F, -5.0F, FTOL); + fn_suite(-5.0F, 5.0F, FTOL); + apex_test_tools::memory_test::stop(); + ASSERT_FALSE(osrf_testing_tools_cpp::memory_tools::is_working()); + // TODO(c.ho) ints etc. or typed test +} + +TEST(Interpolation, Interpolation) +{ + const auto fn_floating = [](auto a, auto b, auto t, auto res, auto tol) + { + EXPECT_LT(std::fabs(motion::motion_common::interpolate(a, b, t) - res), tol) << + a << ", " << b << ", " << t << ", " << res << ", " << tol; + }; + constexpr auto TOL = 1.0E-5F; + constexpr auto lim = std::numeric_limits::max(); + // Base case + apex_test_tools::memory_test::start(); + fn_floating(4.0F, 8.0F, -lim, 4.0F, TOL); + fn_floating(4.0F, 8.0F, lim, 8.0F, TOL); + fn_floating(4.0F, 8.0F, 0.0F, 4.0F, TOL); + fn_floating(4.0F, 8.0F, 1.0F, 8.0F, TOL); + fn_floating(4.0F, 8.0F, 0.5F, 6.0F, TOL); + fn_floating(4.0F, 8.0F, 0.25F, 5.0F, TOL); + fn_floating(4.0F, 8.0F, 0.75F, 7.0F, TOL); + // Communitive property + fn_floating(8.0F, 4.0F, -lim, 8.0F, TOL); + fn_floating(8.0F, 4.0F, lim, 4.0F, TOL); + fn_floating(8.0F, 4.0F, 0.0F, 8.0F, TOL); + fn_floating(8.0F, 4.0F, 1.0F, 4.0F, TOL); + fn_floating(8.0F, 4.0F, 0.5F, 6.0F, TOL); + fn_floating(8.0F, 4.0F, 0.25F, 7.0F, TOL); + fn_floating(8.0F, 4.0F, 0.75F, 5.0F, TOL); + apex_test_tools::memory_test::stop(); +} + +TEST(Interpolation, Slerp2d) +{ + using motion::motion_common::Orientation; + using motion::motion_common::Real; + const auto angle_distance = [](Real a, Real b) -> Real + { + // https://stackoverflow.com/questions/1878907/the-smallest-difference-between-2-angles?rq=1 + const auto d = a - b; + return std::atan2(std::sin(d), std::cos(d)); + }; + // Plain check + const auto test_case = [ = ](Orientation a, Orientation b, Real t, Orientation res, Real tol) + { + using motion::motion_common::slerp; + const auto ret = slerp(a, b, t); + EXPECT_LT( + std::fabs(to_angle(ret) - to_angle(res)), + tol) << to_angle(ret) << ", " << to_angle(res); + }; + // Check using alternate computation path + const auto test_case_dual_path = [ = ](Orientation a, Orientation b, Real t, double tol) + { + // Compute result using angles + const auto th_a = to_angle(a); + const auto th_b = to_angle(b); + const auto ab = angle_distance(th_a, th_b); + const auto t_ = motion::motion_common::clamp(t, 0.0F, 1.0F); + const auto th_t = th_a + (t_ * ab); + const auto res_th = from_angle(th_t); + using motion::motion_common::slerp; + test_case(a, b, t, res_th, tol); + if (HasFailure()) { + const auto ret = slerp(a, b, t); + std::cout << "Angles: " << th_a << ", " << th_b << "; " << th_t << ", " << to_angle(ret) << + "\n"; + } + }; + const auto test_suite = + [ = ](Orientation a, Orientation b, Real tol, Real tol2, bool b_snap = false) + { + constexpr auto lim = std::numeric_limits::max(); + // If you do something crazy, make b less crazy wrt a + if (!b_snap) { + test_case(a, b, lim, b, tol); + } + test_case(a, b, -lim, a, tol); + if (!b_snap) { + test_case(a, b, 1.0F, b, tol); + } + test_case(a, b, 0.0F, a, tol); + test_case_dual_path(a, b, 0.25F, tol2); + test_case_dual_path(a, b, 0.5F, tol2); + test_case_dual_path(a, b, 0.75F, tol2); + }; + constexpr auto TOL = 1.0E-5F; + apex_test_tools::memory_test::start(); + // 0 - 180 -> 90 + test_case(from_angle(0.0), from_angle(M_PI_2), 0.5F, from_angle(M_PI_4), TOL); + // 0 - -180 -> -90 + test_case(from_angle(0.0), from_angle(-M_PI_2), 0.5F, from_angle(-M_PI_4), TOL); + // Commutivity + test_case(from_angle(M_PI_2), from_angle(0.0), 0.5F, from_angle(M_PI_4), TOL); + test_case(from_angle(-M_PI_2), from_angle(0.0), 0.5F, from_angle(-M_PI_4), TOL); + // test suite + test_suite(from_angle(0.0), from_angle(M_PI_2), TOL, 0.07F); + test_suite(from_angle(0.0), from_angle(-M_PI_2), TOL, 0.07F); + test_suite(from_angle(0.0), from_angle(M_PI), TOL, TOL, true); + test_suite(from_angle(M_PI_2), from_angle(-M_PI_2), TOL, TOL, true); + apex_test_tools::memory_test::stop(); + // Test case below doesn't work, but I'm pretty sure it's more because the test doesn't work for + // crazy angles + // test_suite(make(-cs45, -cs45), make(-cs45, cs45), TOL, TOL); + // TODO(c.ho) harder cases past the limits of 180, -180 +} +// TODO(c.ho) point interpolation sanity check + +TEST(Interpolation, AngleArithmetic) +{ + const auto test_fn = [](auto a, auto b, auto res, auto tol) { + const auto qa = from_angle(a); + const auto qb = from_angle(b); + const auto dq = qa - qb; + const auto dth = to_angle(dq); + EXPECT_LT(std::fabs(dth - res), tol) << a << ", " << b << ", " << res << ", " << dth; + }; + const auto TOL = 1.0E-3F; + apex_test_tools::memory_test::start(); + test_fn(0.0F, 1.0F, -1.0F, TOL); + test_fn(1.0F, 0.0F, 1.0F, TOL); + test_fn(1.0F, -1.0F, 2.0F, TOL); + test_fn(-1.0F, 1.0F, -2.0F, TOL); + test_fn(-1.0F, -2.0F, 1.0F, TOL); + test_fn(-2.0F, -1.0F, -1.0F, TOL); + test_fn(-3.14145F, 3.14159F, 0.0F, TOL); + test_fn(3.14145F, -3.14159F, 0.0F, TOL); + apex_test_tools::memory_test::stop(); +} + +// These are generic and/or not represented by the CATR model +void generic_checks(const Point & s, const Point & p, std::chrono::nanoseconds dt, float TOL) +{ + EXPECT_LT(std::fabs(s.lateral_velocity_mps - p.lateral_velocity_mps), TOL); + EXPECT_LT(std::fabs(s.front_wheel_angle_rad - p.front_wheel_angle_rad), TOL); + EXPECT_LT(std::fabs(s.rear_wheel_angle_rad - p.rear_wheel_angle_rad), TOL); + EXPECT_TRUE((dt < milliseconds(1)) && (dt > milliseconds(-1))) << + std::chrono::duration_cast(dt).count(); +} +TEST(Interpolation, TrajectorySubsample) +{ + using std::chrono::milliseconds; + const auto dt0 = milliseconds(100LL); + const auto dt = milliseconds(200LL); + const auto t_ref = constant_velocity_trajectory({}, {}, 1.0F, 1.0F, dt0); + const auto t_res = constant_velocity_trajectory({}, {}, 1.0F, 1.0F, dt); + Trajectory result{}; + result.points.reserve(Trajectory::CAPACITY); + apex_test_tools::memory_test::start(); + motion::motion_common::sample(t_ref, result, dt); + EXPECT_LE(result.points.size(), t_res.points.size()); + EXPECT_GT(result.points.size(), 1U); + // TODO(c.ho) check header + for (auto idx = 0U; idx < result.points.size(); ++idx) { + const auto & pt = result.points[idx]; + const auto & ref = t_res.points[idx]; + // Check... + constexpr auto TOL = 1.0E-3F; + EXPECT_LT(std::fabs(pt.pose.position.x - ref.pose.position.x), static_cast(TOL)); + EXPECT_LT(std::fabs(pt.pose.position.y - ref.pose.position.y), static_cast(TOL)); + EXPECT_LT(std::fabs(to_angle(pt.pose.orientation) - to_angle(ref.pose.orientation)), TOL); + EXPECT_LT(std::fabs(pt.longitudinal_velocity_mps - ref.longitudinal_velocity_mps), TOL); + EXPECT_LT(std::fabs(pt.acceleration_mps2 - ref.acceleration_mps2), TOL); + EXPECT_LT(std::fabs(pt.heading_rate_rps - ref.heading_rate_rps), TOL); + const auto dt_err = (dt * idx) - from_message(pt.time_from_start); + generic_checks(pt, ref, dt_err, TOL); + } + apex_test_tools::memory_test::stop(); +} + +TEST(Interpolation, TrajectorySupersample) +{ + using std::chrono::milliseconds; + const auto dt0 = milliseconds(200LL); + const auto dt = milliseconds(100LL); + const auto t_ref = constant_velocity_trajectory({}, {}, 1.0F, 1.0F, dt0); + const auto t_res = constant_velocity_trajectory({}, {}, 1.0F, 1.0F, dt); + Trajectory result{}; + result.points.reserve(Trajectory::CAPACITY); + apex_test_tools::memory_test::start(); + motion::motion_common::sample(t_ref, result, dt); + EXPECT_LE(result.points.size(), t_res.points.size()); + EXPECT_GT(result.points.size(), 1U); + for (auto idx = 0U; idx < result.points.size(); ++idx) { + const auto & pt = result.points[idx]; + const auto & ref = t_res.points[idx]; + // Check... + constexpr auto TOL = 1.0E-3F; + EXPECT_LT(std::fabs(pt.pose.position.x - ref.pose.position.x), static_cast(TOL)); + EXPECT_LT(std::fabs(pt.pose.position.y - ref.pose.position.y), static_cast(TOL)); + EXPECT_LT(std::fabs(to_angle(pt.pose.orientation) - to_angle(ref.pose.orientation)), TOL); + EXPECT_LT(std::fabs(pt.longitudinal_velocity_mps - ref.longitudinal_velocity_mps), TOL); + EXPECT_LT(std::fabs(pt.acceleration_mps2 - ref.acceleration_mps2), TOL); + EXPECT_LT(std::fabs(pt.heading_rate_rps - ref.heading_rate_rps), TOL); + const auto dt_err = (dt * idx) - from_message(pt.time_from_start); + generic_checks(pt, ref, dt_err, TOL); + } + apex_test_tools::memory_test::start(); +} diff --git a/common/motion_common/test/sanity_checks.cpp b/common/motion_common/test/sanity_checks.cpp new file mode 100644 index 0000000000000..fd21502b32fc0 --- /dev/null +++ b/common/motion_common/test/sanity_checks.cpp @@ -0,0 +1,57 @@ +// Copyright 2017-2021 the Autoware Foundation +// +// 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. +// +/// \file +/// \brief This file includes basic tests for utility functions in motion_common + +#include + +#include +#include +#include +#include + +using autoware::common::types::float64_t; +using autoware_auto_geometry_msgs::msg::Complex32; +using geometry_msgs::msg::Quaternion; +using motion::motion_common::from_quat; + +struct MyQuaternion +{ + float64_t x{0.0}; + float64_t y{0.0}; + float64_t z{0.0}; + float64_t w{1.0}; +}; + +TEST(HeadingFuncs, FromQuat) { + Quaternion gm_quat{}; + gm_quat.z = 0.5f; + gm_quat.w = 0.5f; + + Complex32 complex_heading{}; + complex_heading = from_quat(gm_quat); + + EXPECT_FLOAT_EQ(complex_heading.imag, gm_quat.z); + EXPECT_FLOAT_EQ(complex_heading.real, gm_quat.w); + + MyQuaternion my_quat{}; + my_quat.z = 0.75; + my_quat.w = 0.25; + + complex_heading = from_quat(my_quat); + + EXPECT_FLOAT_EQ(complex_heading.imag, my_quat.z); + EXPECT_FLOAT_EQ(complex_heading.real, my_quat.w); +} diff --git a/common/motion_common/test/trajectory.cpp b/common/motion_common/test/trajectory.cpp new file mode 100644 index 0000000000000..7ac5ebd5d2969 --- /dev/null +++ b/common/motion_common/test/trajectory.cpp @@ -0,0 +1,405 @@ +// Copyright 2021 the Autoware Foundation +// +// 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. +// +/// \file +/// \brief This file includes tests for functions in trajectory_common + +#include +#include + +#include "common/types.hpp" +#include "gtest/gtest.h" +#include "helper_functions/angle_utils.hpp" +#include "motion_common/trajectory_common.hpp" +#include "tf2/LinearMath/Quaternion.h" + + +using autoware::common::types::float64_t; + +TEST(TrajectoryCommonTests, ValidateNonEmpty) { + using autoware::motion::motion_common::validateNonEmpty; + using autoware::motion::motion_common::Point; + using autoware::motion::motion_common::Points; + + Points points; + EXPECT_THROW(validateNonEmpty(points), std::invalid_argument); + + points.emplace_back(); + EXPECT_NO_THROW(validateNonEmpty(points)); +} + +TEST(TrajectoryCommonTests, CalcYawDeviation) { + using autoware::motion::motion_common::calcYawDeviation; + EXPECT_EQ(calcYawDeviation(0.0, 0.0), 0.0); + EXPECT_EQ(calcYawDeviation(M_PI, 0.0), -M_PI); + EXPECT_EQ(calcYawDeviation(0.0, M_PI), -M_PI); + EXPECT_EQ(calcYawDeviation(2 * M_PI, 0.0), 0.0); + EXPECT_EQ(calcYawDeviation(0.0, 2 * M_PI), 0); + EXPECT_EQ(calcYawDeviation(3 * M_PI, 0.0), -M_PI); + EXPECT_EQ(calcYawDeviation(0.0, 3 * M_PI), -M_PI); + EXPECT_EQ(calcYawDeviation(M_PI, -M_PI), 0.0); + EXPECT_EQ(calcYawDeviation(M_PI, M_PI_2), -M_PI_2); + EXPECT_EQ(calcYawDeviation(M_PI_2, M_PI), M_PI_2); +} + +TEST(TrajectoryCommonTests, SearchZeroVelocityIndex) { + using autoware::motion::motion_common::searchZeroVelocityIndex; + using autoware::motion::motion_common::Point; + using autoware::motion::motion_common::Points; + + Points points; + EXPECT_THROW(searchZeroVelocityIndex(points), std::invalid_argument); + + Point p; + p.longitudinal_velocity_mps = 1.0; + points.push_back(p); + EXPECT_EQ(searchZeroVelocityIndex(points), std::experimental::nullopt); + + // Making a trajectory with velocities [1 0 0 2 0] + p.longitudinal_velocity_mps = 0.0; + points.push_back(p); + p.longitudinal_velocity_mps = 0.0; + points.push_back(p); + p.longitudinal_velocity_mps = 2.0; + points.push_back(p); + p.longitudinal_velocity_mps = 0.0; + points.push_back(p); + ASSERT_NE(searchZeroVelocityIndex(points), std::experimental::nullopt); + EXPECT_EQ(searchZeroVelocityIndex(points).value(), size_t(1)); + ASSERT_NE(searchZeroVelocityIndex(points, 3, 5), std::experimental::nullopt); + EXPECT_EQ(searchZeroVelocityIndex(points, 3, 5).value(), size_t(4)); + EXPECT_EQ(searchZeroVelocityIndex(points, 0, 1), std::experimental::nullopt); + EXPECT_EQ(searchZeroVelocityIndex(points, 3, 4), std::experimental::nullopt); + + // Changing the epsilon for zero velocity + EXPECT_EQ(searchZeroVelocityIndex(points, 0, points.size(), -1.0), std::experimental::nullopt); + EXPECT_EQ(searchZeroVelocityIndex(points, 0, points.size(), 2.0).value(), size_t(0)); + EXPECT_EQ(searchZeroVelocityIndex(points, 3, 4, 2.0), std::experimental::nullopt); + EXPECT_EQ(searchZeroVelocityIndex(points, 3, 4, 2.5).value(), size_t(3)); +} + +TEST(TrajectoryCommonTests, FindNearestIndex) { + using autoware::motion::motion_common::findNearestIndex; + using autoware::motion::motion_common::Point; + using autoware::motion::motion_common::Points; + using geometry_msgs::msg::Pose; + using motion::motion_common::from_angle; + + Points points; + Pose pose; + tf2::Quaternion quat; + + EXPECT_THROW(findNearestIndex(points, pose), std::invalid_argument); + + // Making a trajectory with positions [(0,0) (1,1) (2,2) (2,4) (4,4)] + Point p; + p.pose.position.x = 0.0; + p.pose.position.y = 0.0; + p.pose.orientation = from_angle(M_PI_4); + points.push_back(p); + p.pose.position.x = 1.0; + p.pose.position.y = 1.0; + p.pose.orientation = from_angle(M_PI_4); + points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = 2.0; + p.pose.orientation = from_angle(M_PI_2); + points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = 4.0; + p.pose.orientation = from_angle(0.0); + points.push_back(p); + p.pose.position.x = 4.0; + p.pose.position.y = 4.0; + p.pose.orientation = from_angle(0.0); + points.push_back(p); + + // No limits on the distance/orientation + pose.position.x = 0.0; + pose.position.y = 0.0; + EXPECT_EQ(findNearestIndex(points, pose).value(), size_t(0)); + + pose.position.x = -100.0; + pose.position.y = -100.0; + EXPECT_EQ(findNearestIndex(points, pose).value(), size_t(0)); + + pose.position.x = 4.0; + pose.position.y = 4.0; + EXPECT_EQ(findNearestIndex(points, pose).value(), size_t(4)); + + pose.position.x = 100.0; + pose.position.y = 100.0; + EXPECT_EQ(findNearestIndex(points, pose).value(), size_t(4)); + + // limit on the distance + EXPECT_EQ(findNearestIndex(points, pose, 10.0), std::experimental::nullopt); + pose.position.x = 10.0; + pose.position.y = 10.0; + EXPECT_EQ(findNearestIndex(points, pose, 10.0).value(), size_t(4)); + + // limit on the orientation + quat.setRPY(0.0, 0.0, M_PI_2); + pose.orientation = tf2::toMsg(quat); + EXPECT_EQ(findNearestIndex(points, pose, 100.0, 0.1).value(), size_t(2)); + pose.position.x = 0.0; + pose.position.y = 0.0; + EXPECT_EQ(findNearestIndex(points, pose, 100.0, 0.1).value(), size_t(2)); + EXPECT_EQ(findNearestIndex(points, pose, 100.0, M_PI_2).value(), size_t(0)); +} + +TEST(TrajectoryCommonTests, FindNearestSegmentIndex) { + using autoware::motion::motion_common::findNearestSegmentIndex; + using autoware::motion::motion_common::Point; + using autoware::motion::motion_common::Points; + + Points points; + // Making a trajectory with positions [(0,0) (1,1) (2,2) (2,4) (4,4)] + Point p; + p.pose.position.x = 0.0; + p.pose.position.y = 0.0; + points.push_back(p); + p.pose.position.x = 1.0; + p.pose.position.y = 1.0; + points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = 2.0; + points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = 4.0; + points.push_back(p); + p.pose.position.x = 4.0; + p.pose.position.y = 4.0; + points.push_back(p); + + geometry_msgs::msg::Point target; + target.x = 0.0; + target.y = 0.0; + EXPECT_EQ(findNearestSegmentIndex(points, target), size_t(0)); + + target.x = 0.9; + target.y = 0.9; + EXPECT_EQ(findNearestSegmentIndex(points, target), size_t(0)); + + target.x = 1.9; + target.y = 1.9; + EXPECT_EQ(findNearestSegmentIndex(points, target), size_t(1)); + + target.x = 3.0; + target.y = 3.0; + EXPECT_EQ(findNearestSegmentIndex(points, target), size_t(2)); + + target.x = 2.5; + target.y = 4.5; + EXPECT_EQ(findNearestSegmentIndex(points, target), size_t(3)); + + target.x = 5.0; + target.y = 5.0; + EXPECT_EQ(findNearestSegmentIndex(points, target), size_t(3)); +} + +TEST(TrajectoryCommonTests, CalcLongitudinalOffsetToSegment) { + using autoware::motion::motion_common::calcLongitudinalOffsetToSegment; + using autoware::motion::motion_common::Point; + using autoware::motion::motion_common::Points; + + Points points; + geometry_msgs::msg::Point target; + EXPECT_THROW(calcLongitudinalOffsetToSegment(points, 0, target), std::invalid_argument); + + Point p; + p.pose.position.x = 0.0; + p.pose.position.y = 0.0; + points.push_back(p); + points.push_back(p); + EXPECT_THROW(calcLongitudinalOffsetToSegment(points, 0, target), std::runtime_error); + + // Making a trajectory with positions [(0,0) (1,0) (2,0) (2,1) (3,1)] ___|‾ + points.clear(); + p.pose.position.x = 0.0; + p.pose.position.y = 0.0; + points.push_back(p); + p.pose.position.x = 1.0; + p.pose.position.y = 0.0; + points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = 0.0; + points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = 1.0; + points.push_back(p); + p.pose.position.x = 3.0; + p.pose.position.y = 1.0; + points.push_back(p); + + target.x = 0.0; + target.y = 0.0; + EXPECT_EQ(calcLongitudinalOffsetToSegment(points, 0, target), 0.0); + EXPECT_EQ(calcLongitudinalOffsetToSegment(points, 1, target), -1.0); + EXPECT_EQ(calcLongitudinalOffsetToSegment(points, 2, target), 0.0); + EXPECT_EQ(calcLongitudinalOffsetToSegment(points, 3, target), -2.0); +} + +TEST(TrajectoryCommonTests, CalcSignedArcLengthIndexToIndex) { + using autoware::motion::motion_common::calcSignedArcLength; + using autoware::motion::motion_common::Point; + using autoware::motion::motion_common::Points; + + Points points; + EXPECT_THROW(calcSignedArcLength(points, 0, 0), std::invalid_argument); + + // Making a trajectory with positions [(0,0) (1,0) (2,0) (2,1) (3,1)] ___|‾ + Point p; + p.pose.position.x = 0.0; + p.pose.position.y = 0.0; + points.push_back(p); + p.pose.position.x = 1.0; + p.pose.position.y = 0.0; + points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = 0.0; + points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = 1.0; + points.push_back(p); + p.pose.position.x = 3.0; + p.pose.position.y = 1.0; + points.push_back(p); + + // Out of range + EXPECT_THROW(calcSignedArcLength(points, 0, points.size() + 1), std::out_of_range); + + // Same point + EXPECT_EQ(calcSignedArcLength(points, 3, 3), 0.0); + + // Forward + EXPECT_EQ(calcSignedArcLength(points, 0, 3), 3.0); + + // Backward + EXPECT_EQ(calcSignedArcLength(points, 4, 2), -2.0); +} + +TEST(Trajectory, CalcSignedArcLengthPointToIndex) +{ + using autoware::motion::motion_common::calcSignedArcLength; + using autoware::motion::motion_common::Point; + using autoware::motion::motion_common::Points; + + Points points; + geometry_msgs::msg::Point src; + EXPECT_THROW(calcSignedArcLength(points, src, 0), std::invalid_argument); + + // Making a trajectory with positions [(0,0) (1,0) (2,0) (2,1) (3,1)] ___|‾ + Point p; + p.pose.position.x = 0.0; + p.pose.position.y = 0.0; + points.push_back(p); + p.pose.position.x = 1.0; + p.pose.position.y = 0.0; + points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = 0.0; + points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = 1.0; + points.push_back(p); + p.pose.position.x = 3.0; + p.pose.position.y = 1.0; + points.push_back(p); + + // Same point + src.x = 2.0; + src.y = 1.0; + EXPECT_EQ(calcSignedArcLength(points, src, 3), 0.0); + + // Forward + EXPECT_EQ(calcSignedArcLength(points, src, 4), 1.0); + + // Backward + EXPECT_EQ(calcSignedArcLength(points, src, 0), -3.0); + + // Point before start point + src.x = -1.0; + src.y = 0.0; + EXPECT_EQ(calcSignedArcLength(points, src, 2), 3.0); + + // Point after end point but without longitudinal offset + src.x = 3.0; + src.y = 2.0; + EXPECT_EQ(calcSignedArcLength(points, src, 0), -4.0); + + // Point after end point with longitudinal offset + src.x = 4.0; + src.y = 2.0; + EXPECT_EQ(calcSignedArcLength(points, src, 0), -5.0); +} + +TEST(Trajectory, CalcSignedArcLengthPointToPoint) +{ + using autoware::motion::motion_common::calcSignedArcLength; + using autoware::motion::motion_common::Point; + using autoware::motion::motion_common::Points; + + Points points; + geometry_msgs::msg::Point src; + geometry_msgs::msg::Point dest; + EXPECT_THROW(calcSignedArcLength(points, src, dest), std::invalid_argument); + + // Making a trajectory with positions [(0,0) (1,0) (2,0) (2,1) (3,1)] ___|‾ + Point p; + p.pose.position.x = 0.0; + p.pose.position.y = 0.0; + points.push_back(p); + p.pose.position.x = 1.0; + p.pose.position.y = 0.0; + points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = 0.0; + points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = 1.0; + points.push_back(p); + p.pose.position.x = 3.0; + p.pose.position.y = 1.0; + points.push_back(p); + + // Same point + src.x = 1.0; + src.y = 0.0; + dest.x = src.x; + dest.y = src.y; + EXPECT_EQ(calcSignedArcLength(points, src, dest), 0.0); + + // Forward + dest.x = 2.0; + dest.y = 1.0; + EXPECT_EQ(calcSignedArcLength(points, src, dest), 2.0); + + // Backward + EXPECT_EQ(calcSignedArcLength(points, dest, src), -2.0); + + // Point before start point + src.x = -1.0; + src.y = 0.0; + EXPECT_EQ(calcSignedArcLength(points, src, dest), 4.0); + + // Point before start point and after end point + dest.x = 4.0; + dest.y = 1.0; + EXPECT_EQ(calcSignedArcLength(points, src, dest), 6.0); + + // Point after end point + src.x = 0.0; + src.y = 0.0; + EXPECT_EQ(calcSignedArcLength(points, src, dest), 5.0); +} diff --git a/common/motion_testing/CMakeLists.txt b/common/motion_testing/CMakeLists.txt new file mode 100644 index 0000000000000..cf999f59f0b99 --- /dev/null +++ b/common/motion_testing/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.6) + +project(motion_testing) + +### Dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +### Build +ament_auto_add_library(${PROJECT_NAME} SHARED src/motion_testing/motion_testing.cpp) +autoware_set_compile_options(${PROJECT_NAME}) + +### Test +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # Linters + ament_lint_auto_find_test_dependencies() + # Unit test + ament_add_gtest(motion_testing_unit_tests + test/gtest_main.cpp + test/constant_trajectory.cpp + test/trajectory_checks.cpp) + autoware_set_compile_options(motion_testing_unit_tests) + target_compile_options(motion_testing_unit_tests PRIVATE -Wno-sign-conversion) + target_link_libraries(motion_testing_unit_tests ${PROJECT_NAME}) + add_dependencies(motion_testing_unit_tests ${PROJECT_NAME}) +endif() + +ament_auto_package() diff --git a/common/motion_testing/include/motion_testing/motion_testing.hpp b/common/motion_testing/include/motion_testing/motion_testing.hpp new file mode 100644 index 0000000000000..17318552e9982 --- /dev/null +++ b/common/motion_testing/include/motion_testing/motion_testing.hpp @@ -0,0 +1,127 @@ +// Copyright 2019 Christopher Ho +// +// 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 MOTION_TESTING__MOTION_TESTING_HPP_ +#define MOTION_TESTING__MOTION_TESTING_HPP_ + +#include +#include +#include +#include + +#include +#include + +namespace motion +{ +namespace motion_testing +{ +using Generator = std::mt19937; +using State = autoware_auto_vehicle_msgs::msg::VehicleKinematicState; +using Point = autoware_auto_planning_msgs::msg::TrajectoryPoint; +using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; +using Index = decltype(Trajectory::points)::size_type; +using Real = decltype(Point::longitudinal_velocity_mps); +// TODO(c.ho) Make these more modular + +/// \brief Makes a state, intended to make message generation more terse +MOTION_TESTING_PUBLIC State make_state( + Real x0, + Real y0, + Real heading, + Real v0, + Real a0, + Real turn_rate, + std::chrono::system_clock::time_point t); + +/// \brief Generates a state from a normal distribution with the following bounds: +/// TODO(c.ho) +MOTION_TESTING_PUBLIC State generate_state(Generator & gen); + +/// \brief Generates a trajectory assuming the starting state, a bicycle model, and +/// additive noise applied to XXX +/// Note: not implemented +MOTION_TESTING_PUBLIC Trajectory generate_trajectory(const State & start_state, Generator & gen); + +/// \brief Generate a trajectory given the start state, assuming the highest derivatives are held +/// constant. +/// Note: heading_rate behavior will be kind of off TODO(c.ho) +/// Note: lateral_velocity_mps will not be respected TODO(cho) +MOTION_TESTING_PUBLIC +Trajectory constant_trajectory(const State & start_state, std::chrono::nanoseconds dt); +/// \brief Generates a constant velocity trajectory + +MOTION_TESTING_PUBLIC +Trajectory bad_heading_trajectory(const State & start_state, std::chrono::nanoseconds dt); +/// \brief Generates a constant velocity trajectory with invalid heading values + +MOTION_TESTING_PUBLIC Trajectory constant_velocity_trajectory( + float x0, + float y0, + float heading, + float v0, + std::chrono::nanoseconds dt); +/// \brief Generates a constant acceleration trajectory +MOTION_TESTING_PUBLIC Trajectory constant_acceleration_trajectory( + float x0, + float y0, + float heading, + float v0, + float a0, + std::chrono::nanoseconds dt); +/// \brief Generates a constant velocity and constant turn rate trajectory +MOTION_TESTING_PUBLIC Trajectory constant_velocity_turn_rate_trajectory( + float x0, + float y0, + float heading, + float v0, + float turn_rate, + std::chrono::nanoseconds dt); +/// \brief Generates a constant acceleration and constant turn rate trajectory +MOTION_TESTING_PUBLIC Trajectory constant_acceleration_turn_rate_trajectory( + float x0, + float y0, + float heading, + float v0, + float a0, + float turn_rate, + std::chrono::nanoseconds dt); + +/// Given a trajectory, advance state to next trajectory point, with normally distributed noise +/// Note: This version takes "hint" as gospel, and doesn't try to do any time/space matching +/// Note: not implemented +MOTION_TESTING_PUBLIC void next_state( + const Trajectory & trajectory, + State & state, + uint32_t hint, + Generator * gen = nullptr); // TODO(c.ho) std::optional NOLINT +// TODO(c.ho) version that takes control commands + +/// Checks that a trajectory makes constant progress towards a target; returns first +/// index of point that doesn't advance towards target, otherwise size of trajectory +/// heading tolerance is in dot product space of 2d quaternion +MOTION_TESTING_PUBLIC +Index progresses_towards_target( + const Trajectory & trajectory, + const Point & target, + Real heading_tolerance = Real{0.006F}); + +/// Checks that a trajectory is more or less dynamically feasible given the derivatives; +/// tolerance is relative tolerance of trajectory, index is first point that is not dynamically +/// feasible, trajectory.size() if completely feasible +MOTION_TESTING_PUBLIC +Index dynamically_feasible(const Trajectory & trajectory, Real tolerance = 0.05F); +} // namespace motion_testing +} // namespace motion + +#endif // MOTION_TESTING__MOTION_TESTING_HPP_ diff --git a/common/motion_testing/include/motion_testing/visibility_control.hpp b/common/motion_testing/include/motion_testing/visibility_control.hpp new file mode 100644 index 0000000000000..c536894b789a0 --- /dev/null +++ b/common/motion_testing/include/motion_testing/visibility_control.hpp @@ -0,0 +1,36 @@ +// Copyright 2019 Christopher Ho +// +// 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 MOTION_TESTING__VISIBILITY_CONTROL_HPP_ +#define MOTION_TESTING__VISIBILITY_CONTROL_HPP_ + +#if defined(__WIN32) + #if defined(MOTION_TESTING_BUILDING_DLL) || defined(MOTION_TESTING_EXPORTS) + #define MOTION_TESTING_PUBLIC __declspec(dllexport) + #define MOTION_TESTING_LOCAL + #else // defined(MOTION_TESTING_BUILDING_DLL) || defined(MOTION_TESTING_EXPORTS) + #define MOTION_TESTING_PUBLIC __declspec(dllimport) + #define MOTION_TESTING_LOCAL + #endif // defined(MOTION_TESTING_BUILDING_DLL) || defined(MOTION_TESTING_EXPORTS) +#elif defined(__linux__) + #define MOTION_TESTING_PUBLIC __attribute__((visibility("default"))) + #define MOTION_TESTING_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) + #define MOTION_TESTING_PUBLIC __attribute__((visibility("default"))) + #define MOTION_TESTING_LOCAL __attribute__((visibility("hidden"))) +#else // defined(_LINUX) + #error "Unsupported Build Configuration" +#endif // defined(_WINDOWS) + +#endif // MOTION_TESTING__VISIBILITY_CONTROL_HPP_ diff --git a/common/motion_testing/package.xml b/common/motion_testing/package.xml new file mode 100644 index 0000000000000..dcfffa7164e21 --- /dev/null +++ b/common/motion_testing/package.xml @@ -0,0 +1,30 @@ + + + + motion_testing + 1.0.0 + + Helper functions to aid in the testing of motion planner and motion controllers + + Christopher Ho + Apache License 2.0 + + ament_cmake_auto + autoware_auto_cmake + + autoware_auto_cmake + + autoware_auto_planning_msgs + autoware_auto_vehicle_msgs + autoware_auto_planning_msgs + autoware_auto_vehicle_msgs + time_utils + tf2 + tf2_geometry_msgs + + ament_cmake_gtest + + + + ament_cmake + diff --git a/common/motion_testing/src/motion_testing/motion_testing.cpp b/common/motion_testing/src/motion_testing/motion_testing.cpp new file mode 100644 index 0000000000000..e3f6704c2284c --- /dev/null +++ b/common/motion_testing/src/motion_testing/motion_testing.cpp @@ -0,0 +1,319 @@ +// Copyright 2019 Christopher Ho +// +// 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 "motion_testing/motion_testing.hpp" + +#include +#include +#include +#include +#include + +#include + +namespace motion +{ +namespace motion_testing +{ +State make_state( + Real x0, + Real y0, + Real heading, + Real v0, + Real a0, + Real turn_rate, + std::chrono::system_clock::time_point t) +{ + State start_state{rosidl_runtime_cpp::MessageInitialization::ALL}; + start_state.state.pose.position.x = x0; + start_state.state.pose.position.y = y0; + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, heading); + start_state.state.pose.orientation = tf2::toMsg(quat); + start_state.state.longitudinal_velocity_mps = v0; + start_state.state.acceleration_mps2 = a0; + start_state.state.heading_rate_rps = turn_rate; + start_state.state.lateral_velocity_mps = 0.0F; + + start_state.header.stamp = time_utils::to_message(t); + + return start_state; +} + +//////////////////////////////////////////////////////////////////////////////// +State generate_state(Generator & gen) +{ + State ret{rosidl_runtime_cpp::MessageInitialization::ALL}; + // Parameters with positive and negative supports + std::normal_distribution normal{0.0, 1.0}; + std::normal_distribution normalF{0.0F, 1.0F}; + ret.state.pose.position.x = 10.0 * normal(gen); + ret.state.pose.position.y = 10.0 * normal(gen); + ret.state.lateral_velocity_mps = 0.5F * normalF(gen); + ret.state.acceleration_mps2 = normalF(gen); + ret.state.heading_rate_rps = 0.1F * normalF(gen); + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, normal(gen)); + ret.state.pose.orientation = tf2::toMsg(quat); + // Parameters with only positive supports + std::exponential_distribution + exponential{1.0F}; + ret.state.longitudinal_velocity_mps = 5.0F * exponential(gen); + + return ret; +} + +//////////////////////////////////////////////////////////////////////////////// +Trajectory generate_trajectory(const State & start_state, Generator & gen) +{ + Trajectory ret{rosidl_runtime_cpp::MessageInitialization::ALL}; + (void)start_state; + (void)gen; + return ret; +} + +//////////////////////////////////////////////////////////////////////////////// +Trajectory constant_trajectory(const State & start_state, const std::chrono::nanoseconds dt) +{ + Trajectory ret{rosidl_runtime_cpp::MessageInitialization::ALL}; + const auto capacity = 100LL; // TEMP + ret.points.reserve(capacity); + const auto dt_s = std::chrono::duration_cast>(dt).count(); + ret.points.push_back(start_state.state); + ret.points.back().time_from_start = time_utils::to_message(std::chrono::nanoseconds::zero()); + // quaternion increments + tf2::Quaternion orientation_increment; + orientation_increment.setRPY(0.0, 0.0, start_state.state.heading_rate_rps * dt_s); + // fill out trajectory + for (auto i = 1LL; i < capacity; ++i) { + const auto & last_state = ret.points.back(); + decltype(ret.points)::value_type next_state{last_state}; + // longitudinal velocity update; lateral assumed fixed + next_state.longitudinal_velocity_mps += dt_s * next_state.acceleration_mps2; + // heading update + tf2::Quaternion last_orientation; + tf2::fromMsg(last_state.pose.orientation, last_orientation); + next_state.pose.orientation = tf2::toMsg(last_orientation * orientation_increment); + // pose.position update: simplified heading effects + const auto ds = + static_cast(dt_s * + (last_state.longitudinal_velocity_mps + (0.5F * dt_s * last_state.acceleration_mps2))); + const auto yaw = tf2::getYaw(next_state.pose.orientation); + next_state.pose.position.x += std::cos(yaw) * ds; + next_state.pose.position.y += std::sin(yaw) * ds; + + next_state.time_from_start = time_utils::to_message(dt * i); + + ret.points.push_back(next_state); + } + ret.header.stamp = time_utils::to_message(std::chrono::system_clock::now()); + return ret; +} + +//////////////////////////////////////////////////////////////////////////////// +Trajectory bad_heading_trajectory(const State & start_state, const std::chrono::nanoseconds dt) +{ + Trajectory ret{rosidl_runtime_cpp::MessageInitialization::ALL}; + const auto capacity = 100LL; // TEMP + ret.points.reserve(capacity); + const auto dt_s = std::chrono::duration_cast>(dt).count(); + ret.points.push_back(start_state.state); + ret.points.back().pose.orientation.x = 0.0; + ret.points.back().pose.orientation.y = 0.0; + ret.points.back().pose.orientation.z = 0.0; + ret.points.back().pose.orientation.w = 0.0; + ret.points.back().heading_rate_rps = 0.0F; + + ret.points.back().time_from_start = time_utils::to_message(std::chrono::nanoseconds::zero()); + + // fill out trajectory + for (auto i = 1LL; i < capacity; ++i) { + const auto & last_state = ret.points.back(); + decltype(ret.points)::value_type next_state{last_state}; + + next_state.pose.position.x += static_cast(dt_s * last_state.longitudinal_velocity_mps); + next_state.pose.position.y += 0.0; + next_state.time_from_start = time_utils::to_message(dt * i); + + ret.points.push_back(next_state); + } + ret.header.stamp = time_utils::to_message(std::chrono::system_clock::now()); + return ret; +} + +//////////////////////////////////////////////////////////////////////////////// +Trajectory constant_velocity_trajectory( + const Real x0, + const Real y0, + const Real heading, + const Real v0, + const std::chrono::nanoseconds dt) +{ + return constant_acceleration_turn_rate_trajectory(x0, y0, heading, v0, 0.0F, 0.0F, dt); +} + +//////////////////////////////////////////////////////////////////////////////// +Trajectory constant_acceleration_trajectory( + const Real x0, + const Real y0, + const Real heading, + const Real v0, + const Real a0, + const std::chrono::nanoseconds dt) +{ + return constant_acceleration_turn_rate_trajectory(x0, y0, heading, v0, a0, 0.0F, dt); +} + +//////////////////////////////////////////////////////////////////////////////// +Trajectory constant_velocity_turn_rate_trajectory( + const Real x0, + const Real y0, + const Real heading, + const Real v0, + const Real turn_rate, + const std::chrono::nanoseconds dt) +{ + return constant_acceleration_turn_rate_trajectory(x0, y0, heading, v0, 0.0F, turn_rate, dt); +} + +//////////////////////////////////////////////////////////////////////////////// +Trajectory constant_acceleration_turn_rate_trajectory( + const Real x0, + const Real y0, + const Real heading, + const Real v0, + const Real a0, + const Real turn_rate, + const std::chrono::nanoseconds dt) +{ + State start_state = + make_state(x0, y0, heading, v0, a0, turn_rate, std::chrono::system_clock::now()); + + return constant_trajectory(start_state, dt); +} + +//////////////////////////////////////////////////////////////////////////////// +void next_state( + const Trajectory & trajectory, + State & state, + const uint32_t hint, + Generator * const gen) +{ + (void)trajectory; + (void)state; + (void)gen; + (void)hint; +} + +//////////////////////////////////////////////////////////////////////////////// +Index progresses_towards_target( + const Trajectory & trajectory, + const Point & target, + const Real heading_tolerance) +{ + auto last_err = std::numeric_limits::max(); + auto last_heading_err = -std::numeric_limits::max(); + for (auto idx = Index{}; idx < trajectory.points.size(); ++idx) { + const auto & pt = trajectory.points[idx]; + // Pose + const auto dx = pt.pose.position.x - target.pose.position.x; + const auto dy = pt.pose.position.y - target.pose.position.y; + const auto err = (dx * dx) + (dy * dy); + if (err > last_err) { + return idx; + } + last_err = err; + // Heading: dot product should tend towards 1 + tf2::Quaternion pt_orientation; + tf2::Quaternion target_orientation; + tf2::fromMsg(pt.pose.orientation, pt_orientation); + tf2::fromMsg(target.pose.orientation, target_orientation); + const auto dot = static_cast(tf2::dot(pt_orientation, target_orientation)); + if (dot < last_heading_err - heading_tolerance) { // Allow for some error + return idx; + } + last_heading_err = dot; + } + return trajectory.points.size(); +} + +//////////////////////////////////////////////////////////////////////////////// +Index dynamically_feasible(const Trajectory & trajectory, const Real tolerance) +{ + if (trajectory.points.empty()) { + return trajectory.points.size(); + } + auto last_pt = trajectory.points.front(); + for (auto idx = Index{1}; idx < trajectory.points.size(); ++idx) { + const auto & pt = trajectory.points[idx]; + const auto dt_ = time_utils::from_message(pt.time_from_start) - + time_utils::from_message(last_pt.time_from_start); + const auto dt = std::chrono::duration_cast>(dt_).count(); + const auto dv = last_pt.acceleration_mps2 * dt; + const auto ds = + (Real{0.5} *dv * dt) + (last_pt.longitudinal_velocity_mps * dt); + const auto dn = last_pt.lateral_velocity_mps * dt; + const auto dth = last_pt.heading_rate_rps * dt; + const auto check_fn = [tolerance](auto expect, auto val, auto str) -> bool { + bool success = true; + if (static_cast(std::fabs(expect)) < Real{1}) { + success = static_cast(std::fabs(expect - val)) < tolerance; + } else { + success = static_cast(std::fabs(expect - val) / expect) < tolerance; + } + (void)str; + return success; + }; + bool ok = true; + { + const auto v = last_pt.longitudinal_velocity_mps + dv; + ok = check_fn(v, pt.longitudinal_velocity_mps, "vel") && ok; + } + { + tf2::Quaternion th; + tf2::fromMsg(last_pt.pose.orientation, th); + tf2::Quaternion new_th; + tf2::fromMsg(pt.pose.orientation, new_th); + { + // Dot product between angles to "check" magnitude of rotation + const auto dot = static_cast(tf2::dot(th, new_th)); + ok = check_fn(dot, std::cos(dth), "th_mag") && ok; + // cross product between angles to check for consistent sign of change + if (std::fabs(dth) > Real{0.001F}) { + const auto angle = static_cast(th.angle(new_th)); + // Negative product -> not pointing in same direction + ok = (dth * angle > Real{}) && ok; + } + } + // Check changes either from current or next heading + const tf2::Vector3 ds_dn(ds, dn, 0.0); + const tf2::Transform tf(th); + const tf2::Vector3 delta = tf * ds_dn; + const tf2::Transform tf2(new_th); + const tf2::Vector3 delta2 = tf2 * ds_dn; + ok = (check_fn(last_pt.pose.position.x + delta.getX(), pt.pose.position.x, "x") || + check_fn(last_pt.pose.position.x + delta2.getX(), pt.pose.position.x, "x2")) && + ok; + ok = (check_fn(last_pt.pose.position.y + delta.getY(), pt.pose.position.y, "y") || + check_fn(last_pt.pose.position.y + delta2.getY(), pt.pose.position.y, "y2")) && + ok; + } + if (!ok) { + return idx; + } + last_pt = pt; + } + return trajectory.points.size(); +} +} // namespace motion_testing +} // namespace motion diff --git a/common/motion_testing/test/constant_trajectory.cpp b/common/motion_testing/test/constant_trajectory.cpp new file mode 100644 index 0000000000000..b9546352dd25a --- /dev/null +++ b/common/motion_testing/test/constant_trajectory.cpp @@ -0,0 +1,125 @@ +// Copyright 2019 Christopher Ho +// +// 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 +#include + +using motion::motion_testing::State; +using motion::motion_testing::Trajectory; +using time_utils::from_message; + +TEST(ConstantTrajectory, Stationary) +{ + const auto x0 = 3.0F; + const auto y0 = -5.0F; + const auto traj = motion::motion_testing::constant_velocity_trajectory( + x0, y0, 0.0F, 0.0F, std::chrono::milliseconds(100LL)); + for (auto i = 0U; i < traj.points.size(); ++i) { + const auto & t = traj.points[i]; + EXPECT_DOUBLE_EQ(t.pose.position.x, x0); + EXPECT_DOUBLE_EQ(t.pose.position.y, y0); + EXPECT_DOUBLE_EQ(t.pose.orientation.w, 1.0F); + EXPECT_DOUBLE_EQ(t.pose.orientation.x, 0.0F); + EXPECT_DOUBLE_EQ(t.pose.orientation.y, 0.0F); + EXPECT_DOUBLE_EQ(t.pose.orientation.z, 0.0F); + EXPECT_FLOAT_EQ(t.longitudinal_velocity_mps, 0.0F); + EXPECT_FLOAT_EQ(t.lateral_velocity_mps, 0.0F); + EXPECT_FLOAT_EQ(t.acceleration_mps2, 0.0F); + EXPECT_FLOAT_EQ(t.heading_rate_rps, 0.0F); + // TODO(c.ho) check time + if (i > 0U) { + const auto dt_curr = from_message(t.time_from_start); + const auto dt_last = from_message(traj.points[i - 1U].time_from_start); + EXPECT_GT(dt_curr, dt_last); + } + } +} + +TEST(ConstantTrajectory, ConstantVelocity) +{ + const auto x0 = 3.0F; + const auto y0 = -5.0F; + const auto v0 = 2.0F; + const auto heading = 30.0F * (3.14159F / 180.0F); + const auto w = std::cos(heading / 2.0F); + const auto z = std::sin(heading / 2.0F); + const auto c = (w + z) * (w - z); + const auto s = 2.0F * w * z; + EXPECT_FLOAT_EQ(std::sin(heading), s); + EXPECT_FLOAT_EQ(std::cos(heading), c); + const auto dt = std::chrono::milliseconds(100LL); + const auto traj = motion::motion_testing::constant_velocity_trajectory( + x0, y0, heading, v0, dt); + const auto dt_s = std::chrono::duration_cast>(dt).count(); + for (auto i = 0U; i < traj.points.size(); ++i) { + const auto & t = traj.points[i]; + const auto ds = v0 * static_cast(i) * dt_s; + constexpr auto TOL = 1.0E-4F; + EXPECT_LT(std::fabs(static_cast(t.pose.position.x) - (x0 + (ds * c))), TOL); + EXPECT_LT(std::fabs(static_cast(t.pose.position.y) - (y0 + (ds * s))), TOL); + EXPECT_LT(std::fabs(static_cast(t.pose.orientation.w) - (w)), TOL); + EXPECT_LT(std::fabs(static_cast(t.pose.orientation.z) - (z)), TOL); + EXPECT_LT(std::fabs(t.longitudinal_velocity_mps - (v0)), TOL); + EXPECT_LT(std::fabs(t.lateral_velocity_mps - (0.0F)), TOL); + EXPECT_LT(std::fabs(t.acceleration_mps2 - (0.0F)), TOL); + EXPECT_LT(std::fabs(t.heading_rate_rps - (0.0F)), TOL); + // TODO(c.ho) check time + if (i > 0U) { + const auto dt_curr = from_message(t.time_from_start); + const auto dt_last = from_message(traj.points[i - 1U].time_from_start); + EXPECT_GT(dt_curr, dt_last); + } + } +} + +TEST(ConstantTrajectory, ConstantAcceleration) +{ + const auto x0 = 3.0F; + const auto y0 = -5.0F; + const auto v0 = 0.0F; + const auto a0 = 1.0F; + const auto heading = -45.0F * (3.14159F / 180.0F); + const auto w = std::cos(heading / 2.0F); + const auto z = std::sin(heading / 2.0F); + const auto c = (w + z) * (w - z); + const auto s = 2.0F * w * z; + EXPECT_FLOAT_EQ(std::sin(heading), s); + EXPECT_FLOAT_EQ(std::cos(heading), c); + const auto dt = std::chrono::milliseconds(100LL); + const auto traj = motion::motion_testing::constant_acceleration_trajectory( + x0, y0, heading, v0, a0, dt); + const auto dt_s = std::chrono::duration_cast>(dt).count(); + for (auto i = 0U; i < traj.points.size(); ++i) { + const auto & t = traj.points[i]; + const auto dT = static_cast(i) * dt_s; + const auto ds = dT * (v0 + (0.5F * dT * a0)); + constexpr auto TOL = 1.0E-4F; + EXPECT_LT(std::abs(static_cast(t.pose.position.x) - (x0 + (ds * c))), TOL); + EXPECT_LT(std::abs(static_cast(t.pose.position.y) - (y0 + (ds * s))), TOL); + EXPECT_LT(std::abs(static_cast(t.pose.orientation.w) - (w)), TOL); + EXPECT_LT(std::abs(static_cast(t.pose.orientation.z) - (z)), TOL); + EXPECT_LT(std::abs(t.longitudinal_velocity_mps - (v0 + (dT * a0))), TOL); + EXPECT_LT(std::abs(t.lateral_velocity_mps - (0.0F)), TOL); + EXPECT_LT(std::abs(t.acceleration_mps2 - (a0)), TOL); + EXPECT_LT(std::abs(t.heading_rate_rps - (0.0F)), TOL); + // TODO(c.ho) check time + if (i > 0U) { + const auto dt_curr = from_message(t.time_from_start); + const auto dt_last = from_message(traj.points[i - 1U].time_from_start); + EXPECT_GT(dt_curr, dt_last); + } + } +} + +// TODO(c.ho) turn rate tests... diff --git a/common/motion_testing/test/gtest_main.cpp b/common/motion_testing/test/gtest_main.cpp new file mode 100644 index 0000000000000..809ccb3951198 --- /dev/null +++ b/common/motion_testing/test/gtest_main.cpp @@ -0,0 +1,21 @@ +// Copyright 2019 Christopher Ho +// +// 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 + +int32_t main(int32_t argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/motion_testing/test/trajectory_checks.cpp b/common/motion_testing/test/trajectory_checks.cpp new file mode 100644 index 0000000000000..7b4498b292f96 --- /dev/null +++ b/common/motion_testing/test/trajectory_checks.cpp @@ -0,0 +1,40 @@ +// Copyright 2019 Christopher Ho +// +// 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 + +#include + +using motion::motion_testing::State; +using motion::motion_testing::Trajectory; +using motion::motion_testing::make_state; +using motion::motion_testing::constant_velocity_trajectory; + +TEST(TrajectoryChecks, Basic) +{ + Trajectory traj{}; + const auto target = + make_state(-100.0F, 100.0F, 2.0F, 1.0F, 0.0F, 0.0F, std::chrono::system_clock::now()); + using motion::motion_testing::progresses_towards_target; + using motion::motion_testing::dynamically_feasible; + // Empty case + ASSERT_TRUE(traj.points.empty()); + EXPECT_EQ(dynamically_feasible(traj), {}); + EXPECT_EQ(progresses_towards_target(traj, target.state), {}); + // Normal case + traj = constant_velocity_trajectory(1.0F, 1.0F, 2.0F, 3.0F, std::chrono::milliseconds{100LL}); + ASSERT_FALSE(traj.points.empty()); + EXPECT_EQ(dynamically_feasible(traj), traj.points.size()); + EXPECT_EQ(progresses_towards_target(traj, target.state), traj.points.size()); +} diff --git a/common/time_utils/CMakeLists.txt b/common/time_utils/CMakeLists.txt new file mode 100644 index 0000000000000..63e28887ed288 --- /dev/null +++ b/common/time_utils/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.6) + +project(time_utils) + +### Dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +### Build +ament_auto_add_library(${PROJECT_NAME} SHARED src/time_utils/time_utils.cpp) +autoware_set_compile_options(${PROJECT_NAME}) + +### Test +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # Linters + ament_lint_auto_find_test_dependencies() + # TODO(c.ho) unit tests +endif() + +ament_auto_package() diff --git a/common/time_utils/include/time_utils/stopwatch.hpp b/common/time_utils/include/time_utils/stopwatch.hpp new file mode 100644 index 0000000000000..afb3c45746161 --- /dev/null +++ b/common/time_utils/include/time_utils/stopwatch.hpp @@ -0,0 +1,84 @@ +// Copyright 2019-2021 Apex.AI, 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#ifndef TIME_UTILS__STOPWATCH_HPP_ +#define TIME_UTILS__STOPWATCH_HPP_ + +#include + +#include +#include + +namespace autoware +{ +namespace common +{ +namespace time_utils +{ + +namespace detail +{ + +template +struct is_duration : public std::false_type {}; + +template +struct is_duration>: public std::true_type {}; + +} // namespace detail + +/// +/// @brief This class describes a timer used to quickly measure elapsed time. +/// +class TIME_UTILS_PUBLIC Stopwatch +{ + using Clock = std::chrono::high_resolution_clock; + using TimePoint = std::chrono::time_point; + +public: + /// Creating a timer automatically starts it. + Stopwatch() = default; + + /// Restart a running timer. + inline void restart() noexcept {m_start = Clock::now();} + + /// + /// @brief Get the elapsed time in the specified duration. + /// + /// @tparam UnitsT A type that must be a specialization of std::chrono::duration. + /// + /// @return Time elapsed since the start (or a restart) of the timer represented in the + /// provided duration format. + /// + template + inline UnitsT measure() const noexcept + { + static_assert( + detail::is_duration::value, + "The provided type must be an std::chrono::duration<...> type, " + "e.g. std::chrono::milliseconds or similar."); + return std::chrono::duration_cast(Clock::now() - m_start); + } + +private: + TimePoint m_start{Clock::now()}; +}; + +} // namespace time_utils +} // namespace common +} // namespace autoware + +#endif // TIME_UTILS__STOPWATCH_HPP_ diff --git a/common/time_utils/include/time_utils/time_utils.hpp b/common/time_utils/include/time_utils/time_utils.hpp new file mode 100644 index 0000000000000..6dee7947f9eaf --- /dev/null +++ b/common/time_utils/include/time_utils/time_utils.hpp @@ -0,0 +1,50 @@ +// Copyright 2019-2021 Apex.AI, 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. +#ifndef TIME_UTILS__TIME_UTILS_HPP_ +#define TIME_UTILS__TIME_UTILS_HPP_ + +#include +#include +#include + +#include + +namespace time_utils +{ +/// Convert from std::chrono::time_point to time message +TIME_UTILS_PUBLIC builtin_interfaces::msg::Time to_message(std::chrono::system_clock::time_point t); +/// Convert from std::chrono::duration to duration message +TIME_UTILS_PUBLIC builtin_interfaces::msg::Duration to_message(std::chrono::nanoseconds dt); +/// Convert from std::chrono::time_point from time message +TIME_UTILS_PUBLIC +std::chrono::system_clock::time_point from_message(builtin_interfaces::msg::Time t) noexcept; +/// Convert from std::chrono::duration from duration message +TIME_UTILS_PUBLIC +std::chrono::nanoseconds from_message(builtin_interfaces::msg::Duration dt) noexcept; +/// Standard interpolation +TIME_UTILS_PUBLIC std::chrono::nanoseconds interpolate( + std::chrono::nanoseconds a, + std::chrono::nanoseconds b, + float t) noexcept; + +namespace details +{ +template +TimeT duration_to_msg(std::chrono::nanoseconds dt); +} // namespace details +} // namespace time_utils + +#endif // TIME_UTILS__TIME_UTILS_HPP_ diff --git a/common/time_utils/include/time_utils/visibility_control.hpp b/common/time_utils/include/time_utils/visibility_control.hpp new file mode 100644 index 0000000000000..7573c99b55c8d --- /dev/null +++ b/common/time_utils/include/time_utils/visibility_control.hpp @@ -0,0 +1,36 @@ +// Copyright 2019 Christopher Ho +// +// 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 TIME_UTILS__VISIBILITY_CONTROL_HPP_ +#define TIME_UTILS__VISIBILITY_CONTROL_HPP_ + +#if defined(__WIN32) + #if defined(TIME_UTILS_BUILDING_DLL) || defined(TIME_UTILS_EXPORTS) + #define TIME_UTILS_PUBLIC __declspec(dllexport) + #define TIME_UTILS_LOCAL + #else // defined(TIME_UTILS_BUILDING_DLL) || defined(TIME_UTILS_EXPORTS) + #define TIME_UTILS_PUBLIC __declspec(dllimport) + #define TIME_UTILS_LOCAL + #endif // defined(TIME_UTILS_BUILDING_DLL) || defined(TIME_UTILS_EXPORTS) +#elif defined(__linux__) + #define TIME_UTILS_PUBLIC __attribute__((visibility("default"))) + #define TIME_UTILS_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) + #define TIME_UTILS_PUBLIC __attribute__((visibility("default"))) + #define TIME_UTILS_LOCAL __attribute__((visibility("hidden"))) +#else // defined(_LINUX) + #error "Unsupported Build Configuration" +#endif // defined(_WINDOWS) + +#endif // TIME_UTILS__VISIBILITY_CONTROL_HPP_ diff --git a/common/time_utils/package.xml b/common/time_utils/package.xml new file mode 100644 index 0000000000000..74c81b4238084 --- /dev/null +++ b/common/time_utils/package.xml @@ -0,0 +1,23 @@ + + + + time_utils + 1.0.0 + + Simple conversion methods to/from std::chrono to simplify algorithm development + + Christopher Ho + Apache License 2.0 + + ament_cmake_auto + autoware_auto_cmake + autoware_auto_cmake + + builtin_interfaces + + ament_cmake_gtest + + + + ament_cmake + diff --git a/common/time_utils/src/time_utils/time_utils.cpp b/common/time_utils/src/time_utils/time_utils.cpp new file mode 100644 index 0000000000000..aa034b4078796 --- /dev/null +++ b/common/time_utils/src/time_utils/time_utils.cpp @@ -0,0 +1,108 @@ +// Copyright 2019 Christopher Ho +// +// 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 "time_utils/time_utils.hpp" + +#include +#include +#include + +namespace time_utils +{ +namespace details +{ +template +TimeT duration_to_msg(std::chrono::nanoseconds dt) +{ + // Round down seconds + const auto negative = decltype(dt)::zero() > dt; + if (negative) { + dt -= std::chrono::seconds(1); + } + // rounds towards zero + const auto dt_sec = std::chrono::duration_cast(dt); + TimeT ret{rosidl_runtime_cpp::MessageInitialization::ALL}; + // Clamp value to receptive field of retval seconds + { + using SecT = decltype(ret.sec); + using Rep = decltype(dt_sec)::rep; + const auto sec_max = static_cast(std::numeric_limits::max()); + const auto sec_min = static_cast(std::numeric_limits::min()); + const auto secs = std::min(std::max(sec_min, dt_sec.count()), sec_max); + ret.sec = static_cast(secs); + } + // Get raw nanoseconds + const auto dt_nsec = std::chrono::duration_cast(dt); + auto nanosec = dt_nsec - std::chrono::duration_cast(dt_sec); + if (negative) { + nanosec += std::chrono::seconds(1); + if (0 > nanosec.count()) { + throw std::logic_error{"Bug: Nanosec should never be negative"}; + } + } + // Clamp down to receptive field of retval nanoseconds + { + using NsecT = decltype(ret.nanosec); + using Rep = decltype(nanosec)::rep; + const auto nsec_max = static_cast(std::numeric_limits::max()); + const auto nsec_min = static_cast(std::numeric_limits::min()); + const auto nanosec_clamp = std::min(std::max(nsec_min, nanosec.count()), nsec_max); + ret.nanosec = static_cast(nanosec_clamp); + } + return ret; +} +} // namespace details + +//////////////////////////////////////////////////////////////////////////////// +builtin_interfaces::msg::Time to_message(const std::chrono::system_clock::time_point t) +{ + const auto dt = t.time_since_epoch(); + return details::duration_to_msg(dt); +} + +//////////////////////////////////////////////////////////////////////////////// +builtin_interfaces::msg::Duration to_message(std::chrono::nanoseconds dt) +{ + return details::duration_to_msg(dt); +} + +//////////////////////////////////////////////////////////////////////////////// +std::chrono::system_clock::time_point from_message(builtin_interfaces::msg::Time t) noexcept +{ + const auto dt_ns = std::chrono::seconds(t.sec) + std::chrono::nanoseconds(t.nanosec); + const auto dt = std::chrono::duration_cast(dt_ns); + return std::chrono::system_clock::time_point{} + dt; +} + +//////////////////////////////////////////////////////////////////////////////// +std::chrono::nanoseconds from_message(builtin_interfaces::msg::Duration dt) noexcept +{ + const auto ns = std::chrono::nanoseconds{dt.nanosec}; + const auto us = std::chrono::duration_cast(ns); + return std::chrono::seconds{dt.sec} + us; +} + +//////////////////////////////////////////////////////////////////////////////// +std::chrono::nanoseconds interpolate( + std::chrono::nanoseconds a, + std::chrono::nanoseconds b, + float t) noexcept +{ + // TODO(c.ho) consider long double + const auto t_ = static_cast(std::min(std::max(t, 0.0F), 1.0F)); + const auto del = std::chrono::duration_cast>(b - a); + const auto del_ = std::chrono::duration_cast(t_ * del); + return a + del_; +} + +} // namespace time_utils diff --git a/common/vehicle_constants_manager/CMakeLists.txt b/common/vehicle_constants_manager/CMakeLists.txt new file mode 100644 index 0000000000000..87a1b70faa670 --- /dev/null +++ b/common/vehicle_constants_manager/CMakeLists.txt @@ -0,0 +1,50 @@ +# Copyright 2021 The Autoware Foundation +# +# 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. + +cmake_minimum_required(VERSION 3.5) + +project(vehicle_constants_manager) + +# require that dependencies from package.xml be available +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +set(VEHICLE_CONSTANTS_MANAGER_LIB_SRC + src/vehicle_constants_manager.cpp) + +set(VEHICLE_CONSTANTS_MANAGER_LIB_HEADERS + include/vehicle_constants_manager/vehicle_constants_manager.hpp + include/vehicle_constants_manager/visibility_control.hpp) + +# generate library +ament_auto_add_library(${PROJECT_NAME} SHARED + ${VEHICLE_CONSTANTS_MANAGER_LIB_SRC} + ${VEHICLE_CONSTANTS_MANAGER_LIB_HEADERS}) +autoware_set_compile_options(${PROJECT_NAME}) + +# Testing +# if(BUILD_TESTING) +# find_package(ament_lint_auto REQUIRED) +# ament_lint_auto_find_test_dependencies() + +# # Unit tests +# set(TEST_SOURCES test/test_vehicle_constants_manager.cpp) +# set(TEST_VEHICLE_CONSTANTS_MANAGER_EXE test_vehicle_constants_manager) +# ament_add_gtest(${TEST_VEHICLE_CONSTANTS_MANAGER_EXE} ${TEST_SOURCES}) +# autoware_set_compile_options(${TEST_VEHICLE_CONSTANTS_MANAGER_EXE}) +# target_link_libraries(${TEST_VEHICLE_CONSTANTS_MANAGER_EXE} ${PROJECT_NAME}) +# endif() + +# ament package generation and installing +ament_auto_package(INSTALL_TO_SHARE) diff --git a/common/vehicle_constants_manager/design/vehicle_constants_manager-design.md b/common/vehicle_constants_manager/design/vehicle_constants_manager-design.md new file mode 100644 index 0000000000000..8e4a9882d5ff0 --- /dev/null +++ b/common/vehicle_constants_manager/design/vehicle_constants_manager-design.md @@ -0,0 +1,145 @@ +vehicle_constants_manager {#vehicle-constants-manager-package-design} +=========== + +This is the design document for the `vehicle_constants_manager` package. + +# Purpose / Use cases + + + + +This library provides a struct for holding vehicle specific constants. It also +provides a helper method to declare vehicle specific constants which have +already been passed into a node and provide a `VehicleConstants` object. + +# Design + + + +Provided `VehicleConstants` struct holds vehicle parameters. Its parameters can +be split in 2 categories: +(The detailed descriptions and units of the variables is in the +`vehicle_constants_manager.hpp` file.) + +- Primary Constants + - wheel_radius + - wheel_width + - wheel_base + - wheel_tread + - overhang_front + - overhang_rear + - overhang_left + - overhang_right + - vehicle_height + - cg_to_rear + - tire_cornering_stiffness_front_n_per_deg + - tire_cornering_stiffness_rear_n_per_deg + - mass_vehicle + - inertia_yaw_kg_m_2 +- Derived Constants + - cg_to_front + - vehicle_length + - vehicle_width + - offset_longitudinal_min + - offset_longitudinal_max + - offset_lateral_min + - offset_lateral_max + - offset_height_min + - offset_height_max + +The `VehicleConstants` constructor is initialized with the primary parameters. + +The library also provides a `declare_and_get_vehicle_constants` method. Using +this method, the user can declare vehicle parameters that are already passed +into the node and obtain a `VehicleConstants` object. + +## Assumptions / Known limits + + + +This library assumes the vehicle is defined with Ackermann steering geometry. + +`declare_and_get_vehicle_constants` method requires the passed node to have following parameters overridden: + +(Pay attention to the `vehicle` namespace) +```yaml +vehicle: + wheel_radius: + wheel_width: + wheel_base: + wheel_tread: + overhang_front: + overhang_rear: + overhang_left: + overhang_right: + vehicle_height: + cg_to_rear: + tire_cornering_stiffness_front_n_per_deg: + tire_cornering_stiffness_rear_n_per_deg: + mass_vehicle: + inertia_yaw_kg_m_2: +``` + + +## Inputs / Outputs / API {#vehicle-constants-manager-package-design-inputs} + + + + +The constructor of `VehicleConstants` takes the primary vehicle constants and +generates the derived parameters. + +`declare_and_get_vehicle_constants` method takes a `rclcpp::Node` object. And +returns a `VehicleConstants` object if it succeeds. + +Example usage: +```cpp +// In the constructor of a node which received primary vehicle parameters from a +// .yaml file or run args. +auto vehicle_constants = declare_and_get_vehicle_constants(*this); +``` + +## Inner-workings / Algorithms + + +Not Available. + +## Error detection and handling + + + +The `VehicleConstants` struct performs some sanity checks upon construction. + +It will throw `std::runtime_error` in case certain parameters are negative or +cg_to_rear is larger than wheel_base (to ensure center of gravity is within +front and rear axles.) + +# Security considerations + + + +To Be Determined. + +# References / External links + + +Not Available. + +# Future extensions / Unimplemented parts + + +Not Available. + +# Related issues + + +https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1294 diff --git a/common/vehicle_constants_manager/include/vehicle_constants_manager/vehicle_constants_manager.hpp b/common/vehicle_constants_manager/include/vehicle_constants_manager/vehicle_constants_manager.hpp new file mode 100644 index 0000000000000..72e53fff0c53f --- /dev/null +++ b/common/vehicle_constants_manager/include/vehicle_constants_manager/vehicle_constants_manager.hpp @@ -0,0 +1,186 @@ +// Copyright 2021 The Autoware Foundation +// +// 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. + +/// @copyright Copyright 2021 The Autoware Foundation +/// @file vehicle_constants_manager.hpp +/// @brief This file defines the vehicle_constants_manager class. + +#ifndef VEHICLE_CONSTANTS_MANAGER__VEHICLE_CONSTANTS_MANAGER_HPP_ +#define VEHICLE_CONSTANTS_MANAGER__VEHICLE_CONSTANTS_MANAGER_HPP_ + +#include +#include +#include + +#include +#include +#include + +namespace autoware +{ +namespace common +{ +namespace vehicle_constants_manager +{ +/// @brief A struct that holds vehicle specific parameters that don't change over time. +/// @details These parameters include wheel size, vehicle mass, vehicle size, tire cornering +/// stiffness, moment of inertia, center of gravity. +struct VEHICLE_CONSTANTS_MANAGER_PUBLIC VehicleConstants +{ + using SharedPtr = std::shared_ptr; + using ConstSharedPtr = const SharedPtr; + + using float64_t = autoware::common::types::float64_t; + + /// @brief Construct a new instance of the measurement from a state vector. + /// @param[in] wheel_radius Radius of a wheel + /// @param[in] wheel_width Width of a wheel + /// @param[in] wheel_base Distance between front and rear axles + /// @param[in] wheel_tread Distance between centres of left and right wheels + /// @param[in] overhang_front Distance from front axle to fore-most point of the vehicle + /// @param[in] overhang_rear Distance from rear axle to rear-most point of the vehicle + /// @param[in] overhang_left Distance from left wheel center to left-most point of the vehicle + /// @param[in] overhang_right Distance from left wheel center to right-most point of the vehicle + /// @param[in] vehicle_height Distance from ground plane to the top-most point of the vehicle + /// @param[in] cg_to_rear Distance between center of gravity to rear axle + /// @param[in] tire_cornering_stiffness_front Cornering stiffness for front wheels + /// @param[in] tire_cornering_stiffness_rear Cornering stiffness for rear wheels + /// @param[in] mass_vehicle Mass of the vehicle + /// @param[in] inertia_yaw_kg_m_2 Moment of Inertia of the vehicle on Z axis + /// @param[in] maximum_turning_angle_rad Maximum turning angle for cars front axis + /// @throws std::runtime_error if certain parameters are negative. + /// @throws std::runtime_error if cg_to_rear is larger than wheel_base (center of gravity must be + /// within front and rear axles.) + explicit VehicleConstants( + float64_t wheel_radius, float64_t wheel_width, float64_t wheel_base, + float64_t wheel_tread, float64_t overhang_front, + float64_t overhang_rear, float64_t overhang_left, + float64_t overhang_right, float64_t vehicle_height, + float64_t cg_to_rear, float64_t tire_cornering_stiffness_front, + float64_t tire_cornering_stiffness_rear, float64_t mass_vehicle, + float64_t inertia_yaw_kg_m_2, float64_t maximum_turning_angle_rad); + + // Primary Constants + + /// @brief [m] Radius of the wheel including the tires + const float64_t wheel_radius; + + /// @brief [m] Horizontal distance between 2 circular sides of the wheel + const float64_t wheel_width; + + /// @brief [m] Absolute distance between axis centers of front and rear wheels. + const float64_t wheel_base; + + /// @brief [m] Absolute distance between axis centers of left and right wheels. + const float64_t wheel_tread; + + /// @brief [m] Absolute distance between the vertical plane passing through the centres of the + /// front wheels and the foremost point of the vehicle + const float64_t overhang_front; + + /// @brief [m] Absolute distance between the vertical plane passing through the centres of the + /// rear wheels and the rearmost point of the vehicle + const float64_t overhang_rear; + + /// @brief [m] Absolute distance between axis centers of left wheels and the leftmost point of the + /// vehicle + const float64_t overhang_left; + + /// @brief [m] Absolute distance between axis centers of right wheels and the rightmost point of + /// the vehicle + const float64_t overhang_right; + + /// @brief [m] Absolute vertical distance between ground and topmost point of the vehicle + /// including mounted sensors + const float64_t vehicle_height; + + /// @brief [m] Absolute value of longitudinal distance between Center of Gravity and center of + /// rear axle + const float64_t cg_to_rear; + + /// @brief [kg/deg] The nominal cornering stiffness is equal to the side force in Newtons divided + /// by the slip angle in Degrees for small angles for front wheels + /// @details + /// https://www.mchenrysoftware.com/medit32/readme/msmac/default.htm?turl=examplestirecorneringstiffnesscalculation1.htm + const float64_t tire_cornering_stiffness_front; + + /// @brief [kg/deg] The nominal cornering stiffness for rear wheels + const float64_t tire_cornering_stiffness_rear; + + /// @brief [kg] Total mass of the vehicle including sensors in kilograms + const float64_t mass_vehicle; + + /// @brief [kg * m^2] Moment of inertia around vertical axis of the vehicle + const float64_t inertia_yaw_kg_m2; + + /// @brief [rad] Maximum turning angle for cars front axis + const float64_t maximum_turning_angle_rad; + + + // Derived Constants + + /// @brief [m] Absolute value of longitudinal distance between Center of Gravity and center of + /// front axle + const float64_t cg_to_front; + + /// @brief [m] Horizontal distance between foremost and rearmost points of the vehicle + const float64_t vehicle_length; + + /// @brief [m] Horizontal distance between leftmost and rightmost points of the vehicle + const float64_t vehicle_width; + + /// @brief Offsets from base_link + /// + /// These values assume X+ is forward, Y+ is left, Z+ is up + + /// @brief [m] Signed distance from base_link to the rear-most point of the vehicle. (Negative) + const float64_t offset_longitudinal_min; + + /// @brief [m] Signed distance from base_link to the fore-most point of the vehicle. + const float64_t offset_longitudinal_max; + + /// @brief [m] Signed distance from base_link to the right-most point of the vehicle. (Negative) + const float64_t offset_lateral_min; + + /// @brief [m] Signed distance from base_link to the left-most point of the vehicle. + const float64_t offset_lateral_max; + + /// @brief [m] Signed distance from base_link to the bottom-most point of the vehicle. (Negative) + const float64_t offset_height_min; + + /// @brief [m] Signed distance from base_link to the top-most point of the vehicle. + const float64_t offset_height_max; + + /// @brief [rad] Minimum turning radius + float64_t minimum_turning_radius; + + /// @brief Retrieves a list of vehicle parameters names and values as a string. + std::string str_pretty() const; +}; + +/// @brief Declares the vehicle parameters for the node and creates a VehicleConstants object. +/// @details It creates a `rclcpp::SyncParametersClient` object to reach parameters of the +/// `vehicle_constants_manager_node` and attempts to retrieve all required parameters from the node. +/// @throws std::runtime_error if `VehicleConstants` object fails to initialize +/// @throws rclcpp::exceptions::InvalidParameterTypeException if declare_parameter gets a value with +/// wrong type +/// @throws rclcpp::exceptions::InvalidParameterValueException if initial value fails to be set. +/// @return A VehicleConstants object containing vehicle constant parameters. +VEHICLE_CONSTANTS_MANAGER_PUBLIC VehicleConstants +declare_and_get_vehicle_constants(rclcpp::Node & node); +} // namespace vehicle_constants_manager +} // namespace common +} // namespace autoware + +#endif // VEHICLE_CONSTANTS_MANAGER__VEHICLE_CONSTANTS_MANAGER_HPP_ diff --git a/common/vehicle_constants_manager/include/vehicle_constants_manager/visibility_control.hpp b/common/vehicle_constants_manager/include/vehicle_constants_manager/visibility_control.hpp new file mode 100644 index 0000000000000..df84d51780bd2 --- /dev/null +++ b/common/vehicle_constants_manager/include/vehicle_constants_manager/visibility_control.hpp @@ -0,0 +1,39 @@ +// Copyright 2021 The Autoware Foundation +// +// 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_CONSTANTS_MANAGER__VISIBILITY_CONTROL_HPP_ +#define VEHICLE_CONSTANTS_MANAGER__VISIBILITY_CONTROL_HPP_ + +//////////////////////////////////////////////////////////////////////////////// +#if defined(__WIN32) + #if defined(VEHICLE_CONSTANTS_MANAGER_BUILDING_DLL) || defined(VEHICLE_CONSTANTS_MANAGER_EXPORTS) + #define VEHICLE_CONSTANTS_MANAGER_PUBLIC __declspec(dllexport) + #define VEHICLE_CONSTANTS_MANAGER_LOCAL + #else // defined(VEHICLE_CONSTANTS_MANAGER_BUILDING_DLL) +// || defined(VEHICLE_CONSTANTS_MANAGER_EXPORTS) + #define VEHICLE_CONSTANTS_MANAGER_PUBLIC __declspec(dllimport) + #define VEHICLE_CONSTANTS_MANAGER_LOCAL + #endif // defined(VEHICLE_CONSTANTS_MANAGER_BUILDING_DLL) +// || defined(VEHICLE_CONSTANTS_MANAGER_EXPORTS) +#elif defined(__linux__) + #define VEHICLE_CONSTANTS_MANAGER_PUBLIC __attribute__((visibility("default"))) + #define VEHICLE_CONSTANTS_MANAGER_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) + #define VEHICLE_CONSTANTS_MANAGER_PUBLIC __attribute__((visibility("default"))) + #define VEHICLE_CONSTANTS_MANAGER_LOCAL __attribute__((visibility("hidden"))) +#else + #error "Unsupported Build Configuration" +#endif + +#endif // VEHICLE_CONSTANTS_MANAGER__VISIBILITY_CONTROL_HPP_ diff --git a/common/vehicle_constants_manager/package.xml b/common/vehicle_constants_manager/package.xml new file mode 100644 index 0000000000000..97068350b0927 --- /dev/null +++ b/common/vehicle_constants_manager/package.xml @@ -0,0 +1,25 @@ + + + + vehicle_constants_manager + 1.0.0 + Library package to help nodes access vehicle specific constants. + M. Fatih Cırıt + Apache 2.0 + + ament_cmake_auto + autoware_auto_cmake + + rclcpp + rclcpp_components + autoware_auto_common + + ament_cmake_gtest + + autoware_auto_cmake + + + ament_cmake + + diff --git a/common/vehicle_constants_manager/param/params_lexus_rx_hybrid_2016.yaml b/common/vehicle_constants_manager/param/params_lexus_rx_hybrid_2016.yaml new file mode 100644 index 0000000000000..95121d84e0f6e --- /dev/null +++ b/common/vehicle_constants_manager/param/params_lexus_rx_hybrid_2016.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + vehicle: + wheel_radius: 0.37 # Measured from SVL + wheel_width: 0.27 # Measured from SVL + wheel_base: 2.734 # Measured from SVL + wheel_tread: 1.571 # Measured from SVL + overhang_front: 1.033 # Measured from SVL + overhang_rear: 1.021 # Measured from SVL + overhang_left: 0.3135 # Measured from SVL + overhang_right: 0.3135 # Measured from SVL + vehicle_height: 1.662 # Measured from SVL + cg_to_rear: 1.367 # Assuming it is in middle of front and rear axle centers + tire_cornering_stiffness_front: 0.1 # Taken from AVP demo params, can't verify + tire_cornering_stiffness_rear: 0.1 # Taken from AVP demo params, can't verify + mass_vehicle: 2120.0 # Measured from SVL + inertia_yaw_kg_m2: 12.0 # Taken from AVP demo params, can't verify + maximum_turning_angle_rad: 0.53 + + diff --git a/common/vehicle_constants_manager/src/vehicle_constants_manager.cpp b/common/vehicle_constants_manager/src/vehicle_constants_manager.cpp new file mode 100644 index 0000000000000..e895194e098a5 --- /dev/null +++ b/common/vehicle_constants_manager/src/vehicle_constants_manager.cpp @@ -0,0 +1,196 @@ +// Copyright 2021 The Autoware Foundation +// +// 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 "vehicle_constants_manager/vehicle_constants_manager.hpp" +#include + +#include +#include +#include +#include + +namespace autoware +{ +namespace common +{ +namespace vehicle_constants_manager +{ + +using float64_t = VehicleConstants::float64_t; + + +VehicleConstants::VehicleConstants( + float64_t wheel_radius, + float64_t wheel_width, + float64_t wheel_base, + float64_t wheel_tread, + float64_t overhang_front, + float64_t overhang_rear, + float64_t overhang_left, + float64_t overhang_right, + float64_t vehicle_height, + float64_t cg_to_rear, + float64_t tire_cornering_stiffness_front, + float64_t tire_cornering_stiffness_rear, + float64_t mass_vehicle, + float64_t inertia_yaw_kg_m_2, + float64_t maximum_turning_angle_rad) +: wheel_radius(wheel_radius), + wheel_width(wheel_width), + wheel_base(wheel_base), + wheel_tread(wheel_tread), + overhang_front(overhang_front), + overhang_rear(overhang_rear), + overhang_left(overhang_left), + overhang_right(overhang_right), + vehicle_height(vehicle_height), + cg_to_rear(cg_to_rear), + tire_cornering_stiffness_front(tire_cornering_stiffness_front), + tire_cornering_stiffness_rear(tire_cornering_stiffness_rear), + mass_vehicle(mass_vehicle), + inertia_yaw_kg_m2(inertia_yaw_kg_m_2), + maximum_turning_angle_rad(maximum_turning_angle_rad), + cg_to_front(wheel_base - cg_to_rear), + vehicle_length(overhang_front + wheel_base + overhang_rear), + vehicle_width(overhang_left + wheel_tread + overhang_right), + offset_longitudinal_min(-overhang_rear), + offset_longitudinal_max(wheel_base + overhang_front), + offset_lateral_min(-(wheel_tread / 2.0 + overhang_right)), + offset_lateral_max(wheel_tread / 2.0 + overhang_left), + offset_height_min(-wheel_radius), + offset_height_max(vehicle_height - wheel_radius) +{ + // Sanity Checks + // Center of gravity must be between front and rear axles + if (wheel_base < cg_to_rear) { + throw std::runtime_error("wheel_base must be larger than cg_to_rear"); + } + + // These values must be positive + auto throw_if_negative = [](float64_t number, const std::string & name) { + if (number < 0.0) { + throw std::runtime_error( + name + " = " + std::to_string(number) + + " shouldn't be negative."); + } + }; + throw_if_negative(wheel_radius, "wheel_radius"); + throw_if_negative(wheel_width, "wheel_width"); + throw_if_negative(wheel_base, "wheel_base"); + throw_if_negative(wheel_tread, "wheel_tread"); + throw_if_negative(overhang_front, "overhang_front"); + throw_if_negative(overhang_rear, "overhang_rear"); + throw_if_negative(overhang_left, "overhang_left"); + throw_if_negative(overhang_right, "overhang_right"); + throw_if_negative(vehicle_height, "vehicle_height"); + throw_if_negative(cg_to_rear, "cg_to_rear"); + throw_if_negative(tire_cornering_stiffness_front, "tire_cornering_stiffness_front"); + throw_if_negative(tire_cornering_stiffness_rear, "tire_cornering_stiffness_rear"); + throw_if_negative(mass_vehicle, "mass_vehicle"); + throw_if_negative(inertia_yaw_kg_m_2, "inertia_yaw_kg_m_2"); + throw_if_negative(maximum_turning_angle_rad, "maximum_turning_angle_rad"); + + if (!(0.0 < maximum_turning_angle_rad && maximum_turning_angle_rad < (M_PI / 2.0))) { + throw std::runtime_error( + "maximum_turning_angle_rad must be positive and cannot be greater than 0.5*PI."); + } + minimum_turning_radius = wheel_base / tan(maximum_turning_angle_rad); +} + +std::string VehicleConstants::str_pretty() const +{ + return std::string{ + "wheel_radius: " + std::to_string(wheel_radius) + "\n" + "wheel_width: " + std::to_string(wheel_width) + "\n" + "wheel_base: " + std::to_string(wheel_base) + "\n" + "wheel_tread: " + std::to_string(wheel_tread) + "\n" + "overhang_front: " + std::to_string(overhang_front) + "\n" + "overhang_rear: " + std::to_string(overhang_rear) + "\n" + "overhang_left: " + std::to_string(overhang_left) + "\n" + "overhang_right: " + std::to_string(overhang_right) + "\n" + "vehicle_height: " + std::to_string(vehicle_height) + "\n" + "cg_to_rear: " + std::to_string(cg_to_rear) + "\n" + "tire_cornering_stiffness_front: " + std::to_string(tire_cornering_stiffness_front) + "\n" + "tire_cornering_stiffness_rear: " + std::to_string(tire_cornering_stiffness_rear) + "\n" + "mass_vehicle: " + std::to_string(mass_vehicle) + "\n" + "inertia_yaw_kg_m2: " + std::to_string(inertia_yaw_kg_m2) + "\n" + "maximum_turning_angle_rad: " + std::to_string(maximum_turning_angle_rad) + "\n" + "cg_to_front: " + std::to_string(cg_to_front) + "\n" + "vehicle_length: " + std::to_string(vehicle_length) + "\n" + "vehicle_width: " + std::to_string(vehicle_width) + "\n" + "offset_longitudinal_min: " + std::to_string(offset_longitudinal_min) + "\n" + "offset_longitudinal_max: " + std::to_string(offset_longitudinal_max) + "\n" + "offset_lateral_min: " + std::to_string(offset_lateral_min) + "\n" + "offset_lateral_max: " + std::to_string(offset_lateral_max) + "\n" + "offset_height_min: " + std::to_string(offset_height_min) + "\n" + "offset_height_max: " + std::to_string(offset_height_max) + "\n" + "minimum_turning_radius: " + std::to_string(minimum_turning_radius) + "\n" + }; +} + +VehicleConstants declare_and_get_vehicle_constants(rclcpp::Node & node) +{ + // Initialize the parameters + const std::string ns = "vehicle."; + std::map params{ + std::make_pair(ns + "wheel_radius", -1.0), + std::make_pair(ns + "wheel_width", -1.0), + std::make_pair(ns + "wheel_base", -1.0), + std::make_pair(ns + "wheel_tread", -1.0), + std::make_pair(ns + "overhang_front", -1.0), + std::make_pair(ns + "overhang_rear", -1.0), + std::make_pair(ns + "overhang_left", -1.0), + std::make_pair(ns + "overhang_right", -1.0), + std::make_pair(ns + "vehicle_height", -1.0), + std::make_pair(ns + "cg_to_rear", -1.0), + std::make_pair(ns + "tire_cornering_stiffness_front", -1.0), + std::make_pair(ns + "tire_cornering_stiffness_rear", -1.0), + std::make_pair(ns + "mass_vehicle", -1.0), + std::make_pair(ns + "inertia_yaw_kg_m2", -1.0), + std::make_pair(ns + "maximum_turning_angle_rad", -1.0) + }; + + // Try to get parameter values from parameter_overrides set either from .yaml + // or with args. + for (auto & pair : params) { + // If it is already declared + if (node.has_parameter(pair.first)) { + node.get_parameter(pair.first, pair.second); + continue; + } + + pair.second = node.declare_parameter(pair.first, rclcpp::ParameterType::PARAMETER_DOUBLE).get(); + } + + return VehicleConstants( + params.at(ns + "wheel_radius"), + params.at(ns + "wheel_width"), + params.at(ns + "wheel_base"), + params.at(ns + "wheel_tread"), + params.at(ns + "overhang_front"), + params.at(ns + "overhang_rear"), + params.at(ns + "overhang_left"), + params.at(ns + "overhang_right"), + params.at(ns + "vehicle_height"), + params.at(ns + "cg_to_rear"), + params.at(ns + "tire_cornering_stiffness_front"), + params.at(ns + "tire_cornering_stiffness_rear"), + params.at(ns + "mass_vehicle"), + params.at(ns + "inertia_yaw_kg_m2"), + params.at(ns + "maximum_turning_angle_rad") + ); +} +} // namespace vehicle_constants_manager +} // namespace common +} // namespace autoware diff --git a/common/vehicle_constants_manager/test/test_vehicle_constants_manager.cpp b/common/vehicle_constants_manager/test/test_vehicle_constants_manager.cpp new file mode 100644 index 0000000000000..b57c362c9d588 --- /dev/null +++ b/common/vehicle_constants_manager/test/test_vehicle_constants_manager.cpp @@ -0,0 +1,124 @@ +// Copyright 2021 The Autoware Foundation +// +// 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 +#include +#include + +#include "vehicle_constants_manager/vehicle_constants_manager.hpp" +#include "gtest/gtest.h" + +TEST(TestVehicleConstantsManager, TestInitializationConstructor) { + using float64_t = autoware::common::types::float64_t; + using VehicleConstants = + autoware::common::vehicle_constants_manager::VehicleConstants; + + const float64_t wheel_radius = 0.37; + const float64_t wheel_width = 0.27; + const float64_t wheel_base = 2.734; + const float64_t wheel_tread = 1.571; + const float64_t overhang_front = 1.033; + const float64_t overhang_rear = 1.021; + const float64_t overhang_left = 0.3135; + const float64_t overhang_right = 0.3135; + const float64_t vehicle_height = 1.662; + const float64_t cg_to_rear = 1.367; + const float64_t tire_cornering_stiffness_front_n_per_deg = 0.1; + const float64_t tire_cornering_stiffness_rear_n_per_deg = 0.1; + const float64_t mass_vehicle = 2120.0; + const float64_t inertia_yaw_kg_m_2 = 12.0; + const float64_t maximum_turning_angle_rad = 0.53; + + // Well set parameters + EXPECT_NO_THROW( + VehicleConstants vc( + wheel_radius, wheel_width, wheel_base, wheel_tread, overhang_front, + overhang_rear, overhang_left, overhang_right, vehicle_height, cg_to_rear, + tire_cornering_stiffness_front_n_per_deg, + tire_cornering_stiffness_rear_n_per_deg, mass_vehicle, + inertia_yaw_kg_m_2, maximum_turning_angle_rad)); + + // Center of gravity is not within wheel_base + const float64_t bad_cg_to_rear = wheel_base + 1.0; + EXPECT_THROW( + VehicleConstants vc( + wheel_radius, wheel_width, wheel_base, + wheel_tread, overhang_front, overhang_rear, + overhang_left, overhang_right, + vehicle_height, bad_cg_to_rear, + tire_cornering_stiffness_front_n_per_deg, + tire_cornering_stiffness_rear_n_per_deg, + mass_vehicle, inertia_yaw_kg_m_2, + maximum_turning_angle_rad), + std::runtime_error); + + // Some supposedly absolute parameters are negative + EXPECT_THROW( + VehicleConstants vc( + wheel_radius, wheel_width, -wheel_base, + wheel_tread, -overhang_front, overhang_rear, + overhang_left, -overhang_right, + vehicle_height, cg_to_rear, + tire_cornering_stiffness_front_n_per_deg, + tire_cornering_stiffness_rear_n_per_deg, + mass_vehicle, inertia_yaw_kg_m_2, + maximum_turning_angle_rad), + std::runtime_error); +} + +TEST(TestVehicleConstantsManager, TestGetVehicleConstantsMissing) { + rclcpp::init(0, nullptr); + const std::string ns_node = "TestGetVehicleConstantsMissing"; + rclcpp::Node node("some_node", ns_node); + EXPECT_THROW( + autoware::common::vehicle_constants_manager:: + declare_and_get_vehicle_constants(node), + std::runtime_error); + rclcpp::shutdown(); +} + +TEST(TestVehicleConstantsManager, TestGetVehicleConstants) { + rclcpp::init(0, nullptr); + const std::string ns_node = "TestGetVehicleConstants"; + std::vector params; + const std::string ns_vehicle = "vehicle."; + params.emplace_back(ns_vehicle + "wheel_radius", 0.37); + params.emplace_back(ns_vehicle + "wheel_width", 0.27); + params.emplace_back(ns_vehicle + "wheel_base", 2.734); + params.emplace_back(ns_vehicle + "wheel_tread", 1.571); + params.emplace_back(ns_vehicle + "overhang_front", 1.033); + params.emplace_back(ns_vehicle + "overhang_rear", 1.021); + params.emplace_back(ns_vehicle + "overhang_left", 0.3135); + params.emplace_back(ns_vehicle + "overhang_right", 0.3135); + params.emplace_back(ns_vehicle + "vehicle_height", 1.662); + params.emplace_back(ns_vehicle + "cg_to_rear", 1.367); + params.emplace_back(ns_vehicle + "tire_cornering_stiffness_front", 0.1); + params.emplace_back(ns_vehicle + "tire_cornering_stiffness_rear", 0.1); + params.emplace_back(ns_vehicle + "mass_vehicle", 2120.0); + params.emplace_back(ns_vehicle + "inertia_yaw_kg_m2", 12.0); + params.emplace_back(ns_vehicle + "maximum_turning_angle_rad", 0.53); + + rclcpp::NodeOptions node_options; + node_options.parameter_overrides(params); + + const rclcpp::Node::SharedPtr node = + std::make_shared("some_node", ns_node, node_options); + + EXPECT_NO_THROW( + autoware::common::vehicle_constants_manager:: + declare_and_get_vehicle_constants(*node)); + + rclcpp::shutdown(); +}