From 313a383089533b59373847acc367ac673d82f655 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Thu, 19 Sep 2024 11:32:26 +0900 Subject: [PATCH 01/95] refactor(ndt_scan_matcher)!: prefix package and namespace with autoware (#8904) add autoware_ prefix Signed-off-by: a-maumau --- .github/CODEOWNERS | 2 +- .../ndt_scan_matcher.launch.xml | 2 +- launch/tier4_localization_launch/package.xml | 2 +- .../CMakeLists.txt | 4 +-- .../README.md | 26 +++++++++--------- .../config/ndt_scan_matcher.param.yaml | 0 .../ndt_scan_matcher/hyper_parameters.hpp | 6 ++-- .../ndt_scan_matcher/map_update_module.hpp | 10 +++---- .../ndt_scan_matcher_core.hpp | 10 +++---- .../autoware}/ndt_scan_matcher/particle.hpp | 6 ++-- .../launch/ndt_scan_matcher.launch.xml | 4 +-- .../media/bridge_map.jpg | Bin .../media/bridge_map_less_feature.jpg | Bin .../media/calculation_of_ndt_covariance.png | Bin ...gnostic_initial_pose_subscriber_status.png | Bin .../media/diagnostic_map_update_status.png | Bin .../diagnostic_ndt_align_service_status.png | Bin ..._regularization_pose_subscriber_status.png | Bin .../media/diagnostic_scan_matching_status.png | Bin ...diagnostic_trigger_node_service_status.png | Bin .../media/differential_area_loading.gif | Bin .../media/trajectory_with_regularization.png | Bin .../trajectory_without_regularization.png | Bin .../package.xml | 4 +-- .../schema/ndt_scan_matcher.schema.json | 0 .../schema/sub/covariance.json | 0 .../sub/covariance_covariance_estimation.json | 0 .../schema/sub/dynamic_map_loading.json | 0 .../schema/sub/frame.json | 0 .../schema/sub/initial_pose_estimation.json | 0 .../schema/sub/ndt.json | 0 .../schema/sub/ndt_regularization.json | 0 .../schema/sub/score_estimation.json | 0 .../score_estimation_no_ground_points.json | 0 .../schema/sub/sensor_points.json | 0 .../schema/sub/validation.json | 0 .../src/map_update_module.cpp | 2 +- .../src/ndt_scan_matcher_core.cpp | 4 +-- .../src/particle.cpp | 2 +- .../test/stub_initialpose_client.hpp | 0 .../test/stub_pcd_loader.hpp | 0 .../test/stub_sensor_pcd_publisher.hpp | 0 .../test/stub_trigger_node_client.hpp | 0 ...t_out_of_map_then_initialize_correctly.cpp | 0 ...d_sequence_for_initial_pose_estimation.cpp | 0 .../test/test_fixture.hpp | 7 +++-- .../test/test_ndt_scan_matcher_launch.py | 2 +- .../test/test_util.hpp | 0 .../autoware_pose_estimator_arbiter/README.md | 2 +- map/map_loader/README.md | 2 +- 50 files changed, 49 insertions(+), 48 deletions(-) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/CMakeLists.txt (95%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/README.md (97%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/config/ndt_scan_matcher.param.yaml (100%) rename localization/{ndt_scan_matcher/include => autoware_ndt_scan_matcher/include/autoware}/ndt_scan_matcher/hyper_parameters.hpp (97%) rename localization/{ndt_scan_matcher/include => autoware_ndt_scan_matcher/include/autoware}/ndt_scan_matcher/map_update_module.hpp (91%) rename localization/{ndt_scan_matcher/include => autoware_ndt_scan_matcher/include/autoware}/ndt_scan_matcher/ndt_scan_matcher_core.hpp (96%) rename localization/{ndt_scan_matcher/include => autoware_ndt_scan_matcher/include/autoware}/ndt_scan_matcher/particle.hpp (90%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/launch/ndt_scan_matcher.launch.xml (86%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/media/bridge_map.jpg (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/media/bridge_map_less_feature.jpg (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/media/calculation_of_ndt_covariance.png (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/media/diagnostic_initial_pose_subscriber_status.png (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/media/diagnostic_map_update_status.png (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/media/diagnostic_ndt_align_service_status.png (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/media/diagnostic_regularization_pose_subscriber_status.png (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/media/diagnostic_scan_matching_status.png (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/media/diagnostic_trigger_node_service_status.png (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/media/differential_area_loading.gif (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/media/trajectory_with_regularization.png (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/media/trajectory_without_regularization.png (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/package.xml (94%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/schema/ndt_scan_matcher.schema.json (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/schema/sub/covariance.json (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/schema/sub/covariance_covariance_estimation.json (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/schema/sub/dynamic_map_loading.json (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/schema/sub/frame.json (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/schema/sub/initial_pose_estimation.json (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/schema/sub/ndt.json (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/schema/sub/ndt_regularization.json (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/schema/sub/score_estimation.json (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/schema/sub/score_estimation_no_ground_points.json (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/schema/sub/sensor_points.json (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/schema/sub/validation.json (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/src/map_update_module.cpp (99%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/src/ndt_scan_matcher_core.cpp (99%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/src/particle.cpp (98%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/test/stub_initialpose_client.hpp (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/test/stub_pcd_loader.hpp (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/test/stub_sensor_pcd_publisher.hpp (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/test/stub_trigger_node_client.hpp (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/test/test_cases/once_initialize_at_out_of_map_then_initialize_correctly.cpp (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/test/test_cases/standard_sequence_for_initial_pose_estimation.cpp (100%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/test/test_fixture.hpp (93%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/test/test_ndt_scan_matcher_launch.py (97%) rename localization/{ndt_scan_matcher => autoware_ndt_scan_matcher}/test/test_util.hpp (100%) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 1e6e29dd67051..d009a73809074 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -92,7 +92,7 @@ localization/autoware_pose_instability_detector/** anh.nguyen.2@tier4.jp kento.y localization/autoware_stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/autoware_ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_common/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml index ef3522b215227..b47c4d99c8dd2 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 20c39c0b58f2e..1cd48aee74469 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -23,6 +23,7 @@ autoware_geo_pose_projector autoware_gyro_odometer autoware_lidar_marker_localizer + autoware_ndt_scan_matcher autoware_pointcloud_preprocessor autoware_pose_estimator_arbiter autoware_pose_initializer @@ -30,7 +31,6 @@ eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_rt - ndt_scan_matcher topic_tools yabloc_common yabloc_image_processing diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/autoware_ndt_scan_matcher/CMakeLists.txt similarity index 95% rename from localization/ndt_scan_matcher/CMakeLists.txt rename to localization/autoware_ndt_scan_matcher/CMakeLists.txt index b7a70b8442458..98bdf9be8761e 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/autoware_ndt_scan_matcher/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(ndt_scan_matcher) +project(autoware_ndt_scan_matcher) find_package(autoware_cmake REQUIRED) autoware_package() @@ -32,7 +32,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) link_directories(${PCL_LIBRARY_DIRS}) -target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES}) rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "autoware::ndt_scan_matcher::NDTScanMatcher" diff --git a/localization/ndt_scan_matcher/README.md b/localization/autoware_ndt_scan_matcher/README.md similarity index 97% rename from localization/ndt_scan_matcher/README.md rename to localization/autoware_ndt_scan_matcher/README.md index 1881deab23d05..22e56930a0048 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/autoware_ndt_scan_matcher/README.md @@ -1,8 +1,8 @@ -# ndt_scan_matcher +# autoware_ndt_scan_matcher ## Purpose -ndt_scan_matcher is a package for position estimation using the NDT scan matching method. +autoware_ndt_scan_matcher is a package for position estimation using the NDT scan matching method. There are two main functions in this package: @@ -58,31 +58,31 @@ One optional function is regularization. Please see the regularization chapter i #### Frame -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/frame.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/frame.json") }} #### Sensor Points -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/sensor_points.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/sensor_points.json") }} #### Ndt -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/ndt.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/ndt.json") }} #### Initial Pose Estimation -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/initial_pose_estimation.json") }} #### Validation -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/validation.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/validation.json") }} #### Score Estimation -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/score_estimation.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/score_estimation.json") }} #### Covariance -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/covariance.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/covariance.json") }} ## Regularization @@ -158,7 +158,7 @@ This is because if the base position is far off from the true value, NDT scan ma ### Parameters -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/ndt_regularization.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/ndt_regularization.json") }} Regularization is disabled by default because GNSS is not always accurate enough to serve the appropriate base position in any scenes. @@ -208,7 +208,7 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma ### Parameters -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/dynamic_map_loading.json") }} ### Notes for dynamic map loading @@ -235,7 +235,7 @@ This is a function that uses no ground LiDAR scan to estimate the scan matching ### Parameters -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json") }} ## 2D real-time covariance estimation @@ -262,7 +262,7 @@ initial_pose_offset_model is rotated around (x,y) = (0,0) in the direction of th initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. In MULTI_NDT_SCORE mode, the scale of the output 2D covariance can be adjusted according to the temperature. -{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json") }} +{{ json_to_markdown("localization/autoware_ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json") }} ## Diagnostics diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/autoware_ndt_scan_matcher/config/ndt_scan_matcher.param.yaml similarity index 100% rename from localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml rename to localization/autoware_ndt_scan_matcher/config/ndt_scan_matcher.param.yaml diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/hyper_parameters.hpp similarity index 97% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp rename to localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/hyper_parameters.hpp index 8244bb7ab4b92..c1ffbe696fda3 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/hyper_parameters.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ -#define NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ +#ifndef AUTOWARE__NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ +#define AUTOWARE__NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ #include @@ -196,4 +196,4 @@ struct HyperParameters } // namespace autoware::ndt_scan_matcher -#endif // NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ +#endif // AUTOWARE__NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp similarity index 91% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp rename to localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp index 24ecc17ff7af1..8e7bf78845674 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ -#define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ +#ifndef AUTOWARE__NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ +#define AUTOWARE__NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ +#include "autoware/ndt_scan_matcher/hyper_parameters.hpp" +#include "autoware/ndt_scan_matcher/particle.hpp" #include "localization_util/diagnostics_module.hpp" #include "localization_util/util_func.hpp" -#include "ndt_scan_matcher/hyper_parameters.hpp" -#include "ndt_scan_matcher/particle.hpp" #include #include @@ -100,4 +100,4 @@ class MapUpdateModule } // namespace autoware::ndt_scan_matcher -#endif // NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ +#endif // AUTOWARE__NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp similarity index 96% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp rename to localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 649a0995e0d63..b4062535503c1 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ -#define NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ +#ifndef AUTOWARE__NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ +#define AUTOWARE__NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ #define FMT_HEADER_ONLY +#include "autoware/ndt_scan_matcher/hyper_parameters.hpp" +#include "autoware/ndt_scan_matcher/map_update_module.hpp" #include "localization_util/diagnostics_module.hpp" #include "localization_util/smart_pose_buffer.hpp" -#include "ndt_scan_matcher/hyper_parameters.hpp" -#include "ndt_scan_matcher/map_update_module.hpp" #include #include @@ -225,4 +225,4 @@ class NDTScanMatcher : public rclcpp::Node } // namespace autoware::ndt_scan_matcher -#endif // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ +#endif // AUTOWARE__NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/particle.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/particle.hpp similarity index 90% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/particle.hpp rename to localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/particle.hpp index c5ecc7f2e9837..71b5d89ceb151 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/particle.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/particle.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__PARTICLE_HPP_ -#define NDT_SCAN_MATCHER__PARTICLE_HPP_ +#ifndef AUTOWARE__NDT_SCAN_MATCHER__PARTICLE_HPP_ +#define AUTOWARE__NDT_SCAN_MATCHER__PARTICLE_HPP_ #include #include @@ -43,4 +43,4 @@ void push_debug_markers( } // namespace autoware::ndt_scan_matcher -#endif // NDT_SCAN_MATCHER__PARTICLE_HPP_ +#endif // AUTOWARE__NDT_SCAN_MATCHER__PARTICLE_HPP_ diff --git a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml b/localization/autoware_ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml similarity index 86% rename from localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml rename to localization/autoware_ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml index 671e2ace56cad..2d1a5ce4c6c35 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/autoware_ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -1,5 +1,5 @@ - + @@ -14,7 +14,7 @@ - + diff --git a/localization/ndt_scan_matcher/media/bridge_map.jpg b/localization/autoware_ndt_scan_matcher/media/bridge_map.jpg similarity index 100% rename from localization/ndt_scan_matcher/media/bridge_map.jpg rename to localization/autoware_ndt_scan_matcher/media/bridge_map.jpg diff --git a/localization/ndt_scan_matcher/media/bridge_map_less_feature.jpg b/localization/autoware_ndt_scan_matcher/media/bridge_map_less_feature.jpg similarity index 100% rename from localization/ndt_scan_matcher/media/bridge_map_less_feature.jpg rename to localization/autoware_ndt_scan_matcher/media/bridge_map_less_feature.jpg diff --git a/localization/ndt_scan_matcher/media/calculation_of_ndt_covariance.png b/localization/autoware_ndt_scan_matcher/media/calculation_of_ndt_covariance.png similarity index 100% rename from localization/ndt_scan_matcher/media/calculation_of_ndt_covariance.png rename to localization/autoware_ndt_scan_matcher/media/calculation_of_ndt_covariance.png diff --git a/localization/ndt_scan_matcher/media/diagnostic_initial_pose_subscriber_status.png b/localization/autoware_ndt_scan_matcher/media/diagnostic_initial_pose_subscriber_status.png similarity index 100% rename from localization/ndt_scan_matcher/media/diagnostic_initial_pose_subscriber_status.png rename to localization/autoware_ndt_scan_matcher/media/diagnostic_initial_pose_subscriber_status.png diff --git a/localization/ndt_scan_matcher/media/diagnostic_map_update_status.png b/localization/autoware_ndt_scan_matcher/media/diagnostic_map_update_status.png similarity index 100% rename from localization/ndt_scan_matcher/media/diagnostic_map_update_status.png rename to localization/autoware_ndt_scan_matcher/media/diagnostic_map_update_status.png diff --git a/localization/ndt_scan_matcher/media/diagnostic_ndt_align_service_status.png b/localization/autoware_ndt_scan_matcher/media/diagnostic_ndt_align_service_status.png similarity index 100% rename from localization/ndt_scan_matcher/media/diagnostic_ndt_align_service_status.png rename to localization/autoware_ndt_scan_matcher/media/diagnostic_ndt_align_service_status.png diff --git a/localization/ndt_scan_matcher/media/diagnostic_regularization_pose_subscriber_status.png b/localization/autoware_ndt_scan_matcher/media/diagnostic_regularization_pose_subscriber_status.png similarity index 100% rename from localization/ndt_scan_matcher/media/diagnostic_regularization_pose_subscriber_status.png rename to localization/autoware_ndt_scan_matcher/media/diagnostic_regularization_pose_subscriber_status.png diff --git a/localization/ndt_scan_matcher/media/diagnostic_scan_matching_status.png b/localization/autoware_ndt_scan_matcher/media/diagnostic_scan_matching_status.png similarity index 100% rename from localization/ndt_scan_matcher/media/diagnostic_scan_matching_status.png rename to localization/autoware_ndt_scan_matcher/media/diagnostic_scan_matching_status.png diff --git a/localization/ndt_scan_matcher/media/diagnostic_trigger_node_service_status.png b/localization/autoware_ndt_scan_matcher/media/diagnostic_trigger_node_service_status.png similarity index 100% rename from localization/ndt_scan_matcher/media/diagnostic_trigger_node_service_status.png rename to localization/autoware_ndt_scan_matcher/media/diagnostic_trigger_node_service_status.png diff --git a/localization/ndt_scan_matcher/media/differential_area_loading.gif b/localization/autoware_ndt_scan_matcher/media/differential_area_loading.gif similarity index 100% rename from localization/ndt_scan_matcher/media/differential_area_loading.gif rename to localization/autoware_ndt_scan_matcher/media/differential_area_loading.gif diff --git a/localization/ndt_scan_matcher/media/trajectory_with_regularization.png b/localization/autoware_ndt_scan_matcher/media/trajectory_with_regularization.png similarity index 100% rename from localization/ndt_scan_matcher/media/trajectory_with_regularization.png rename to localization/autoware_ndt_scan_matcher/media/trajectory_with_regularization.png diff --git a/localization/ndt_scan_matcher/media/trajectory_without_regularization.png b/localization/autoware_ndt_scan_matcher/media/trajectory_without_regularization.png similarity index 100% rename from localization/ndt_scan_matcher/media/trajectory_without_regularization.png rename to localization/autoware_ndt_scan_matcher/media/trajectory_without_regularization.png diff --git a/localization/ndt_scan_matcher/package.xml b/localization/autoware_ndt_scan_matcher/package.xml similarity index 94% rename from localization/ndt_scan_matcher/package.xml rename to localization/autoware_ndt_scan_matcher/package.xml index c88760d3db92c..f27cd63ff523e 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/autoware_ndt_scan_matcher/package.xml @@ -1,9 +1,9 @@ - ndt_scan_matcher + autoware_ndt_scan_matcher 0.1.0 - The ndt_scan_matcher package + The autoware_ndt_scan_matcher package Yamato Ando Kento Yabuuchi Masahiro Sakamoto diff --git a/localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json b/localization/autoware_ndt_scan_matcher/schema/ndt_scan_matcher.schema.json similarity index 100% rename from localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json rename to localization/autoware_ndt_scan_matcher/schema/ndt_scan_matcher.schema.json diff --git a/localization/ndt_scan_matcher/schema/sub/covariance.json b/localization/autoware_ndt_scan_matcher/schema/sub/covariance.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/covariance.json rename to localization/autoware_ndt_scan_matcher/schema/sub/covariance.json diff --git a/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json b/localization/autoware_ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json rename to localization/autoware_ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json diff --git a/localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json b/localization/autoware_ndt_scan_matcher/schema/sub/dynamic_map_loading.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json rename to localization/autoware_ndt_scan_matcher/schema/sub/dynamic_map_loading.json diff --git a/localization/ndt_scan_matcher/schema/sub/frame.json b/localization/autoware_ndt_scan_matcher/schema/sub/frame.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/frame.json rename to localization/autoware_ndt_scan_matcher/schema/sub/frame.json diff --git a/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json b/localization/autoware_ndt_scan_matcher/schema/sub/initial_pose_estimation.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json rename to localization/autoware_ndt_scan_matcher/schema/sub/initial_pose_estimation.json diff --git a/localization/ndt_scan_matcher/schema/sub/ndt.json b/localization/autoware_ndt_scan_matcher/schema/sub/ndt.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/ndt.json rename to localization/autoware_ndt_scan_matcher/schema/sub/ndt.json diff --git a/localization/ndt_scan_matcher/schema/sub/ndt_regularization.json b/localization/autoware_ndt_scan_matcher/schema/sub/ndt_regularization.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/ndt_regularization.json rename to localization/autoware_ndt_scan_matcher/schema/sub/ndt_regularization.json diff --git a/localization/ndt_scan_matcher/schema/sub/score_estimation.json b/localization/autoware_ndt_scan_matcher/schema/sub/score_estimation.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/score_estimation.json rename to localization/autoware_ndt_scan_matcher/schema/sub/score_estimation.json diff --git a/localization/ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json b/localization/autoware_ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json rename to localization/autoware_ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json diff --git a/localization/ndt_scan_matcher/schema/sub/sensor_points.json b/localization/autoware_ndt_scan_matcher/schema/sub/sensor_points.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/sensor_points.json rename to localization/autoware_ndt_scan_matcher/schema/sub/sensor_points.json diff --git a/localization/ndt_scan_matcher/schema/sub/validation.json b/localization/autoware_ndt_scan_matcher/schema/sub/validation.json similarity index 100% rename from localization/ndt_scan_matcher/schema/sub/validation.json rename to localization/autoware_ndt_scan_matcher/schema/sub/validation.json diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp similarity index 99% rename from localization/ndt_scan_matcher/src/map_update_module.cpp rename to localization/autoware_ndt_scan_matcher/src/map_update_module.cpp index 611d15783ea38..31e0ae2eb1a59 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ndt_scan_matcher/map_update_module.hpp" +#include "autoware/ndt_scan_matcher/map_update_module.hpp" namespace autoware::ndt_scan_matcher { diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp similarity index 99% rename from localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp rename to localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 47436832cdb5e..e9619375dc60a 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ndt_scan_matcher/ndt_scan_matcher_core.hpp" +#include "autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp" +#include "autoware/ndt_scan_matcher/particle.hpp" #include "localization_util/matrix_type.hpp" #include "localization_util/tree_structured_parzen_estimator.hpp" #include "localization_util/util_func.hpp" -#include "ndt_scan_matcher/particle.hpp" #include #include diff --git a/localization/ndt_scan_matcher/src/particle.cpp b/localization/autoware_ndt_scan_matcher/src/particle.cpp similarity index 98% rename from localization/ndt_scan_matcher/src/particle.cpp rename to localization/autoware_ndt_scan_matcher/src/particle.cpp index fce6cdce3326c..238e0ea14a65f 100644 --- a/localization/ndt_scan_matcher/src/particle.cpp +++ b/localization/autoware_ndt_scan_matcher/src/particle.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ndt_scan_matcher/particle.hpp" +#include "autoware/ndt_scan_matcher/particle.hpp" #include "localization_util/util_func.hpp" namespace autoware::ndt_scan_matcher diff --git a/localization/ndt_scan_matcher/test/stub_initialpose_client.hpp b/localization/autoware_ndt_scan_matcher/test/stub_initialpose_client.hpp similarity index 100% rename from localization/ndt_scan_matcher/test/stub_initialpose_client.hpp rename to localization/autoware_ndt_scan_matcher/test/stub_initialpose_client.hpp diff --git a/localization/ndt_scan_matcher/test/stub_pcd_loader.hpp b/localization/autoware_ndt_scan_matcher/test/stub_pcd_loader.hpp similarity index 100% rename from localization/ndt_scan_matcher/test/stub_pcd_loader.hpp rename to localization/autoware_ndt_scan_matcher/test/stub_pcd_loader.hpp diff --git a/localization/ndt_scan_matcher/test/stub_sensor_pcd_publisher.hpp b/localization/autoware_ndt_scan_matcher/test/stub_sensor_pcd_publisher.hpp similarity index 100% rename from localization/ndt_scan_matcher/test/stub_sensor_pcd_publisher.hpp rename to localization/autoware_ndt_scan_matcher/test/stub_sensor_pcd_publisher.hpp diff --git a/localization/ndt_scan_matcher/test/stub_trigger_node_client.hpp b/localization/autoware_ndt_scan_matcher/test/stub_trigger_node_client.hpp similarity index 100% rename from localization/ndt_scan_matcher/test/stub_trigger_node_client.hpp rename to localization/autoware_ndt_scan_matcher/test/stub_trigger_node_client.hpp diff --git a/localization/ndt_scan_matcher/test/test_cases/once_initialize_at_out_of_map_then_initialize_correctly.cpp b/localization/autoware_ndt_scan_matcher/test/test_cases/once_initialize_at_out_of_map_then_initialize_correctly.cpp similarity index 100% rename from localization/ndt_scan_matcher/test/test_cases/once_initialize_at_out_of_map_then_initialize_correctly.cpp rename to localization/autoware_ndt_scan_matcher/test/test_cases/once_initialize_at_out_of_map_then_initialize_correctly.cpp diff --git a/localization/ndt_scan_matcher/test/test_cases/standard_sequence_for_initial_pose_estimation.cpp b/localization/autoware_ndt_scan_matcher/test/test_cases/standard_sequence_for_initial_pose_estimation.cpp similarity index 100% rename from localization/ndt_scan_matcher/test/test_cases/standard_sequence_for_initial_pose_estimation.cpp rename to localization/autoware_ndt_scan_matcher/test/test_cases/standard_sequence_for_initial_pose_estimation.cpp diff --git a/localization/ndt_scan_matcher/test/test_fixture.hpp b/localization/autoware_ndt_scan_matcher/test/test_fixture.hpp similarity index 93% rename from localization/ndt_scan_matcher/test/test_fixture.hpp rename to localization/autoware_ndt_scan_matcher/test/test_fixture.hpp index c14c0b2ffe087..36704aaa99ef2 100644 --- a/localization/ndt_scan_matcher/test/test_fixture.hpp +++ b/localization/autoware_ndt_scan_matcher/test/test_fixture.hpp @@ -15,7 +15,7 @@ #ifndef TEST_FIXTURE_HPP_ #define TEST_FIXTURE_HPP_ -#include "../include/ndt_scan_matcher/ndt_scan_matcher_core.hpp" +#include "../include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp" #include "stub_initialpose_client.hpp" #include "stub_pcd_loader.hpp" #include "stub_sensor_pcd_publisher.hpp" @@ -37,8 +37,9 @@ class TestNDTScanMatcher : public ::testing::Test protected: void SetUp() override { - const std::string yaml_path = ament_index_cpp::get_package_share_directory("ndt_scan_matcher") + - "/config/ndt_scan_matcher.param.yaml"; + const std::string yaml_path = + ament_index_cpp::get_package_share_directory("autoware_ndt_scan_matcher") + + "/config/ndt_scan_matcher.param.yaml"; rcl_params_t * params_st = rcl_yaml_node_struct_init(rcl_get_default_allocator()); if (!rcl_parse_yaml_file(yaml_path.c_str(), params_st)) { diff --git a/localization/ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py b/localization/autoware_ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py similarity index 97% rename from localization/ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py rename to localization/autoware_ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py index 2dc4958c4704f..fb182c8c51bfb 100644 --- a/localization/ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py +++ b/localization/autoware_ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py @@ -33,7 +33,7 @@ @pytest.mark.launch_test def generate_test_description(): test_ndt_scan_matcher_launch_file = os.path.join( - get_package_share_directory("ndt_scan_matcher"), + get_package_share_directory("autoware_ndt_scan_matcher"), "launch", "ndt_scan_matcher.launch.xml", ) diff --git a/localization/ndt_scan_matcher/test/test_util.hpp b/localization/autoware_ndt_scan_matcher/test/test_util.hpp similarity index 100% rename from localization/ndt_scan_matcher/test/test_util.hpp rename to localization/autoware_ndt_scan_matcher/test/test_util.hpp diff --git a/localization/autoware_pose_estimator_arbiter/README.md b/localization/autoware_pose_estimator_arbiter/README.md index 938bb20e252ac..87fcbc252cd8e 100644 --- a/localization/autoware_pose_estimator_arbiter/README.md +++ b/localization/autoware_pose_estimator_arbiter/README.md @@ -32,7 +32,7 @@ Also, even if both can be activated at the same time, the Kalman Filter may be a ### Supporting pose_estimators -- [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) +- [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/autoware_ndt_scan_matcher) - [eagleye](https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/launch-autoware/localization/eagleye/) - [yabloc](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/yabloc) - [landmark_based_localizer](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/autoware_landmark_based_localizer) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 7870ff73c5408..bc3eb80d80339 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -25,7 +25,7 @@ You may provide either a single .pcd file or multiple .pcd files. If you are usi 1. **The pointcloud map should be projected on the same coordinate defined in `map_projection_loader`**, in order to be consistent with the lanelet2 map and other packages that converts between local and geodetic coordinates. For more information, please refer to [the readme of `map_projection_loader`](https://github.com/autowarefoundation/autoware.universe/tree/main/map/autoware_map_projection_loader/README.md). 2. **It must be divided by straight lines parallel to the x-axis and y-axis**. The system does not support division by diagonal lines or curved lines. 3. **The division size along each axis should be equal.** -4. **The division size should be about 20m x 20m.** Particularly, care should be taken as using too large division size (for example, more than 100m) may have adverse effects on dynamic map loading features in [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [autoware_compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/autoware_compare_map_segmentation). +4. **The division size should be about 20m x 20m.** Particularly, care should be taken as using too large division size (for example, more than 100m) may have adverse effects on dynamic map loading features in [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/autoware_ndt_scan_matcher) and [autoware_compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/autoware_compare_map_segmentation). 5. **All the split maps should not overlap with each other.** 6. **Metadata file should also be provided.** The metadata structure description is provided below. From 1bf24e49e89043166a3f3350522d86530442574e Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 19 Sep 2024 12:00:24 +0900 Subject: [PATCH 02/95] fix(goal_planner): fix typo (#8910) Signed-off-by: Fumiya Watanabe --- .../examples/plot_map.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp index 6b750bc9674f3..a51b2fd337512 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp @@ -258,10 +258,10 @@ int main(int argc, char ** argv) const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo(); autoware::lane_departure_checker::LaneDepartureChecker lane_departure_checker{}; lane_departure_checker.setVehicleInfo(vehicle_info); - autoware::lane_departure_checker::Param lane_depature_checker_params; - lane_depature_checker_params.footprint_extra_margin = + autoware::lane_departure_checker::Param lane_departure_checker_params; + lane_departure_checker_params.footprint_extra_margin = goal_planner_parameter.lane_departure_check_expansion_margin; - lane_departure_checker.setParam(lane_depature_checker_params); + lane_departure_checker.setParam(lane_departure_checker_params); auto shift_pull_over_planner = autoware::behavior_path_planner::ShiftPullOver( *node, goal_planner_parameter, lane_departure_checker); const auto pull_over_path_opt = From 2791c53423a2a5614f6064fd01700c9299cdf5f8 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 19 Sep 2024 13:03:00 +0900 Subject: [PATCH 03/95] fix(intersection): fix typo (#8911) * fix(intersection): fix typo Signed-off-by: Fumiya Watanabe * fix(intersection): fix typo Signed-off-by: Fumiya Watanabe * style(pre-commit): autofix --------- Signed-off-by: Fumiya Watanabe Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/util.cpp | 39 ++++++++++--------- .../src/util.hpp | 6 +-- 2 files changed, 23 insertions(+), 22 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp index c48588bc5386d..a446493793c53 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp @@ -217,31 +217,32 @@ std::optional getFirstPointInsidePolygon( } void retrievePathsBackward( - const std::vector> & adjacency, const size_t src_ind, - const std::vector & visited_inds, std::vector> & paths) + const std::vector> & adjacency, const size_t src_index, + const std::vector & visited_indices, std::vector> & paths) { - const auto & nexts = adjacency.at(src_ind); - const bool is_terminal = (std::find(nexts.begin(), nexts.end(), true) == nexts.end()); + const auto & next_indices = adjacency.at(src_index); + const bool is_terminal = + (std::find(next_indices.begin(), next_indices.end(), true) == next_indices.end()); if (is_terminal) { - std::vector path(visited_inds.begin(), visited_inds.end()); - path.push_back(src_ind); + std::vector path(visited_indices.begin(), visited_indices.end()); + path.push_back(src_index); paths.emplace_back(std::move(path)); return; } - for (size_t next = 0; next < nexts.size(); next++) { - if (!nexts.at(next)) { + for (size_t next = 0; next < next_indices.size(); next++) { + if (!next_indices.at(next)) { continue; } - if (std::find(visited_inds.begin(), visited_inds.end(), next) != visited_inds.end()) { + if (std::find(visited_indices.begin(), visited_indices.end(), next) != visited_indices.end()) { // loop detected - std::vector path(visited_inds.begin(), visited_inds.end()); - path.push_back(src_ind); + std::vector path(visited_indices.begin(), visited_indices.end()); + path.push_back(src_index); paths.emplace_back(std::move(path)); continue; } - auto new_visited_inds = visited_inds; - new_visited_inds.push_back(src_ind); - retrievePathsBackward(adjacency, next, new_visited_inds, paths); + auto new_visited_indices = visited_indices; + new_visited_indices.push_back(src_index); + retrievePathsBackward(adjacency, next, new_visited_indices, paths); } return; } @@ -263,10 +264,10 @@ mergeLaneletsByTopologicalSort( ind2Id[ind] = Id; Id2lanelet[Id] = lanelet; } - std::set terminal_inds; + std::set terminal_indices; for (const auto & terminal_lanelet : terminal_lanelets) { if (Id2ind.count(terminal_lanelet.id()) > 0) { - terminal_inds.insert(Id2ind[terminal_lanelet.id()]); + terminal_indices.insert(Id2ind[terminal_lanelet.id()]); } } @@ -292,7 +293,7 @@ mergeLaneletsByTopologicalSort( } std::unordered_map>> branches; - for (const auto & terminal_ind : terminal_inds) { + for (const auto & terminal_ind : terminal_indices) { std::vector> paths; std::vector visited; retrievePathsBackward(adjacency, terminal_ind, visited, paths); @@ -311,11 +312,11 @@ mergeLaneletsByTopologicalSort( if (sub_branches.size() == 0) { continue; } - for (const auto & sub_inds : sub_branches) { + for (const auto & sub_indices : sub_branches) { lanelet::ConstLanelets to_be_merged; originals.push_back(lanelet::ConstLanelets({})); auto & original = originals.back(); - for (const auto & sub_ind : sub_inds) { + for (const auto & sub_ind : sub_indices) { to_be_merged.push_back(Id2lanelet[ind2Id[sub_ind]]); original.push_back(Id2lanelet[ind2Id[sub_ind]]); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp index 6167e7a366295..0fac44f6cdf97 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp @@ -105,11 +105,11 @@ mergeLaneletsByTopologicalSort( /** * @brief this functions retrieves all the paths from the given source to terminal nodes on the tree - @param[in] visited_inds visited node indices excluding src_ind so far + @param[in] visited_indices visited node indices excluding src_ind so far */ void retrievePathsBackward( - const std::vector> & adjacency, const size_t src_ind, - const std::vector & visited_inds, std::vector> & paths); + const std::vector> & adjacency, const size_t src_index, + const std::vector & visited_indices, std::vector> & paths); /** * @brief find the index of the first point where vehicle footprint intersects with the given From b1dddf1e11d009bceafc7e1fc8aa9f4a7d489d83 Mon Sep 17 00:00:00 2001 From: Nagi70 <114723428+Nagi70@users.noreply.github.com> Date: Thu, 19 Sep 2024 14:27:20 +0900 Subject: [PATCH 04/95] fix(autoware_perception_rviz_plugin): fix unusedFunction (#8784) fix: unusedFunction Signed-off-by: Nagi70 --- .../src/object_detection/object_polygon_detail.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index 28ff67433efe5..04d07462e8695 100644 --- a/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -538,6 +538,7 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( return marker_ptr; } +// cppcheck-suppress unusedFunction visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, From c1aac51443b6fafc4acd6b3e718be39851396f44 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 19 Sep 2024 14:32:27 +0900 Subject: [PATCH 05/95] refactor(start_planner,raw_vechile_cmd_converter): align parameter with autoware_launch's parameter (#8913) * align autoware_raw_vehicle_cmd_converter's parameter Signed-off-by: kyoichi-sugahara * align start_planner's parameter Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../config/start_planner.param.yaml | 11 +++++++++-- .../config/raw_vehicle_cmd_converter.param.yaml | 2 +- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml index f93800450a479..a71c202b05043 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml @@ -69,10 +69,11 @@ search_configs: theta_size: 120 angle_goal_range: 6.0 - curve_weight: 0.5 - reverse_weight: 1.0 lateral_goal_range: 0.5 longitudinal_goal_range: 2.0 + curve_weight: 0.5 + reverse_weight: 1.0 + direction_change_weight: 1.5 # costmap configs costmap_configs: obstacle_threshold: 30 @@ -81,7 +82,13 @@ search_method: "forward" # options: forward, backward only_behind_solutions: false use_back: true + adapt_expansion_distance: true + expansion_distance: 0.5 + near_goal_distance: 3.0 distance_heuristic_weight: 2.0 + smoothness_weight: 0.5 + obstacle_distance_weight: 1.75 + goal_lat_distance_weight: 5.0 # -- RRT* search Configurations -- rrtstar: enable_update: true diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml b/vehicle/autoware_raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml index 4eb9f0ab6d442..e108de527ecbc 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml +++ b/vehicle/autoware_raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml @@ -5,7 +5,7 @@ csv_path_steer_map: $(find-pkg-share autoware_raw_vehicle_cmd_converter)/data/default/steer_map.csv convert_accel_cmd: true convert_brake_cmd: true - convert_steer_cmd: true + convert_steer_cmd: false use_steer_ff: true use_steer_fb: true is_debugging: false From 7819089327e2469f3371bcb106fb6d99432230f7 Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Thu, 19 Sep 2024 14:44:08 +0900 Subject: [PATCH 06/95] fix(interpolation): fix bug of interpolation (#8903) * fix(interpolation): fix bug of interpolation Signed-off-by: Y.Hisaki * add const Signed-off-by: Y.Hisaki * auto -> int64_t Signed-off-by: Y.Hisaki * add const Signed-off-by: Y.Hisaki * add const Signed-off-by: Y.Hisaki * add const Signed-off-by: Y.Hisaki --------- Signed-off-by: Y.Hisaki --- .../interpolation/spline_interpolation.hpp | 29 +-- common/interpolation/package.xml | 1 + .../src/spline_interpolation.cpp | 225 ++++++++---------- .../test/src/test_spline_interpolation.cpp | 10 +- .../test_spline_interpolation_points_2d.cpp | 19 +- 5 files changed, 117 insertions(+), 167 deletions(-) diff --git a/common/interpolation/include/interpolation/spline_interpolation.hpp b/common/interpolation/include/interpolation/spline_interpolation.hpp index 38e7b3fed5b8b..54603a68d37a5 100644 --- a/common/interpolation/include/interpolation/spline_interpolation.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation.hpp @@ -18,6 +18,8 @@ #include "autoware/universe_utils/geometry/geometry.hpp" #include "interpolation/interpolation_utils.hpp" +#include + #include #include #include @@ -26,25 +28,6 @@ namespace interpolation { -// NOTE: X(s) = a_i (s - s_i)^3 + b_i (s - s_i)^2 + c_i (s - s_i) + d_i : (i = 0, 1, ... N-1) -struct MultiSplineCoef -{ - MultiSplineCoef() = default; - - explicit MultiSplineCoef(const size_t num_spline) - { - a.resize(num_spline); - b.resize(num_spline); - c.resize(num_spline); - d.resize(num_spline); - } - - std::vector a; - std::vector b; - std::vector c; - std::vector d; -}; - // static spline interpolation functions std::vector spline( const std::vector & base_keys, const std::vector & base_values, @@ -98,11 +81,17 @@ class SplineInterpolation size_t getSize() const { return base_keys_.size(); } private: + Eigen::VectorXd a_; + Eigen::VectorXd b_; + Eigen::VectorXd c_; + Eigen::VectorXd d_; + std::vector base_keys_; - interpolation::MultiSplineCoef multi_spline_coef_; void calcSplineCoefficients( const std::vector & base_keys, const std::vector & base_values); + + Eigen::Index get_index(const double & key) const; }; #endif // INTERPOLATION__SPLINE_INTERPOLATION_HPP_ diff --git a/common/interpolation/package.xml b/common/interpolation/package.xml index bb4af924dd252..330c87df18423 100644 --- a/common/interpolation/package.xml +++ b/common/interpolation/package.xml @@ -12,6 +12,7 @@ autoware_cmake autoware_universe_utils + eigen ament_cmake_ros ament_lint_auto diff --git a/common/interpolation/src/spline_interpolation.cpp b/common/interpolation/src/spline_interpolation.cpp index 1275b47346d78..5cf8f68ca519d 100644 --- a/common/interpolation/src/spline_interpolation.cpp +++ b/common/interpolation/src/spline_interpolation.cpp @@ -14,65 +14,40 @@ #include "interpolation/spline_interpolation.hpp" +#include #include namespace { -// solve Ax = d -// where A is tridiagonal matrix -// [b_0 c_0 ... ] -// [a_0 b_1 c_1 ... O ] -// A = [ ... ] -// [ O ... a_N-3 b_N-2 c_N-2] -// [ ... a_N-2 b_N-1] -struct TDMACoef +Eigen::VectorXd solve_tridiagonal_matrix_algorithm( + const Eigen::Ref & a, const Eigen::Ref & b, + const Eigen::Ref & c, const Eigen::Ref & d) { - explicit TDMACoef(const size_t num_row) - { - a.resize(num_row - 1); - b.resize(num_row); - c.resize(num_row - 1); - d.resize(num_row); + const auto n = d.size(); + + if (n == 1) { + return d.array() / b.array(); } - std::vector a; - std::vector b; - std::vector c; - std::vector d; -}; + Eigen::VectorXd c_prime = Eigen::VectorXd::Zero(n); + Eigen::VectorXd d_prime = Eigen::VectorXd::Zero(n); + Eigen::VectorXd x = Eigen::VectorXd::Zero(n); -inline std::vector solveTridiagonalMatrixAlgorithm(const TDMACoef & tdma_coef) -{ - const auto & a = tdma_coef.a; - const auto & b = tdma_coef.b; - const auto & c = tdma_coef.c; - const auto & d = tdma_coef.d; - - const size_t num_row = b.size(); - - std::vector x(num_row); - if (num_row != 1) { - // calculate p and q - std::vector p; - std::vector q; - p.push_back(-c[0] / b[0]); - q.push_back(d[0] / b[0]); - - for (size_t i = 1; i < num_row; ++i) { - const double den = b[i] + a[i - 1] * p[i - 1]; - p.push_back(-c[i - 1] / den); - q.push_back((d[i] - a[i - 1] * q[i - 1]) / den); - } + // Forward sweep + c_prime(0) = c(0) / b(0); + d_prime(0) = d(0) / b(0); - // calculate solution - x[num_row - 1] = q[num_row - 1]; + for (auto i = 1; i < n; i++) { + const double m = 1.0 / (b(i) - a(i - 1) * c_prime(i - 1)); + c_prime(i) = i < n - 1 ? c(i) * m : 0; + d_prime(i) = (d(i) - a(i - 1) * d_prime(i - 1)) * m; + } - for (size_t i = 1; i < num_row; ++i) { - const size_t j = num_row - 1 - i; - x[j] = p[j] * x[j + 1] + q[j]; - } - } else { - x[0] = (d[0] / b[0]); + // Back substitution + x(n - 1) = d_prime(n - 1); + + for (int64_t i = n - 2; i >= 0; i--) { + x(i) = d_prime(i) - c_prime(i) * x(i + 1); } return x; @@ -166,53 +141,59 @@ std::vector splineByAkima( } } // namespace interpolation +Eigen::Index SplineInterpolation::get_index(const double & key) const +{ + const auto it = std::lower_bound(base_keys_.begin(), base_keys_.end(), key); + return std::clamp( + static_cast(std::distance(base_keys_.begin(), it)) - 1, 0, + static_cast(base_keys_.size()) - 2); +} + void SplineInterpolation::calcSplineCoefficients( const std::vector & base_keys, const std::vector & base_values) { // throw exceptions for invalid arguments interpolation_utils::validateKeysAndValues(base_keys, base_values); - - const size_t num_base = base_keys.size(); // N+1 - - std::vector diff_keys; // N - std::vector diff_values; // N - for (size_t i = 0; i < num_base - 1; ++i) { - diff_keys.push_back(base_keys.at(i + 1) - base_keys.at(i)); - diff_values.push_back(base_values.at(i + 1) - base_values.at(i)); - } - - std::vector v = {0.0}; - if (num_base > 2) { - // solve tridiagonal matrix algorithm - TDMACoef tdma_coef(num_base - 2); // N-1 - - for (size_t i = 0; i < num_base - 2; ++i) { - tdma_coef.b[i] = 2 * (diff_keys[i] + diff_keys[i + 1]); - if (i != num_base - 3) { - tdma_coef.a[i] = diff_keys[i + 1]; - tdma_coef.c[i] = diff_keys[i + 1]; - } - tdma_coef.d[i] = - 6.0 * (diff_values[i + 1] / diff_keys[i + 1] - diff_values[i] / diff_keys[i]); - } - - const std::vector tdma_res = solveTridiagonalMatrixAlgorithm(tdma_coef); - - // calculate v - v.insert(v.end(), tdma_res.begin(), tdma_res.end()); - } - v.push_back(0.0); - - // calculate a, b, c, d of spline coefficients - multi_spline_coef_ = interpolation::MultiSplineCoef{num_base - 1}; // N - for (size_t i = 0; i < num_base - 1; ++i) { - multi_spline_coef_.a[i] = (v[i + 1] - v[i]) / 6.0 / diff_keys[i]; - multi_spline_coef_.b[i] = v[i] / 2.0; - multi_spline_coef_.c[i] = - diff_values[i] / diff_keys[i] - diff_keys[i] * (2 * v[i] + v[i + 1]) / 6.0; - multi_spline_coef_.d[i] = base_values[i]; + const Eigen::VectorXd x = Eigen::Map( + base_keys.data(), static_cast(base_keys.size())); + const Eigen::VectorXd y = Eigen::Map( + base_values.data(), static_cast(base_values.size())); + + const auto n = x.size(); + + if (n == 2) { + a_ = Eigen::VectorXd::Zero(1); + b_ = Eigen::VectorXd::Zero(1); + c_ = Eigen::VectorXd::Zero(1); + d_ = Eigen::VectorXd::Zero(1); + c_[0] = (y[1] - y[0]) / (x[1] - x[0]); + d_[0] = y[0]; + base_keys_ = base_keys; + return; } + // Create Tridiagonal matrix + Eigen::VectorXd v(n); + const Eigen::VectorXd h = x.segment(1, n - 1) - x.segment(0, n - 1); + const Eigen::VectorXd a = h.segment(1, n - 3); + const Eigen::VectorXd b = 2 * (h.segment(0, n - 2) + h.segment(1, n - 2)); + const Eigen::VectorXd c = h.segment(1, n - 3); + const Eigen::VectorXd y_diff = y.segment(1, n - 1) - y.segment(0, n - 1); + const Eigen::VectorXd d = 6 * (y_diff.segment(1, n - 2).array() / h.tail(n - 2).array() - + y_diff.segment(0, n - 2).array() / h.head(n - 2).array()); + + // Solve tridiagonal matrix + v.segment(1, n - 2) = solve_tridiagonal_matrix_algorithm(a, b, c, d); + v[0] = 0; + v[n - 1] = 0; + + // Calculate spline coefficients + a_ = (v.tail(n - 1) - v.head(n - 1)).array() / 6.0 / (x.tail(n - 1) - x.head(n - 1)).array(); + b_ = v.segment(0, n - 1) / 2.0; + c_ = (y.tail(n - 1) - y.head(n - 1)).array() / (x.tail(n - 1) - x.head(n - 1)).array() - + (x.tail(n - 1) - x.head(n - 1)).array() * + (2 * v.segment(0, n - 1).array() + v.segment(1, n - 1).array()) / 6.0; + d_ = y.head(n - 1); base_keys_ = base_keys; } @@ -221,24 +202,17 @@ std::vector SplineInterpolation::getSplineInterpolatedValues( { // throw exceptions for invalid arguments const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); - - const auto & a = multi_spline_coef_.a; - const auto & b = multi_spline_coef_.b; - const auto & c = multi_spline_coef_.c; - const auto & d = multi_spline_coef_.d; - - std::vector res; - size_t j = 0; - for (const auto & query_key : validated_query_keys) { - while (base_keys_.at(j + 1) < query_key) { - ++j; - } - - const double ds = query_key - base_keys_.at(j); - res.push_back(d.at(j) + (c.at(j) + (b.at(j) + a.at(j) * ds) * ds) * ds); + std::vector interpolated_values; + interpolated_values.reserve(query_keys.size()); + + for (const auto & key : query_keys) { + const auto idx = get_index(key); + const auto dx = key - base_keys_[idx]; + interpolated_values.emplace_back( + a_[idx] * dx * dx * dx + b_[idx] * dx * dx + c_[idx] * dx + d_[idx]); } - return res; + return interpolated_values; } std::vector SplineInterpolation::getSplineInterpolatedDiffValues( @@ -246,23 +220,16 @@ std::vector SplineInterpolation::getSplineInterpolatedDiffValues( { // throw exceptions for invalid arguments const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); + std::vector interpolated_diff_values; + interpolated_diff_values.reserve(query_keys.size()); - const auto & a = multi_spline_coef_.a; - const auto & b = multi_spline_coef_.b; - const auto & c = multi_spline_coef_.c; - - std::vector res; - size_t j = 0; - for (const auto & query_key : validated_query_keys) { - while (base_keys_.at(j + 1) < query_key) { - ++j; - } - - const double ds = query_key - base_keys_.at(j); - res.push_back(c.at(j) + (2.0 * b.at(j) + 3.0 * a.at(j) * ds) * ds); + for (const auto & key : query_keys) { + const auto idx = get_index(key); + const auto dx = key - base_keys_[idx]; + interpolated_diff_values.emplace_back(3 * a_[idx] * dx * dx + 2 * b_[idx] * dx + c_[idx]); } - return res; + return interpolated_diff_values; } std::vector SplineInterpolation::getSplineInterpolatedQuadDiffValues( @@ -270,20 +237,14 @@ std::vector SplineInterpolation::getSplineInterpolatedQuadDiffValues( { // throw exceptions for invalid arguments const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); + std::vector interpolated_quad_diff_values; + interpolated_quad_diff_values.reserve(query_keys.size()); - const auto & a = multi_spline_coef_.a; - const auto & b = multi_spline_coef_.b; - - std::vector res; - size_t j = 0; - for (const auto & query_key : validated_query_keys) { - while (base_keys_.at(j + 1) < query_key) { - ++j; - } - - const double ds = query_key - base_keys_.at(j); - res.push_back(2.0 * b.at(j) + 6.0 * a.at(j) * ds); + for (const auto & key : query_keys) { + const auto idx = get_index(key); + const auto dx = key - base_keys_[idx]; + interpolated_quad_diff_values.emplace_back(6 * a_[idx] * dx + 2 * b_[idx]); } - return res; + return interpolated_quad_diff_values; } diff --git a/common/interpolation/test/src/test_spline_interpolation.cpp b/common/interpolation/test/src/test_spline_interpolation.cpp index d3cb2d6d3060b..6e6a9192cf71a 100644 --- a/common/interpolation/test/src/test_spline_interpolation.cpp +++ b/common/interpolation/test/src/test_spline_interpolation.cpp @@ -64,7 +64,7 @@ TEST(spline_interpolation, spline) const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; const std::vector query_keys{0.0, 8.0, 18.0}; - const std::vector ans{-0.075611, 0.997242, 1.573258}; + const std::vector ans{-0.076114, 1.001217, 1.573640}; const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { @@ -112,7 +112,7 @@ TEST(spline_interpolation, spline) const std::vector base_keys = {0.0, 1.0, 1.0001, 2.0, 3.0, 4.0}; const std::vector base_values = {0.0, 0.0, 0.1, 0.1, 0.1, 0.1}; const std::vector query_keys = {0.0, 1.0, 1.5, 2.0, 3.0, 4.0}; - const std::vector ans = {0.0, 0.0, 137.591789, 0.1, 0.1, 0.1}; + const std::vector ans = {0.0, 0.0, 158.738293, 0.1, 0.1, 0.1}; const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { @@ -227,7 +227,7 @@ TEST(spline_interpolation, SplineInterpolation) const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; const std::vector query_keys{0.0, 8.0, 18.0}; - const std::vector ans{-0.075611, 0.997242, 1.573258}; + const std::vector ans{-0.076114, 1.001217, 1.573640}; SplineInterpolation s(base_keys, base_values); const std::vector query_values = s.getSplineInterpolatedValues(query_keys); @@ -242,7 +242,7 @@ TEST(spline_interpolation, SplineInterpolation) const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; const std::vector query_keys{0.0, 8.0, 12.0, 18.0}; - const std::vector ans{0.671301, 0.0509853, 0.209426, -0.253628}; + const std::vector ans{0.671343, 0.049289, 0.209471, -0.253746}; SplineInterpolation s(base_keys, base_values); const std::vector query_values = s.getSplineInterpolatedDiffValues(query_keys); @@ -257,7 +257,7 @@ TEST(spline_interpolation, SplineInterpolation) const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; const std::vector query_keys{0.0, 8.0, 12.0, 18.0}; - const std::vector ans{-0.156582, 0.0440771, -0.0116873, -0.0495025}; + const std::vector ans{-0.155829, 0.043097, -0.011143, -0.049611}; SplineInterpolation s(base_keys, base_values); const std::vector query_values = s.getSplineInterpolatedQuadDiffValues(query_keys); diff --git a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp index 4013832220cd8..7e41980c78bdc 100644 --- a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp +++ b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp @@ -51,8 +51,7 @@ TEST(spline_interpolation, splineYawFromPoints) points.push_back(createPoint(5.0, 10.0, 0.0)); points.push_back(createPoint(10.0, 12.5, 0.0)); - const std::vector ans{1.3593746, 0.9813541, 1.0419655, 0.8935115, 0.2932783}; - + const std::vector ans{1.368174, 0.961318, 1.086098, 0.938357, 0.278594}; const auto yaws = interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); @@ -121,8 +120,8 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) // random const auto random_point = s.getSplineInterpolatedPoint(3, 0.5); - EXPECT_NEAR(random_point.x, 5.3036484, epsilon); - EXPECT_NEAR(random_point.y, 10.3343074, epsilon); + EXPECT_NEAR(random_point.x, 5.28974, epsilon); + EXPECT_NEAR(random_point.y, 10.3450319, epsilon); // out of range of total length const auto front_out_point = s.getSplineInterpolatedPoint(0.0, -0.1); @@ -140,17 +139,17 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) { // yaw // front - EXPECT_NEAR(s.getSplineInterpolatedYaw(0, 0.0), 1.3593746, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(0, 0.0), 1.368174, epsilon); // back - EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.0), 0.2932783, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.0), 0.278594, epsilon); // random - EXPECT_NEAR(s.getSplineInterpolatedYaw(3, 0.5), 0.7757198, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(3, 0.5), 0.808580, epsilon); // out of range of total length - EXPECT_NEAR(s.getSplineInterpolatedYaw(0.0, -0.1), 1.3593746, epsilon); - EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.1), 0.2932783, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(0.0, -0.1), 1.368174, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.1), 0.278594, epsilon); // out of range of index EXPECT_THROW(s.getSplineInterpolatedYaw(-1, 0.0), std::out_of_range); @@ -165,7 +164,7 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) EXPECT_NEAR(s.getSplineInterpolatedCurvature(4, 0.0), 0.0, epsilon); // random - EXPECT_NEAR(s.getSplineInterpolatedCurvature(3, 0.5), -0.2441671, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedCurvature(3, 0.5), -0.271073, epsilon); // out of range of total length EXPECT_NEAR(s.getSplineInterpolatedCurvature(0.0, -0.1), 0.0, epsilon); From 812e6d2cf119461e04a3f2b27ab64a81b80e867f Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 19 Sep 2024 14:50:21 +0900 Subject: [PATCH 07/95] fix(cppcheck_CI): fix unknownMacro (#8894) * fix:cppcheck CI Signed-off-by: kobayu858 * fix:pre-commit.ci Signed-off-by: kobayu858 * doc:remove redundant comments Signed-off-by: kobayu858 * refactor: use -D option Signed-off-by: kobayu858 * test: cppcheck ci Signed-off-by: kobayu858 * test: Completion Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- .github/workflows/cppcheck-differential.yaml | 2 +- .github/workflows/cppcheck-weekly.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/cppcheck-differential.yaml b/.github/workflows/cppcheck-differential.yaml index 6d466f7c6bc45..8c2edd3c76724 100644 --- a/.github/workflows/cppcheck-differential.yaml +++ b/.github/workflows/cppcheck-differential.yaml @@ -71,7 +71,7 @@ jobs: id: cppcheck run: | echo "Running Cppcheck on modified packages: ${{ steps.filter-paths-no-cpp-files.outputs.filtered-full-paths }}" - cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --suppressions-list=.cppcheck_suppressions --inline-suppr ${{ steps.filter-paths-no-cpp-files.outputs.filtered-full-paths }} 2> cppcheck-report.txt + cppcheck --enable=all --inconclusive --check-level=exhaustive -D'PLUGINLIB_EXPORT_CLASS(class_type, base_class)=' -Dslots= -DQ_SLOTS= --error-exitcode=1 --suppressions-list=.cppcheck_suppressions --inline-suppr ${{ steps.filter-paths-no-cpp-files.outputs.filtered-full-paths }} 2> cppcheck-report.txt shell: bash - name: Setup Problem Matchers for cppcheck diff --git a/.github/workflows/cppcheck-weekly.yaml b/.github/workflows/cppcheck-weekly.yaml index c59d663048b9c..573dcc54aa8c0 100644 --- a/.github/workflows/cppcheck-weekly.yaml +++ b/.github/workflows/cppcheck-weekly.yaml @@ -23,7 +23,7 @@ jobs: continue-on-error: true id: cppcheck run: | - cppcheck --enable=all --inconclusive --check-level=exhaustive --suppress=*:*/test/* --error-exitcode=1 --xml --inline-suppr . 2> cppcheck-report.xml + cppcheck --enable=all --inconclusive --check-level=exhaustive -D'PLUGINLIB_EXPORT_CLASS(class_type, base_class)=' -Dslots= -DQ_SLOTS= --suppress=*:*/test/* --error-exitcode=1 --xml --inline-suppr . 2> cppcheck-report.xml shell: bash - name: Count errors by error ID and severity From 3e377d82941fe960a962fde29813cb1f680d51fa Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Thu, 19 Sep 2024 15:03:52 +0900 Subject: [PATCH 08/95] chore: remove duplicate line in mpc_lateral_controller.cpp (#8916) remove duplicate line in mpc_lateral_controller.cpp Signed-off-by: Autumn60 --- .../src/mpc_lateral_controller.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp index 4ff7f329e5498..f4ba74d74f246 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -104,7 +104,6 @@ MpcLateralController::MpcLateralController( m_mpc->setVehicleModel(vehicle_model_ptr); /* QP solver setup */ - m_mpc->setVehicleModel(vehicle_model_ptr); auto qpsolver_ptr = createQPSolverInterface(node); m_mpc->setQPSolver(qpsolver_ptr); From 03ecd6ec6fac24b2832781e68e708c60540589ab Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 19 Sep 2024 15:08:52 +0900 Subject: [PATCH 09/95] fix(tier4_localization_rviz_plugin): fix unusedFunction (#8848) fix:unusedFunction Signed-off-by: kobayu858 --- .../src/pose_history_footprint/display.cpp | 7 ------- .../src/pose_history_footprint/display.hpp | 1 - 2 files changed, 8 deletions(-) diff --git a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp index 662ce97b162cd..391ebdc0c1c77 100644 --- a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp +++ b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp @@ -95,13 +95,6 @@ void PoseHistoryFootprint::update_vehicle_info() } } -void PoseHistoryFootprint::update_visualization() -{ - if (last_msg_ptr_) { - processMessage(last_msg_ptr_); - } -} - void PoseHistoryFootprint::onInitialize() { MFDClass::onInitialize(); diff --git a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp index 241e682924fad..7634f6049b6a0 100644 --- a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp +++ b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp @@ -61,7 +61,6 @@ class PoseHistoryFootprint void update_footprint(); private Q_SLOTS: - void update_visualization(); void update_vehicle_info(); private: // NOLINT : suppress redundancy warnings From c99a8f1d622036353c9a8aae87192660b2ed06d0 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 19 Sep 2024 15:19:37 +0900 Subject: [PATCH 10/95] chore(mpc_lateral_controller): consistent parameters with autoware_launch (#8914) Signed-off-by: Takayuki Murooka --- .../param/lateral_controller_defaults.param.yaml | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml b/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml index de194a8902a7d..b358e95f86f99 100644 --- a/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml +++ b/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml @@ -47,7 +47,7 @@ # -- vehicle model -- vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics input_delay: 0.24 # steering input delay time for delay compensation - vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s] + vehicle_model_steer_tau: 0.27 # steering dynamics time constant (1d approximation) [s] steer_rate_lim_dps_list_by_curvature: [40.0, 50.0, 60.0] # steering angle rate limit list depending on curvature [deg/s] curvature_list_for_steer_rate_lim: [0.001, 0.002, 0.01] # curvature list for steering angle rate limit interpolation in ascending order [/m] steer_rate_lim_dps_list_by_velocity: [60.0, 50.0, 40.0] # steering angle rate limit list depending on velocity [deg/s] @@ -76,13 +76,4 @@ average_num: 1000 steering_offset_limit: 0.02 - # vehicle parameters - mass_kg: 2400.0 - mass_fl: 600.0 - mass_fr: 600.0 - mass_rl: 600.0 - mass_rr: 600.0 - cf: 155494.663 - cr: 155494.663 - publish_debug_trajectories: true # flag to publish predicted trajectory and resampled reference trajectory for debug purpose From 3dd8e7c80865c29ce84d6ab0948faf3a9146c968 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 19 Sep 2024 16:02:09 +0900 Subject: [PATCH 11/95] chore(planning): consistent parameters with autoware_launch (#8915) * chore(planning): consistent parameters with autoware_launch Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * fix json schema Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../config/adaptive_cruise_control.param.yaml | 4 ++-- .../config/obstacle_stop_planner.param.yaml | 23 ++++++++++--------- .../schema/obstacle_stop_planner.schema.json | 5 ++++ .../config/goal_planner.param.yaml | 15 +++++++++--- .../config/behavior_path_planner.param.yaml | 1 + .../config/out_of_lane.param.yaml | 1 + 6 files changed, 33 insertions(+), 16 deletions(-) diff --git a/planning/autoware_obstacle_stop_planner/config/adaptive_cruise_control.param.yaml b/planning/autoware_obstacle_stop_planner/config/adaptive_cruise_control.param.yaml index 691be53a7470f..73e7a578fedb7 100644 --- a/planning/autoware_obstacle_stop_planner/config/adaptive_cruise_control.param.yaml +++ b/planning/autoware_obstacle_stop_planner/config/adaptive_cruise_control.param.yaml @@ -10,16 +10,16 @@ obstacle_velocity_thresh_to_start_acc: 1.5 # start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s] obstacle_velocity_thresh_to_stop_acc: 1.0 # stop adaptive cruise control when the velocity of the forward obstacle falls below this value [m/s] emergency_stop_acceleration: -5.0 # supposed minimum acceleration (deceleration) in emergency stop [m/ss] + obstacle_emergency_stop_acceleration: -5.0 emergency_stop_idling_time: 0.5 # supposed idling time to start emergency stop [s] min_dist_stop: 4.0 # minimum distance of emergency stop [m] - obstacle_emergency_stop_acceleration: -5.0 # supposed minimum acceleration (deceleration) in emergency stop [m/ss] max_standard_acceleration: 0.5 # supposed maximum acceleration in active cruise control [m/ss] min_standard_acceleration: -1.0 # supposed minimum acceleration (deceleration) in active cruise control standard_idling_time: 0.5 # supposed idling time to react object in active cruise control [s] min_dist_standard: 4.0 # minimum distance in active cruise control [m] obstacle_min_standard_acceleration: -1.5 # supposed minimum acceleration of forward obstacle [m/ss] margin_rate_to_change_vel: 0.3 # margin to insert upper velocity [-] - use_time_compensation_to_calc_distance: true # use time-compensation to calculate distance to forward vehicle + use_time_compensation_to_calc_distance: true # pid parameter for ACC p_coefficient_positive: 0.1 # coefficient P in PID control (used when target dist -current_dist >=0) [-] diff --git a/planning/autoware_obstacle_stop_planner/config/obstacle_stop_planner.param.yaml b/planning/autoware_obstacle_stop_planner/config/obstacle_stop_planner.param.yaml index ca5869bce0eb6..8e215a1bcf236 100644 --- a/planning/autoware_obstacle_stop_planner/config/obstacle_stop_planner.param.yaml +++ b/planning/autoware_obstacle_stop_planner/config/obstacle_stop_planner.param.yaml @@ -1,22 +1,23 @@ /**: ros__parameters: - chattering_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] - max_velocity: 20.0 # max velocity [m/s] - enable_slow_down: False # whether to use slow down planner [-] - enable_z_axis_obstacle_filtering: True # filter obstacles in z axis (height) [-] - z_axis_filtering_buffer: 0.0 # additional buffer for z axis filtering [m] - voxel_grid_x: 0.05 # voxel grid x parameter for filtering pointcloud [m] - voxel_grid_y: 0.05 # voxel grid y parameter for filtering pointcloud [m] - voxel_grid_z: 100000.0 # voxel grid z parameter for filtering pointcloud [m] - use_predicted_objects : False # whether to use predicted objects [-] - publish_obstacle_polygon: False # whether to publish obstacle polygon [-] + chattering_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] + lowpass_gain: 0.9 # gain parameter for low pass filter [-] + max_velocity: 20.0 # max velocity [m/s] + enable_slow_down: False # whether to use slow down planner [-] + enable_z_axis_obstacle_filtering: True # filter obstacles in z axis (height) [-] + z_axis_filtering_buffer: 0.0 # additional buffer for z axis filtering [m] + voxel_grid_x: 0.05 # voxel grid x parameter for filtering pointcloud [m] + voxel_grid_y: 0.05 # voxel grid y parameter for filtering pointcloud [m] + voxel_grid_z: 100000.0 # voxel grid z parameter for filtering pointcloud [m] + use_predicted_objects: False # whether to use predicted objects [-] + publish_obstacle_polygon: False # whether to publish obstacle polygon [-] predicted_object_filtering_threshold: 1.5 # threshold for filtering predicted objects (valid only publish_obstacle_polygon true) [m] stop_planner: # params for stop position stop_position: max_longitudinal_margin: 5.0 # stop margin distance from obstacle on the path [m] - max_longitudinal_margin_behind_goal: 2.5 # stop margin distance from obstacle behind the goal on the path [m] + max_longitudinal_margin_behind_goal: 2.5 # stop margin distance from obstacle behind goal on the path [m] min_longitudinal_margin: 5.0 # stop margin distance when any other stop point is inserted in stop margin [m] hold_stop_margin_distance: 0.0 # the ego keeps stopping if the ego is in this margin [m] diff --git a/planning/autoware_obstacle_stop_planner/schema/obstacle_stop_planner.schema.json b/planning/autoware_obstacle_stop_planner/schema/obstacle_stop_planner.schema.json index cb2765233c5a0..cfcce06d519bc 100644 --- a/planning/autoware_obstacle_stop_planner/schema/obstacle_stop_planner.schema.json +++ b/planning/autoware_obstacle_stop_planner/schema/obstacle_stop_planner.schema.json @@ -11,6 +11,11 @@ "description": "even if the obstacle disappears, the stop judgment continues for chattering_threshold [s]", "default": "0.5" }, + "lowpass_gain": { + "type": "number", + "description": "gain parameter for low pass filter [-]", + "default": "0.9" + }, "max_velocity": { "type": "number", "description": "max velocity [m/s]", diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml index 58a8f5d074fca..5eb71126bb838 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -41,13 +41,14 @@ object_recognition: use_object_recognition: true collision_check_soft_margins: [5.0, 4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0] # the maximum margin when ego and objects are oriented. - collision_check_hard_margins: [0.6] # this should be larger than `surround_check_distance` of surround_obstacle_checker. + collision_check_hard_margins: [0.6] # this should be larger than `surround_check_distance` of surround_obstacle_checker object_recognition_collision_check_max_extra_stopping_margin: 1.0 th_moving_object_velocity: 1.0 detection_bound_offset: 15.0 outer_road_detection_offset: 1.0 inner_road_detection_offset: 0.0 + # pull over pull_over: minimum_request_length: 0.0 @@ -101,10 +102,11 @@ search_configs: theta_size: 120 angle_goal_range: 6.0 - curve_weight: 0.5 - reverse_weight: 1.0 lateral_goal_range: 0.5 longitudinal_goal_range: 2.0 + curve_weight: 0.5 + reverse_weight: 1.0 + direction_change_weight: 1.5 # costmap configs costmap_configs: obstacle_threshold: 30 @@ -113,7 +115,13 @@ search_method: "forward" # options: forward, backward only_behind_solutions: false use_back: true + adapt_expansion_distance: true + expansion_distance: 0.5 + near_goal_distance: 3.0 distance_heuristic_weight: 2.0 + smoothness_weight: 0.5 + obstacle_distance_weight: 1.75 + goal_lat_distance_weight: 5.0 # -- RRT* search Configurations -- rrtstar: enable_update: true @@ -130,6 +138,7 @@ ego_predicted_path: min_velocity: 1.0 min_acceleration: 1.0 + max_velocity: 1.0 time_horizon_for_front_object: 10.0 time_horizon_for_rear_object: 10.0 time_resolution: 0.5 diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml index d94d4e2e8a8a1..41f8f53a8f17c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml @@ -1,6 +1,7 @@ /**: ros__parameters: traffic_light_signal_timeout: 1.0 + planning_hz: 10.0 backward_path_length: 5.0 forward_path_length: 300.0 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml index a4ab065539bdb..150478ce268b2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -3,6 +3,7 @@ out_of_lane: # module to stop or slowdown before overlapping another lane with other objects mode: ttc # mode used to consider a conflict with an object. "threshold", or "ttc" skip_if_already_overlapping: false # do not run this module when ego already overlaps another lane + ignore_overlaps_over_lane_changeable_lanelets: true # if true, overlaps on lane changeable lanelets are ignored max_arc_length: 100.0 # [m] maximum trajectory arc length that is checked for out_of_lane collisions threshold: From 1fd012c1aca6db81d3dd5887a65aa61af7c61899 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 19 Sep 2024 16:03:39 +0900 Subject: [PATCH 12/95] feat(goal_planner): move PathDecidingStatus to other controller class (#8872) Signed-off-by: Mamoru Sobue --- .cspell.json | 2 +- .../goal_planner_module.hpp | 130 ++-- .../util.hpp | 9 + .../src/goal_planner_module.cpp | 602 ++++++++---------- .../src/util.cpp | 75 +++ 5 files changed, 420 insertions(+), 398 deletions(-) diff --git a/.cspell.json b/.cspell.json index b0d572db8726b..2d024a5ca589d 100644 --- a/.cspell.json +++ b/.cspell.json @@ -4,5 +4,5 @@ "planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/scripts/**" ], "ignoreRegExpList": [], - "words": ["dltype", "tvmgen", "fromarray"] + "words": ["dltype", "tvmgen", "fromarray", "soblin"] } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index fe859b458c7bb..24ae4342537fb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -91,29 +91,19 @@ public: \ DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) -enum class DecidingPathStatus { - NOT_DECIDED, - DECIDING, - DECIDED, -}; -using DecidingPathStatusWithStamp = std::pair; - -struct PreviousPullOverData +class PathDecisionState { - struct SafetyStatus - { - std::optional safe_start_time{}; - bool is_safe{false}; +public: + enum class DecisionKind { + NOT_DECIDED, + DECIDING, + DECIDED, }; - void reset() - { - safety_status = SafetyStatus{}; - deciding_path_status = DecidingPathStatusWithStamp{}; - } - - SafetyStatus safety_status{}; - DecidingPathStatusWithStamp deciding_path_status{}; + DecisionKind state{DecisionKind::NOT_DECIDED}; + rclcpp::Time stamp{}; + bool is_stable_safe{false}; + std::optional safe_start_time{std::nullopt}; }; class ThreadSafeData @@ -194,7 +184,7 @@ class ThreadSafeData last_path_idx_increment_time_ = std::nullopt; closest_start_pose_ = std::nullopt; last_previous_module_output_ = std::nullopt; - prev_data_.reset(); + prev_data_ = PathDecisionState{}; } DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, pull_over_path) @@ -207,10 +197,10 @@ class ThreadSafeData DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, modified_goal_pose) DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, closest_start_pose) DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, last_previous_module_output) - DEFINE_SETTER_GETTER_WITH_MUTEX(PreviousPullOverData, prev_data) DEFINE_SETTER_GETTER_WITH_MUTEX(CollisionCheckDebugMap, collision_check) DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, static_target_objects) DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, dynamic_target_objects) + DEFINE_SETTER_GETTER_WITH_MUTEX(PathDecisionState, prev_data) private: void set_pull_over_path_no_lock(const PullOverPath & path) @@ -238,7 +228,6 @@ class ThreadSafeData void set_no_lock(const PullOverPath & arg) { set_pull_over_path_no_lock(arg); } void set_no_lock(const GoalCandidate & arg) { modified_goal_pose_ = arg; } void set_no_lock(const BehaviorModuleOutput & arg) { last_previous_module_output_ = arg; } - void set_no_lock(const PreviousPullOverData & arg) { prev_data_ = arg; } void set_no_lock(const CollisionCheckDebugMap & arg) { collision_check_ = arg; } std::shared_ptr pull_over_path_{nullptr}; @@ -250,10 +239,10 @@ class ThreadSafeData std::optional last_path_idx_increment_time_; std::optional closest_start_pose_{}; std::optional last_previous_module_output_{}; - PreviousPullOverData prev_data_{}; CollisionCheckDebugMap collision_check_{}; PredictedObjects static_target_objects_{}; PredictedObjects dynamic_target_objects_{}; + PathDecisionState prev_data_{}; std::recursive_mutex & mutex_; rclcpp::Clock::SharedPtr clock_; @@ -312,12 +301,59 @@ struct PoseWithString struct PullOverContextData { PullOverContextData() = delete; - explicit PullOverContextData(const std::pair & is_safe_path) - : is_safe_path_latched(is_safe_path.first), is_safe_path_instant(is_safe_path.second) + explicit PullOverContextData( + const bool is_stable_safe_path, const PredictedObjects & static_objects, + const PredictedObjects & dynamic_objects) + : is_stable_safe_path(is_stable_safe_path), + static_target_objects(static_objects), + dynamic_target_objects(dynamic_objects) { } - bool is_safe_path_latched{true}; - bool is_safe_path_instant{true}; + const bool is_stable_safe_path; + const PredictedObjects static_target_objects; + const PredictedObjects dynamic_target_objects; +}; + +class PathDecisionStateController +{ +public: + PathDecisionStateController() = default; + + /** + * @brief update current state and save old current state to prev state + */ + void transit_state( + const bool found_pull_over_path, const rclcpp::Time & now, + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const std::optional modified_goal_opt, + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const bool is_current_safe, const GoalPlannerParameters & parameters, + const std::shared_ptr goal_searcher, const bool is_activated, + const std::optional & pull_over_path, + std::vector & ego_polygons_expanded); + + PathDecisionState get_current_state() const { return current_state_; } + + PathDecisionState get_prev_state() const { return prev_state_; } + +private: + PathDecisionState current_state_{}; + PathDecisionState prev_state_{}; + + /** + * @brief update current state and save old current state to prev state + */ + PathDecisionState get_next_state( + const bool found_pull_over_path, const rclcpp::Time & now, + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const std::optional modified_goal_opt, + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const bool is_current_safe, const GoalPlannerParameters & parameters, + const std::shared_ptr goal_searcher, const bool is_activated, + const std::optional & pull_over_path_opt, + std::vector & ego_polygons_expanded) const; }; class GoalPlannerModule : public SceneModuleInterface @@ -519,6 +555,8 @@ class GoalPlannerModule : public SceneModuleInterface // TODO(soblin): organize part of thread_safe_data and previous data to PullOverContextData // context_data_ is initialized in updateData(), used in plan() and refreshed in postProcess() std::optional context_data_{std::nullopt}; + // path_decision_controller is updated in updateData(), and used in plan() + PathDecisionStateController path_decision_controller_{}; std::unique_ptr last_approval_data_{nullptr}; // approximate distance from the start point to the end point of pull_over. @@ -548,11 +586,6 @@ class GoalPlannerModule : public SceneModuleInterface bool checkOccupancyGridCollision( const PathWithLaneId & path, const std::shared_ptr occupancy_grid_map) const; - bool checkObjectsCollision( - const PathWithLaneId & path, const std::vector & curvatures, - const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, - const double collision_check_margin, const bool extract_static_objects, - const bool update_debug_data = false) const; // goal seach Pose calcRefinedGoal(const Pose & goal_pose) const; @@ -580,26 +613,11 @@ class GoalPlannerModule : public SceneModuleInterface const Pose & current_pose, const double path_update_duration, const GoalPlannerParameters & parameters) const; bool isStuck( + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, const GoalPlannerParameters & parameters); - bool hasDecidedPath( - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const PullOverContextData & context_data, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher) const; - bool hasNotDecidedPath( - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const PullOverContextData & context_data, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher) const; - DecidingPathStatusWithStamp checkDecidingPathStatus( - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const PullOverContextData & context_data, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher) const; void decideVelocity(); - bool foundPullOverPath() const; void updateStatus(const BehaviorModuleOutput & output); // validation @@ -617,13 +635,14 @@ class GoalPlannerModule : public SceneModuleInterface std::shared_ptr planner_data, const std::shared_ptr goal_searcher, const std::shared_ptr occupancy_grid_map); - bool canReturnToLaneParking(); + bool canReturnToLaneParking(const PullOverContextData & context_data); // plan pull over path BehaviorModuleOutput planPullOver(const PullOverContextData & context_data); BehaviorModuleOutput planPullOverAsOutput(const PullOverContextData & context_data); BehaviorModuleOutput planPullOverAsCandidate(const PullOverContextData & context_data); std::optional> selectPullOverPath( + const PullOverContextData & context_data, const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const; @@ -633,7 +652,6 @@ class GoalPlannerModule : public SceneModuleInterface // output setter void setOutput(const PullOverContextData & context_data, BehaviorModuleOutput & output); - void updatePreviousData(const PullOverContextData & context_data); void setModifiedGoal(BehaviorModuleOutput & output) const; void setTurnSignalInfo(BehaviorModuleOutput & output); @@ -670,12 +688,12 @@ class GoalPlannerModule : public SceneModuleInterface */ /** * @brief Checks if the current path is safe. - * @return std::pair - * first: If the path is safe for a certain period of time, true. - * second: If the path is safe in the current state, true. + * @return If the path is safe in the current state, true. */ - std::pair isSafePath( - const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, + bool isSafePath( + const std::shared_ptr planner_data, const bool found_pull_over_path, + const std::optional & pull_over_path_opt, + const GoalPlannerParameters & parameters, const std::shared_ptr & ego_predicted_path_params, const std::shared_ptr & objects_filtering_params, const std::shared_ptr & safety_check_params) const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index 70cdacf09f335..a9c9991df3c18 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -129,6 +129,15 @@ bool isWithinAreas( */ std::vector getBusStopAreaPolygons(const lanelet::ConstLanelets & lanes); +bool checkObjectsCollision( + const PathWithLaneId & path, const std::vector & curvatures, + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const BehaviorPathPlannerParameters & behavior_path_parameters, + const double collision_check_margin, const bool extract_static_objects, + const double maximum_deceleration, + const double object_recognition_collision_check_max_extra_stopping_margin, + std::vector & ego_polygons_expanded, const bool update_debug_data = false); + // debug MarkerArray createPullOverAreaMarkerArray( const autoware::universe_utils::MultiPolygon2d area_polygons, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index dde3a554fca3d..e92cd5940e06e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -261,6 +261,7 @@ void GoalPlannerModule::onTimer() } // check if new pull over path candidates are needed to be generated + const auto current_state = thread_safe_data_.get_prev_data().state; const bool need_update = std::invoke([&]() { if (isOnModifiedGoal(local_planner_data->self_odometry->pose.pose, parameters)) { return false; @@ -276,15 +277,9 @@ void GoalPlannerModule::onTimer() RCLCPP_DEBUG(getLogger(), "has previous module path shape changed"); return true; } - // TODO(soblin): never call isSafePath on thread pool side - const auto local_context_data = PullOverContextData(isSafePath( - local_planner_data, parameters, ego_predicted_path_params, objects_filtering_params, - safety_check_params)); if ( hasDeviatedFromLastPreviousModulePath(local_planner_data) && - // TODO(soblin): use DecidingPathStatus in ThreadInputData - !hasDecidedPath( - local_planner_data, occupancy_grid_map, local_context_data, parameters, goal_searcher)) { + current_state != PathDecisionState::DecisionKind::DECIDED) { RCLCPP_DEBUG(getLogger(), "has deviated from last previous module path"); return true; } @@ -428,7 +423,10 @@ void GoalPlannerModule::onFreespaceParkingTimer() (clock_->now() - local_planner_data->costmap->header.stamp).seconds() < 1.0; constexpr double path_update_duration = 1.0; if ( - isStuck(local_planner_data, occupancy_grid_map, parameters) && is_new_costmap && + isStuck( + thread_safe_data_.get_static_target_objects(), thread_safe_data_.get_dynamic_target_objects(), + local_planner_data, occupancy_grid_map, parameters) && + is_new_costmap && needPathUpdate( local_planner_data->self_odometry->pose.pose, path_update_duration, parameters)) { planFreespacePath(local_planner_data, goal_searcher, occupancy_grid_map); @@ -464,35 +462,33 @@ void GoalPlannerModule::updateData() universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // extract static and dynamic objects in extraction polygon for path collision check - { - const auto & p = parameters_; - const auto & rh = *(planner_data_->route_handler); - const auto objects = *(planner_data_->dynamic_object); - const double vehicle_width = planner_data_->parameters.vehicle_width; - const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length); - const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon( - pull_over_lanes, left_side_parking_, p->detection_bound_offset, - p->margin_from_boundary + p->max_lateral_offset + vehicle_width); - if (objects_extraction_polygon.has_value()) { - debug_data_.objects_extraction_polygon = objects_extraction_polygon.value(); - } - PredictedObjects dynamic_target_objects{}; - for (const auto & object : objects.objects) { - const auto object_polygon = universe_utils::toPolygon2d(object); - if ( - objects_extraction_polygon.has_value() && - boost::geometry::intersects(object_polygon, objects_extraction_polygon.value())) { - dynamic_target_objects.objects.push_back(object); - } + const auto & p = parameters_; + const auto & rh = *(planner_data_->route_handler); + const auto objects = *(planner_data_->dynamic_object); + const double vehicle_width = planner_data_->parameters.vehicle_width; + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length); + const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon( + pull_over_lanes, left_side_parking_, p->detection_bound_offset, + p->margin_from_boundary + p->max_lateral_offset + vehicle_width); + if (objects_extraction_polygon.has_value()) { + debug_data_.objects_extraction_polygon = objects_extraction_polygon.value(); + } + PredictedObjects dynamic_target_objects{}; + for (const auto & object : objects.objects) { + const auto object_polygon = universe_utils::toPolygon2d(object); + if ( + objects_extraction_polygon.has_value() && + boost::geometry::intersects(object_polygon, objects_extraction_polygon.value())) { + dynamic_target_objects.objects.push_back(object); } - const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity( - dynamic_target_objects, p->th_moving_object_velocity); - - // these objects are used for path collision check not for safety check - thread_safe_data_.set_static_target_objects(static_target_objects); - thread_safe_data_.set_dynamic_target_objects(dynamic_target_objects); } + const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity( + dynamic_target_objects, p->th_moving_object_velocity); + + // these objects are used for path collision check not for safety check + thread_safe_data_.set_static_target_objects(static_target_objects); + thread_safe_data_.set_dynamic_target_objects(dynamic_target_objects); // In PlannerManager::run(), it calls SceneModuleInterface::setData and // SceneModuleInterface::setPreviousModuleOutput before module_ptr->run(). @@ -550,9 +546,22 @@ void GoalPlannerModule::updateData() resetPathReference(); path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); - context_data_ = PullOverContextData(isSafePath( - planner_data_, *parameters_, ego_predicted_path_params_, objects_filtering_params_, - safety_check_params_)); + const bool found_pull_over_path = thread_safe_data_.foundPullOverPath(); + const std::optional pull_over_path = + found_pull_over_path ? std::make_optional(*thread_safe_data_.get_pull_over_path()) + : std::nullopt; + + const bool is_current_safe = isSafePath( + planner_data_, found_pull_over_path, pull_over_path, *parameters_, ego_predicted_path_params_, + objects_filtering_params_, safety_check_params_); + path_decision_controller_.transit_state( + found_pull_over_path, clock_->now(), static_target_objects, dynamic_target_objects, + thread_safe_data_.get_modified_goal_pose(), planner_data_, occupancy_grid_map_, is_current_safe, + *parameters_, goal_searcher_, isActivated(), pull_over_path, debug_data_.ego_polygons_expanded); + + context_data_.emplace( + path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, + dynamic_target_objects); // update goal searcher and generate goal candidates if (thread_safe_data_.get_goal_candidates().empty()) { @@ -561,6 +570,8 @@ void GoalPlannerModule::updateData() thread_safe_data_.set_goal_candidates(generateGoalCandidates()); } + thread_safe_data_.set_prev_data(path_decision_controller_.get_current_state()); + if (!isActivated()) { return; } @@ -683,11 +694,11 @@ bool GoalPlannerModule::isExecutionRequested() const bool GoalPlannerModule::isExecutionReady() const { + // NOTE(soblin): at least in goal_planner, isExecutionReady is called via super::updateRTCStatus + // from self::postProcess, so returning cached member variable like + // path_decision_controller.get_current_state() is valid if (parameters_->safety_check_params.enable_safety_check && isWaitingApproval()) { - if (!isSafePath( - planner_data_, *parameters_, ego_predicted_path_params_, objects_filtering_params_, - safety_check_params_) - .first) { + if (!path_decision_controller_.get_current_state().is_stable_safe) { RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); return false; } @@ -817,7 +828,7 @@ bool GoalPlannerModule::planFreespacePath( return false; } -bool GoalPlannerModule::canReturnToLaneParking() +bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & context_data) { // return only before starting free space parking if (!isStopped()) { @@ -833,10 +844,13 @@ bool GoalPlannerModule::canReturnToLaneParking() const std::vector curvatures = lane_parking_path->getFullPathCurvatures(); if ( parameters_->use_object_recognition && - checkObjectsCollision( - path, curvatures, planner_data_, *parameters_, + goal_planner_utils::checkObjectsCollision( + path, curvatures, context_data.static_target_objects, context_data.dynamic_target_objects, + planner_data_->parameters, parameters_->object_recognition_collision_check_hard_margins.back(), - /*extract_static_objects=*/false)) { + /*extract_static_objects=*/false, parameters_->maximum_deceleration, + parameters_->object_recognition_collision_check_max_extra_stopping_margin, + debug_data_.ego_polygons_expanded)) { return false; } @@ -888,7 +902,7 @@ BehaviorModuleOutput GoalPlannerModule::plan() } const auto context_data = context_data_.has_value() ? context_data_.value() - : PullOverContextData(std::make_pair(true, true)); + : PullOverContextData(true, PredictedObjects{}, PredictedObjects{}); if (utils::isAllowedGoalModification(planner_data_->route_handler)) { return planPullOver(context_data); @@ -899,6 +913,7 @@ BehaviorModuleOutput GoalPlannerModule::plan() } std::optional> GoalPlannerModule::selectPullOverPath( + const PullOverContextData & context_data, const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const { @@ -1106,9 +1121,13 @@ std::optional> GoalPlannerModule::selectP const PathWithLaneId parking_path = path.getParkingPath(); const auto parking_path_curvatures = path.getParkingPathCurvatures(); if ( - parameters_->use_object_recognition && checkObjectsCollision( - parking_path, parking_path_curvatures, planner_data_, - *parameters_, collision_check_margin, true, true)) { + parameters_->use_object_recognition && + goal_planner_utils::checkObjectsCollision( + parking_path, parking_path_curvatures, context_data.static_target_objects, + context_data.dynamic_target_objects, planner_data_->parameters, collision_check_margin, + true, parameters_->maximum_deceleration, + parameters_->object_recognition_collision_check_max_extra_stopping_margin, + debug_data_.ego_polygons_expanded, true)) { continue; } if ( @@ -1152,7 +1171,7 @@ void GoalPlannerModule::setOutput( } if ( - parameters_->safety_check_params.enable_safety_check && !context_data.is_safe_path_latched && + parameters_->safety_check_params.enable_safety_check && !context_data.is_stable_safe_path && isActivated()) { // situation : not safe against dynamic objects after approval // insert stop point in current path if ego is able to stop with acceleration and jerk @@ -1177,8 +1196,8 @@ void GoalPlannerModule::setOutput( // set hazard and turn signal if ( - hasDecidedPath( - planner_data_, occupancy_grid_map_, context_data, *parameters_, goal_searcher_) && + path_decision_controller_.get_current_state().state == + PathDecisionState::DecisionKind::DECIDED && isActivated()) { setTurnSignalInfo(output); } @@ -1245,138 +1264,6 @@ void GoalPlannerModule::updateSteeringFactor( pose, distance, PlanningBehavior::GOAL_PLANNER, steering_factor_direction, type, ""); } -bool GoalPlannerModule::hasDecidedPath( - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const PullOverContextData & context_data, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher) const -{ - return checkDecidingPathStatus( - planner_data, occupancy_grid_map, context_data, parameters, goal_searcher) - .first == DecidingPathStatus::DECIDED; -} - -bool GoalPlannerModule::hasNotDecidedPath( - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const PullOverContextData & context_data, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher) const -{ - return checkDecidingPathStatus( - planner_data, occupancy_grid_map, context_data, parameters, goal_searcher) - .first == DecidingPathStatus::NOT_DECIDED; -} - -DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus( - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const PullOverContextData & context_data, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher) const -{ - const auto & prev_status = thread_safe_data_.get_prev_data().deciding_path_status; - const bool enable_safety_check = parameters.safety_check_params.enable_safety_check; - - // Once this function returns true, it will continue to return true thereafter - if (prev_status.first == DecidingPathStatus::DECIDED) { - return prev_status; - } - - // if path is not safe, not decided - if (!thread_safe_data_.foundPullOverPath()) { - return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; - } - - // If it is dangerous against dynamic objects before approval, do not determine the path. - // This eliminates a unsafe path to be approved - if (enable_safety_check && !context_data.is_safe_path_latched && !isActivated()) { - RCLCPP_DEBUG( - getLogger(), - "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " - "approval"); - return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; - } - - // if object recognition for path collision check is enabled, transition to DECIDED after - // DECIDING for a certain period of time if there is no collision. - const auto pull_over_path = thread_safe_data_.get_pull_over_path(); - const auto current_path = pull_over_path->getCurrentPath(); - if (prev_status.first == DecidingPathStatus::DECIDING) { - const double hysteresis_factor = 0.9; - - // check goal pose collision - const auto modified_goal_opt = thread_safe_data_.get_modified_goal_pose(); - if ( - modified_goal_opt && !goal_searcher->isSafeGoalWithMarginScaleFactor( - modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map, - planner_data, thread_safe_data_.get_static_target_objects())) { - RCLCPP_DEBUG(getLogger(), "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe"); - return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; - } - - // check current parking path collision - const auto parking_path = pull_over_path->getParkingPath(); - const std::vector parking_path_curvatures = pull_over_path->getParkingPathCurvatures(); - const double margin = - parameters.object_recognition_collision_check_hard_margins.back() * hysteresis_factor; - if (checkObjectsCollision( - parking_path, parking_path_curvatures, planner_data, parameters, margin, - /*extract_static_objects=*/false)) { - RCLCPP_DEBUG( - getLogger(), - "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects"); - return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; - } - - if (enable_safety_check && !context_data.is_safe_path_latched) { - RCLCPP_DEBUG( - getLogger(), - "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path is not safe against dynamic objects"); - return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; - } - - // if enough time has passed since deciding status starts, transition to DECIDED - constexpr double check_collision_duration = 1.0; - const double elapsed_time_from_deciding = (clock_->now() - prev_status.second).seconds(); - if (elapsed_time_from_deciding > check_collision_duration) { - RCLCPP_DEBUG( - getLogger(), "[DecidingPathStatus]: DECIDING->DECIDED. has enough safe time passed"); - return {DecidingPathStatus::DECIDED, clock_->now()}; - } - - // if enough time has NOT passed since deciding status starts, keep DECIDING - RCLCPP_DEBUG( - getLogger(), "[DecidingPathStatus]: keep DECIDING. elapsed_time_from_deciding: %f", - elapsed_time_from_deciding); - return prev_status; - } - - // if ego is sufficiently close to the start of the nearest candidate path, the path is decided - const auto & current_pose = planner_data->self_odometry->pose.pose; - const size_t ego_segment_idx = - autoware::motion_utils::findNearestSegmentIndex(current_path.points, current_pose.position); - - const size_t start_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( - current_path.points, pull_over_path->start_pose.position); - const double dist_to_parking_start_pose = calcSignedArcLength( - current_path.points, current_pose.position, ego_segment_idx, - pull_over_path->start_pose.position, start_pose_segment_idx); - if (dist_to_parking_start_pose > parameters.decide_path_distance) { - return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; - } - - // if object recognition for path collision check is enabled, transition to DECIDING to check - // collision for a certain period of time. Otherwise, transition to DECIDED directly. - if (parameters.use_object_recognition) { - RCLCPP_DEBUG( - getLogger(), - "[DecidingPathStatus]: NOT_DECIDED->DECIDING. start checking collision for certain " - "period of time"); - return {DecidingPathStatus::DECIDING, clock_->now()}; - } - RCLCPP_DEBUG(getLogger(), "[DecidingPathStatus]: NOT_DECIDED->DECIDED"); - return {DecidingPathStatus::DECIDED, clock_->now()}; -} - void GoalPlannerModule::decideVelocity() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1395,9 +1282,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOver(const PullOverContextData & { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - // TODO(soblin): move DecidingPathStatus to context_data - if (!hasDecidedPath( - planner_data_, occupancy_grid_map_, context_data, *parameters_, goal_searcher_)) { + if ( + path_decision_controller_.get_current_state().state != + PathDecisionState::DecisionKind::DECIDED) { return planPullOverAsCandidate(context_data); } @@ -1451,8 +1338,8 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( } if ( - hasNotDecidedPath( - planner_data_, occupancy_grid_map_, context_data, *parameters_, goal_searcher_) && + path_decision_controller_.get_current_state().state == + PathDecisionState::DecisionKind::NOT_DECIDED && needPathUpdate( planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, *parameters_)) { // if the final path is not decided and enough time has passed since last path update, @@ -1470,7 +1357,8 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( // Select a path that is as safe as possible and has a high priority. const auto pull_over_path_candidates = thread_safe_data_.get_pull_over_path_candidates(); - const auto path_and_goal_opt = selectPullOverPath(pull_over_path_candidates, goal_candidates); + const auto path_and_goal_opt = + selectPullOverPath(context_data, pull_over_path_candidates, goal_candidates); // update thread_safe_data_ if (path_and_goal_opt) { @@ -1492,7 +1380,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( // return to lane parking if it is possible if ( thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE && - canReturnToLaneParking()) { + canReturnToLaneParking(context_data)) { thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path()); } @@ -1511,8 +1399,6 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( path_candidate_ = std::make_shared(thread_safe_data_.get_pull_over_path()->getFullPath()); - updatePreviousData(context_data); - return output; } @@ -1525,12 +1411,10 @@ void GoalPlannerModule::postProcess() } const auto context_data = context_data_.has_value() ? context_data_.value() - : PullOverContextData(std::make_pair(true, true)); + : PullOverContextData(true, PredictedObjects{}, PredictedObjects{}); - // TODO(Mamoru Sobue): repetitive call to checkDecidingPathStatus() in the main thread is a - // waste of time because it gives same result throughout the main thread. const bool has_decided_path = - hasDecidedPath(planner_data_, occupancy_grid_map_, context_data, *parameters_, goal_searcher_); + path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::DECIDED; context_data_ = std::nullopt; @@ -1553,40 +1437,6 @@ void GoalPlannerModule::postProcess() setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath()); } -void GoalPlannerModule::updatePreviousData(const PullOverContextData & context_data) -{ - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - - // for the next loop setOutput(). - // this is used to determine whether to generate a new stop path or keep the current stop path. - // TODO(Mamoru Sobue): put prev_data_ out of ThreadSafeData - auto prev_data = thread_safe_data_.get_prev_data(); - - prev_data.deciding_path_status = checkDecidingPathStatus( - planner_data_, occupancy_grid_map_, context_data, *parameters_, goal_searcher_); - - // This is related to safety check, so if it is disabled, return here. - if (!parameters_->safety_check_params.enable_safety_check) { - prev_data.safety_status.is_safe = true; - } else { - // Even if the current path is safe, it will not be safe unless it stands for a certain period - // of time. Record the time when the current path has become safe - const auto is_safe = context_data.is_safe_path_latched, - current_is_safe = context_data.is_safe_path_instant; - if (current_is_safe) { - if (!prev_data.safety_status.safe_start_time) { - prev_data.safety_status.safe_start_time = clock_->now(); - } - } else { - prev_data.safety_status.safe_start_time = std::nullopt; - } - prev_data.safety_status.is_safe = is_safe; - } - - // commit the change - thread_safe_data_.set_prev_data(prev_data); -} - BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1597,7 +1447,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() } const auto context_data = context_data_.has_value() ? context_data_.value() - : PullOverContextData(std::make_pair(true, true)); + : PullOverContextData(true, PredictedObjects{}, PredictedObjects{}); if (utils::isAllowedGoalModification(planner_data_->route_handler)) { return planPullOverAsCandidate(context_data); @@ -1811,6 +1661,7 @@ bool GoalPlannerModule::isStopped() } bool GoalPlannerModule::isStuck( + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, const GoalPlannerParameters & parameters) @@ -1839,10 +1690,12 @@ bool GoalPlannerModule::isStuck( if (parameters.use_object_recognition) { const auto path = pull_over_path->getCurrentPath(); const auto curvatures = autoware::motion_utils::calcCurvature(path.points); - if (checkObjectsCollision( - path, curvatures, planner_data, parameters, + if (goal_planner_utils::checkObjectsCollision( + path, curvatures, static_target_objects, dynamic_target_objects, planner_data->parameters, parameters.object_recognition_collision_check_hard_margins.back(), - /*extract_static_objects=*/false)) { + /*extract_static_objects=*/false, parameters.maximum_deceleration, + parameters.object_recognition_collision_check_max_extra_stopping_margin, + debug_data_.ego_polygons_expanded)) { return true; } } @@ -1993,77 +1846,6 @@ bool GoalPlannerModule::checkOccupancyGridCollision( return occupancy_grid_map->hasObstacleOnPath(path, check_out_of_range); } -bool GoalPlannerModule::checkObjectsCollision( - const PathWithLaneId & path, const std::vector & curvatures, - const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, - const double collision_check_margin, const bool extract_static_objects, - const bool update_debug_data) const -{ - const auto target_objects = extract_static_objects - ? thread_safe_data_.get_static_target_objects() - : thread_safe_data_.get_dynamic_target_objects(); - if (target_objects.objects.empty()) { - return false; - } - - // check collision roughly with {min_distance, max_distance} between ego footprint and objects - // footprint - std::pair has_collision_rough = - utils::path_safety_checker::checkObjectsCollisionRough( - path, target_objects, collision_check_margin, planner_data_->parameters, false); - if (!has_collision_rough.first) { - return false; - } - if (has_collision_rough.second) { - return true; - } - - std::vector obj_polygons; - for (const auto & object : target_objects.objects) { - obj_polygons.push_back(autoware::universe_utils::toPolygon2d(object)); - } - - /* Expand ego collision check polygon - * - `collision_check_margin` is added in all directions. - * - `extra_stopping_margin` adds stopping margin under deceleration constraints forward. - * - `extra_lateral_margin` adds the lateral margin on curves. - */ - if (path.points.size() != curvatures.size()) { - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, - "path.points.size() != curvatures.size() in checkObjectsCollision(). judge as non collision"); - return false; - } - std::vector ego_polygons_expanded{}; - for (size_t i = 0; i < path.points.size(); ++i) { - const auto p = path.points.at(i); - const double extra_stopping_margin = std::min( - std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / parameters.maximum_deceleration, - parameters.object_recognition_collision_check_max_extra_stopping_margin); - - // The square is meant to imply centrifugal force, but it is not a very well-founded formula. - // TODO(kosuke55): It is needed to consider better way because there is an inherently - // different conception of the inside and outside margins. - const double extra_lateral_margin = std::min( - extra_stopping_margin, - std::abs(curvatures.at(i) * std::pow(p.point.longitudinal_velocity_mps, 2))); - - const auto ego_polygon = autoware::universe_utils::toFootprint( - p.point.pose, - planner_data->parameters.base_link2front + collision_check_margin + extra_stopping_margin, - planner_data->parameters.base_link2rear + collision_check_margin, - planner_data->parameters.vehicle_width + collision_check_margin * 2.0 + - extra_lateral_margin * 2.0); - ego_polygons_expanded.push_back(ego_polygon); - } - - if (update_debug_data) { - debug_data_.ego_polygons_expanded = ego_polygons_expanded; - } - - return utils::path_safety_checker::checkPolygonsIntersects(ego_polygons_expanded, obj_polygons); -} - bool GoalPlannerModule::hasEnoughDistance( const PullOverPath & pull_over_path, const PathWithLaneId & long_tail_reference_path) const { @@ -2368,16 +2150,18 @@ static std::vector filterOb return refined_filtered_objects; } -std::pair GoalPlannerModule::isSafePath( - const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, +bool GoalPlannerModule::isSafePath( + const std::shared_ptr planner_data, const bool found_pull_over_path, + const std::optional & pull_over_path_opt, const GoalPlannerParameters & parameters, const std::shared_ptr & ego_predicted_path_params, const std::shared_ptr & objects_filtering_params, const std::shared_ptr & safety_check_params) const { - if (!thread_safe_data_.get_pull_over_path()) { - return {false, false}; + if (!found_pull_over_path || !pull_over_path_opt) { + return false; } - const auto pull_over_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); + const auto & pull_over_path = pull_over_path_opt.value(); + const auto current_pull_over_path = pull_over_path.getCurrentPath(); const auto & current_pose = planner_data->self_odometry->pose.pose; const double current_velocity = std::hypot( planner_data->self_odometry->twist.twist.linear.x, @@ -2390,11 +2174,10 @@ std::pair GoalPlannerModule::isSafePath( const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *route_handler, left_side_parking_, parameters.backward_goal_search_length, parameters.forward_goal_search_length); - const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(pull_over_path.points); + const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(current_pull_over_path.points); const std::pair terminal_velocity_and_accel = utils::parking_departure::getPairsTerminalVelocityAndAccel( - thread_safe_data_.get_pull_over_path()->pairs_terminal_velocity_and_accel, - thread_safe_data_.get_pull_over_path()->path_idx); + pull_over_path.pairs_terminal_velocity_and_accel, pull_over_path.path_idx); RCLCPP_DEBUG( getLogger(), "pairs_terminal_velocity_and_accel for goal_planner: %f, %f", terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); @@ -2405,8 +2188,8 @@ std::pair GoalPlannerModule::isSafePath( const bool limit_to_max_velocity = true; const auto ego_predicted_path = autoware::behavior_path_planner::utils::path_safety_checker::createPredictedPath( - ego_predicted_path_params, pull_over_path.points, current_pose, current_velocity, ego_seg_idx, - is_object_front, limit_to_max_velocity); + ego_predicted_path_params, current_pull_over_path.points, current_pose, current_velocity, + ego_seg_idx, is_object_front, limit_to_max_velocity); // ========================================================================================== // if ego is before the entry of pull_over_lanes, the beginning of the safety check area @@ -2436,8 +2219,8 @@ std::pair GoalPlannerModule::isSafePath( first_road_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(lane_yaw); // if current ego pose is before pull over lanes segment, use first road lanelet center pose if ( - calcSignedArcLength(pull_over_path.points, first_road_pose.position, current_pose.position) < - 0.0) { + calcSignedArcLength( + current_pull_over_path.points, first_road_pose.position, current_pose.position) < 0.0) { return first_road_pose; } // if current ego pose is in pull over lanes segment, use current ego pose @@ -2459,13 +2242,13 @@ std::pair GoalPlannerModule::isSafePath( const auto prev_data = thread_safe_data_.get_prev_data(); const double hysteresis_factor = - prev_data.safety_status.is_safe ? 1.0 : parameters.hysteresis_factor_expand_rate; + prev_data.is_stable_safe ? 1.0 : parameters.hysteresis_factor_expand_rate; CollisionCheckDebugMap collision_check{}; const bool current_is_safe = std::invoke([&]() { if (parameters.safety_check_params.method == "RSS") { return autoware::behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( - pull_over_path, ego_predicted_path, filtered_objects, collision_check, + current_pull_over_path, ego_predicted_path, filtered_objects, collision_check, planner_data->parameters, safety_check_params->rss_params, objects_filtering_params->use_all_predicted_path, hysteresis_factor, safety_check_params->collision_check_yaw_diff_threshold); @@ -2496,16 +2279,7 @@ std::pair GoalPlannerModule::isSafePath( * | | | | | | | | * 0 =========================-------==========-- t */ - if (current_is_safe) { - if ( - prev_data.safety_status.safe_start_time && - (clock_->now() - prev_data.safety_status.safe_start_time.value()).seconds() > - parameters.safety_check_params.keep_unsafe_time) { - return {true /*is_safe*/, true /*current_is_safe*/}; - } - } - - return {false /*is_safe*/, current_is_safe}; + return current_is_safe; } void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) @@ -2536,10 +2310,10 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) }; if (utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas - const auto color = - hasDecidedPath(planner_data_, occupancy_grid_map_, context_data, *parameters_, goal_searcher_) - ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow - : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green + const auto color = path_decision_controller_.get_current_state().state == + PathDecisionState::DecisionKind::DECIDED + ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow + : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green const double z = planner_data_->route_handler->getGoalPose().position.z; add(goal_planner_utils::createPullOverAreaMarkerArray( goal_searcher_->getAreaPolygons(), header, color, z)); @@ -2664,17 +2438,17 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) { const auto prev_data = thread_safe_data_.get_prev_data(); visualization_msgs::msg::MarkerArray marker_array{}; - const auto color = prev_data.safety_status.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = prev_data.is_stable_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "safety_status", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); marker.pose = planner_data_->self_odometry->pose.pose; - marker.text += "is_safe: " + std::to_string(prev_data.safety_status.is_safe) + "\n"; - if (prev_data.safety_status.safe_start_time) { + marker.text += "is_safe: " + std::to_string(prev_data.is_stable_safe) + "\n"; + if (prev_data.safe_start_time) { const double elapsed_time_from_safe_start = - (clock_->now() - prev_data.safety_status.safe_start_time.value()).seconds(); + (clock_->now() - prev_data.safe_start_time.value()).seconds(); marker.text += "elapsed_time_from_safe_start: " + std::to_string(elapsed_time_from_safe_start) + "\n"; } @@ -2702,7 +2476,9 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) std::to_string(thread_safe_data_.get_pull_over_path()->partial_paths.size() - 1); } - if (isStuck(planner_data_, occupancy_grid_map_, *parameters_)) { + if (isStuck( + context_data.static_target_objects, context_data.dynamic_target_objects, planner_data_, + occupancy_grid_map_, *parameters_)) { marker.text += " stuck"; } else if (isStopped()) { marker.text += " stopped"; @@ -2822,4 +2598,148 @@ void GoalPlannerModule::GoalPlannerData::update( goal_searcher->setReferenceGoal(goal_searcher_->getReferenceGoal()); } +void PathDecisionStateController::transit_state( + const bool found_pull_over_path, const rclcpp::Time & now, + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const std::optional modified_goal_opt, + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const bool is_current_safe, const GoalPlannerParameters & parameters, + const std::shared_ptr goal_searcher, const bool is_activated, + const std::optional & pull_over_path, + std::vector & ego_polygons_expanded) +{ + const auto next_state = get_next_state( + found_pull_over_path, now, static_target_objects, dynamic_target_objects, modified_goal_opt, + planner_data, occupancy_grid_map, is_current_safe, parameters, goal_searcher, is_activated, + pull_over_path, ego_polygons_expanded); + prev_state_ = current_state_; + current_state_ = next_state; +} + +PathDecisionState PathDecisionStateController::get_next_state( + const bool found_pull_over_path, const rclcpp::Time & now, + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const std::optional modified_goal_opt, + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const bool is_current_safe, const GoalPlannerParameters & parameters, + const std::shared_ptr goal_searcher, const bool is_activated, + const std::optional & pull_over_path_opt, + std::vector & ego_polygons_expanded) const +{ + auto next_state = prev_state_; + + // update timestamp + next_state.stamp = now; + + // update safety + if (!parameters.safety_check_params.enable_safety_check) { + next_state.is_stable_safe = true; + } else { + if (is_current_safe) { + if (!next_state.safe_start_time) { + next_state.safe_start_time = now; + next_state.is_stable_safe = false; + } else { + next_state.is_stable_safe = + ((now - next_state.safe_start_time.value()).seconds() > + parameters.safety_check_params.keep_unsafe_time); + } + } else { + next_state.safe_start_time = std::nullopt; + next_state.is_stable_safe = false; + } + } + + // Once this function returns true, it will continue to return true thereafter + if (next_state.state == PathDecisionState::DecisionKind::DECIDED) { + next_state.state = PathDecisionState::DecisionKind::DECIDED; + return next_state; + } + + // if path is not safe, not decided + if (!found_pull_over_path || !pull_over_path_opt) { + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + return next_state; + } + + const auto & pull_over_path = pull_over_path_opt.value(); + const bool enable_safety_check = parameters.safety_check_params.enable_safety_check; + // If it is dangerous against dynamic objects before approval, do not determine the path. + // This eliminates a unsafe path to be approved + if (enable_safety_check && !next_state.is_stable_safe && !is_activated) { + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + return next_state; + } + + const auto & current_path = pull_over_path.getCurrentPath(); + if (prev_state_.state == PathDecisionState::DecisionKind::DECIDING) { + const double hysteresis_factor = 0.9; + + // check goal pose collision + if ( + modified_goal_opt && !goal_searcher->isSafeGoalWithMarginScaleFactor( + modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map, + planner_data, static_target_objects)) { + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + return next_state; + } + + // check current parking path collision + const auto & parking_path = pull_over_path.getParkingPath(); + const std::vector parking_path_curvatures = pull_over_path.getParkingPathCurvatures(); + const double margin = + parameters.object_recognition_collision_check_hard_margins.back() * hysteresis_factor; + if (goal_planner_utils::checkObjectsCollision( + parking_path, parking_path_curvatures, static_target_objects, dynamic_target_objects, + planner_data->parameters, margin, + /*extract_static_objects=*/false, parameters.maximum_deceleration, + parameters.object_recognition_collision_check_max_extra_stopping_margin, + ego_polygons_expanded, true)) { + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + return next_state; + } + + if (enable_safety_check && !next_state.is_stable_safe) { + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + return next_state; + } + + // if enough time has passed since deciding status starts, transition to DECIDED + constexpr double check_collision_duration = 1.0; + const double elapsed_time_from_deciding = (now - prev_state_.stamp).seconds(); + if (elapsed_time_from_deciding > check_collision_duration) { + next_state.state = PathDecisionState::DecisionKind::DECIDED; + return next_state; + } + + // if enough time has NOT passed since deciding status starts, keep DECIDING + return next_state; + } + + // if ego is sufficiently close to the start of the nearest candidate path, the path is decided + const auto & current_pose = planner_data->self_odometry->pose.pose; + const size_t ego_segment_idx = + autoware::motion_utils::findNearestSegmentIndex(current_path.points, current_pose.position); + + const size_t start_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( + current_path.points, pull_over_path.start_pose.position); + const double dist_to_parking_start_pose = calcSignedArcLength( + current_path.points, current_pose.position, ego_segment_idx, pull_over_path.start_pose.position, + start_pose_segment_idx); + if (dist_to_parking_start_pose > parameters.decide_path_distance) { + next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + return next_state; + } + + // if object recognition for path collision check is enabled, transition to DECIDING to check + // collision for a certain period of time. Otherwise, transition to DECIDED directly. + if (parameters.use_object_recognition) { + next_state.state = PathDecisionState::DecisionKind::DECIDED; + return next_state; + } + return {PathDecisionState::DecisionKind::DECIDED, now}; +} + } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index 01247e11aef3e..4749029e04ba3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -15,6 +15,7 @@ #include "autoware/behavior_path_goal_planner_module/util.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware_lanelet2_extension/regulatory_elements/bus_stop_area.hpp" @@ -299,6 +300,80 @@ std::vector getBusStopAreaPolygons(const lanelet::Const return area_polygons; } +bool checkObjectsCollision( + const PathWithLaneId & path, const std::vector & curvatures, + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const BehaviorPathPlannerParameters & behavior_path_parameters, + const double collision_check_margin, const bool extract_static_objects, + const double maximum_deceleration, + const double object_recognition_collision_check_max_extra_stopping_margin, + std::vector & debug_ego_polygons_expanded, const bool update_debug_data) +{ + if (path.points.size() != curvatures.size()) { + RCLCPP_WARN( + rclcpp::get_logger("goal_planner_util"), + "path.points.size() != curvatures.size() in checkObjectsCollision(). judge as non collision"); + return false; + } + + const auto & target_objects = + extract_static_objects ? static_target_objects : dynamic_target_objects; + if (target_objects.objects.empty()) { + return false; + } + + // check collision roughly with {min_distance, max_distance} between ego footprint and objects + // footprint + std::pair has_collision_rough = + utils::path_safety_checker::checkObjectsCollisionRough( + path, target_objects, collision_check_margin, behavior_path_parameters, false); + if (!has_collision_rough.first) { + return false; + } + if (has_collision_rough.second) { + return true; + } + + std::vector obj_polygons; + for (const auto & object : target_objects.objects) { + obj_polygons.push_back(autoware::universe_utils::toPolygon2d(object)); + } + + /* Expand ego collision check polygon + * - `collision_check_margin` is added in all directions. + * - `extra_stopping_margin` adds stopping margin under deceleration constraints forward. + * - `extra_lateral_margin` adds the lateral margin on curves. + */ + std::vector ego_polygons_expanded{}; + for (size_t i = 0; i < path.points.size(); ++i) { + const auto p = path.points.at(i); + const double extra_stopping_margin = std::min( + std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / maximum_deceleration, + object_recognition_collision_check_max_extra_stopping_margin); + + // The square is meant to imply centrifugal force, but it is not a very well-founded formula. + // TODO(kosuke55): It is needed to consider better way because there is an inherently + // different conception of the inside and outside margins. + const double extra_lateral_margin = std::min( + extra_stopping_margin, + std::abs(curvatures.at(i) * std::pow(p.point.longitudinal_velocity_mps, 2))); + + const auto ego_polygon = autoware::universe_utils::toFootprint( + p.point.pose, + behavior_path_parameters.base_link2front + collision_check_margin + extra_stopping_margin, + behavior_path_parameters.base_link2rear + collision_check_margin, + behavior_path_parameters.vehicle_width + collision_check_margin * 2.0 + + extra_lateral_margin * 2.0); + ego_polygons_expanded.push_back(ego_polygon); + } + + if (update_debug_data) { + debug_ego_polygons_expanded = ego_polygons_expanded; + } + + return utils::path_safety_checker::checkPolygonsIntersects(ego_polygons_expanded, obj_polygons); +} + MarkerArray createPullOverAreaMarkerArray( const autoware::universe_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, const std_msgs::msg::ColorRGBA & color, const double z) From bdf7a422e4fb82eb2cfc365b20c1924082f174b6 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 19 Sep 2024 16:21:09 +0900 Subject: [PATCH 13/95] refactor(goal_planner): remove unnecessary GoalPlannerData member (#8920) Signed-off-by: Mamoru Sobue --- .../goal_planner_module.hpp | 11 ++----- .../src/goal_planner_module.cpp | 31 +++++-------------- 2 files changed, 10 insertions(+), 32 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 24ae4342537fb..6c96ca0eafbb6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -446,9 +446,6 @@ class GoalPlannerModule : public SceneModuleInterface initializeOccupancyGridMap(planner_data, parameters); }; GoalPlannerParameters parameters; - std::shared_ptr ego_predicted_path_params; - std::shared_ptr objects_filtering_params; - std::shared_ptr safety_check_params; autoware::universe_utils::LinearRing2d vehicle_footprint; PlannerData planner_data; @@ -464,12 +461,8 @@ class GoalPlannerModule : public SceneModuleInterface void updateOccupancyGrid(); GoalPlannerData clone() const; void update( - const GoalPlannerParameters & parameters, - const std::shared_ptr & ego_predicted_path_params_, - const std::shared_ptr & objects_filtering_params_, - const std::shared_ptr & safety_check_params_, - const PlannerData & planner_data, const ModuleStatus & current_status, - const BehaviorModuleOutput & previous_module_output, + const GoalPlannerParameters & parameters, const PlannerData & planner_data, + const ModuleStatus & current_status, const BehaviorModuleOutput & previous_module_output, const std::shared_ptr goal_searcher_, const autoware::universe_utils::LinearRing2d & vehicle_footprint); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index e92cd5940e06e..ec7a8e12de573 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -209,9 +209,6 @@ void GoalPlannerModule::onTimer() std::optional previous_module_output_opt{std::nullopt}; std::shared_ptr occupancy_grid_map{nullptr}; std::optional parameters_opt{std::nullopt}; - std::shared_ptr ego_predicted_path_params{nullptr}; - std::shared_ptr objects_filtering_params{nullptr}; - std::shared_ptr safety_check_params{nullptr}; std::shared_ptr goal_searcher{nullptr}; // begin of critical section @@ -224,23 +221,18 @@ void GoalPlannerModule::onTimer() previous_module_output_opt = gp_planner_data.previous_module_output; occupancy_grid_map = gp_planner_data.occupancy_grid_map; parameters_opt = gp_planner_data.parameters; - ego_predicted_path_params = gp_planner_data.ego_predicted_path_params; - objects_filtering_params = gp_planner_data.objects_filtering_params; - safety_check_params = gp_planner_data.safety_check_params; goal_searcher = gp_planner_data.goal_searcher; } } // end of critical section if ( !local_planner_data || !current_status_opt || !previous_module_output_opt || - !occupancy_grid_map || !parameters_opt || !ego_predicted_path_params || - !objects_filtering_params || !safety_check_params || !goal_searcher) { + !occupancy_grid_map || !parameters_opt || !goal_searcher) { RCLCPP_ERROR( getLogger(), "failed to get valid " "local_planner_data/current_status/previous_module_output/occupancy_grid_map/parameters_opt/" - "ego_predicted_path_params/objects_filtering_params/safety_check_params/goal_searcher in " - "onTimer"); + "goal_searcher in onTimer"); return; } const auto & current_status = current_status_opt.value(); @@ -514,8 +506,7 @@ void GoalPlannerModule::updateData() // **re-pointing** by `planner_data_->foo = msg` in behavior_path_planner::onCallbackFor(msg) // and if these two coincided, only the reference count is affected gp_planner_data.update( - *parameters_, ego_predicted_path_params_, objects_filtering_params_, safety_check_params_, - *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), goal_searcher_, + *parameters_, *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), goal_searcher_, vehicle_footprint_); // NOTE: RouteHandler holds several shared pointers in it, so just copying PlannerData as // value does not adds the reference counts of RouteHandler.lanelet_map_ptr_ and others. Since @@ -583,6 +574,7 @@ void GoalPlannerModule::updateData() if (!last_approval_data_) { last_approval_data_ = std::make_unique(clock_->now(), planner_data_->self_odometry->pose.pose); + // TODO(soblin): do not "plan" in updateData decideVelocity(); } } @@ -2565,25 +2557,18 @@ GoalPlannerModule::GoalPlannerData GoalPlannerModule::GoalPlannerData::clone() c { GoalPlannerModule::GoalPlannerData gp_planner_data(planner_data, parameters); gp_planner_data.update( - parameters, ego_predicted_path_params, objects_filtering_params, safety_check_params, - planner_data, current_status, previous_module_output, goal_searcher, vehicle_footprint); + parameters, planner_data, current_status, previous_module_output, goal_searcher, + vehicle_footprint); return gp_planner_data; } void GoalPlannerModule::GoalPlannerData::update( - const GoalPlannerParameters & parameters_, - const std::shared_ptr & ego_predicted_path_params_, - const std::shared_ptr & objects_filtering_params_, - const std::shared_ptr & safety_check_params_, - const PlannerData & planner_data_, const ModuleStatus & current_status_, - const BehaviorModuleOutput & previous_module_output_, + const GoalPlannerParameters & parameters_, const PlannerData & planner_data_, + const ModuleStatus & current_status_, const BehaviorModuleOutput & previous_module_output_, const std::shared_ptr goal_searcher_, const autoware::universe_utils::LinearRing2d & vehicle_footprint_) { parameters = parameters_; - ego_predicted_path_params = ego_predicted_path_params_; - objects_filtering_params = objects_filtering_params_; - safety_check_params = safety_check_params_; vehicle_footprint = vehicle_footprint_; planner_data = planner_data_; From f8dc16fe97a7b2801124ca800a11735ab069b714 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Thu, 19 Sep 2024 17:10:47 +0900 Subject: [PATCH 14/95] refactor(localization_util)!: prefix package and namespace with autoware (#8922) add autoware prefix to localization_util Signed-off-by: a-maumau --- .github/CODEOWNERS | 2 +- .../autoware_ekf_localizer/package.xml | 2 +- .../src/ekf_localizer.cpp | 2 +- .../autoware_gyro_odometer/package.xml | 2 +- .../src/gyro_odometer_core.cpp | 3 +- .../src/gyro_odometer_core.hpp | 4 +- .../package.xml | 2 +- .../src/ar_tag_based_localizer.cpp | 8 ++-- .../src/ar_tag_based_localizer.hpp | 4 +- .../autoware_landmark_manager/package.xml | 2 +- .../src/landmark_manager.cpp | 10 +++-- .../package.xml | 2 +- .../src/lidar_marker_localizer.cpp | 11 ++--- .../src/lidar_marker_localizer.hpp | 8 ++-- .../package.xml | 2 +- .../src/localization_error_monitor.cpp | 3 +- .../src/localization_error_monitor.hpp | 6 +-- .../CMakeLists.txt | 4 +- .../autoware_localization_util/README.md | 5 +++ .../localization_util/covariance_ellipse.hpp | 6 +-- .../localization_util/diagnostics_module.hpp | 10 +++-- .../localization_util/matrix_type.hpp | 9 ++-- .../localization_util/smart_pose_buffer.hpp | 11 +++-- .../tree_structured_parzen_estimator.hpp | 9 ++-- .../autoware}/localization_util/util_func.hpp | 10 +++-- .../package.xml | 4 +- .../src/covariance_ellipse.cpp | 2 +- .../src/diagnostics_module.cpp | 5 ++- .../src/smart_pose_buffer.cpp | 5 ++- .../src/tree_structured_parzen_estimator.cpp | 5 ++- .../src/util_func.cpp | 7 ++- .../test/test_smart_pose_buffer.cpp | 13 +++--- .../test/test_tpe.cpp | 4 +- .../ndt_scan_matcher/map_update_module.hpp | 5 ++- .../ndt_scan_matcher_core.hpp | 8 ++-- .../autoware_ndt_scan_matcher/package.xml | 2 +- .../src/ndt_scan_matcher_core.cpp | 45 ++++++++++++------- .../src/particle.cpp | 4 +- .../autoware_pose_initializer/package.xml | 2 +- .../src/pose_initializer_core.cpp | 3 +- .../src/pose_initializer_core.hpp | 4 +- localization/localization_util/README.md | 5 --- 42 files changed, 157 insertions(+), 103 deletions(-) rename localization/{localization_util => autoware_localization_util}/CMakeLists.txt (87%) create mode 100644 localization/autoware_localization_util/README.md rename localization/{localization_util/include => autoware_localization_util/include/autoware}/localization_util/covariance_ellipse.hpp (87%) rename localization/{localization_util/include => autoware_localization_util/include/autoware}/localization_util/diagnostics_module.hpp (87%) rename localization/{localization_util/include => autoware_localization_util/include/autoware}/localization_util/matrix_type.hpp (75%) rename localization/{localization_util/include => autoware_localization_util/include/autoware}/localization_util/smart_pose_buffer.hpp (86%) rename localization/{localization_util/include => autoware_localization_util/include/autoware}/localization_util/tree_structured_parzen_estimator.hpp (87%) rename localization/{localization_util/include => autoware_localization_util/include/autoware}/localization_util/util_func.hpp (92%) rename localization/{localization_util => autoware_localization_util}/package.xml (92%) rename localization/{localization_util => autoware_localization_util}/src/covariance_ellipse.cpp (98%) rename localization/{localization_util => autoware_localization_util}/src/diagnostics_module.cpp (95%) rename localization/{localization_util => autoware_localization_util}/src/smart_pose_buffer.cpp (97%) rename localization/{localization_util => autoware_localization_util}/src/tree_structured_parzen_estimator.cpp (97%) rename localization/{localization_util => autoware_localization_util}/src/util_func.cpp (97%) rename localization/{localization_util => autoware_localization_util}/test/test_smart_pose_buffer.cpp (88%) rename localization/{localization_util => autoware_localization_util}/test/test_tpe.cpp (93%) delete mode 100644 localization/localization_util/README.md diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index d009a73809074..07b35e3e33045 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -91,7 +91,7 @@ localization/autoware_pose_initializer/** anh.nguyen.2@tier4.jp isamu.takagi@tie localization/autoware_pose_instability_detector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/autoware_localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_common/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp diff --git a/localization/autoware_ekf_localizer/package.xml b/localization/autoware_ekf_localizer/package.xml index 7f33fb0b946d1..e0f4c9319d0d5 100644 --- a/localization/autoware_ekf_localizer/package.xml +++ b/localization/autoware_ekf_localizer/package.xml @@ -23,11 +23,11 @@ eigen autoware_kalman_filter + autoware_localization_util autoware_universe_utils diagnostic_msgs fmt geometry_msgs - localization_util nav_msgs rclcpp rclcpp_components diff --git a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp b/localization/autoware_ekf_localizer/src/ekf_localizer.cpp index f678412a25438..1b747c750977c 100644 --- a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp +++ b/localization/autoware_ekf_localizer/src/ekf_localizer.cpp @@ -17,7 +17,7 @@ #include "autoware/ekf_localizer/diagnostics.hpp" #include "autoware/ekf_localizer/string.hpp" #include "autoware/ekf_localizer/warning_message.hpp" -#include "localization_util/covariance_ellipse.hpp" +#include "autoware/localization_util/covariance_ellipse.hpp" #include #include diff --git a/localization/autoware_gyro_odometer/package.xml b/localization/autoware_gyro_odometer/package.xml index 0e78d0dea51b6..77de1d582fc0f 100644 --- a/localization/autoware_gyro_odometer/package.xml +++ b/localization/autoware_gyro_odometer/package.xml @@ -17,11 +17,11 @@ ament_cmake_auto autoware_cmake + autoware_localization_util autoware_universe_utils diagnostic_msgs fmt geometry_msgs - localization_util rclcpp rclcpp_components sensor_msgs diff --git a/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp index bc2abb4a8a321..9511f168f346f 100644 --- a/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp @@ -72,7 +72,8 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) twist_with_covariance_pub_ = create_publisher( "twist_with_covariance", rclcpp::QoS{10}); - diagnostics_ = std::make_unique(this, "gyro_odometer_status"); + diagnostics_ = + std::make_unique(this, "gyro_odometer_status"); // TODO(YamatoAndo) createTimer } diff --git a/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp index 1b2c4246a037a..036b3d7cab527 100644 --- a/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp @@ -15,10 +15,10 @@ #ifndef GYRO_ODOMETER_CORE_HPP_ #define GYRO_ODOMETER_CORE_HPP_ +#include "autoware/localization_util/diagnostics_module.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" #include "autoware/universe_utils/ros/transform_listener.hpp" -#include "localization_util/diagnostics_module.hpp" #include @@ -80,7 +80,7 @@ class GyroOdometerNode : public rclcpp::Node std::deque vehicle_twist_queue_; std::deque gyro_queue_; - std::unique_ptr diagnostics_; + std::unique_ptr diagnostics_; }; } // namespace autoware::gyro_odometer diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml index e8597a6fc9a4e..4f6726abb33ef 100644 --- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml @@ -19,9 +19,9 @@ ament_index_cpp aruco autoware_landmark_manager + autoware_localization_util cv_bridge diagnostic_msgs - localization_util rclcpp rclcpp_components tf2_eigen diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index 80845402a59f3..2418cf3f5dd1c 100644 --- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -44,7 +44,7 @@ #include "ar_tag_based_localizer.hpp" -#include "localization_util/util_func.hpp" +#include "autoware/localization_util/util_func.hpp" #include #include @@ -94,7 +94,7 @@ ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) RCLCPP_ERROR_STREAM(this->get_logger(), "Invalid detection_mode: " << detection_mode); return; } - ekf_pose_buffer_ = std::make_unique( + ekf_pose_buffer_ = std::make_unique( this->get_logger(), ekf_time_tolerance_, ekf_position_tolerance_); /* @@ -168,8 +168,8 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) const builtin_interfaces::msg::Time sensor_stamp = msg->header.stamp; // get self pose - const std::optional interpolate_result = - ekf_pose_buffer_->interpolate(sensor_stamp); + const std::optional + interpolate_result = ekf_pose_buffer_->interpolate(sensor_stamp); if (!interpolate_result) { return; } diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.hpp b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.hpp index 9826c04e3a86f..ddfdf852be2b5 100644 --- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.hpp +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.hpp @@ -46,7 +46,7 @@ #define AR_TAG_BASED_LOCALIZER_HPP_ #include "autoware/landmark_manager/landmark_manager.hpp" -#include "localization_util/smart_pose_buffer.hpp" +#include "autoware/localization_util/smart_pose_buffer.hpp" #include @@ -122,7 +122,7 @@ class ArTagBasedLocalizer : public rclcpp::Node aruco::MarkerDetector detector_; aruco::CameraParameters cam_param_; bool cam_info_received_; - std::unique_ptr ekf_pose_buffer_; + std::unique_ptr ekf_pose_buffer_; landmark_manager::LandmarkManager landmark_manager_; }; diff --git a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml index d69adc7904411..43ef73b8c5226 100644 --- a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml @@ -19,10 +19,10 @@ eigen autoware_lanelet2_extension + autoware_localization_util autoware_map_msgs autoware_universe_utils geometry_msgs - localization_util rclcpp tf2_eigen diff --git a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp index 488f250e325d2..04823a71febe9 100644 --- a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp @@ -14,9 +14,9 @@ #include "autoware/landmark_manager/landmark_manager.hpp" +#include "autoware/localization_util/util_func.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware_lanelet2_extension/utility/message_conversion.hpp" -#include "localization_util/util_func.hpp" #include #include @@ -178,12 +178,14 @@ geometry_msgs::msg::Pose LandmarkManager::calculate_new_self_pose( } if (consider_orientation) { - const Eigen::Affine3d landmark_pose = pose_to_affine3d(mapped_landmark_on_map); + const Eigen::Affine3d landmark_pose = + autoware::localization_util::pose_to_affine3d(mapped_landmark_on_map); const Eigen::Affine3d landmark_to_base_link = - pose_to_affine3d(detected_landmark_on_base_link).inverse(); + autoware::localization_util::pose_to_affine3d(detected_landmark_on_base_link).inverse(); const Eigen::Affine3d new_self_pose_eigen = landmark_pose * landmark_to_base_link; - const Pose new_self_pose = matrix4f_to_pose(new_self_pose_eigen.matrix().cast()); + const Pose new_self_pose = + autoware::localization_util::matrix4f_to_pose(new_self_pose_eigen.matrix().cast()); // update min_distance = curr_distance; diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml index bf60d96311ff1..2b19b46b55424 100755 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml @@ -15,10 +15,10 @@ autoware_cmake autoware_landmark_manager + autoware_localization_util autoware_map_msgs autoware_point_types autoware_universe_utils - localization_util pcl_conversions pcl_ros rclcpp diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp index 83fcae2352f51..06d2acf8a55fc 100644 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp @@ -81,7 +81,7 @@ LidarMarkerLocalizer::LidarMarkerLocalizer(const rclcpp::NodeOptions & node_opti param_.save_file_name = this->declare_parameter("save_file_name"); param_.save_frame_id = this->declare_parameter("save_frame_id"); - ekf_pose_buffer_ = std::make_unique( + ekf_pose_buffer_ = std::make_unique( this->get_logger(), param_.self_pose_timeout_sec, param_.self_pose_distance_tolerance_m); rclcpp::CallbackGroup::SharedPtr points_callback_group; @@ -122,7 +122,8 @@ LidarMarkerLocalizer::LidarMarkerLocalizer(const rclcpp::NodeOptions & node_opti tf_buffer_ = std::make_shared(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_, this, false); - diagnostics_module_.reset(new DiagnosticsModule(this, "marker_detection_status")); + diagnostics_module_.reset( + new autoware::localization_util::DiagnosticsModule(this, "marker_detection_status")); } void LidarMarkerLocalizer::initialize_diagnostics() @@ -195,8 +196,8 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin } // (2) get Self Pose - const std::optional interpolate_result = - ekf_pose_buffer_->interpolate(sensor_ros_time); + const std::optional + interpolate_result = ekf_pose_buffer_->interpolate(sensor_ros_time); const bool is_received_self_pose = interpolate_result != std::nullopt; diagnostics_module_->add_key_value("is_received_self_pose", is_received_self_pose); @@ -607,7 +608,7 @@ void LidarMarkerLocalizer::transform_sensor_measurement( const geometry_msgs::msg::PoseStamped target_to_source_pose_stamped = autoware::universe_utils::transform2pose(transform); const Eigen::Matrix4f base_to_sensor_matrix = - pose_to_matrix4f(target_to_source_pose_stamped.pose); + autoware::localization_util::pose_to_matrix4f(target_to_source_pose_stamped.pose); pcl_ros::transformPointCloud( base_to_sensor_matrix, *sensor_points_input_ptr, *sensor_points_output_ptr); } diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp index 11a6e0b34abdc..d1482c6912592 100644 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp @@ -15,8 +15,8 @@ #ifndef LIDAR_MARKER_LOCALIZER_HPP_ #define LIDAR_MARKER_LOCALIZER_HPP_ -#include "localization_util/diagnostics_module.hpp" -#include "localization_util/smart_pose_buffer.hpp" +#include "autoware/localization_util/diagnostics_module.hpp" +#include "autoware/localization_util/smart_pose_buffer.hpp" #include @@ -134,11 +134,11 @@ class LidarMarkerLocalizer : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_debug_pose_with_covariance_; rclcpp::Publisher::SharedPtr pub_marker_pointcloud_; - std::shared_ptr diagnostics_module_; + std::shared_ptr diagnostics_module_; Param param_; bool is_activated_; - std::unique_ptr ekf_pose_buffer_; + std::unique_ptr ekf_pose_buffer_; landmark_manager::LandmarkManager landmark_manager_; }; diff --git a/localization/autoware_localization_error_monitor/package.xml b/localization/autoware_localization_error_monitor/package.xml index 426b0d6b6c432..463e7aee347d7 100644 --- a/localization/autoware_localization_error_monitor/package.xml +++ b/localization/autoware_localization_error_monitor/package.xml @@ -19,9 +19,9 @@ autoware_cmake eigen + autoware_localization_util autoware_universe_utils diagnostic_msgs - localization_util nav_msgs rclcpp_components tf2 diff --git a/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp b/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp index 3f9a02834c74d..63edbe5f6a9c7 100644 --- a/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp +++ b/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp @@ -58,7 +58,8 @@ LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & o logger_configure_ = std::make_unique(this); - diagnostics_error_monitor_ = std::make_unique(this, "ellipse_error_status"); + diagnostics_error_monitor_ = + std::make_unique(this, "ellipse_error_status"); } void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg) diff --git a/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp b/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp index 49abacfa60f38..7b26573964b38 100644 --- a/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp +++ b/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp @@ -15,8 +15,8 @@ #ifndef LOCALIZATION_ERROR_MONITOR_HPP_ #define LOCALIZATION_ERROR_MONITOR_HPP_ -#include "localization_util/covariance_ellipse.hpp" -#include "localization_util/diagnostics_module.hpp" +#include "autoware/localization_util/covariance_ellipse.hpp" +#include "autoware/localization_util/diagnostics_module.hpp" #include #include @@ -39,7 +39,7 @@ class LocalizationErrorMonitor : public rclcpp::Node std::unique_ptr logger_configure_; - std::unique_ptr diagnostics_error_monitor_; + std::unique_ptr diagnostics_error_monitor_; double scale_; double error_ellipse_size_; diff --git a/localization/localization_util/CMakeLists.txt b/localization/autoware_localization_util/CMakeLists.txt similarity index 87% rename from localization/localization_util/CMakeLists.txt rename to localization/autoware_localization_util/CMakeLists.txt index 2e4d4b321ec54..dd18f3cbad932 100644 --- a/localization/localization_util/CMakeLists.txt +++ b/localization/autoware_localization_util/CMakeLists.txt @@ -1,10 +1,10 @@ cmake_minimum_required(VERSION 3.14) -project(localization_util) +project(autoware_localization_util) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(localization_util SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/util_func.cpp src/diagnostics_module.cpp src/smart_pose_buffer.cpp diff --git a/localization/autoware_localization_util/README.md b/localization/autoware_localization_util/README.md new file mode 100644 index 0000000000000..f7fddd9eebf05 --- /dev/null +++ b/localization/autoware_localization_util/README.md @@ -0,0 +1,5 @@ +# autoware_localization_util + +`autoware_localization_util` is a localization utility package. + +This package does not have a node, it is just a library. diff --git a/localization/localization_util/include/localization_util/covariance_ellipse.hpp b/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp similarity index 87% rename from localization/localization_util/include/localization_util/covariance_ellipse.hpp rename to localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp index 1f187d411dc63..abd0af46856b0 100644 --- a/localization/localization_util/include/localization_util/covariance_ellipse.hpp +++ b/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ -#define LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ +#ifndef AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ +#define AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ #include @@ -41,4 +41,4 @@ visualization_msgs::msg::Marker create_ellipse_marker( } // namespace autoware::localization_util -#endif // LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ +#endif // AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ diff --git a/localization/localization_util/include/localization_util/diagnostics_module.hpp b/localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp similarity index 87% rename from localization/localization_util/include/localization_util/diagnostics_module.hpp rename to localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp index 0ec52cfe814af..8c19c94127630 100644 --- a/localization/localization_util/include/localization_util/diagnostics_module.hpp +++ b/localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ -#define LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ +#ifndef AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ +#define AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ #include @@ -22,6 +22,8 @@ #include #include +namespace autoware::localization_util +{ class DiagnosticsModule { public: @@ -57,4 +59,6 @@ void DiagnosticsModule::add_key_value(const std::string & key, const std::string template <> void DiagnosticsModule::add_key_value(const std::string & key, const bool & value); -#endif // LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ +} // namespace autoware::localization_util + +#endif // AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ diff --git a/localization/localization_util/include/localization_util/matrix_type.hpp b/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp similarity index 75% rename from localization/localization_util/include/localization_util/matrix_type.hpp rename to localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp index d9ec9b369127a..bda6ff19f2867 100644 --- a/localization/localization_util/include/localization_util/matrix_type.hpp +++ b/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ -#define LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ +#ifndef AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ +#define AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ #include +namespace autoware::localization_util +{ using Matrix6d = Eigen::Matrix; using RowMatrixXd = Eigen::Matrix; +} // namespace autoware::localization_util -#endif // LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ +#endif // AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ diff --git a/localization/localization_util/include/localization_util/smart_pose_buffer.hpp b/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp similarity index 86% rename from localization/localization_util/include/localization_util/smart_pose_buffer.hpp rename to localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp index 2a2a775a9280c..8c10506c36753 100644 --- a/localization/localization_util/include/localization_util/smart_pose_buffer.hpp +++ b/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ -#define LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ +#ifndef AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ +#define AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ -#include "localization_util/util_func.hpp" +#include "autoware/localization_util/util_func.hpp" #include @@ -23,6 +23,8 @@ #include +namespace autoware::localization_util +{ class SmartPoseBuffer { private: @@ -64,5 +66,6 @@ class SmartPoseBuffer const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const; }; +} // namespace autoware::localization_util -#endif // LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ +#endif // AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ diff --git a/localization/localization_util/include/localization_util/tree_structured_parzen_estimator.hpp b/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp similarity index 87% rename from localization/localization_util/include/localization_util/tree_structured_parzen_estimator.hpp rename to localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp index ee25ac175c0b4..ddf7625c202ec 100644 --- a/localization/localization_util/include/localization_util/tree_structured_parzen_estimator.hpp +++ b/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ -#define LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ +#ifndef AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ +#define AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ /* A implementation of tree-structured parzen estimator (TPE) @@ -28,6 +28,8 @@ Optuna is also used as a reference for implementation. #include #include +namespace autoware::localization_util +{ class TreeStructuredParzenEstimator { public: @@ -80,5 +82,6 @@ class TreeStructuredParzenEstimator const std::vector sample_stddev_; Input base_stddev_; }; +} // namespace autoware::localization_util -#endif // LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ +#endif // AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ diff --git a/localization/localization_util/include/localization_util/util_func.hpp b/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp similarity index 92% rename from localization/localization_util/include/localization_util/util_func.hpp rename to localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp index 0b31333da44d3..bef9968f34b6f 100644 --- a/localization/localization_util/include/localization_util/util_func.hpp +++ b/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_UTIL__UTIL_FUNC_HPP_ -#define LOCALIZATION_UTIL__UTIL_FUNC_HPP_ +#ifndef AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ +#define AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ #include #include @@ -36,6 +36,8 @@ #include #include +namespace autoware::localization_util +{ // ref by http://takacity.blog.fc2.com/blog-entry-69.html std_msgs::msg::ColorRGBA exchange_color_crc(double x); @@ -81,4 +83,6 @@ void output_pose_with_cov_to_log( const rclcpp::Logger & logger, const std::string & prefix, const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov); -#endif // LOCALIZATION_UTIL__UTIL_FUNC_HPP_ +} // namespace autoware::localization_util + +#endif // AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ diff --git a/localization/localization_util/package.xml b/localization/autoware_localization_util/package.xml similarity index 92% rename from localization/localization_util/package.xml rename to localization/autoware_localization_util/package.xml index 86c74bb92f2f8..48abf542ddd07 100644 --- a/localization/localization_util/package.xml +++ b/localization/autoware_localization_util/package.xml @@ -1,9 +1,9 @@ - localization_util + autoware_localization_util 0.1.0 - The localization_util package + The autoware_localization_util package Yamato Ando Masahiro Sakamoto Shintaro Sakoda diff --git a/localization/localization_util/src/covariance_ellipse.cpp b/localization/autoware_localization_util/src/covariance_ellipse.cpp similarity index 98% rename from localization/localization_util/src/covariance_ellipse.cpp rename to localization/autoware_localization_util/src/covariance_ellipse.cpp index 885ce2dcee19c..4a7d71909fb7a 100644 --- a/localization/localization_util/src/covariance_ellipse.cpp +++ b/localization/autoware_localization_util/src/covariance_ellipse.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "localization_util/covariance_ellipse.hpp" +#include "autoware/localization_util/covariance_ellipse.hpp" #include #ifdef ROS_DISTRO_GALACTIC diff --git a/localization/localization_util/src/diagnostics_module.cpp b/localization/autoware_localization_util/src/diagnostics_module.cpp similarity index 95% rename from localization/localization_util/src/diagnostics_module.cpp rename to localization/autoware_localization_util/src/diagnostics_module.cpp index fb9e122a71e24..2b68dbf4f5890 100644 --- a/localization/localization_util/src/diagnostics_module.cpp +++ b/localization/autoware_localization_util/src/diagnostics_module.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "localization_util/diagnostics_module.hpp" +#include "autoware/localization_util/diagnostics_module.hpp" #include @@ -21,6 +21,8 @@ #include #include +namespace autoware::localization_util +{ DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name) : clock_(node->get_clock()) { @@ -103,3 +105,4 @@ diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_arra return diagnostics_msg; } +} // namespace autoware::localization_util diff --git a/localization/localization_util/src/smart_pose_buffer.cpp b/localization/autoware_localization_util/src/smart_pose_buffer.cpp similarity index 97% rename from localization/localization_util/src/smart_pose_buffer.cpp rename to localization/autoware_localization_util/src/smart_pose_buffer.cpp index 201c743471efd..3b529d68cf6c5 100644 --- a/localization/localization_util/src/smart_pose_buffer.cpp +++ b/localization/autoware_localization_util/src/smart_pose_buffer.cpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "localization_util/smart_pose_buffer.hpp" +#include "autoware/localization_util/smart_pose_buffer.hpp" +namespace autoware::localization_util +{ SmartPoseBuffer::SmartPoseBuffer( const rclcpp::Logger & logger, const double & pose_timeout_sec, const double & pose_distance_tolerance_meters) @@ -153,3 +155,4 @@ bool SmartPoseBuffer::validate_position_difference( } return success; } +} // namespace autoware::localization_util diff --git a/localization/localization_util/src/tree_structured_parzen_estimator.cpp b/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp similarity index 97% rename from localization/localization_util/src/tree_structured_parzen_estimator.cpp rename to localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp index 8d594310d79bc..064b33ebe3ad9 100644 --- a/localization/localization_util/src/tree_structured_parzen_estimator.cpp +++ b/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp @@ -12,13 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "localization_util/tree_structured_parzen_estimator.hpp" +#include "autoware/localization_util/tree_structured_parzen_estimator.hpp" #include #include #include #include +namespace autoware::localization_util +{ // random number generator std::mt19937_64 TreeStructuredParzenEstimator::engine(0); @@ -177,3 +179,4 @@ double TreeStructuredParzenEstimator::log_gaussian_pdf( } return result; } +} // namespace autoware::localization_util diff --git a/localization/localization_util/src/util_func.cpp b/localization/autoware_localization_util/src/util_func.cpp similarity index 97% rename from localization/localization_util/src/util_func.cpp rename to localization/autoware_localization_util/src/util_func.cpp index 133442df68dcd..6b019f7750471 100644 --- a/localization/localization_util/src/util_func.cpp +++ b/localization/autoware_localization_util/src/util_func.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "localization_util/util_func.hpp" +#include "autoware/localization_util/util_func.hpp" -#include "localization_util/matrix_type.hpp" +#include "autoware/localization_util/matrix_type.hpp" +namespace autoware::localization_util +{ // ref by http://takacity.blog.fc2.com/blog-entry-69.html std_msgs::msg::ColorRGBA exchange_color_crc(double x) { @@ -249,3 +251,4 @@ void output_pose_with_cov_to_log( << covariance(1, 1) << "," << covariance(2, 2) << "," << covariance(3, 3) << "," << covariance(4, 4) << "," << covariance(5, 5)); } +} // namespace autoware::localization_util diff --git a/localization/localization_util/test/test_smart_pose_buffer.cpp b/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp similarity index 88% rename from localization/localization_util/test/test_smart_pose_buffer.cpp rename to localization/autoware_localization_util/test/test_smart_pose_buffer.cpp index a8ed6d98b6064..d55555682be84 100644 --- a/localization/localization_util/test/test_smart_pose_buffer.cpp +++ b/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "localization_util/smart_pose_buffer.hpp" -#include "localization_util/util_func.hpp" +#include "autoware/localization_util/smart_pose_buffer.hpp" +#include "autoware/localization_util/util_func.hpp" #include #include @@ -26,6 +26,7 @@ #include using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; +using SmartPoseBuffer = autoware::localization_util::SmartPoseBuffer; bool compare_pose( const PoseWithCovarianceStamped & pose_a, const PoseWithCovarianceStamped & pose_b) @@ -56,7 +57,8 @@ TEST(TestSmartPoseBuffer, interpolate_pose) // NOLINT old_pose_ptr->pose.pose.position.x = 10.0; old_pose_ptr->pose.pose.position.y = 20.0; old_pose_ptr->pose.pose.position.z = 0.0; - old_pose_ptr->pose.pose.orientation = rpy_deg_to_quaternion(0.0, 0.0, 0.0); + old_pose_ptr->pose.pose.orientation = + autoware::localization_util::rpy_deg_to_quaternion(0.0, 0.0, 0.0); smart_pose_buffer.push_back(old_pose_ptr); // second data @@ -66,7 +68,8 @@ TEST(TestSmartPoseBuffer, interpolate_pose) // NOLINT new_pose_ptr->pose.pose.position.x = 20.0; new_pose_ptr->pose.pose.position.y = 40.0; new_pose_ptr->pose.pose.position.z = 0.0; - new_pose_ptr->pose.pose.orientation = rpy_deg_to_quaternion(0.0, 0.0, 90.0); + new_pose_ptr->pose.pose.orientation = + autoware::localization_util::rpy_deg_to_quaternion(0.0, 0.0, 90.0); smart_pose_buffer.push_back(new_pose_ptr); // interpolate @@ -90,7 +93,7 @@ TEST(TestSmartPoseBuffer, interpolate_pose) // NOLINT EXPECT_EQ(result.interpolated_pose.pose.pose.position.x, 15.0); EXPECT_EQ(result.interpolated_pose.pose.pose.position.y, 30.0); EXPECT_EQ(result.interpolated_pose.pose.pose.position.z, 0.0); - const auto rpy = get_rpy(result.interpolated_pose.pose.pose); + const auto rpy = autoware::localization_util::get_rpy(result.interpolated_pose.pose.pose); EXPECT_NEAR(rpy.x * 180 / M_PI, 0.0, 1e-6); EXPECT_NEAR(rpy.y * 180 / M_PI, 0.0, 1e-6); EXPECT_NEAR(rpy.z * 180 / M_PI, 45.0, 1e-6); diff --git a/localization/localization_util/test/test_tpe.cpp b/localization/autoware_localization_util/test/test_tpe.cpp similarity index 93% rename from localization/localization_util/test/test_tpe.cpp rename to localization/autoware_localization_util/test/test_tpe.cpp index fd9afe8b2a75f..6cbe3c2a62571 100644 --- a/localization/localization_util/test/test_tpe.cpp +++ b/localization/autoware_localization_util/test/test_tpe.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "localization_util/tree_structured_parzen_estimator.hpp" +#include "autoware/localization_util/tree_structured_parzen_estimator.hpp" #include +using TreeStructuredParzenEstimator = autoware::localization_util::TreeStructuredParzenEstimator; + TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphere_function) { auto sphere_function = [](const TreeStructuredParzenEstimator::Input & input) { diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp index 8e7bf78845674..9b140977f4971 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp @@ -15,10 +15,10 @@ #ifndef AUTOWARE__NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ #define AUTOWARE__NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ +#include "autoware/localization_util/diagnostics_module.hpp" +#include "autoware/localization_util/util_func.hpp" #include "autoware/ndt_scan_matcher/hyper_parameters.hpp" #include "autoware/ndt_scan_matcher/particle.hpp" -#include "localization_util/diagnostics_module.hpp" -#include "localization_util/util_func.hpp" #include #include @@ -42,6 +42,7 @@ namespace autoware::ndt_scan_matcher { +using DiagnosticsModule = autoware::localization_util::DiagnosticsModule; class MapUpdateModule { diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp index b4062535503c1..c47af70f4c189 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -17,10 +17,10 @@ #define FMT_HEADER_ONLY +#include "autoware/localization_util/diagnostics_module.hpp" +#include "autoware/localization_util/smart_pose_buffer.hpp" #include "autoware/ndt_scan_matcher/hyper_parameters.hpp" #include "autoware/ndt_scan_matcher/map_update_module.hpp" -#include "localization_util/diagnostics_module.hpp" -#include "localization_util/smart_pose_buffer.hpp" #include #include @@ -202,13 +202,13 @@ class NDTScanMatcher : public rclcpp::Node Eigen::Matrix4f base_to_sensor_matrix_; std::mutex ndt_ptr_mtx_; - std::unique_ptr initial_pose_buffer_; + std::unique_ptr initial_pose_buffer_; // Keep latest position for dynamic map loading std::mutex latest_ekf_position_mtx_; std::optional latest_ekf_position_ = std::nullopt; - std::unique_ptr regularization_pose_buffer_; + std::unique_ptr regularization_pose_buffer_; std::atomic is_activated_; std::unique_ptr diagnostics_scan_points_; diff --git a/localization/autoware_ndt_scan_matcher/package.xml b/localization/autoware_ndt_scan_matcher/package.xml index f27cd63ff523e..a4e5d71fa5c59 100644 --- a/localization/autoware_ndt_scan_matcher/package.xml +++ b/localization/autoware_ndt_scan_matcher/package.xml @@ -17,13 +17,13 @@ ament_cmake_auto autoware_cmake + autoware_localization_util autoware_map_msgs autoware_universe_utils diagnostic_msgs fmt geometry_msgs libpcl-all-dev - localization_util nav_msgs ndt_omp pcl_conversions diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index e9619375dc60a..8cc397c4531b5 100644 --- a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -14,10 +14,10 @@ #include "autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp" +#include "autoware/localization_util/matrix_type.hpp" +#include "autoware/localization_util/tree_structured_parzen_estimator.hpp" +#include "autoware/localization_util/util_func.hpp" #include "autoware/ndt_scan_matcher/particle.hpp" -#include "localization_util/matrix_type.hpp" -#include "localization_util/tree_structured_parzen_estimator.hpp" -#include "localization_util/util_func.hpp" #include #include @@ -39,6 +39,14 @@ namespace autoware::ndt_scan_matcher { +using autoware::localization_util::exchange_color_crc; +using autoware::localization_util::matrix4f_to_pose; +using autoware::localization_util::point_to_vector3d; +using autoware::localization_util::pose_to_matrix4f; + +using autoware::localization_util::DiagnosticsModule; +using autoware::localization_util::SmartPoseBuffer; +using autoware::localization_util::TreeStructuredParzenEstimator; tier4_debug_msgs::msg::Float32Stamped make_float32_stamped( const builtin_interfaces::msg::Time & stamp, const float data) @@ -593,8 +601,8 @@ bool NDTScanMatcher::callback_sensor_points_main( } // check distance_initial_to_result - const auto distance_initial_to_result = static_cast( - norm(interpolation_result.interpolated_pose.pose.pose.position, result_pose_msg.position)); + const auto distance_initial_to_result = static_cast(autoware::localization_util::norm( + interpolation_result.interpolated_pose.pose.pose.position, result_pose_msg.position)); diagnostics_scan_points_->add_key_value("distance_initial_to_result", distance_initial_to_result); if (distance_initial_to_result > param_.validation.initial_to_result_distance_tolerance_m) { std::stringstream message; @@ -798,18 +806,18 @@ void NDTScanMatcher::publish_initial_to_result( initial_to_result_relative_pose_stamped.header.frame_id = param_.frame.map_frame; initial_to_result_relative_pose_pub_->publish(initial_to_result_relative_pose_stamped); - const auto initial_to_result_distance = - static_cast(norm(initial_pose_cov_msg.pose.pose.position, result_pose_msg.position)); + const auto initial_to_result_distance = static_cast(autoware::localization_util::norm( + initial_pose_cov_msg.pose.pose.position, result_pose_msg.position)); initial_to_result_distance_pub_->publish( make_float32_stamped(sensor_ros_time, initial_to_result_distance)); - const auto initial_to_result_distance_old = - static_cast(norm(initial_pose_old_msg.pose.pose.position, result_pose_msg.position)); + const auto initial_to_result_distance_old = static_cast(autoware::localization_util::norm( + initial_pose_old_msg.pose.pose.position, result_pose_msg.position)); initial_to_result_distance_old_pub_->publish( make_float32_stamped(sensor_ros_time, initial_to_result_distance_old)); - const auto initial_to_result_distance_new = - static_cast(norm(initial_pose_new_msg.pose.pose.position, result_pose_msg.position)); + const auto initial_to_result_distance_new = static_cast(autoware::localization_util::norm( + initial_pose_new_msg.pose.pose.position, result_pose_msg.position)); initial_to_result_distance_new_pub_->publish( make_float32_stamped(sensor_ros_time, initial_to_result_distance_new)); } @@ -1010,7 +1018,8 @@ void NDTScanMatcher::service_ndt_align_main( diagnostics_ndt_align_->add_key_value("is_succeed_transform_initial_pose", true); // transform pose_frame to map_frame - auto initial_pose_msg_in_map_frame = transform(req->pose_with_covariance, transform_s2t); + auto initial_pose_msg_in_map_frame = + autoware::localization_util::transform(req->pose_with_covariance, transform_s2t); initial_pose_msg_in_map_frame.header.stamp = req->pose_with_covariance.header.stamp; map_update_module_->update_map( initial_pose_msg_in_map_frame.pose.pose.position, diagnostics_ndt_align_); @@ -1062,10 +1071,11 @@ void NDTScanMatcher::service_ndt_align_main( std::tuple NDTScanMatcher::align_pose( const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov) { - output_pose_with_cov_to_log(get_logger(), "align_pose_input", initial_pose_with_cov); + autoware::localization_util::output_pose_with_cov_to_log( + get_logger(), "align_pose_input", initial_pose_with_cov); - const auto base_rpy = get_rpy(initial_pose_with_cov); - const Eigen::Map covariance = { + const auto base_rpy = autoware::localization_util::get_rpy(initial_pose_with_cov); + const Eigen::Map covariance = { initial_pose_with_cov.pose.covariance.data(), 6, 6}; const double stddev_x = std::sqrt(covariance(0, 0)); const double stddev_y = std::sqrt(covariance(1, 1)); @@ -1127,7 +1137,7 @@ std::tuple NDTScanMatcher } const geometry_msgs::msg::Pose pose = matrix4f_to_pose(ndt_result.pose); - const geometry_msgs::msg::Vector3 rpy = get_rpy(pose); + const geometry_msgs::msg::Vector3 rpy = autoware::localization_util::get_rpy(pose); TreeStructuredParzenEstimator::Input result(6); result[0] = pose.position.x; @@ -1154,7 +1164,8 @@ std::tuple NDTScanMatcher result_pose_with_cov_msg.header.frame_id = param_.frame.map_frame; result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose; - output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg); + autoware::localization_util::output_pose_with_cov_to_log( + get_logger(), "align_pose_output", result_pose_with_cov_msg); diagnostics_ndt_align_->add_key_value("best_particle_score", best_particle_ptr->score); return std::make_tuple(result_pose_with_cov_msg, best_particle_ptr->score); diff --git a/localization/autoware_ndt_scan_matcher/src/particle.cpp b/localization/autoware_ndt_scan_matcher/src/particle.cpp index 238e0ea14a65f..a5db875fc3ff7 100644 --- a/localization/autoware_ndt_scan_matcher/src/particle.cpp +++ b/localization/autoware_ndt_scan_matcher/src/particle.cpp @@ -14,10 +14,12 @@ #include "autoware/ndt_scan_matcher/particle.hpp" -#include "localization_util/util_func.hpp" +#include "autoware/localization_util/util_func.hpp" namespace autoware::ndt_scan_matcher { +using autoware::localization_util::exchange_color_crc; + void push_debug_markers( visualization_msgs::msg::MarkerArray & marker_array, const builtin_interfaces::msg::Time & stamp, const std::string & map_frame_, const Particle & particle, const size_t i) diff --git a/localization/autoware_pose_initializer/package.xml b/localization/autoware_pose_initializer/package.xml index 67079c476ac8a..d1ffc4ca4aef5 100644 --- a/localization/autoware_pose_initializer/package.xml +++ b/localization/autoware_pose_initializer/package.xml @@ -19,13 +19,13 @@ ament_cmake autoware_cmake + autoware_localization_util autoware_map_height_fitter autoware_motion_utils autoware_universe_utils component_interface_specs component_interface_utils geometry_msgs - localization_util rclcpp rclcpp_components std_srvs diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp index 777f92826de73..912c6ec9b57a2 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp @@ -39,7 +39,8 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) output_pose_covariance_ = get_covariance_parameter(this, "output_pose_covariance"); gnss_particle_covariance_ = get_covariance_parameter(this, "gnss_particle_covariance"); - diagnostics_pose_reliable_ = std::make_unique(this, "pose_initializer_status"); + diagnostics_pose_reliable_ = std::make_unique( + this, "pose_initializer_status"); if (declare_parameter("ekf_enabled")) { ekf_localization_trigger_ = std::make_unique(this); diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp index e8fef885cf577..a44f7f70b4b43 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp @@ -15,7 +15,7 @@ #ifndef POSE_INITIALIZER_CORE_HPP_ #define POSE_INITIALIZER_CORE_HPP_ -#include "localization_util/diagnostics_module.hpp" +#include "autoware/localization_util/diagnostics_module.hpp" #include #include @@ -59,7 +59,7 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr ekf_localization_trigger_; std::unique_ptr ndt_localization_trigger_; std::unique_ptr logger_configure_; - std::unique_ptr diagnostics_pose_reliable_; + std::unique_ptr diagnostics_pose_reliable_; double stop_check_duration_; void change_node_trigger(bool flag, bool need_spin = false); diff --git a/localization/localization_util/README.md b/localization/localization_util/README.md deleted file mode 100644 index a7daea33e0273..0000000000000 --- a/localization/localization_util/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# localization_util - -`localization_util` is a localization utility package. - -This package does not have a node, it is just a library. From 2a2d037820bdda445675061168a4d1d8378a72dc Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Thu, 19 Sep 2024 18:46:22 +0900 Subject: [PATCH 15/95] fix(lane_change): set initail rtc state properly (#8902) set initail rtc state properly Signed-off-by: Go Sakayori --- .../src/interface.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 61daf1183e993..7ddafdbd6c935 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -125,16 +125,21 @@ BehaviorModuleOutput LaneChangeInterface::plan() } else { const auto path = assignToCandidate(module_type_->getLaneChangePath(), module_type_->getEgoPosition()); - const auto force_activated = std::any_of( + const auto is_registered = std::any_of( rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), - [&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); }); - if (!force_activated) { + [&](const auto & rtc) { return rtc.second->isRegistered(uuid_map_.at(rtc.first)); }); + + if (!is_registered) { updateRTCStatus( path.start_distance_to_path_change, path.finish_distance_to_path_change, true, - State::RUNNING); + State::WAITING_FOR_EXECUTION); } else { + const auto force_activated = std::any_of( + rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), + [&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); }); + const bool safe = force_activated ? false : true; updateRTCStatus( - path.start_distance_to_path_change, path.finish_distance_to_path_change, false, + path.start_distance_to_path_change, path.finish_distance_to_path_change, safe, State::RUNNING); } } From 59838b6e7552ef1bc61867f9dc65de4dfa472529 Mon Sep 17 00:00:00 2001 From: Giovanni Muhammad Raditya Date: Fri, 20 Sep 2024 10:23:23 +0900 Subject: [PATCH 16/95] refactor(universe_utils): remove raw pointers from the triangulation function (#8893) --- .../universe_utils/geometry/ear_clipping.hpp | 146 ++- .../src/geometry/ear_clipping.cpp | 834 ++++++++++-------- 2 files changed, 530 insertions(+), 450 deletions(-) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp index 5ba6501a1ec18..cf93740e956d8 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp @@ -17,91 +17,89 @@ #include "autoware/universe_utils/geometry/boost_geometry.hpp" -#include +#include #include namespace autoware::universe_utils { -class EarClipping -{ -public: - std::vector indices; - std::size_t vertices = 0; - using Polygon2d = autoware::universe_utils::Polygon2d; - using Point2d = autoware::universe_utils::Point2d; - using LinearRing2d = autoware::universe_utils::LinearRing2d; - - void operator()(const Polygon2d & polygon); +using Polygon2d = autoware::universe_utils::Polygon2d; +using Point2d = autoware::universe_utils::Point2d; +using LinearRing2d = autoware::universe_utils::LinearRing2d; - ~EarClipping() +struct LinkedPoint +{ + explicit LinkedPoint(const Point2d & point) + : pt(point), steiner(false), prev_index(std::nullopt), next_index(std::nullopt) { - for (auto * p : points_) { - delete p; - } } -private: - struct Point - { - Point(const std::size_t index, Point2d point) : i(index), pt(std::move(point)) {} - - const std::size_t i; // Index of the point in the original polygon - const Point2d pt; // The Point2d object representing the coordinates - - // Previous and next vertices (Points) in the polygon ring - Point * prev = nullptr; - Point * next = nullptr; - bool steiner = false; - - [[nodiscard]] double x() const { return pt.x(); } - [[nodiscard]] double y() const { return pt.y(); } - }; - - std::vector points_; - - Point * linked_list(const LinearRing2d & points, bool clockwise); - static Point * filter_points(Point * start, Point * end = nullptr); - Point * cure_local_intersections(Point * start); - static Point * get_leftmost(Point * start); - Point * split_polygon(Point * a, Point * b); - Point * insert_point(std::size_t i, const Point2d & p, Point * last); - Point * eliminate_holes( - const std::vector & inners, EarClipping::Point * outer_point); - Point * eliminate_hole(Point * hole, Point * outer_point); - static Point * find_hole_bridge(Point * hole, Point * outer_point); - void ear_clipping_linked(Point * ear, int pass = 0); - void split_ear_clipping(Point * start); - static void remove_point(Point * p); - static bool is_ear(Point * ear); - static bool sector_contains_sector(const Point * m, const Point * p); - [[nodiscard]] static bool point_in_triangle( - double ax, double ay, double bx, double by, double cx, double cy, double px, double py); - static bool is_valid_diagonal(Point * a, Point * b); - static bool equals(const Point * p1, const Point * p2); - static bool intersects(const Point * p1, const Point * q1, const Point * p2, const Point * q2); - static bool on_segment(const Point * p, const Point * q, const Point * r); - static bool intersects_polygon(const Point * a, const Point * b); - static bool locally_inside(const Point * a, const Point * b); - static bool middle_inside(const Point * a, const Point * b); - static int sign(double val); - static double area(const Point * p, const Point * q, const Point * r); - - // Function to construct a new Point object - EarClipping::Point * construct_point(std::size_t index, const Point2d & point) - { - auto * new_point = new Point(index, point); - points_.push_back(new_point); - return new_point; - } + Point2d pt; + bool steiner; + std::optional prev_index; + std::optional next_index; + [[nodiscard]] double x() const { return pt.x(); } + [[nodiscard]] double y() const { return pt.y(); } }; -/// @brief Triangulate based on ear clipping algorithm -/// @param polygon concave/convex polygon with/without holes -/// @details algorithm based on https://github.com/mapbox/earclipping with modification -std::vector triangulate( - const autoware::universe_utils::Polygon2d & poly); - +/** + * @brief main ear slicing loop which triangulates a polygon using linked list + * @details iterates over the linked list of polygon points, cutting off triangular ears one by one + * handles different stages for fixing intersections and splitting if necessary + */ +void ear_clipping_linked( + std::size_t ear_index, std::vector & indices, std::vector & points, + const int pass = 0); +void split_ear_clipping( + std::vector & points, std::size_t start_index, std::vector & indices); + +/** + * @brief creates a linked list from a ring of points + * @details converts a polygon ring into a doubly linked list with optional clockwise ordering + * @return the last index of the created linked list + */ +std::size_t linked_list( + const LinearRing2d & ring, const bool clockwise, std::size_t & vertices, + std::vector & points); + +/** + * @brief David Eberly's algorithm for finding a bridge between hole and outer polygon + * @details connects a hole to the outer polygon by finding the closest bridge point + * @return index of the bridge point + */ +std::size_t find_hole_bridge( + const std::size_t hole_index, const std::size_t outer_point_index, + const std::vector & points); + +/** + * @brief eliminates a single hole by connecting it to the outer polygon + * @details finds a bridge and modifies the linked list to remove the hole + * @return the updated outer_index after the hole is eliminated + */ +std::size_t eliminate_hole( + const std::size_t hole_index, const std::size_t outer_index, std::vector & points); + +/** + * @brief eliminates all holes from a polygon + * @details processes multiple holes by connecting each to the outer polygon in sequence + * @return the updated outer_index after all holes are eliminated + */ +std::size_t eliminate_holes( + const std::vector & inners, std::size_t outer_index, std::size_t & vertices, + std::vector & points); + +/** + * @brief triangulates a polygon into convex triangles + * @details simplifies a concave polygon, with or without holes, into a set of triangles + * the size of the `points` vector at the end of the perform_triangulation algorithm is described as + * follow: + * - `outer_points`: Number of points in the initial outer linked list. + * - `hole_points`: Number of points in all inner polygons. + * - `2 * n_holes`: Additional points for bridging holes, where `n_holes` is the number of holes. + * the final size of `points` vector is: `outer_points + hole_points + 2 * n_holes`. + * @return A vector of convex triangles representing the triangulated polygon. + */ +std::vector triangulate(const Polygon2d & polygon); } // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__EAR_CLIPPING_HPP_ diff --git a/common/autoware_universe_utils/src/geometry/ear_clipping.cpp b/common/autoware_universe_utils/src/geometry/ear_clipping.cpp index 741f6fb901c07..03b39e9536b62 100644 --- a/common/autoware_universe_utils/src/geometry/ear_clipping.cpp +++ b/common/autoware_universe_utils/src/geometry/ear_clipping.cpp @@ -14,525 +14,607 @@ #include "autoware/universe_utils/geometry/ear_clipping.hpp" -#ifndef EAR_CUT_IMPL_HPP -#define EAR_CUT_IMPL_HPP - namespace autoware::universe_utils { -void EarClipping::operator()(const Polygon2d & polygon) +void remove_point(const std::size_t p_index, std::vector & points) { - indices.clear(); - vertices = 0; - - if (polygon.outer().size() == 0) return; - - std::size_t len = 0; - - const auto & outer_ring = polygon.outer(); - len = outer_ring.size(); - points_.reserve(len * 3 / 2); - indices.reserve(len + outer_ring.size()); + std::size_t prev_index = points[p_index].prev_index.value(); + std::size_t next_index = points[p_index].next_index.value(); - EarClipping::Point * outer_point = linked_list(outer_ring, true); - if (!outer_point || outer_point->prev == outer_point->next) return; - if (polygon.inners().size() > 0) outer_point = eliminate_holes(polygon.inners(), outer_point); - ear_clipping_linked(outer_point); - points_.clear(); + points[prev_index].next_index = next_index; + points[next_index].prev_index = prev_index; } -/// @brief create a circular doubly linked list from polygon points in the specified winding order -EarClipping::Point * EarClipping::linked_list(const LinearRing2d & points, bool clockwise) +std::size_t get_leftmost(const std::size_t start_idx, const std::vector & points) { - double sum = 0; - const std::size_t len = points.size(); - EarClipping::Point * last = nullptr; - - for (size_t i = 0, j = len > 0 ? len - 1 : 0; i < len; j = i++) { - const auto & p1 = points[i]; - const auto & p2 = points[j]; - const double p10 = p1.x(); - const double p11 = p1.y(); - const double p20 = p2.x(); - const double p21 = p2.y(); - sum += (p20 - p10) * (p11 + p21); - } + std::optional p_idx = points[start_idx].next_index; + std::size_t left_most_idx = start_idx; - if (clockwise == (sum > 0)) { - for (size_t i = 0; i < len; i++) { - last = insert_point(vertices + i, points[i], last); - } - } else { - for (size_t i = len; i-- > 0;) { - last = insert_point(vertices + i, points[i], last); + while (p_idx.has_value() && p_idx.value() != start_idx) { + if ( + points[p_idx.value()].x() < points[left_most_idx].x() || + (points[p_idx.value()].x() == points[left_most_idx].x() && + points[p_idx.value()].y() < points[left_most_idx].y())) { + left_most_idx = p_idx.value(); } + p_idx = points[p_idx.value()].next_index; } - if (last && equals(last, last->next)) { - remove_point(last); - last = last->next; - } - - vertices += len; + return left_most_idx; +} - return last; +bool point_in_triangle( + const double ax, const double ay, const double bx, const double by, const double cx, + const double cy, const double px, const double py) +{ + return (cx - px) * (ay - py) >= (ax - px) * (cy - py) && + (ax - px) * (by - py) >= (bx - px) * (ay - py) && + (bx - px) * (cy - py) >= (cx - px) * (by - py); } -/// @brief eliminate colinear or duplicate points -EarClipping::Point * EarClipping::filter_points( - EarClipping::Point * start, EarClipping::Point * end) +double area( + const std::vector & points, const std::size_t p_idx, const std::size_t q_idx, + const std::size_t r_idx) { - if (!end) end = start; + const LinkedPoint & p = points[p_idx]; + const LinkedPoint & q = points[q_idx]; + const LinkedPoint & r = points[r_idx]; + return (q.y() - p.y()) * (r.x() - q.x()) - (q.x() - p.x()) * (r.y() - q.y()); +} - EarClipping::Point * p = start; - bool again = false; - do { - again = false; +bool middle_inside( + const std::size_t a_idx, const std::size_t b_idx, const std::vector & points) +{ + std::optional p_idx = a_idx; + bool inside = false; + double px = (points[a_idx].x() + points[b_idx].x()) / 2; + double py = (points[a_idx].y() + points[b_idx].y()) / 2; + std::size_t start_idx = a_idx; - if (!p->steiner && (equals(p, p->next) || area(p->prev, p, p->next) == 0)) { - remove_point(p); - p = end = p->prev; + while (p_idx.has_value()) { + std::size_t current_idx = p_idx.value(); + std::size_t next_idx = points[current_idx].next_index.value(); - if (p == p->next) break; - again = true; + if ( + ((points[current_idx].y() > py) != (points[next_idx].y() > py)) && + points[next_idx].y() != points[current_idx].y() && + (px < (points[next_idx].x() - points[current_idx].x()) * (py - points[current_idx].y()) / + (points[next_idx].y() - points[current_idx].y()) + + points[current_idx].x())) { + inside = !inside; + } - } else { - p = p->next; + if (next_idx == start_idx) { + break; // Break the loop if we have cycled back to the start index } - } while (again || p != end); - return end; + p_idx = next_idx; + } + + return inside; } -/// @brief find a bridge between vertices that connects hole with an outer ring and and link it -EarClipping::Point * EarClipping::eliminate_hole( - EarClipping::Point * hole, EarClipping::Point * outer_point) +bool equals( + const std::size_t p1_idx, const std::size_t p2_idx, const std::vector & points) { - EarClipping::Point * bridge = find_hole_bridge(hole, outer_point); - if (!bridge) { - return outer_point; - } - EarClipping::Point * bridge_reverse = split_polygon(bridge, hole); + return points[p1_idx].x() == points[p2_idx].x() && points[p1_idx].y() == points[p2_idx].y(); +} - // filter collinear points around the cuts - filter_points(bridge_reverse, bridge_reverse->next); +int sign(const double val) +{ + return (0.0 < val) - (val < 0.0); +} - // Check if input node was removed by the filtering - return filter_points(bridge, bridge->next); +bool on_segment( + const std::vector & points, const std::size_t p_idx, const std::size_t q_idx, + const std::size_t r_idx) +{ + const LinkedPoint & p = points[p_idx]; + const LinkedPoint & q = points[q_idx]; + const LinkedPoint & r = points[r_idx]; + return q.x() <= std::max(p.x(), r.x()) && q.x() >= std::min(p.x(), r.x()) && + q.y() <= std::max(p.y(), r.y()) && q.y() >= std::min(p.y(), r.y()); } -EarClipping::Point * EarClipping::eliminate_holes( - const std::vector & inners, EarClipping::Point * outer_point) +bool locally_inside( + const std::size_t a_idx, const std::size_t b_idx, const std::vector & points) { - const size_t len = inners.size(); - - std::vector queue; - for (size_t i = 0; i < len; i++) { - Point * list = linked_list(inners[i], false); - if (list) { - if (list == list->next) list->steiner = true; - queue.push_back(get_leftmost(list)); - } - } - std::sort( - queue.begin(), queue.end(), [](const Point * a, const Point * b) { return a->x() < b->x(); }); - for (const auto & q : queue) { - outer_point = eliminate_hole(q, outer_point); + const auto & prev_idx = points[a_idx].prev_index; + const auto & next_idx = points[a_idx].next_index; + + if (!prev_idx.has_value() || !next_idx.has_value()) { + return false; } - return outer_point; + double area_prev = area(points, prev_idx.value(), a_idx, next_idx.value()); + double area_a_b_next = area(points, a_idx, b_idx, next_idx.value()); + double area_a_prev_b = area(points, a_idx, prev_idx.value(), b_idx); + double area_a_b_prev = area(points, a_idx, b_idx, prev_idx.value()); + double area_a_next_b = area(points, a_idx, next_idx.value(), b_idx); + + return area_prev < 0 ? area_a_b_next >= 0 && area_a_prev_b >= 0 + : area_a_b_prev < 0 || area_a_next_b < 0; } -// cspell: ignore Eberly -/// @brief David Eberly's algorithm for finding a bridge between hole and outer polygon -EarClipping::Point * EarClipping::find_hole_bridge(Point * hole, Point * outer_point) +bool intersects( + const std::size_t p1_idx, const std::size_t q1_idx, const std::size_t p2_idx, + const std::size_t q2_idx, const std::vector & points) { - Point * p = outer_point; - double hx = hole->x(); - double hy = hole->y(); - double qx = -std::numeric_limits::infinity(); - Point * m = nullptr; - do { - if (hy <= p->y() && hy >= p->next->y() && p->next->y() != p->y()) { - double x = p->x() + (hy - p->y()) * (p->next->x() - p->x()) / (p->next->y() - p->y()); - if (x <= hx && x > qx) { - qx = x; - m = p->x() < p->next->x() ? p : p->next; - if (x == hx) return m; - } - } - p = p->next; - } while (p != outer_point); + int o1 = sign(area(points, p1_idx, q1_idx, p2_idx)); + int o2 = sign(area(points, p1_idx, q1_idx, q2_idx)); + int o3 = sign(area(points, p2_idx, q2_idx, p1_idx)); + int o4 = sign(area(points, p2_idx, q2_idx, q1_idx)); - if (!m) return nullptr; + if (o1 != o2 && o3 != o4) return true; - const Point * stop = m; - double tan_min = std::numeric_limits::infinity(); + if (o1 == 0 && on_segment(points, p1_idx, p2_idx, q1_idx)) return true; + if (o2 == 0 && on_segment(points, p1_idx, q2_idx, q1_idx)) return true; + if (o3 == 0 && on_segment(points, p2_idx, p1_idx, q2_idx)) return true; + if (o4 == 0 && on_segment(points, p2_idx, q1_idx, q2_idx)) return true; - p = m; - double mx = m->x(); - double my = m->y(); + return false; +} - do { - if ( - hx >= p->x() && p->x() >= mx && hx != p->x() && - point_in_triangle(hy < my ? hx : qx, hy, mx, my, hy < my ? qx : hx, hy, p->x(), p->y())) { - const auto tan_cur = std::abs(hy - p->y()) / (hx - p->x()); +bool intersects_polygon( + const std::vector & points, const std::size_t a_idx, const std::size_t b_idx) +{ + std::size_t p_idx = a_idx; + std::optional p_next_opt = points[p_idx].next_index; - if ( - locally_inside(p, hole) && - (tan_cur < tan_min || - (tan_cur == tan_min && (p->x() > m->x() || sector_contains_sector(m, p))))) { - m = p; - tan_min = tan_cur; + while (p_next_opt.has_value() && p_next_opt.value() != a_idx) { + std::size_t p_next_idx = p_next_opt.value(); + + if (p_idx != a_idx && p_next_idx != a_idx && p_idx != b_idx && p_next_idx != b_idx) { + if (intersects(p_idx, p_next_idx, a_idx, b_idx, points)) { + return true; } } - p = p->next; - } while (p != stop); + p_idx = p_next_idx; + p_next_opt = points[p_idx].next_index; + } - return m; + return false; } -/// @brief main ear slicing loop which triangulates a polygon using linked list -void EarClipping::ear_clipping_linked(EarClipping::Point * ear, int pass) +bool is_valid_diagonal( + const std::size_t a_idx, const std::size_t b_idx, const std::vector & points) { - if (!ear) return; + if ( + !points[a_idx].next_index.has_value() || !points[a_idx].prev_index.has_value() || + !points[b_idx].next_index.has_value() || !points[b_idx].prev_index.has_value()) { + return false; + } - EarClipping::Point * stop = ear; - EarClipping::Point * next = nullptr; + std::size_t a_next_idx = points[a_idx].next_index.value(); + std::size_t a_prev_idx = points[a_idx].prev_index.value(); + std::size_t b_next_idx = points[b_idx].next_index.value(); + std::size_t b_prev_idx = points[b_idx].prev_index.value(); - // Iterate through ears, slicing them one by one - while (ear->prev != ear->next) { - next = ear->next; + if (a_next_idx == b_idx || a_prev_idx == b_idx || intersects_polygon(points, a_idx, b_idx)) { + return false; + } - if (is_ear(ear)) { - // Cut off the triangle - indices.emplace_back(ear->prev->i); - indices.emplace_back(ear->i); - indices.emplace_back(next->i); + bool is_locally_inside_ab = locally_inside(a_idx, b_idx, points); + bool is_locally_inside_ba = locally_inside(b_idx, a_idx, points); + bool is_middle_inside = middle_inside(a_idx, b_idx, points); - remove_point(ear); - ear = next->next; - stop = next->next; + bool is_valid_diagonal = + (is_locally_inside_ab && is_locally_inside_ba && is_middle_inside && + (area(points, a_prev_idx, a_idx, b_prev_idx) != 0.0 || + area(points, a_idx, b_prev_idx, b_idx) != 0.0)) || + (equals(a_idx, b_idx, points) && area(points, a_prev_idx, a_idx, a_next_idx) > 0 && + area(points, b_prev_idx, b_idx, b_next_idx) > 0); - continue; - } + return is_valid_diagonal; +} - ear = next; - if (ear == stop) { - if (!pass) { - ear_clipping_linked(filter_points(ear), 1); - } else if (pass == 1) { - ear = cure_local_intersections(filter_points(ear)); - ear_clipping_linked(ear, 2); - } else if (pass == 2) { - split_ear_clipping(ear); - } - break; +std::size_t insert_point( + const Point2d & pt, std::vector & points, + const std::optional last_index) +{ + std::size_t p_idx = points.size(); + points.push_back(LinkedPoint(pt)); + + // Making sure all next_index and prev_index will always have values + if (!last_index.has_value()) { + points[p_idx].prev_index = p_idx; + points[p_idx].next_index = p_idx; + } else { + std::size_t last = last_index.value(); + std::size_t next = points[last].next_index.value(); + points[p_idx].prev_index = last; + points[p_idx].next_index = next; + points[last].next_index = p_idx; + if (next != p_idx) { + points[next].prev_index = p_idx; } } + + return p_idx; } -/// @brief check whether ear is valid -bool EarClipping::is_ear(EarClipping::Point * ear) +std::size_t linked_list( + const LinearRing2d & ring, const bool clockwise, std::size_t & vertices, + std::vector & points) { - const EarClipping::Point * a = ear->prev; - const EarClipping::Point * b = ear; - const EarClipping::Point * c = ear->next; + double sum = 0; + const std::size_t len = ring.size(); + std::optional last_index = std::nullopt; + + for (std::size_t i = 0, j = len > 0 ? len - 1 : 0; i < len; j = i++) { + const auto & p1 = ring[i]; + const auto & p2 = ring[j]; + sum += (p2.x() - p1.x()) * (p1.y() + p2.y()); + } - if (area(a, b, c) >= 0) return false; // Reflex, can't be an ear + if (clockwise == (sum > 0)) { + for (const auto & point : ring) { + last_index = insert_point(point, points, last_index); + } + } else { + for (auto it = ring.rbegin(); it != ring.rend(); ++it) { + last_index = insert_point(*it, points, last_index); + } + } - EarClipping::Point * p = ear->next->next; + if (last_index.has_value()) { + std::size_t last_idx_value = last_index.value(); + std::optional next_index = points[last_idx_value].next_index; - while (p != ear->prev) { - if ( - point_in_triangle(a->x(), a->y(), b->x(), b->y(), c->x(), c->y(), p->x(), p->y()) && - area(p->prev, p, p->next) >= 0) - return false; - p = p->next; + if (next_index.has_value() && equals(last_idx_value, next_index.value(), points)) { + std::size_t next_idx_value = next_index.value(); + remove_point(last_idx_value, points); + last_index = next_idx_value; + } } - return true; + vertices += len; + return last_index.value(); } -/// @brief go through all polygon Points and cure small local self-intersections -EarClipping::Point * EarClipping::cure_local_intersections(EarClipping::Point * start) +bool sector_contains_sector( + const std::size_t m_idx, const std::size_t p_idx, const std::vector & points) { - EarClipping::Point * p = start; - do { - EarClipping::Point * a = p->prev; - EarClipping::Point * b = p->next->next; + if (!points[m_idx].prev_index.has_value() || !points[m_idx].next_index.has_value()) { + return false; + } - if ( - !equals(a, b) && intersects(a, p, p->next, b) && locally_inside(a, b) && - locally_inside(b, a)) { - indices.emplace_back(a->i); - indices.emplace_back(p->i); - indices.emplace_back(b->i); - remove_point(p); - remove_point(p->next); - - p = start = b; - } - p = p->next; - } while (p != start); + std::size_t m_prev_idx = points[m_idx].prev_index.value(); + std::size_t m_next_idx = points[m_idx].next_index.value(); - return filter_points(p); + return area(points, m_prev_idx, m_idx, p_idx) < 0 && area(points, p_idx, m_next_idx, m_idx) < 0; } -/// @brief splitting polygon into two and triangulate them independently -void EarClipping::split_ear_clipping(EarClipping::Point * start) +std::size_t find_hole_bridge( + const std::size_t hole_index, const std::size_t outer_point_index, + const std::vector & points) { - EarClipping::Point * a = start; - do { - EarClipping::Point * b = a->next->next; - while (b != a->prev) { - if (a->i != b->i && is_valid_diagonal(a, b)) { - EarClipping::Point * c = split_polygon(a, b); + std::size_t p = outer_point_index; + double hx = points[hole_index].x(); + double hy = points[hole_index].y(); + double qx = -std::numeric_limits::infinity(); + std::optional bridge_index = std::nullopt; + std::size_t next_index = points[p].next_index.value(); + + while (p != outer_point_index) { + if ( + hy <= points[p].y() && hy >= points[next_index].y() && + points[next_index].y() != points[p].y()) { + double x = points[p].x() + (hy - points[p].y()) * (points[next_index].x() - points[p].x()) / + (points[next_index].y() - points[p].y()); + if (x <= hx && x > qx) { + qx = x; + bridge_index = (points[p].x() < points[next_index].x()) ? p : next_index; + if (x == hx) return bridge_index.value(); + } + } + p = next_index; + next_index = points[p].next_index.value(); + } - a = filter_points(a, a->next); - c = filter_points(c, c->next); + if (!bridge_index.has_value()) return outer_point_index; - ear_clipping_linked(a); - ear_clipping_linked(c); - return; + const std::size_t stop = bridge_index.value(); + double min_tan = std::numeric_limits::infinity(); + + p = bridge_index.value(); + double mx = points[p].x(); + double my = points[p].y(); + next_index = points[p].next_index.value(); + + while (p != stop) { + if ( + hx >= points[p].x() && points[p].x() >= mx && hx != points[p].x() && + point_in_triangle( + hy < my ? hx : qx, hy, mx, my, hy < my ? qx : hx, hy, points[p].x(), points[p].y())) { + double current_tan = std::abs(hy - points[p].y()) / (hx - points[p].x()); + + if ( + locally_inside(p, hole_index, points) && + (current_tan < min_tan || + (current_tan == min_tan && (points[p].x() > points[bridge_index.value()].x() || + sector_contains_sector(bridge_index.value(), p, points))))) { + bridge_index = p; + min_tan = current_tan; } - b = b->next; } - a = a->next; - } while (a != start); -} -/// @brief check whether sector in vertex m contains sector in vertex p in the same coordinates -bool EarClipping::sector_contains_sector(const EarClipping::Point * m, const EarClipping::Point * p) -{ - return area(m->prev, m, p->prev) < 0 && area(p->next, m, m->next) < 0; + p = next_index; + next_index = points[p].next_index.value(); + } + + return bridge_index.value(); } -/// @brief find the leftmost Point of a polygon ring -EarClipping::Point * EarClipping::get_leftmost(EarClipping::Point * start) +std::size_t split_polygon( + std::size_t a_index, std::size_t b_index, std::vector & points) { - EarClipping::Point * p = start; - EarClipping::Point * leftmost = start; - do { - if (p->x() < leftmost->x() || (p->x() == leftmost->x() && p->y() < leftmost->y())) leftmost = p; - p = p->next; - } while (p != start); + std::size_t an_idx = points[a_index].next_index.value(); + std::size_t bp_idx = points[b_index].prev_index.value(); - return leftmost; -} + std::size_t a2_idx = points.size(); + std::size_t b2_idx = points.size() + 1; + points.push_back(points[a_index]); + points.push_back(points[b_index]); -/// @brief check if a point lies within a convex triangle -bool EarClipping::point_in_triangle( - double ax, double ay, double bx, double by, double cx, double cy, double px, double py) -{ - return (cx - px) * (ay - py) >= (ax - px) * (cy - py) && - (ax - px) * (by - py) >= (bx - px) * (ay - py) && - (bx - px) * (cy - py) >= (cx - px) * (by - py); -} + points[a_index].next_index = b_index; + points[a2_idx].prev_index = b2_idx; + points[a2_idx].next_index = an_idx; -/// @brief check if a diagonal between two polygon Points is valid -bool EarClipping::is_valid_diagonal(EarClipping::Point * a, EarClipping::Point * b) -{ - return a->next->i != b->i && a->prev->i != b->i && !intersects_polygon(a, b) && - ((locally_inside(a, b) && locally_inside(b, a) && middle_inside(a, b) && - (area(a->prev, a, b->prev) != 0.0 || area(a, b->prev, b) != 0.0)) || - (equals(a, b) && area(a->prev, a, a->next) > 0 && area(b->prev, b, b->next) > 0)); -} + points[b_index].prev_index = a_index; + points[an_idx].prev_index = b2_idx; + points[b2_idx].next_index = a2_idx; + points[b2_idx].prev_index = bp_idx; -/// @brief signed area of a triangle -double EarClipping::area( - const EarClipping::Point * p, const EarClipping::Point * q, const EarClipping::Point * r) -{ - return (q->y() - p->y()) * (r->x() - q->x()) - (q->x() - p->x()) * (r->y() - q->y()); -} + if (bp_idx != b_index) { + points[bp_idx].next_index = b2_idx; + } -/// @brief check if two points are equal -bool EarClipping::equals(const EarClipping::Point * p1, const EarClipping::Point * p2) -{ - return p1->x() == p2->x() && p1->y() == p2->y(); + return b2_idx; } -/// @brief check if two segments intersect -bool EarClipping::intersects( - const EarClipping::Point * p1, const EarClipping::Point * q1, const EarClipping::Point * p2, - const EarClipping::Point * q2) +std::size_t filter_points( + const std::size_t start_index, const std::size_t end_index, std::vector & points) { - int o1 = sign(area(p1, q1, p2)); - int o2 = sign(area(p1, q1, q2)); - int o3 = sign(area(p2, q2, p1)); - int o4 = sign(area(p2, q2, q1)); + auto p = start_index; + bool again = true; - if (o1 != o2 && o3 != o4) return true; + while (again && p != end_index) { + again = false; - if (o1 == 0 && on_segment(p1, p2, q1)) return true; - if (o2 == 0 && on_segment(p1, q2, q1)) return true; - if (o3 == 0 && on_segment(p2, p1, q2)) return true; - if (o4 == 0 && on_segment(p2, q1, q2)) return true; + if ( + !points[p].steiner && + (equals(p, points[p].next_index.value(), points) || + area(points, points[p].prev_index.value(), p, points[p].next_index.value()) == 0)) { + remove_point(p, points); + p = points[p].prev_index.value(); + + if (p == points[p].next_index.value()) { + break; + } + again = true; + } else { + p = points[p].next_index.value(); + } + } - return false; + return end_index; } -/// @brief for collinear points p, q, r, check if point q lies on segment pr -bool EarClipping::on_segment( - const EarClipping::Point * p, const EarClipping::Point * q, const EarClipping::Point * r) +std::size_t eliminate_hole( + const std::size_t hole_index, const std::size_t outer_index, std::vector & points) { - return q->x() <= std::max(p->x(), r->x()) && q->x() >= std::min(p->x(), r->x()) && - q->y() <= std::max(p->y(), r->y()) && q->y() >= std::min(p->y(), r->y()); + auto bridge = find_hole_bridge(hole_index, outer_index, points); + auto bridge_reverse = split_polygon(bridge, hole_index, points); + + auto next_index_bridge_reverse = points[bridge_reverse].next_index.value(); + filter_points(bridge_reverse, next_index_bridge_reverse, points); + + auto next_index_bridge = points[bridge].next_index.value(); + return filter_points(bridge, next_index_bridge, points); } -/// @brief Sign function for area calculation -int EarClipping::sign(double val) +std::size_t eliminate_holes( + const std::vector & inners, std::size_t outer_index, std::size_t & vertices, + std::vector & points) { - return (0.0 < val) - (val < 0.0); + std::vector queue; + + for (const auto & ring : inners) { + auto inner_index = linked_list(ring, false, vertices, points); + + if (points[inner_index].next_index.value() == inner_index) { + points[inner_index].steiner = true; + } + + queue.push_back(get_leftmost(inner_index, points)); + } + + std::sort(queue.begin(), queue.end(), [&](std::size_t a, std::size_t b) { + return points[a].x() < points[b].x(); + }); + + for (const auto & q : queue) { + outer_index = eliminate_hole(q, outer_index, points); + } + + return outer_index; } -/// @brief Check if a polygon diagonal intersects any polygon segments -bool EarClipping::intersects_polygon(const EarClipping::Point * a, const EarClipping::Point * b) +bool is_ear(const std::size_t ear_index, const std::vector & points) { - const EarClipping::Point * p = a; - do { + const auto a_index = points[ear_index].prev_index.value(); + const auto b_index = ear_index; + const auto c_index = points[ear_index].next_index.value(); + + const auto a = points[a_index]; + const auto b = points[b_index]; + const auto c = points[c_index]; + + if (area(points, a_index, b_index, c_index) >= 0) return false; + auto p_index = points[c_index].next_index.value(); + while (p_index != a_index) { + const auto p = points[p_index]; if ( - p->i != a->i && p->next->i != a->i && p->i != b->i && p->next->i != b->i && - intersects(p, p->next, a, b)) - return true; - p = p->next; - } while (p != a); + point_in_triangle(a.x(), a.y(), b.x(), b.y(), c.x(), c.y(), p.x(), p.y()) && + area(points, p.prev_index.value(), p_index, p.next_index.value()) >= 0) { + return false; + } + p_index = points[p_index].next_index.value(); + } - return false; + return true; } -/// @brief Check if a polygon diagonal is locally inside the polygon -bool EarClipping::locally_inside(const EarClipping::Point * a, const EarClipping::Point * b) +std::size_t cure_local_intersections( + std::size_t start_index, std::vector & indices, std::vector & points) { - return area(a->prev, a, a->next) < 0 ? area(a, b, a->next) >= 0 && area(a, a->prev, b) >= 0 - : area(a, b, a->prev) < 0 || area(a, a->next, b) < 0; + auto p = start_index; + bool updated = false; + + while (p != start_index || updated) { + updated = false; + auto a_idx = points[p].prev_index.value(); + auto b_idx = points[points[p].next_index.value()].next_index.value(); + + if ( + !equals(a_idx, b_idx, points) && + intersects( + a_idx, p, points[points[p].next_index.value()].next_index.value(), b_idx, points) && + locally_inside(a_idx, b_idx, points) && locally_inside(b_idx, a_idx, points)) { + indices.push_back(a_idx); + indices.push_back(p); + indices.push_back(b_idx); + + remove_point(p, points); + remove_point(points[p].next_index.value(), points); + + p = start_index = b_idx; + updated = true; + } else { + p = points[p].next_index.value(); + } + } + + return filter_points(p, p, points); } -/// @brief Check if the middle vertex of a polygon diagonal is inside the polygon -bool EarClipping::middle_inside(const EarClipping::Point * a, const EarClipping::Point * b) +void split_ear_clipping( + std::vector & points, const std::size_t start_idx, + std::vector & indices) { - const EarClipping::Point * p = a; - bool inside = false; - double px = (a->x() + b->x()) / 2; - double py = (a->y() + b->y()) / 2; + std::size_t a_idx = start_idx; do { - if ( - ((p->y() > py) != (p->next->y() > py)) && p->next->y() != p->y() && - (px < (p->next->x() - p->x()) * (py - p->y()) / (p->next->y() - p->y()) + p->x())) - inside = !inside; - p = p->next; - } while (p != a); + std::size_t b_idx = points[points[a_idx].next_index.value()].next_index.value(); + while (b_idx != points[a_idx].prev_index.value()) { + if (a_idx != b_idx && is_valid_diagonal(a_idx, b_idx, points)) { + std::size_t c_idx = split_polygon(a_idx, b_idx, points); - return inside; + a_idx = filter_points(start_idx, points[a_idx].next_index.value(), points); + c_idx = filter_points(start_idx, points[c_idx].next_index.value(), points); + + ear_clipping_linked(a_idx, indices, points); + ear_clipping_linked(c_idx, indices, points); + return; + } + b_idx = points[b_idx].next_index.value(); + } + a_idx = points[a_idx].next_index.value(); + } while (a_idx != start_idx); } -/// @brief Link two polygon vertices with a bridge -EarClipping::Point * EarClipping::split_polygon(EarClipping::Point * a, EarClipping::Point * b) +void ear_clipping_linked( + std::size_t ear_index, std::vector & indices, std::vector & points, + const int pass) { - Point2d a_point(a->x(), a->y()); - Point2d b_point(b->x(), b->y()); + auto stop = ear_index; + std::optional next = std::nullopt; - // Use construct_point to create new Point objects - EarClipping::Point * a2 = construct_point(a->i, a_point); - EarClipping::Point * b2 = construct_point(b->i, b_point); + while (points[ear_index].prev_index.value() != points[ear_index].next_index.value()) { + next = points[ear_index].next_index; - EarClipping::Point * an = a->next; - EarClipping::Point * bp = b->prev; + if (is_ear(ear_index, points)) { + indices.push_back(points[ear_index].prev_index.value()); + indices.push_back(ear_index); + indices.push_back(next.value()); - // Update the linked list connections - a->next = b; - b->prev = a; + remove_point(ear_index, points); - a2->next = an; - if (an) { - an->prev = a2; - } + ear_index = points[next.value()].next_index.value(); + stop = points[next.value()].next_index.value(); + continue; + } - b2->next = a2; - a2->prev = b2; + ear_index = next.value(); - if (bp) { - bp->next = b2; + if (ear_index == stop) { + if (pass == 0) { + ear_clipping_linked(filter_points(ear_index, ear_index, points), indices, points, 1); + } else if (pass == 1) { + ear_index = + cure_local_intersections(filter_points(ear_index, ear_index, points), indices, points); + ear_clipping_linked(ear_index, indices, points, 2); + } else if (pass == 2) { + split_ear_clipping(points, ear_index, indices); + } + break; + } } - b2->prev = bp; - - return b2; } -/// @brief create a Point and optionally link it with the previous one (in a circular doubly linked -/// list) -EarClipping::Point * EarClipping::insert_point( - std::size_t i, const Point2d & pt, EarClipping::Point * last) +std::vector perform_triangulation( + const Polygon2d & polygon, std::vector & indices) { - EarClipping::Point * p = construct_point(static_cast(i), pt); + indices.clear(); + std::vector points; + std::size_t vertices = 0; + const auto & outer_ring = polygon.outer(); + std::size_t len = outer_ring.size(); + points.reserve(len * 3 / 2); - if (!last) { - p->prev = p; - p->next = p; - } else { - assert(last); - p->next = last->next; - p->prev = last; - last->next->prev = p; - last->next = p; + if (polygon.outer().empty()) return points; + + indices.reserve(len + outer_ring.size()); + auto outer_point_index = linked_list(outer_ring, true, vertices, points); + if ( + !points[outer_point_index].prev_index.has_value() || + outer_point_index == points[outer_point_index].prev_index.value()) { + return points; } - return p; -} -/// @brief remove a Point from the linked list -void EarClipping::remove_point(EarClipping::Point * p) -{ - p->next->prev = p->prev; - p->prev->next = p->next; + if (!polygon.inners().empty()) { + outer_point_index = eliminate_holes(polygon.inners(), outer_point_index, vertices, points); + } + + ear_clipping_linked(outer_point_index, indices, points); + return points; } -/// @brief main triangulate function std::vector triangulate(const Polygon2d & poly) { - autoware::universe_utils::EarClipping triangulate; - triangulate(poly); + std::vector indices; + auto points = perform_triangulation(poly, indices); std::vector triangles; - - const auto & indices = triangulate.indices; const std::size_t num_indices = indices.size(); if (num_indices % 3 != 0) { throw std::runtime_error("Indices size should be a multiple of 3"); } - // Gather all vertices from outer and inner rings - std::vector all_vertices; - const auto & outer_ring = poly.outer(); - all_vertices.insert(all_vertices.end(), outer_ring.begin(), outer_ring.end()); - - for (const auto & inner_ring : poly.inners()) { - all_vertices.insert(all_vertices.end(), inner_ring.begin(), inner_ring.end()); - } - - const std::size_t total_vertices = all_vertices.size(); - for (std::size_t i = 0; i < num_indices; i += 3) { - if ( - indices[i] >= total_vertices || indices[i + 1] >= total_vertices || - indices[i + 2] >= total_vertices) { - throw std::runtime_error("Index out of range"); - } - Polygon2d triangle; - triangle.outer().push_back(all_vertices[indices[i]]); - triangle.outer().push_back(all_vertices[indices[i + 1]]); - triangle.outer().push_back(all_vertices[indices[i + 2]]); - triangle.outer().push_back(all_vertices[indices[i]]); // Close the triangle + triangle.outer().push_back(points[indices[i]].pt); + triangle.outer().push_back(points[indices[i + 1]].pt); + triangle.outer().push_back(points[indices[i + 2]].pt); + triangle.outer().push_back(points[indices[i]].pt); triangles.push_back(triangle); } - + points.clear(); return triangles; } } // namespace autoware::universe_utils - -#endif // EAR_CUT_IMPL_HPP From be9a40494da5197b208567fbee176320db11a55b Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Fri, 20 Sep 2024 13:37:53 +0900 Subject: [PATCH 17/95] chore(motion_velocity_planner): add Alqudah Mohammad as maintainer (#8877) Signed-off-by: Maxime CLEMENT --- .../autoware_motion_velocity_out_of_lane_module/package.xml | 1 + .../autoware_motion_velocity_planner_common/package.xml | 1 + .../autoware_motion_velocity_planner_node/package.xml | 1 + 3 files changed, 3 insertions(+) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml index 3336278c761c6..e0fb81e2d7a5a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml @@ -10,6 +10,7 @@ Shumpei Wakabayashi Takayuki Murooka Mamoru Sobue + Alqudah Mohammad Apache License 2.0 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml index 515a30348cbee..dfe15579eed2a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml @@ -6,6 +6,7 @@ Common functions and interfaces for motion_velocity_planner modules Maxime Clement + Alqudah Mohammad Apache License 2.0 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml index c1e68a232c55d..7919917c2256a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml @@ -6,6 +6,7 @@ Node of the motion_velocity_planner Maxime Clement + Alqudah Mohammad Apache License 2.0 From 583d85678e05ac52251cc07f6aa6ec908c8a748a Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Fri, 20 Sep 2024 14:16:30 +0900 Subject: [PATCH 18/95] feat(emergency_handler): delete package (#8917) * feat(emergency_handler): delete package Signed-off-by: veqcc --- .github/CODEOWNERS | 1 - .../launch/system.launch.xml | 28 +- launch/tier4_system_launch/package.xml | 1 - .../system_launch.drawio.svg | 6 +- system/emergency_handler/CMakeLists.txt | 20 - system/emergency_handler/README.md | 42 -- .../config/emergency_handler.param.yaml | 12 - .../image/fail-safe-state.drawio.svg | 239 ---------- .../emergency_handler_core.hpp | 143 ------ .../launch/emergency_handler.launch.xml | 35 -- system/emergency_handler/package.xml | 31 -- .../schema/emergency_handler.schema.json | 65 --- .../emergency_handler_core.cpp | 451 ------------------ 13 files changed, 5 insertions(+), 1069 deletions(-) delete mode 100644 system/emergency_handler/CMakeLists.txt delete mode 100644 system/emergency_handler/README.md delete mode 100644 system/emergency_handler/config/emergency_handler.param.yaml delete mode 100644 system/emergency_handler/image/fail-safe-state.drawio.svg delete mode 100644 system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp delete mode 100644 system/emergency_handler/launch/emergency_handler.launch.xml delete mode 100644 system/emergency_handler/package.xml delete mode 100644 system/emergency_handler/schema/emergency_handler.schema.json delete mode 100644 system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 07b35e3e33045..e21c06112069f 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -231,7 +231,6 @@ system/diagnostic_graph_utils/** isamu.takagi@tier4.jp system/dummy_diag_publisher/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/dummy_infrastructure/** ryohsuke.mitsudome@tier4.jp system/duplicated_node_checker/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp uken.ryu@tier4.jp -system/emergency_handler/** makoto.kurihara@tier4.jp ryuta.kambe@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/hazard_status_converter/** isamu.takagi@tier4.jp system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index d11d00c43d55b..07f6ae8d4bdc2 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -2,7 +2,6 @@ - @@ -26,8 +25,6 @@ - - @@ -97,27 +94,6 @@ - - - - - - - - - - - - - - - - - - - - - @@ -127,12 +103,12 @@ - + - + diff --git a/launch/tier4_system_launch/package.xml b/launch/tier4_system_launch/package.xml index 789cf136f1152..673596fad9972 100644 --- a/launch/tier4_system_launch/package.xml +++ b/launch/tier4_system_launch/package.xml @@ -12,7 +12,6 @@ autoware_cmake component_state_monitor - emergency_handler system_error_monitor system_monitor diff --git a/launch/tier4_system_launch/system_launch.drawio.svg b/launch/tier4_system_launch/system_launch.drawio.svg index 07a33aff8e30f..987158ba00c2b 100644 --- a/launch/tier4_system_launch/system_launch.drawio.svg +++ b/launch/tier4_system_launch/system_launch.drawio.svg @@ -287,15 +287,15 @@ >
- emergency_handler.launch.xml + mrm_handler.launch.xml

-
package: emergency_handler
+
package: mrm_handler
- emergency_handler.launch.xml... + mrm_handler.launch.xml... diff --git a/system/emergency_handler/CMakeLists.txt b/system/emergency_handler/CMakeLists.txt deleted file mode 100644 index 0475cdbe57dd8..0000000000000 --- a/system/emergency_handler/CMakeLists.txt +++ /dev/null @@ -1,20 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(emergency_handler) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/emergency_handler/emergency_handler_core.cpp -) - -rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "EmergencyHandler" - EXECUTABLE ${PROJECT_NAME}_node - EXECUTOR MultiThreadedExecutor -) - -ament_auto_package(INSTALL_TO_SHARE - launch - config -) diff --git a/system/emergency_handler/README.md b/system/emergency_handler/README.md deleted file mode 100644 index f34ebcf8d9b9a..0000000000000 --- a/system/emergency_handler/README.md +++ /dev/null @@ -1,42 +0,0 @@ -# emergency_handler - -## Purpose - -Emergency Handler is a node to select proper MRM from from system failure state contained in HazardStatus. - -## Inner-workings / Algorithms - -### State Transitions - -![fail-safe-state](image/fail-safe-state.drawio.svg) - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| ----------------------------------------- | ------------------------------------------------ | ----------------------------------------------------------------------------- | -| `/system/emergency/hazard_status` | `autoware_system_msgs::msg::HazardStatusStamped` | Used to select proper MRM from system failure state contained in HazardStatus | -| `/control/vehicle_cmd` | `autoware_control_msgs::msg::Control` | Used as reference when generate Emergency Control Command | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not | -| `/vehicle/status/control_mode` | `autoware_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual | -| `/system/api/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available | -| `/system/api/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available | - -### Output - -| Name | Type | Description | -| ------------------------------------------ | ------------------------------------------------- | ----------------------------------------------------- | -| `/system/emergency/shift_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | Required to execute proper MRM (send gear cmd) | -| `/system/emergency/hazard_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) | -| `/api/fail_safe/mrm_state` | `autoware_adapi_v1_msgs::msg::MrmState` | Inform MRM execution state and selected MRM behavior | -| `/system/api/mrm/comfortable_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM comfortable stop | -| `/system/api/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM emergency stop | - -## Parameters - -{{ json_to_markdown("system/emergency_handler/schema/emergency_handler.schema.json") }} - -## Assumptions / Known limits - -TBD. diff --git a/system/emergency_handler/config/emergency_handler.param.yaml b/system/emergency_handler/config/emergency_handler.param.yaml deleted file mode 100644 index 2309e7f23deb0..0000000000000 --- a/system/emergency_handler/config/emergency_handler.param.yaml +++ /dev/null @@ -1,12 +0,0 @@ -# Default configuration for emergency handler ---- -/**: - ros__parameters: - update_rate: 10 - timeout_hazard_status: 0.5 - use_parking_after_stopped: false - use_comfortable_stop: false - - # setting whether to turn hazard lamp on for each situation - turning_hazard_on: - emergency: true diff --git a/system/emergency_handler/image/fail-safe-state.drawio.svg b/system/emergency_handler/image/fail-safe-state.drawio.svg deleted file mode 100644 index a9cbb1026616b..0000000000000 --- a/system/emergency_handler/image/fail-safe-state.drawio.svg +++ /dev/null @@ -1,239 +0,0 @@ - - - - - - - - - - - - -
-
-
- Normal -
-
-
- - Normal - - - - - - - - - Emergency - - - - - -
-
-
- OverrideRequesting -
-
-
- - OverrideRequesting - - - - - - - - - -
-
-
- MrmOperating -
-
-
- - MrmOperating - - - - - - - - - - -
-
-
- MrmFailed -
-
-
- - MrmFailed - - - - - - - - -
-
-
- MrmSucceeded -
-
-
- - MrmSucceeded - - - - - - - -
-
-
- no reaction -
-
-
- - no reaction - - - - - -
-
-
- failure -
-
-
- - failure - - - - - -
-
-
- warning -
-
-
- - warning - - - - - -
-
-
- emergency -
-
-
- - emergency - - - - - -
-
-
- recovery -
-
-
- - recovery - - - - - -
-
-
- success -
-
-
- - success - - - - - -
-
-
fatal
-
-
- - fatal - - - - - - - Viewer does not support full SVG 1.1 - - - diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp deleted file mode 100644 index b77797b2a4205..0000000000000 --- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp +++ /dev/null @@ -1,143 +0,0 @@ -// 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 EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_ -#define EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_ - -// Core -#include -#include - -// Autoware -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -// ROS 2 core -#include -#include - -#include -#include - -struct HazardLampPolicy -{ - bool emergency; -}; - -struct Param -{ - int update_rate; - double timeout_hazard_status; - bool use_parking_after_stopped; - bool use_comfortable_stop; - HazardLampPolicy turning_hazard_on{}; -}; - -class EmergencyHandler : public rclcpp::Node -{ -public: - explicit EmergencyHandler(const rclcpp::NodeOptions & options); - -private: - // Subscribers with callback - rclcpp::Subscription::SharedPtr - sub_hazard_status_stamped_; - rclcpp::Subscription::SharedPtr sub_prev_control_command_; - // Subscribers without callback - autoware::universe_utils::InterProcessPollingSubscriber sub_odom_{ - this, "~/input/odometry"}; - autoware::universe_utils::InterProcessPollingSubscriber< - autoware_vehicle_msgs::msg::ControlModeReport> - sub_control_mode_{this, "~/input/control_mode"}; - autoware::universe_utils::InterProcessPollingSubscriber - sub_mrm_comfortable_stop_status_{this, "~/input/mrm/comfortable_stop/status"}; - autoware::universe_utils::InterProcessPollingSubscriber - sub_mrm_emergency_stop_status_{this, "~/input/mrm/emergency_stop/status"}; - - autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_stamped_; - autoware_control_msgs::msg::Control::ConstSharedPtr prev_control_command_; - - void onHazardStatusStamped( - const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg); - void onPrevControlCommand(const autoware_control_msgs::msg::Control::ConstSharedPtr msg); - - // Publisher - rclcpp::Publisher::SharedPtr pub_control_command_; - - // rclcpp::Publisher::SharedPtr pub_shift_; - // rclcpp::Publisher::SharedPtr pub_turn_signal_; - rclcpp::Publisher::SharedPtr pub_hazard_cmd_; - rclcpp::Publisher::SharedPtr pub_gear_cmd_; - - autoware_vehicle_msgs::msg::HazardLightsCommand createHazardCmdMsg(); - autoware_vehicle_msgs::msg::GearCommand createGearCmdMsg(); - void publishControlCommands(); - - rclcpp::Publisher::SharedPtr pub_mrm_state_; - - autoware_adapi_v1_msgs::msg::MrmState mrm_state_; - void publishMrmState(); - - // Clients - rclcpp::CallbackGroup::SharedPtr client_mrm_comfortable_stop_group_; - rclcpp::Client::SharedPtr client_mrm_comfortable_stop_; - rclcpp::CallbackGroup::SharedPtr client_mrm_emergency_stop_group_; - rclcpp::Client::SharedPtr client_mrm_emergency_stop_; - - void callMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const; - void cancelMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const; - void logMrmCallingResult( - const tier4_system_msgs::srv::OperateMrm::Response & result, const std::string & behavior, - bool is_call) const; - - // Timer - rclcpp::TimerBase::SharedPtr timer_; - - // Parameters - Param param_; - - bool isDataReady(); - void onTimer(); - - // Heartbeat - rclcpp::Time stamp_hazard_status_; - bool is_hazard_status_timeout_; - void checkHazardStatusTimeout(); - - // Algorithm - uint8_t last_gear_command_{autoware_vehicle_msgs::msg::GearCommand::DRIVE}; - void transitionTo(const int new_state); - void updateMrmState(); - void operateMrm(); - autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); - - bool isAutonomous(); - bool isDrivingBackwards(); - bool isEmergency(); - bool isStopped(); - bool isComfortableStopStatusAvailable(); - bool isEmergencyStopStatusAvailable(); -}; - -#endif // EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_ diff --git a/system/emergency_handler/launch/emergency_handler.launch.xml b/system/emergency_handler/launch/emergency_handler.launch.xml deleted file mode 100644 index 203c13bd94e0a..0000000000000 --- a/system/emergency_handler/launch/emergency_handler.launch.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/system/emergency_handler/package.xml b/system/emergency_handler/package.xml deleted file mode 100644 index e16c1b60a5be0..0000000000000 --- a/system/emergency_handler/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - emergency_handler - 0.1.0 - The emergency_handler ROS 2 package - Makoto Kurihara - Ryuta Kambe - Tetsuhiro Kawaguchi - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - autoware_adapi_v1_msgs - autoware_control_msgs - autoware_system_msgs - autoware_universe_utils - autoware_vehicle_msgs - nav_msgs - rclcpp - rclcpp_components - tier4_system_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/system/emergency_handler/schema/emergency_handler.schema.json b/system/emergency_handler/schema/emergency_handler.schema.json deleted file mode 100644 index 3276ae3fa1553..0000000000000 --- a/system/emergency_handler/schema/emergency_handler.schema.json +++ /dev/null @@ -1,65 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for emergency handler", - "type": "object", - "definitions": { - "emergency_handler": { - "type": "object", - "properties": { - "update_rate": { - "type": "integer", - "description": "Timer callback period.", - "default": 10 - }, - "timeout_hazard_status": { - "type": "number", - "description": "If the input `hazard_status` topic cannot be received for more than `timeout_hazard_status`, vehicle will make an emergency stop.", - "default": 0.5 - }, - "use_parking_after_stopped": { - "type": "boolean", - "description": "If this parameter is true, it will publish PARKING shift command.", - "default": "false" - }, - "use_comfortable_stop": { - "type": "boolean", - "description": "If this parameter is true, operate comfortable stop when latent faults occur.", - "default": "false" - }, - "turning_hazard_on": { - "type": "object", - "properties": { - "emergency": { - "type": "boolean", - "description": "If this parameter is true, hazard lamps will be turned on during emergency state.", - "default": "true" - } - }, - "required": ["emergency"] - } - }, - "required": [ - "update_rate", - "timeout_hazard_status", - "use_parking_after_stopped", - "use_comfortable_stop", - "turning_hazard_on" - ], - "additionalProperties": false - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/emergency_handler" - } - }, - "required": ["ros__parameters"], - "additionalProperties": false - } - }, - "required": ["/**"], - "additionalProperties": false -} diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp deleted file mode 100644 index b2ee12afc9c76..0000000000000 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp +++ /dev/null @@ -1,451 +0,0 @@ -// 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. - -#include "emergency_handler/emergency_handler_core.hpp" - -#include -#include -#include - -EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options) -: Node("emergency_handler", options) -{ - // Parameter - param_.update_rate = declare_parameter("update_rate"); - param_.timeout_hazard_status = declare_parameter("timeout_hazard_status"); - param_.use_parking_after_stopped = declare_parameter("use_parking_after_stopped"); - param_.use_comfortable_stop = declare_parameter("use_comfortable_stop"); - param_.turning_hazard_on.emergency = declare_parameter("turning_hazard_on.emergency"); - - using std::placeholders::_1; - - // Subscribers with callback - sub_hazard_status_stamped_ = create_subscription( - "~/input/hazard_status", rclcpp::QoS{1}, - std::bind(&EmergencyHandler::onHazardStatusStamped, this, _1)); - sub_prev_control_command_ = create_subscription( - "~/input/prev_control_command", rclcpp::QoS{1}, - std::bind(&EmergencyHandler::onPrevControlCommand, this, _1)); - - // Publisher - pub_control_command_ = create_publisher( - "~/output/control_command", rclcpp::QoS{1}); - pub_hazard_cmd_ = create_publisher( - "~/output/hazard", rclcpp::QoS{1}); - pub_gear_cmd_ = - create_publisher("~/output/gear", rclcpp::QoS{1}); - pub_mrm_state_ = - create_publisher("~/output/mrm/state", rclcpp::QoS{1}); - - // Clients - client_mrm_comfortable_stop_group_ = - create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - client_mrm_comfortable_stop_ = create_client( - "~/output/mrm/comfortable_stop/operate", rmw_qos_profile_services_default, - client_mrm_comfortable_stop_group_); - client_mrm_emergency_stop_group_ = - create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - client_mrm_emergency_stop_ = create_client( - "~/output/mrm/emergency_stop/operate", rmw_qos_profile_services_default, - client_mrm_emergency_stop_group_); - - // Initialize - mrm_state_.stamp = this->now(); - mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL; - mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE; - is_hazard_status_timeout_ = false; - - // Timer - const auto update_period_ns = rclcpp::Rate(param_.update_rate).period(); - timer_ = rclcpp::create_timer( - this, get_clock(), update_period_ns, std::bind(&EmergencyHandler::onTimer, this)); -} - -void EmergencyHandler::onHazardStatusStamped( - const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg) -{ - hazard_status_stamped_ = msg; - stamp_hazard_status_ = this->now(); -} - -void EmergencyHandler::onPrevControlCommand( - const autoware_control_msgs::msg::Control::ConstSharedPtr msg) -{ - auto control_command = new autoware_control_msgs::msg::Control(*msg); - control_command->stamp = msg->stamp; - prev_control_command_ = autoware_control_msgs::msg::Control::ConstSharedPtr(control_command); -} - -autoware_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHazardCmdMsg() -{ - using autoware_vehicle_msgs::msg::HazardLightsCommand; - HazardLightsCommand msg; - - // Check emergency - const bool is_emergency = isEmergency(); - - if (hazard_status_stamped_->status.emergency_holding) { - // turn hazard on during emergency holding - msg.command = HazardLightsCommand::ENABLE; - } else if (is_emergency && param_.turning_hazard_on.emergency) { - // turn hazard on if vehicle is in emergency state and - // turning hazard on if emergency flag is true - msg.command = HazardLightsCommand::ENABLE; - - } else { - msg.command = HazardLightsCommand::NO_COMMAND; - } - return msg; -} - -void EmergencyHandler::publishControlCommands() -{ - using autoware_vehicle_msgs::msg::GearCommand; - - // Create timestamp - const auto stamp = this->now(); - - // Publish hazard command - pub_hazard_cmd_->publish(createHazardCmdMsg()); - - // Publish gear - { - GearCommand msg; - msg.stamp = stamp; - const auto command = [&]() { - // If stopped and use_parking is not true, send the last gear command - if (isStopped()) - return (param_.use_parking_after_stopped) ? GearCommand::PARK : last_gear_command_; - return (isDrivingBackwards()) ? GearCommand::REVERSE : GearCommand::DRIVE; - }(); - - msg.command = command; - last_gear_command_ = msg.command; - pub_gear_cmd_->publish(msg); - return; - } -} - -void EmergencyHandler::publishMrmState() -{ - mrm_state_.stamp = this->now(); - pub_mrm_state_->publish(mrm_state_); -} - -void EmergencyHandler::operateMrm() -{ - using autoware_adapi_v1_msgs::msg::MrmState; - - if (mrm_state_.state == MrmState::NORMAL) { - // Cancel MRM behavior when returning to NORMAL state - const auto current_mrm_behavior = MrmState::NONE; - if (current_mrm_behavior != mrm_state_.behavior) { - cancelMrmBehavior(mrm_state_.behavior); - mrm_state_.behavior = current_mrm_behavior; - } - return; - } - if (mrm_state_.state == MrmState::MRM_OPERATING) { - const auto current_mrm_behavior = getCurrentMrmBehavior(); - if (current_mrm_behavior != mrm_state_.behavior) { - cancelMrmBehavior(mrm_state_.behavior); - callMrmBehavior(current_mrm_behavior); - mrm_state_.behavior = current_mrm_behavior; - } - return; - } - if (mrm_state_.state == MrmState::MRM_SUCCEEDED) { - // TODO(mkuri): operate MRC behavior - // Do nothing - return; - } - if (mrm_state_.state == MrmState::MRM_FAILED) { - // Do nothing - return; - } - RCLCPP_WARN(this->get_logger(), "invalid MRM state: %d", mrm_state_.state); -} - -void EmergencyHandler::callMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const -{ - using autoware_adapi_v1_msgs::msg::MrmState; - - auto request = std::make_shared(); - request->operate = true; - - if (mrm_behavior == MrmState::NONE) { - RCLCPP_WARN(this->get_logger(), "MRM behavior is None. Do nothing."); - return; - } - if (mrm_behavior == MrmState::COMFORTABLE_STOP) { - auto result = client_mrm_comfortable_stop_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Comfortable stop is operated"); - } else { - RCLCPP_ERROR(this->get_logger(), "Comfortable stop is failed to operate"); - } - return; - } - if (mrm_behavior == MrmState::EMERGENCY_STOP) { - auto result = client_mrm_emergency_stop_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Emergency stop is operated"); - } else { - RCLCPP_ERROR(this->get_logger(), "Emergency stop is failed to operate"); - } - return; - } - RCLCPP_WARN(this->get_logger(), "invalid MRM behavior: %d", mrm_behavior); -} - -void EmergencyHandler::cancelMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const -{ - using autoware_adapi_v1_msgs::msg::MrmState; - - auto request = std::make_shared(); - request->operate = false; - - if (mrm_behavior == MrmState::NONE) { - // Do nothing - return; - } - if (mrm_behavior == MrmState::COMFORTABLE_STOP) { - auto result = client_mrm_comfortable_stop_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Comfortable stop is canceled"); - } else { - RCLCPP_ERROR(this->get_logger(), "Comfortable stop is failed to cancel"); - } - return; - } - if (mrm_behavior == MrmState::EMERGENCY_STOP) { - auto result = client_mrm_emergency_stop_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Emergency stop is canceled"); - } else { - RCLCPP_ERROR(this->get_logger(), "Emergency stop is failed to cancel"); - } - return; - } - RCLCPP_WARN(this->get_logger(), "invalid MRM behavior: %d", mrm_behavior); -} - -bool EmergencyHandler::isDataReady() -{ - if (!hazard_status_stamped_) { - RCLCPP_INFO_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), - "waiting for hazard_status_stamped msg..."); - return false; - } - - if (param_.use_comfortable_stop && !isComfortableStopStatusAvailable()) { - RCLCPP_INFO_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), - "waiting for mrm comfortable stop to become available..."); - return false; - } - - if (!isEmergencyStopStatusAvailable()) { - RCLCPP_INFO_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), - "waiting for mrm emergency stop to become available..."); - return false; - } - - return true; -} - -void EmergencyHandler::checkHazardStatusTimeout() -{ - if ((this->now() - stamp_hazard_status_).seconds() > param_.timeout_hazard_status) { - is_hazard_status_timeout_ = true; - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), - "heartbeat_hazard_status is timeout"); - } else { - is_hazard_status_timeout_ = false; - } -} - -void EmergencyHandler::onTimer() -{ - if (!isDataReady()) { - return; - } - - // Check whether heartbeat hazard_status is timeout - checkHazardStatusTimeout(); - - // Update Emergency State - updateMrmState(); - - // Publish control commands - publishControlCommands(); - operateMrm(); - publishMrmState(); -} - -void EmergencyHandler::transitionTo(const int new_state) -{ - using autoware_adapi_v1_msgs::msg::MrmState; - - const auto state2string = [](const int state) { - if (state == MrmState::NORMAL) { - return "NORMAL"; - } - if (state == MrmState::MRM_OPERATING) { - return "MRM_OPERATING"; - } - if (state == MrmState::MRM_SUCCEEDED) { - return "MRM_SUCCEEDED"; - } - if (state == MrmState::MRM_FAILED) { - return "MRM_FAILED"; - } - - const auto msg = "invalid state: " + std::to_string(state); - throw std::runtime_error(msg); - }; - - RCLCPP_DEBUG( - this->get_logger(), "MRM State changed: %s -> %s", state2string(mrm_state_.state), - state2string(new_state)); - - mrm_state_.state = new_state; -} - -void EmergencyHandler::updateMrmState() -{ - using autoware_adapi_v1_msgs::msg::MrmState; - using autoware_vehicle_msgs::msg::ControlModeReport; - - // Check emergency - const bool is_emergency = isEmergency(); - - // Get mode - const bool is_auto_mode = isAutonomous(); - - // State Machine - if (mrm_state_.state == MrmState::NORMAL) { - // NORMAL - if (is_auto_mode && is_emergency) { - transitionTo(MrmState::MRM_OPERATING); - return; - } - } else { - // Emergency - // Send recovery events if "not emergency" - if (!is_emergency) { - transitionTo(MrmState::NORMAL); - return; - } - - if (mrm_state_.state == MrmState::MRM_OPERATING) { - // TODO(Kenji Miyake): Check MRC is accomplished - if (isStopped()) { - transitionTo(MrmState::MRM_SUCCEEDED); - return; - } - } else if (mrm_state_.state == MrmState::MRM_SUCCEEDED) { - // Do nothing(only checking common recovery events) - } else if (mrm_state_.state == MrmState::MRM_FAILED) { - // Do nothing(only checking common recovery events) - } else { - const auto msg = "invalid state: " + std::to_string(mrm_state_.state); - throw std::runtime_error(msg); - } - } -} - -autoware_adapi_v1_msgs::msg::MrmState::_behavior_type EmergencyHandler::getCurrentMrmBehavior() -{ - using autoware_adapi_v1_msgs::msg::MrmState; - using autoware_system_msgs::msg::HazardStatus; - - // Get hazard level - auto level = hazard_status_stamped_->status.level; - if (is_hazard_status_timeout_) { - level = HazardStatus::SINGLE_POINT_FAULT; - } - - // State machine - if (mrm_state_.behavior == MrmState::NONE) { - if (level == HazardStatus::LATENT_FAULT) { - if (param_.use_comfortable_stop) { - return MrmState::COMFORTABLE_STOP; - } - return MrmState::EMERGENCY_STOP; - } - if (level == HazardStatus::SINGLE_POINT_FAULT) { - return MrmState::EMERGENCY_STOP; - } - } - if (mrm_state_.behavior == MrmState::COMFORTABLE_STOP) { - if (level == HazardStatus::SINGLE_POINT_FAULT) { - return MrmState::EMERGENCY_STOP; - } - } - - return mrm_state_.behavior; -} - -bool EmergencyHandler::isAutonomous() -{ - using autoware_vehicle_msgs::msg::ControlModeReport; - auto mode = sub_control_mode_.takeData(); - if (mode == nullptr) return false; - return mode->mode == ControlModeReport::AUTONOMOUS; -} - -bool EmergencyHandler::isEmergency() -{ - return hazard_status_stamped_->status.emergency || - hazard_status_stamped_->status.emergency_holding || is_hazard_status_timeout_; -} - -bool EmergencyHandler::isStopped() -{ - auto odom = sub_odom_.takeData(); - if (odom == nullptr) return false; - constexpr auto th_stopped_velocity = 0.001; - return (std::abs(odom->twist.twist.linear.x) < th_stopped_velocity); -} - -bool EmergencyHandler::isComfortableStopStatusAvailable() -{ - auto status = sub_mrm_comfortable_stop_status_.takeData(); - if (status == nullptr) return false; - return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; -} - -bool EmergencyHandler::isEmergencyStopStatusAvailable() -{ - auto status = sub_mrm_emergency_stop_status_.takeData(); - if (status == nullptr) return false; - return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; -} - -bool EmergencyHandler::isDrivingBackwards() -{ - auto odom = sub_odom_.takeData(); - if (odom == nullptr) return false; - constexpr auto th_moving_backwards = -0.001; - return odom->twist.twist.linear.x < th_moving_backwards; -} - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(EmergencyHandler) From 43373a7c6ca186dea3d4289f2842e1bc1571628b Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 20 Sep 2024 15:05:26 +0900 Subject: [PATCH 19/95] fix(tier4_localization_rviz_plugin): fix unmatchedSuppression (#8919) fix:unmatchedSuppression Signed-off-by: kobayu858 --- .../src/pose_history/pose_history_display.cpp | 1 - .../pose_with_covariance_history_display.cpp | 4 ---- 2 files changed, 5 deletions(-) diff --git a/common/tier4_localization_rviz_plugin/src/pose_history/pose_history_display.cpp b/common/tier4_localization_rviz_plugin/src/pose_history/pose_history_display.cpp index 85c8a81ea7932..d427f63628d69 100644 --- a/common/tier4_localization_rviz_plugin/src/pose_history/pose_history_display.cpp +++ b/common/tier4_localization_rviz_plugin/src/pose_history/pose_history_display.cpp @@ -44,7 +44,6 @@ PoseHistory::PoseHistory() : last_stamp_(0, 0, RCL_ROS_TIME) PoseHistory::~PoseHistory() = default; // Properties are deleted by Qt -// cppcheck-suppress unusedFunction void PoseHistory::onInitialize() { MFDClass::onInitialize(); diff --git a/common/tier4_localization_rviz_plugin/src/pose_with_covariance_history/pose_with_covariance_history_display.cpp b/common/tier4_localization_rviz_plugin/src/pose_with_covariance_history/pose_with_covariance_history_display.cpp index c8fde566c5211..42f36ed6f4c08 100644 --- a/common/tier4_localization_rviz_plugin/src/pose_with_covariance_history/pose_with_covariance_history_display.cpp +++ b/common/tier4_localization_rviz_plugin/src/pose_with_covariance_history/pose_with_covariance_history_display.cpp @@ -92,19 +92,16 @@ void PoseWithCovarianceHistory::onInitialize() lines_ = std::make_unique(scene_manager_, scene_node_); } -// cppcheck-suppress unusedFunction void PoseWithCovarianceHistory::onEnable() { subscribe(); } -// cppcheck-suppress unusedFunction void PoseWithCovarianceHistory::onDisable() { unsubscribe(); } -// cppcheck-suppress unusedFunction void PoseWithCovarianceHistory::update(float wall_dt, float ros_dt) { (void)wall_dt; @@ -151,7 +148,6 @@ void PoseWithCovarianceHistory::update_shape_type() property_arrow_color_->setHidden(!is_arrow); } -// cppcheck-suppress unusedFunction void PoseWithCovarianceHistory::processMessage( const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr message) { From a98cfe8864e7a4bc1d7202973c660b1058b13a1a Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Fri, 20 Sep 2024 15:48:44 +0900 Subject: [PATCH 20/95] fix(motion_velocity_planner): disable randomly failing test (#8923) Signed-off-by: Maxime CLEMENT --- .../test/test_collision_checker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp index fd9a7d6e6649b..5988272b39ae1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp @@ -74,7 +74,7 @@ bool all_within(const MultiPoint2d & pts1, const MultiPoint2d & pts2) return true; } -TEST(TestCollisionChecker, Benchmark) +TEST(TestCollisionChecker, DISABLED_Benchmark) { constexpr auto nb_ego_footprints = 1000; constexpr auto nb_obstacles = 1000; From c70daa044194de87f9472098447ac2f157ce6e5f Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Fri, 20 Sep 2024 17:15:45 +0900 Subject: [PATCH 21/95] fix(static_obstacle_avoidance, avoidance_by_lane_change): remove unused variable (#8926) remove unused variables Signed-off-by: Go Sakayori --- .../config/avoidance_by_lane_change.param.yaml | 8 -------- .../src/manager.cpp | 1 - .../data_structs.hpp | 2 -- .../src/utils.cpp | 2 -- 4 files changed, 13 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml index 384ef7bd285e9..f6974e38a2a50 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml @@ -7,7 +7,6 @@ # avoidance is performed for the object type with true target_object: car: - execute_num: 2 # [-] th_moving_speed: 1.0 # [m/s] th_moving_time: 1.0 # [s] max_expand_ratio: 0.0 # [-] @@ -17,7 +16,6 @@ hard_margin: 0.0 # [m] hard_margin_for_parked_vehicle: 0.0 # [m] truck: - execute_num: 2 th_moving_speed: 1.0 # 3.6km/h th_moving_time: 1.0 max_expand_ratio: 0.0 @@ -27,7 +25,6 @@ hard_margin: 0.0 # [m] hard_margin_for_parked_vehicle: 0.0 # [m] bus: - execute_num: 2 th_moving_speed: 1.0 # 3.6km/h th_moving_time: 1.0 max_expand_ratio: 0.0 @@ -37,7 +34,6 @@ hard_margin: 0.0 # [m] hard_margin_for_parked_vehicle: 0.0 # [m] trailer: - execute_num: 2 th_moving_speed: 1.0 # 3.6km/h th_moving_time: 1.0 max_expand_ratio: 0.0 @@ -47,7 +43,6 @@ hard_margin: 0.0 # [m] hard_margin_for_parked_vehicle: 0.0 # [m] unknown: - execute_num: 1 th_moving_speed: 0.28 # 1.0km/h th_moving_time: 1.0 max_expand_ratio: 0.0 @@ -57,7 +52,6 @@ hard_margin: 0.0 # [m] hard_margin_for_parked_vehicle: 0.0 # [m] bicycle: - execute_num: 2 th_moving_speed: 0.28 # 1.0km/h th_moving_time: 1.0 max_expand_ratio: 0.0 @@ -67,7 +61,6 @@ hard_margin: 1.0 # [m] hard_margin_for_parked_vehicle: 1.0 # [m] motorcycle: - execute_num: 2 th_moving_speed: 1.0 # 3.6km/h th_moving_time: 1.0 max_expand_ratio: 0.0 @@ -77,7 +70,6 @@ hard_margin: 1.0 # [m] hard_margin_for_parked_vehicle: 1.0 # [m] pedestrian: - execute_num: 2 th_moving_speed: 0.28 # 1.0km/h th_moving_time: 1.0 max_expand_ratio: 0.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index ddcfda50d18c4..ef02bfaa7b0dd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -66,7 +66,6 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) { const auto get_object_param = [&](std::string && ns) { ObjectParameter param{}; - param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); param.moving_speed_threshold = getOrDeclareParameter(*node, ns + "th_moving_speed"); param.moving_time_threshold = getOrDeclareParameter(*node, ns + "th_moving_time"); param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp index 35ee73ca8423e..c8236c4e50cd8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -73,8 +73,6 @@ struct ObjectParameter bool is_safety_check_target{false}; - size_t execute_num{1}; - double moving_speed_threshold{0.0}; double moving_time_threshold{1.0}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index a7166b52fc4e5..0b8abb96cfc1f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -256,8 +256,6 @@ bool isWithinIntersection( return false; } - const auto object_polygon = autoware::universe_utils::toPolygon2d(object.object); - const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id.c_str())); From 3c7327a3a6c92ab85488a26f443f36ec9e3b19e6 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Fri, 20 Sep 2024 17:29:39 +0900 Subject: [PATCH 22/95] feat(lane_change): add checks to ensure the edge of vehicle do not exceed target lane boundary when changing lanes (#8750) * check if LC candidate path footprint exceeds target lane far bound Signed-off-by: mohammad alqudah * add parameter to enable/disable check Signed-off-by: mohammad alqudah * check only lane changing section of cadidate path Signed-off-by: mohammad alqudah * fix spelling Signed-off-by: mohammad alqudah * small refactoring Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah --- .../config/lane_change.param.yaml | 1 + .../utils/data_structs.hpp | 1 + .../utils/utils.hpp | 14 +++++-- .../src/manager.cpp | 2 + .../src/scene.cpp | 9 +++++ .../src/utils/utils.cpp | 38 +++++++++++++++++++ 6 files changed, 61 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index 4c55beb89e813..0a2c9991ec5fd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -34,6 +34,7 @@ # safety check safety_check: allow_loose_check_for_cancel: true + enable_target_lane_bound_check: true collision_check_yaw_diff_threshold: 3.1416 execution: expected_front_deceleration: -1.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 194412dfe01b7..4ba2e1aef893c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -154,6 +154,7 @@ struct Parameters // safety check bool allow_loose_check_for_cancel{true}; + bool enable_target_lane_bound_check{true}; double collision_check_yaw_diff_threshold{3.1416}; utils::path_safety_checker::RSSparams rss_params{}; utils::path_safety_checker::RSSparams rss_params_for_parked{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 84c3f310f0382..bf80b492d533e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -45,6 +45,7 @@ using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVeloc using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; using autoware::route_handler::Direction; using autoware::universe_utils::Polygon2d; +using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; @@ -94,6 +95,10 @@ bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); +bool pathFootprintExceedsTargetLaneBound( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, + const double margin = 0.1); + std::optional constructCandidatePath( const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, @@ -217,8 +222,9 @@ rclcpp::Logger getLogger(const std::string & type); * * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle. */ -Polygon2d getEgoCurrentFootprint( - const Pose & ego_pose, const autoware::vehicle_info_utils::VehicleInfo & ego_info); +Polygon2d getEgoCurrentFootprint(const Pose & ego_pose, const VehicleInfo & ego_info); + +Point getEgoFrontVertex(const Pose & ego_pose, const VehicleInfo & ego_info, bool left); /** * @brief Checks if the given polygon is within an intersection area. @@ -305,8 +311,8 @@ namespace autoware::behavior_path_planner::utils::lane_change::debug geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose); geometry_msgs::msg::Polygon createExecutionArea( - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Pose & pose, - double additional_lon_offset, double additional_lat_offset); + const VehicleInfo & vehicle_info, const Pose & pose, double additional_lon_offset, + double additional_lat_offset); } // namespace autoware::behavior_path_planner::utils::lane_change::debug #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 69df2fe14317a..dd7382d8f7116 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -102,6 +102,8 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) // safety check p.allow_loose_check_for_cancel = getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); + p.enable_target_lane_bound_check = + getOrDeclareParameter(*node, parameter("safety_check.enable_target_lane_bound_check")); p.collision_check_yaw_diff_threshold = getOrDeclareParameter( *node, parameter("safety_check.collision_check_yaw_diff_threshold")); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index e59119230c224..61f84c31c88da 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -1686,6 +1686,15 @@ bool NormalLaneChange::getLaneChangePaths( return false; } + if ( + lane_change_parameters_->enable_target_lane_bound_check && + utils::lane_change::pathFootprintExceedsTargetLaneBound( + common_data_ptr_, candidate_path.value().shifted_path.path, + common_parameters.vehicle_info)) { + debug_print_lat("Reject: Path footprint exceeds target lane boundary. Skip lane change."); + return false; + } + const auto is_safe = std::invoke([&]() { constexpr size_t decel_sampling_num = 1; const auto safety_check_with_normal_rss = isLaneChangePathSafe( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 6ff69abb85d18..d163c00369b12 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -212,6 +212,36 @@ bool isPathInLanelets( return true; } +bool pathFootprintExceedsTargetLaneBound( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, + const double margin) +{ + if (common_data_ptr->direction == Direction::NONE || path.points.empty()) { + return false; + } + + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const bool is_left = common_data_ptr->direction == Direction::LEFT; + + const auto combined_target_lane = lanelet::utils::combineLaneletsShape(target_lanes); + + for (const auto & path_point : path.points) { + const auto & pose = path_point.point.pose; + const auto front_vertex = getEgoFrontVertex(pose, ego_info, is_left); + + const auto sign = is_left ? -1.0 : 1.0; + const auto dist_to_boundary = + sign * utils::getSignedDistanceFromLaneBoundary(combined_target_lane, front_vertex, is_left); + + if (dist_to_boundary < margin) { + RCLCPP_DEBUG(get_logger(), "Path footprint exceeds target lane boundary"); + return true; + } + } + + return false; +} + std::optional constructCandidatePath( const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, @@ -1048,6 +1078,14 @@ Polygon2d getEgoCurrentFootprint( return autoware::universe_utils::toFootprint(ego_pose, base_to_front, base_to_rear, width); } +Point getEgoFrontVertex( + const Pose & ego_pose, const autoware::vehicle_info_utils::VehicleInfo & ego_info, bool left) +{ + const double lon_offset = ego_info.wheel_base_m + ego_info.front_overhang_m; + const double lat_offset = 0.5 * (left ? ego_info.vehicle_width_m : -ego_info.vehicle_width_m); + return autoware::universe_utils::calcOffsetPose(ego_pose, lon_offset, lat_offset, 0.0).position; +} + bool isWithinIntersection( const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon) From fc283c5f8a77d5cce3b63b0c0938ecd90f3a6f63 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Fri, 20 Sep 2024 10:46:56 +0200 Subject: [PATCH 23/95] refactor(signal_processing): prefix package and namespace with autoware (#8541) Signed-off-by: Esteve Fernandez --- .github/CODEOWNERS | 2 +- .../CMakeLists.txt | 2 +- .../README.md | 0 .../documentation/ButterworthFilter.md | 0 .../documentation/img.png | Bin .../autoware}/signal_processing/butterworth.hpp | 10 +++++++--- .../autoware}/signal_processing/lowpass_filter.hpp | 11 +++++++---- .../signal_processing/lowpass_filter_1d.hpp | 10 +++++----- .../package.xml | 2 +- .../src/butterworth.cpp | 5 ++++- .../src/lowpass_filter.cpp | 5 ++++- .../src/lowpass_filter_1d.cpp | 6 +++--- .../test/include/butterworth_filter_test.hpp | 2 +- .../test/src/butterworth_filter_test.cpp | 2 ++ .../test/src/lowpass_filter_1d_test.cpp | 4 +++- .../test/src/lowpass_filter_test.cpp | 4 +++- .../usage_examples/main_butterworth.cpp | 2 +- .../control_performance_analysis_node.hpp | 2 +- control/control_performance_analysis/package.xml | 2 +- localization/autoware_twist2accel/package.xml | 2 +- .../autoware_twist2accel/src/twist2accel.cpp | 2 ++ .../autoware_twist2accel/src/twist2accel.hpp | 4 +++- .../yabloc_common/ground_server/ground_server.hpp | 4 +++- localization/yabloc/yabloc_common/package.xml | 2 +- .../autoware/obstacle_cruise_planner/node.hpp | 2 +- .../pid_based_planner/pid_based_planner.hpp | 4 +++- .../autoware_obstacle_cruise_planner/package.xml | 2 +- .../src/pid_based_planner/pid_based_planner.cpp | 2 ++ .../src/planner_interface.cpp | 8 ++++---- planning/autoware_obstacle_stop_planner/package.xml | 2 +- .../package.xml | 2 +- .../src/scene.cpp | 2 +- .../autoware_behavior_path_planner/package.xml | 2 +- .../package.xml | 2 +- .../package.xml | 2 +- 35 files changed, 72 insertions(+), 43 deletions(-) rename common/{signal_processing => autoware_signal_processing}/CMakeLists.txt (94%) rename common/{signal_processing => autoware_signal_processing}/README.md (100%) rename common/{signal_processing => autoware_signal_processing}/documentation/ButterworthFilter.md (100%) rename common/{signal_processing => autoware_signal_processing}/documentation/img.png (100%) rename common/{signal_processing/include => autoware_signal_processing/include/autoware}/signal_processing/butterworth.hpp (94%) rename common/{signal_processing/include => autoware_signal_processing/include/autoware}/signal_processing/lowpass_filter.hpp (82%) rename common/{signal_processing/include => autoware_signal_processing/include/autoware}/signal_processing/lowpass_filter_1d.hpp (81%) rename common/{signal_processing => autoware_signal_processing}/package.xml (96%) rename common/{signal_processing => autoware_signal_processing}/src/butterworth.cpp (98%) rename common/{signal_processing => autoware_signal_processing}/src/lowpass_filter.cpp (89%) rename common/{signal_processing => autoware_signal_processing}/src/lowpass_filter_1d.cpp (89%) rename common/{signal_processing => autoware_signal_processing}/test/include/butterworth_filter_test.hpp (94%) rename common/{signal_processing => autoware_signal_processing}/test/src/butterworth_filter_test.cpp (98%) rename common/{signal_processing => autoware_signal_processing}/test/src/lowpass_filter_1d_test.cpp (92%) rename common/{signal_processing => autoware_signal_processing}/test/src/lowpass_filter_test.cpp (95%) rename common/{signal_processing => autoware_signal_processing}/usage_examples/main_butterworth.cpp (98%) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index e21c06112069f..b31f6c07d7426 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -10,6 +10,7 @@ common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodr common/autoware_path_distance_calculator/** isamu.takagi@tier4.jp common/autoware_perception_rviz_plugin/** opensource@apex.ai shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp common/autoware_point_types/** david.wong@tier4.jp max.schmeller@tier4.jp +common/autoware_signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp common/autoware_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_universe_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @@ -27,7 +28,6 @@ common/object_recognition_utils/** shunsuke.miura@tier4.jp takayuki.murooka@tier common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/polar_grid/** yukihiro.saito@tier4.jp common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp -common/signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp common/tensorrt_common/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp common/tier4_api_utils/** isamu.takagi@tier4.jp diff --git a/common/signal_processing/CMakeLists.txt b/common/autoware_signal_processing/CMakeLists.txt similarity index 94% rename from common/signal_processing/CMakeLists.txt rename to common/autoware_signal_processing/CMakeLists.txt index 89f14d4339223..7e6a0429fe93f 100644 --- a/common/signal_processing/CMakeLists.txt +++ b/common/autoware_signal_processing/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(signal_processing) +project(autoware_signal_processing) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/common/signal_processing/README.md b/common/autoware_signal_processing/README.md similarity index 100% rename from common/signal_processing/README.md rename to common/autoware_signal_processing/README.md diff --git a/common/signal_processing/documentation/ButterworthFilter.md b/common/autoware_signal_processing/documentation/ButterworthFilter.md similarity index 100% rename from common/signal_processing/documentation/ButterworthFilter.md rename to common/autoware_signal_processing/documentation/ButterworthFilter.md diff --git a/common/signal_processing/documentation/img.png b/common/autoware_signal_processing/documentation/img.png similarity index 100% rename from common/signal_processing/documentation/img.png rename to common/autoware_signal_processing/documentation/img.png diff --git a/common/signal_processing/include/signal_processing/butterworth.hpp b/common/autoware_signal_processing/include/autoware/signal_processing/butterworth.hpp similarity index 94% rename from common/signal_processing/include/signal_processing/butterworth.hpp rename to common/autoware_signal_processing/include/autoware/signal_processing/butterworth.hpp index 579a915cc9968..11099aca1f206 100644 --- a/common/signal_processing/include/signal_processing/butterworth.hpp +++ b/common/autoware_signal_processing/include/autoware/signal_processing/butterworth.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIGNAL_PROCESSING__BUTTERWORTH_HPP_ -#define SIGNAL_PROCESSING__BUTTERWORTH_HPP_ +#ifndef AUTOWARE__SIGNAL_PROCESSING__BUTTERWORTH_HPP_ +#define AUTOWARE__SIGNAL_PROCESSING__BUTTERWORTH_HPP_ #include #include @@ -21,6 +21,9 @@ #include #include +namespace autoware::signal_processing +{ + template const T & append_separator(const T & arg) { @@ -133,5 +136,6 @@ class ButterworthFilter // Computes continuous time roots from the phase angles void computeContinuousTimeRoots(bool const & use_sampling_frequency = false); }; +} // namespace autoware::signal_processing -#endif // SIGNAL_PROCESSING__BUTTERWORTH_HPP_ +#endif // AUTOWARE__SIGNAL_PROCESSING__BUTTERWORTH_HPP_ diff --git a/common/signal_processing/include/signal_processing/lowpass_filter.hpp b/common/autoware_signal_processing/include/autoware/signal_processing/lowpass_filter.hpp similarity index 82% rename from common/signal_processing/include/signal_processing/lowpass_filter.hpp rename to common/autoware_signal_processing/include/autoware/signal_processing/lowpass_filter.hpp index 89b7a6da263f7..f09e1dda461da 100644 --- a/common/signal_processing/include/signal_processing/lowpass_filter.hpp +++ b/common/autoware_signal_processing/include/autoware/signal_processing/lowpass_filter.hpp @@ -12,13 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIGNAL_PROCESSING__LOWPASS_FILTER_HPP_ -#define SIGNAL_PROCESSING__LOWPASS_FILTER_HPP_ +#ifndef AUTOWARE__SIGNAL_PROCESSING__LOWPASS_FILTER_HPP_ +#define AUTOWARE__SIGNAL_PROCESSING__LOWPASS_FILTER_HPP_ -#include "signal_processing/lowpass_filter_1d.hpp" +#include "autoware/signal_processing/lowpass_filter_1d.hpp" #include "geometry_msgs/msg/twist.hpp" +namespace autoware::signal_processing +{ /** * @class First-order low-pass filter * @brief filtering values @@ -51,5 +53,6 @@ class LowpassFilterTwist : public LowpassFilterInterface -namespace signal_processing +namespace autoware::signal_processing { double lowpassFilter(const double current_val, const double prev_val, const double gain); -} /** * @class First-order low-pass filter @@ -43,5 +42,6 @@ class LowpassFilter1d boost::optional getValue() const; double filter(const double u); }; +} // namespace autoware::signal_processing -#endif // SIGNAL_PROCESSING__LOWPASS_FILTER_1D_HPP_ +#endif // AUTOWARE__SIGNAL_PROCESSING__LOWPASS_FILTER_1D_HPP_ diff --git a/common/signal_processing/package.xml b/common/autoware_signal_processing/package.xml similarity index 96% rename from common/signal_processing/package.xml rename to common/autoware_signal_processing/package.xml index 2412f916dbd15..18841347dab9f 100644 --- a/common/signal_processing/package.xml +++ b/common/autoware_signal_processing/package.xml @@ -1,7 +1,7 @@ - signal_processing + autoware_signal_processing 0.1.0 The signal processing package Takayuki Murooka diff --git a/common/signal_processing/src/butterworth.cpp b/common/autoware_signal_processing/src/butterworth.cpp similarity index 98% rename from common/signal_processing/src/butterworth.cpp rename to common/autoware_signal_processing/src/butterworth.cpp index a0e4d61f1412b..7f491d443787b 100644 --- a/common/signal_processing/src/butterworth.cpp +++ b/common/autoware_signal_processing/src/butterworth.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "signal_processing/butterworth.hpp" +#include "autoware/signal_processing/butterworth.hpp" #include @@ -20,6 +20,8 @@ #include #include +namespace autoware::signal_processing +{ /** * @brief Computes the minimum of an analog Butterworth filter order and cut-off frequency give * the pass and stop-band frequencies and ripple magnitude (tolerances). @@ -341,3 +343,4 @@ void ButterworthFilter::printFilterSpecs() const rclcpp::get_logger("rclcpp"), "Cut-off Frequency : %2.2f rad/sec", this->filter_specs_.Wc_rad_sec); } +} // namespace autoware::signal_processing diff --git a/common/signal_processing/src/lowpass_filter.cpp b/common/autoware_signal_processing/src/lowpass_filter.cpp similarity index 89% rename from common/signal_processing/src/lowpass_filter.cpp rename to common/autoware_signal_processing/src/lowpass_filter.cpp index c5119828f4c52..6d275bb5023b3 100644 --- a/common/signal_processing/src/lowpass_filter.cpp +++ b/common/autoware_signal_processing/src/lowpass_filter.cpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "signal_processing/lowpass_filter.hpp" +#include "autoware/signal_processing/lowpass_filter.hpp" +namespace autoware::signal_processing +{ geometry_msgs::msg::Twist LowpassFilterTwist::filter(const geometry_msgs::msg::Twist & u) { if (x_) { @@ -31,3 +33,4 @@ geometry_msgs::msg::Twist LowpassFilterTwist::filter(const geometry_msgs::msg::T x_ = u; return x_.get(); } +} // namespace autoware::signal_processing diff --git a/common/signal_processing/src/lowpass_filter_1d.cpp b/common/autoware_signal_processing/src/lowpass_filter_1d.cpp similarity index 89% rename from common/signal_processing/src/lowpass_filter_1d.cpp rename to common/autoware_signal_processing/src/lowpass_filter_1d.cpp index 08de005f736c1..f5835ae160ba7 100644 --- a/common/signal_processing/src/lowpass_filter_1d.cpp +++ b/common/autoware_signal_processing/src/lowpass_filter_1d.cpp @@ -12,15 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "signal_processing/lowpass_filter_1d.hpp" +#include "autoware/signal_processing/lowpass_filter_1d.hpp" -namespace signal_processing +namespace autoware::signal_processing { double lowpassFilter(const double current_val, const double prev_val, const double gain) { return gain * prev_val + (1.0 - gain) * current_val; } -} // namespace signal_processing LowpassFilter1d::LowpassFilter1d(const double gain) : gain_(gain) { @@ -57,3 +56,4 @@ double LowpassFilter1d::filter(const double u) x_ = u; return x_.get(); } +} // namespace autoware::signal_processing diff --git a/common/signal_processing/test/include/butterworth_filter_test.hpp b/common/autoware_signal_processing/test/include/butterworth_filter_test.hpp similarity index 94% rename from common/signal_processing/test/include/butterworth_filter_test.hpp rename to common/autoware_signal_processing/test/include/butterworth_filter_test.hpp index 6a43b0b95a7e2..83b060373a442 100644 --- a/common/signal_processing/test/include/butterworth_filter_test.hpp +++ b/common/autoware_signal_processing/test/include/butterworth_filter_test.hpp @@ -15,9 +15,9 @@ #ifndef BUTTERWORTH_FILTER_TEST_HPP_ #define BUTTERWORTH_FILTER_TEST_HPP_ +#include "autoware/signal_processing/butterworth.hpp" #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "signal_processing/butterworth.hpp" class ButterWorthTestFixture : public ::testing::Test { diff --git a/common/signal_processing/test/src/butterworth_filter_test.cpp b/common/autoware_signal_processing/test/src/butterworth_filter_test.cpp similarity index 98% rename from common/signal_processing/test/src/butterworth_filter_test.cpp rename to common/autoware_signal_processing/test/src/butterworth_filter_test.cpp index d85b9a37f870f..bd5d1dbbf6948 100644 --- a/common/signal_processing/test/src/butterworth_filter_test.cpp +++ b/common/autoware_signal_processing/test/src/butterworth_filter_test.cpp @@ -14,6 +14,8 @@ #include "butterworth_filter_test.hpp" +using autoware::signal_processing::ButterworthFilter; + TEST_F(ButterWorthTestFixture, butterworthOrderTest) { double tol = 1e-4; diff --git a/common/signal_processing/test/src/lowpass_filter_1d_test.cpp b/common/autoware_signal_processing/test/src/lowpass_filter_1d_test.cpp similarity index 92% rename from common/signal_processing/test/src/lowpass_filter_1d_test.cpp rename to common/autoware_signal_processing/test/src/lowpass_filter_1d_test.cpp index 5cce36884588e..44e8a1ffce437 100644 --- a/common/signal_processing/test/src/lowpass_filter_1d_test.cpp +++ b/common/autoware_signal_processing/test/src/lowpass_filter_1d_test.cpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "signal_processing/lowpass_filter_1d.hpp" +#include "autoware/signal_processing/lowpass_filter_1d.hpp" #include constexpr double epsilon = 1e-6; +using autoware::signal_processing::LowpassFilter1d; + TEST(lowpass_filter_1d, filter) { LowpassFilter1d lowpass_filter_1d(0.1); diff --git a/common/signal_processing/test/src/lowpass_filter_test.cpp b/common/autoware_signal_processing/test/src/lowpass_filter_test.cpp similarity index 95% rename from common/signal_processing/test/src/lowpass_filter_test.cpp rename to common/autoware_signal_processing/test/src/lowpass_filter_test.cpp index 8dfea4dcae02e..864378719dd5c 100644 --- a/common/signal_processing/test/src/lowpass_filter_test.cpp +++ b/common/autoware_signal_processing/test/src/lowpass_filter_test.cpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "signal_processing/lowpass_filter.hpp" +#include "autoware/signal_processing/lowpass_filter.hpp" #include constexpr double epsilon = 1e-6; +using autoware::signal_processing::LowpassFilterTwist; + geometry_msgs::msg::Twist createTwist( const double lx, const double ly, const double lz, const double ax, const double ay, const double az) diff --git a/common/signal_processing/usage_examples/main_butterworth.cpp b/common/autoware_signal_processing/usage_examples/main_butterworth.cpp similarity index 98% rename from common/signal_processing/usage_examples/main_butterworth.cpp rename to common/autoware_signal_processing/usage_examples/main_butterworth.cpp index 2b36bd40b3578..5de87af046879 100644 --- a/common/signal_processing/usage_examples/main_butterworth.cpp +++ b/common/autoware_signal_processing/usage_examples/main_butterworth.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "signal_processing/butterworth.hpp" +#include "autoware/signal_processing/butterworth.hpp" #include #include diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp index b359eb16387db..16e383916eb6d 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp @@ -19,9 +19,9 @@ #include "control_performance_analysis/msg/driving_monitor_stamped.hpp" #include "control_performance_analysis/msg/error_stamped.hpp" +#include #include #include -#include #include #include diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index 878136d837678..a088b4a4a168e 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/control_performance_analysis/package.xml @@ -27,6 +27,7 @@ autoware_control_msgs autoware_motion_utils autoware_planning_msgs + autoware_signal_processing autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs @@ -36,7 +37,6 @@ rclcpp rclcpp_components sensor_msgs - signal_processing std_msgs tf2 tf2_eigen diff --git a/localization/autoware_twist2accel/package.xml b/localization/autoware_twist2accel/package.xml index af8938fd74a9f..6a4e021858f7c 100644 --- a/localization/autoware_twist2accel/package.xml +++ b/localization/autoware_twist2accel/package.xml @@ -17,11 +17,11 @@ ament_cmake_auto autoware_cmake + autoware_signal_processing geometry_msgs nav_msgs rclcpp rclcpp_components - signal_processing tf2 tier4_debug_msgs diff --git a/localization/autoware_twist2accel/src/twist2accel.cpp b/localization/autoware_twist2accel/src/twist2accel.cpp index 2205a9d3a674e..0f904132f6cdc 100644 --- a/localization/autoware_twist2accel/src/twist2accel.cpp +++ b/localization/autoware_twist2accel/src/twist2accel.cpp @@ -22,6 +22,8 @@ #include #include +using autoware::signal_processing::LowpassFilter1d; + namespace autoware::twist2accel { using std::placeholders::_1; diff --git a/localization/autoware_twist2accel/src/twist2accel.hpp b/localization/autoware_twist2accel/src/twist2accel.hpp index 97060c080179b..d338b256fec77 100644 --- a/localization/autoware_twist2accel/src/twist2accel.hpp +++ b/localization/autoware_twist2accel/src/twist2accel.hpp @@ -15,7 +15,7 @@ #ifndef TWIST2ACCEL_HPP_ #define TWIST2ACCEL_HPP_ -#include "signal_processing/lowpass_filter_1d.hpp" +#include "autoware/signal_processing/lowpass_filter_1d.hpp" #include @@ -37,6 +37,8 @@ #include #include +using autoware::signal_processing::LowpassFilter1d; + namespace autoware::twist2accel { class Twist2Accel : public rclcpp::Node diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp index e8f7e6e081e43..0b02fd59b322a 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp +++ b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp @@ -17,8 +17,8 @@ #include "yabloc_common/ground_server/filter/moving_averaging.hpp" +#include #include -#include #include #include @@ -38,6 +38,8 @@ #include +using autoware::signal_processing::LowpassFilter1d; + namespace yabloc::ground_server { class GroundServer : public rclcpp::Node diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index ee0ef087579f7..67fe462282981 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -18,6 +18,7 @@ autoware_lanelet2_extension autoware_map_msgs + autoware_signal_processing autoware_universe_utils cv_bridge geometry_msgs @@ -26,7 +27,6 @@ rclcpp rclcpp_components sensor_msgs - signal_processing sophus std_msgs tf2_ros diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp index 29bd91f5c5b72..35cbf31239272 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp @@ -19,10 +19,10 @@ #include "autoware/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp" #include "autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" #include "autoware/obstacle_cruise_planner/type_alias.hpp" +#include "autoware/signal_processing/lowpass_filter_1d.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" -#include "signal_processing/lowpass_filter_1d.hpp" #include #include diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp index c048c3278e2ec..d063d08d82d92 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp @@ -18,7 +18,7 @@ #include "autoware/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp" #include "autoware/obstacle_cruise_planner/pid_based_planner/pid_controller.hpp" #include "autoware/obstacle_cruise_planner/planner_interface.hpp" -#include "signal_processing/lowpass_filter_1d.hpp" +#include "autoware/signal_processing/lowpass_filter_1d.hpp" #include "visualization_msgs/msg/marker_array.hpp" @@ -26,6 +26,8 @@ #include #include +using autoware::signal_processing::LowpassFilter1d; + class PIDBasedPlanner : public PlannerInterface { public: diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index 79335dc23221b..a781a20388106 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -23,6 +23,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager + autoware_signal_processing autoware_universe_utils autoware_vehicle_info_utils geometry_msgs @@ -32,7 +33,6 @@ osqp_interface rclcpp rclcpp_components - signal_processing std_msgs tf2 tf2_ros diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index a021e87e6199e..74f2002a93420 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -23,6 +23,8 @@ #include "tier4_planning_msgs/msg/velocity_limit.hpp" +using autoware::signal_processing::LowpassFilter1d; + namespace { VelocityLimit createVelocityLimitMsg( diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index 0b9a9eaa9d58c..ba93efc1f10c4 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -19,8 +19,8 @@ #include "autoware/motion_utils/resample/resample.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/signal_processing/lowpass_filter_1d.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" -#include "signal_processing/lowpass_filter_1d.hpp" #include #include @@ -620,7 +620,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( // calculate slow down velocity const double stable_slow_down_vel = [&]() { if (prev_output) { - return signal_processing::lowpassFilter( + return autoware::signal_processing::lowpassFilter( feasible_slow_down_vel, prev_output->target_vel, slow_down_param_.lpf_gain_slow_down_vel); } return feasible_slow_down_vel; @@ -706,7 +706,7 @@ double PlannerInterface::calculateSlowDownVelocity( slow_down_param_.getObstacleParamByLabel(obstacle.classification, is_obstacle_moving); const double stable_precise_lat_dist = [&]() { if (prev_output) { - return signal_processing::lowpassFilter( + return autoware::signal_processing::lowpassFilter( obstacle.precise_lat_dist, prev_output->precise_lat_dist, slow_down_param_.lpf_gain_lat_dist); } @@ -785,7 +785,7 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints( autoware::motion_utils::findNearestSegmentIndex(traj_points, prev_point->position); const double prev_dist_to_slow_down = autoware::motion_utils::calcSignedArcLength(traj_points, 0, prev_point->position, seg_idx); - return signal_processing::lowpassFilter( + return autoware::signal_processing::lowpassFilter( dist_to_slow_down, prev_dist_to_slow_down, slow_down_param_.lpf_gain_dist_to_slow_down); } return dist_to_slow_down; diff --git a/planning/autoware_obstacle_stop_planner/package.xml b/planning/autoware_obstacle_stop_planner/package.xml index 71eec208f7fdc..9e6a8a2dc5f43 100644 --- a/planning/autoware_obstacle_stop_planner/package.xml +++ b/planning/autoware_obstacle_stop_planner/package.xml @@ -25,6 +25,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager + autoware_signal_processing autoware_universe_utils autoware_vehicle_info_utils diagnostic_msgs @@ -34,7 +35,6 @@ rclcpp rclcpp_components sensor_msgs - signal_processing std_msgs tf2 tf2_eigen diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml index 5bf00292d061e..9308aab7642f5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml @@ -22,6 +22,7 @@ autoware_motion_utils autoware_perception_msgs autoware_planning_msgs + autoware_signal_processing autoware_universe_utils autoware_vehicle_msgs geometry_msgs @@ -29,7 +30,6 @@ object_recognition_utils pluginlib rclcpp - signal_processing tf2 tf2_eigen tf2_geometry_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index d91b3fb9aa346..3fa3f04435e87 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -16,9 +16,9 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/signal_processing/lowpass_filter_1d.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" -#include "signal_processing/lowpass_filter_1d.hpp" #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml index 981051a363029..1e52977611759 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml @@ -47,6 +47,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager + autoware_signal_processing autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs @@ -61,7 +62,6 @@ rclcpp rclcpp_components sensor_msgs - signal_processing tf2 tf2_eigen tf2_geometry_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml index 9ede8342856a6..d868025475444 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml @@ -23,6 +23,7 @@ autoware_planning_msgs autoware_planning_test_manager autoware_route_handler + autoware_signal_processing autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs @@ -32,7 +33,6 @@ rclcpp rclcpp_components sensor_msgs - signal_processing tf2 tf2_eigen tf2_geometry_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml index f997b30da9ffa..328ac199aca1a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml @@ -29,6 +29,7 @@ autoware_perception_msgs autoware_route_handler autoware_rtc_interface + autoware_signal_processing autoware_universe_utils autoware_vehicle_info_utils geometry_msgs @@ -36,7 +37,6 @@ pluginlib rclcpp sensor_msgs - signal_processing tf2 tf2_eigen tf2_geometry_msgs From 6465aafcfd2073beb61b2e32b71a48ba0f66811f Mon Sep 17 00:00:00 2001 From: Batuhan Beytekin <71197983+batuhanbeytekin@users.noreply.github.com> Date: Fri, 20 Sep 2024 14:47:14 +0300 Subject: [PATCH 24/95] refactor(detected_object_validation): rework parameters (#7750) * refactor(detected_object_validation): rework parameters Signed-off-by: batuhanbeytekin Signed-off-by: Batuhan Beytekin * style(pre-commit): autofix Signed-off-by: Batuhan Beytekin * Update perception/detected_object_validation/schema/object_lanelet_filter.schema.json Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Signed-off-by: batuhanbeytekin Signed-off-by: Batuhan Beytekin * Update perception/detected_object_validation/schema/object_position_filter.schema.json Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Signed-off-by: batuhanbeytekin Signed-off-by: Batuhan Beytekin * refactor(detected_object_validation): rework parameters Signed-off-by: batuhanbeytekin Signed-off-by: Batuhan Beytekin * refactor(detected_object_validation): rework parameters Signed-off-by: batuhanbeytekin Signed-off-by: Batuhan Beytekin * refactor(detected_object_validation): rework parameters Signed-off-by: batuhanbeytekin Signed-off-by: Batuhan Beytekin * style(pre-commit): autofix Signed-off-by: Batuhan Beytekin * refactor(detected_object_validation): rework parameters Signed-off-by: batuhanbeytekin Signed-off-by: Batuhan Beytekin * refactor(detected_object_validation): rework parameters Signed-off-by: batuhanbeytekin Signed-off-by: Batuhan Beytekin * style(pre-commit): autofix --------- Signed-off-by: batuhanbeytekin Signed-off-by: Batuhan Beytekin Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../README.md | 18 +++ .../occupancy_grid_based_validator.param.yaml | 1 + .../schema/object_lanelet_filter.schema.json | 121 ++++++++++++++++++ .../schema/object_position_filter.schema.json | 106 +++++++++++++++ ...cle_pointcloud_based_validator.schema.json | 68 ++++++++++ ...occupancy_grid_based_validator.schema.json | 38 ++++++ .../src/lanelet_filter/lanelet_filter.cpp | 16 +-- .../obstacle_pointcloud_validator.cpp | 2 +- .../occupancy_grid_map_validator.cpp | 4 +- .../src/position_filter/position_filter.cpp | 24 ++-- 10 files changed, 375 insertions(+), 23 deletions(-) create mode 100644 perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json create mode 100644 perception/autoware_detected_object_validation/schema/object_position_filter.schema.json create mode 100644 perception/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json create mode 100644 perception/autoware_detected_object_validation/schema/occupancy_grid_based_validator.schema.json diff --git a/perception/autoware_detected_object_validation/README.md b/perception/autoware_detected_object_validation/README.md index 12fad4ad9759f..adca7c0fd9678 100644 --- a/perception/autoware_detected_object_validation/README.md +++ b/perception/autoware_detected_object_validation/README.md @@ -10,3 +10,21 @@ The purpose of this package is to eliminate obvious false positives of DetectedO - [Occupancy grid based validator](occupancy-grid-based-validator.md) - [Object lanelet filter](object-lanelet-filter.md) - [Object position filter](object-position-filter.md) + +### Node Parameters + +#### object_lanelet_filter + +{{ json_to_markdown("perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json") }} + +#### object_position_filter + +{{ json_to_markdown("perception/autoware_detected_object_validation/schema/object_position_filter.schema.json") }} + +#### obstacle_pointcloud_based_validator + +{{ json_to_markdown("perception/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json") }} + +#### occupancy_grid_based_validator + +{{ json_to_markdown("perception/autoware_detected_object_validation/schema/occupancy_grid_based_validator.schema.json") }} diff --git a/perception/autoware_detected_object_validation/config/occupancy_grid_based_validator.param.yaml b/perception/autoware_detected_object_validation/config/occupancy_grid_based_validator.param.yaml index fc5c634735e23..bb996f1197155 100644 --- a/perception/autoware_detected_object_validation/config/occupancy_grid_based_validator.param.yaml +++ b/perception/autoware_detected_object_validation/config/occupancy_grid_based_validator.param.yaml @@ -1,3 +1,4 @@ /**: ros__parameters: + mean_threshold: 0.6 enable_debug: false diff --git a/perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json b/perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json new file mode 100644 index 0000000000000..fd4e39f0ca02e --- /dev/null +++ b/perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json @@ -0,0 +1,121 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Object Lanelet Filter", + "type": "object", + "definitions": { + "object_lanelet_filter": { + "type": "object", + "properties": { + "filter_target_label": { + "type": "object", + "properties": { + "UNKNOWN": { + "type": "boolean", + "default": true, + "description": "If true, unknown objects are filtered." + }, + "CAR": { + "type": "boolean", + "default": false, + "description": "If true, car objects are filtered." + }, + "TRUCK": { + "type": "boolean", + "default": false, + "description": "If true, truck objects are filtered." + }, + "BUS": { + "type": "boolean", + "default": false, + "description": "If true, bus objects are filtered." + }, + "TRAILER": { + "type": "boolean", + "default": false, + "description": "If true, trailer objects are filtered." + }, + "MOTORCYCLE": { + "type": "boolean", + "default": false, + "description": "If true, motorcycle objects are filtered." + }, + "BICYCLE": { + "type": "boolean", + "default": false, + "description": "If true, bicycle objects are filtered." + }, + "PEDESTRIAN": { + "type": "boolean", + "default": false, + "description": "If true, pedestrian objects are filtered." + } + }, + "required": [ + "UNKNOWN", + "CAR", + "TRUCK", + "BUS", + "TRAILER", + "MOTORCYCLE", + "BICYCLE", + "PEDESTRIAN" + ] + }, + "filter_settings": { + "type": "object", + "properties": { + "polygon_overlap_filter": { + "type": "object", + "properties": { + "enabled": { + "type": "boolean", + "default": true, + "description": "If true, objects that are not in the lanelet polygon are filtered." + } + }, + "required": ["enabled"] + }, + "lanelet_direction_filter": { + "type": "object", + "properties": { + "enabled": { + "type": "boolean", + "default": false, + "description": "If true, objects that are not in the same direction as the lanelet are filtered." + }, + "velocity_yaw_threshold": { + "type": "number", + "default": 0.785398, + "description": "If the yaw difference between the object and the lanelet is greater than this value, the object is filtered." + }, + "object_speed_threshold": { + "type": "number", + "default": 3.0, + "description": "If the object speed is greater than this value, the object is filtered." + } + }, + "required": ["enabled", "velocity_yaw_threshold", "object_speed_threshold"] + } + }, + "required": ["polygon_overlap_filter", "lanelet_direction_filter"] + } + }, + "required": ["filter_target_label", "filter_settings"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/object_lanelet_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/autoware_detected_object_validation/schema/object_position_filter.schema.json b/perception/autoware_detected_object_validation/schema/object_position_filter.schema.json new file mode 100644 index 0000000000000..9c35280d403ea --- /dev/null +++ b/perception/autoware_detected_object_validation/schema/object_position_filter.schema.json @@ -0,0 +1,106 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Object Position Filter", + "type": "object", + "definitions": { + "object_position_filter_params": { + "type": "object", + "properties": { + "filter_target_label": { + "type": "object", + "properties": { + "UNKNOWN": { + "type": "boolean", + "default": true, + "description": "Filter for UNKNOWN label" + }, + "CAR": { + "type": "boolean", + "default": false, + "description": "Filter for CAR label" + }, + "TRUCK": { + "type": "boolean", + "default": false, + "description": "Filter for TRUCK label" + }, + "BUS": { + "type": "boolean", + "default": false, + "description": "Filter for BUS label" + }, + "TRAILER": { + "type": "boolean", + "default": false, + "description": "Filter for TRAILER label" + }, + "MOTORCYCLE": { + "type": "boolean", + "default": false, + "description": "Filter for MOTORCYCLE label" + }, + "BICYCLE": { + "type": "boolean", + "default": false, + "description": "Filter for BICYCLE label" + }, + "PEDESTRIAN": { + "type": "boolean", + "default": false, + "description": "Filter for PEDESTRIAN label" + } + }, + "required": [ + "UNKNOWN", + "CAR", + "TRUCK", + "BUS", + "TRAILER", + "MOTORCYCLE", + "BICYCLE", + "PEDESTRIAN" + ] + }, + "upper_bound_x": { + "type": "number", + "default": 100.0, + "description": "Upper bound for X coordinate" + }, + "lower_bound_x": { + "type": "number", + "default": 0.0, + "description": "Lower bound for X coordinate" + }, + "upper_bound_y": { + "type": "number", + "default": 10.0, + "description": "Upper bound for Y coordinate" + }, + "lower_bound_y": { + "type": "number", + "default": -10.0, + "description": "Lower bound for Y coordinate" + } + }, + "required": [ + "filter_target_label", + "upper_bound_x", + "lower_bound_x", + "upper_bound_y", + "lower_bound_y" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/object_position_filter_params" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json b/perception/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json new file mode 100644 index 0000000000000..d7aa970993ca1 --- /dev/null +++ b/perception/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json @@ -0,0 +1,68 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Obstacle Pointcloud Based Validator", + "type": "object", + "definitions": { + "obstacle_pointcloud_based_validator": { + "type": "object", + "properties": { + "min_points_num": { + "type": "array", + "items": { + "type": "integer" + }, + "default": [10, 10, 10, 10, 10, 10, 10, 10], + "description": "The minimum number of obstacle point clouds in DetectedObjects" + }, + "max_points_num": { + "type": "array", + "items": { + "type": "integer" + }, + "default": [10, 10, 10, 10, 10, 10, 10, 10], + "description": "The max number of obstacle point clouds in DetectedObjects" + }, + "min_points_and_distance_ratio": { + "type": "array", + "items": { + "type": "number" + }, + "default": [800, 800, 800, 800, 800, 800, 800, 800], + "description": "Threshold value of the number of point clouds per object when the distance from baselink is 1m, because the number of point clouds varies with the distance from baselink." + }, + "using_2d_validator": { + "type": "boolean", + "default": false, + "description": "The xy-plane projected (2D) obstacle point clouds will be used for validation" + }, + "enable_debugger": { + "type": "boolean", + "default": false, + "description": "Whether to create debug topics or not?" + } + }, + "required": [ + "min_points_num", + "max_points_num", + "min_points_and_distance_ratio", + "using_2d_validator", + "enable_debugger" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/obstacle_pointcloud_based_validator" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/autoware_detected_object_validation/schema/occupancy_grid_based_validator.schema.json b/perception/autoware_detected_object_validation/schema/occupancy_grid_based_validator.schema.json new file mode 100644 index 0000000000000..918e94cd9847e --- /dev/null +++ b/perception/autoware_detected_object_validation/schema/occupancy_grid_based_validator.schema.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Occupancy Grid Based Validator", + "type": "object", + "definitions": { + "occupancy_grid_based_validator": { + "type": "object", + "properties": { + "mean_threshold": { + "type": "number", + "default": 0.6, + "description": "The percentage threshold of allowed non-freespace." + }, + "enable_debug": { + "type": "boolean", + "default": false, + "description": "Whether to display debug images or not?" + } + }, + "required": ["mean_threshold", "enable_debug"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/occupancy_grid_based_validator" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp index aed0ba5ab85ea..59de872ae9b90 100644 --- a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp +++ b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp @@ -42,14 +42,14 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod using std::placeholders::_1; // Set parameters - filter_target_.UNKNOWN = declare_parameter("filter_target_label.UNKNOWN", false); - filter_target_.CAR = declare_parameter("filter_target_label.CAR", false); - filter_target_.TRUCK = declare_parameter("filter_target_label.TRUCK", false); - filter_target_.BUS = declare_parameter("filter_target_label.BUS", false); - filter_target_.TRAILER = declare_parameter("filter_target_label.TRAILER", false); - filter_target_.MOTORCYCLE = declare_parameter("filter_target_label.MOTORCYCLE", false); - filter_target_.BICYCLE = declare_parameter("filter_target_label.BICYCLE", false); - filter_target_.PEDESTRIAN = declare_parameter("filter_target_label.PEDESTRIAN", false); + filter_target_.UNKNOWN = declare_parameter("filter_target_label.UNKNOWN"); + filter_target_.CAR = declare_parameter("filter_target_label.CAR"); + filter_target_.TRUCK = declare_parameter("filter_target_label.TRUCK"); + filter_target_.BUS = declare_parameter("filter_target_label.BUS"); + filter_target_.TRAILER = declare_parameter("filter_target_label.TRAILER"); + filter_target_.MOTORCYCLE = declare_parameter("filter_target_label.MOTORCYCLE"); + filter_target_.BICYCLE = declare_parameter("filter_target_label.BICYCLE"); + filter_target_.PEDESTRIAN = declare_parameter("filter_target_label.PEDESTRIAN"); // Set filter settings filter_settings_.polygon_overlap_filter = declare_parameter("filter_settings.polygon_overlap_filter.enabled"); diff --git a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp index c5ecce714735b..7f55c86080fd2 100644 --- a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp +++ b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp @@ -310,7 +310,7 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( debug_publisher_ = std::make_unique( this, "obstacle_pointcloud_based_validator"); - const bool enable_debugger = declare_parameter("enable_debugger", false); + const bool enable_debugger = declare_parameter("enable_debugger"); if (enable_debugger) debugger_ = std::make_shared(this); published_time_publisher_ = std::make_unique(this); diff --git a/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp b/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp index c082b4b0f03f1..51df1929a300c 100644 --- a/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp +++ b/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp @@ -53,8 +53,8 @@ OccupancyGridBasedValidator::OccupancyGridBasedValidator(const rclcpp::NodeOptio objects_pub_ = create_publisher( "~/output/objects", rclcpp::QoS{1}); - mean_threshold_ = declare_parameter("mean_threshold", 0.6); - enable_debug_ = declare_parameter("enable_debug", false); + mean_threshold_ = declare_parameter("mean_threshold"); + enable_debug_ = declare_parameter("enable_debug"); published_time_publisher_ = std::make_unique(this); } diff --git a/perception/autoware_detected_object_validation/src/position_filter/position_filter.cpp b/perception/autoware_detected_object_validation/src/position_filter/position_filter.cpp index ac0bab404c476..3b88628aa6d84 100644 --- a/perception/autoware_detected_object_validation/src/position_filter/position_filter.cpp +++ b/perception/autoware_detected_object_validation/src/position_filter/position_filter.cpp @@ -26,18 +26,18 @@ ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & n using std::placeholders::_1; // Set parameters - upper_bound_x_ = declare_parameter("upper_bound_x", 100.0); - upper_bound_y_ = declare_parameter("upper_bound_y", 50.0); - lower_bound_x_ = declare_parameter("lower_bound_x", 0.0); - lower_bound_y_ = declare_parameter("lower_bound_y", -50.0); - filter_target_.UNKNOWN = declare_parameter("filter_target_label.UNKNOWN", false); - filter_target_.CAR = declare_parameter("filter_target_label.CAR", false); - filter_target_.TRUCK = declare_parameter("filter_target_label.TRUCK", false); - filter_target_.BUS = declare_parameter("filter_target_label.BUS", false); - filter_target_.TRAILER = declare_parameter("filter_target_label.TRAILER", false); - filter_target_.MOTORCYCLE = declare_parameter("filter_target_label.MOTORCYCLE", false); - filter_target_.BICYCLE = declare_parameter("filter_target_label.BICYCLE", false); - filter_target_.PEDESTRIAN = declare_parameter("filter_target_label.PEDESTRIAN", false); + upper_bound_x_ = declare_parameter("upper_bound_x"); + upper_bound_y_ = declare_parameter("upper_bound_y"); + lower_bound_x_ = declare_parameter("lower_bound_x"); + lower_bound_y_ = declare_parameter("lower_bound_y"); + filter_target_.UNKNOWN = declare_parameter("filter_target_label.UNKNOWN"); + filter_target_.CAR = declare_parameter("filter_target_label.CAR"); + filter_target_.TRUCK = declare_parameter("filter_target_label.TRUCK"); + filter_target_.BUS = declare_parameter("filter_target_label.BUS"); + filter_target_.TRAILER = declare_parameter("filter_target_label.TRAILER"); + filter_target_.MOTORCYCLE = declare_parameter("filter_target_label.MOTORCYCLE"); + filter_target_.BICYCLE = declare_parameter("filter_target_label.BICYCLE"); + filter_target_.PEDESTRIAN = declare_parameter("filter_target_label.PEDESTRIAN"); // Set publisher/subscriber object_sub_ = this->create_subscription( From ef0f84675626a17b9b24dfca8939edb313da395c Mon Sep 17 00:00:00 2001 From: Mitsuhiro Sakamoto <50359861+mitukou1109@users.noreply.github.com> Date: Fri, 20 Sep 2024 22:07:07 +0900 Subject: [PATCH 25/95] test(lane_departure_checker): add tests for createVehicleFootprints (#8928) * move createVehicleFootprints() to seperate files Signed-off-by: mitukou1109 * add tests for createVehicleFootprints() Signed-off-by: mitukou1109 --------- Signed-off-by: mitukou1109 --- .../lane_departure_checker.hpp | 5 - .../util/create_vehicle_footprint.hpp | 66 ----- .../autoware/lane_departure_checker/utils.hpp | 31 ++ .../lane_departure_checker.cpp | 58 +--- .../src/lane_departure_checker_node/utils.cpp | 70 +++++ .../test/test_create_vehicle_footprints.cpp | 280 ++++++++++++++++++ .../test/test_cut_trajectory.cpp | 2 +- .../test/test_resample_trajectory.cpp | 2 +- 8 files changed, 396 insertions(+), 118 deletions(-) delete mode 100644 control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/util/create_vehicle_footprint.hpp create mode 100644 control/autoware_lane_departure_checker/test/test_create_vehicle_footprints.cpp diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp index 27107cfd8974b..9ef803d874520 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp @@ -160,11 +160,6 @@ class LaneDepartureChecker const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); - std::vector createVehicleFootprints( - const geometry_msgs::msg::PoseWithCovariance & covariance, const TrajectoryPoints & trajectory, - const Param & param); - std::vector createVehicleFootprints(const PathWithLaneId & path) const; - static std::vector createVehiclePassingAreas( const std::vector & vehicle_footprints); diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/util/create_vehicle_footprint.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/util/create_vehicle_footprint.hpp deleted file mode 100644 index 4435f282054ad..0000000000000 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/util/create_vehicle_footprint.hpp +++ /dev/null @@ -1,66 +0,0 @@ -// 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. -// Copyright 2015-2020 Autoware Foundation. 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. - -#ifndef AUTOWARE__LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ -#define AUTOWARE__LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ - -#include -#include -#include - -#include - -struct FootprintMargin -{ - double lon; - double lat; -}; - -inline FootprintMargin calcFootprintMargin( - const geometry_msgs::msg::PoseWithCovariance & covariance, const double scale) -{ - const auto Cov_in_map = covariance.covariance; - Eigen::Matrix2d Cov_xy_map; - Cov_xy_map << Cov_in_map[0 * 6 + 0], Cov_in_map[0 * 6 + 1], Cov_in_map[1 * 6 + 0], - Cov_in_map[1 * 6 + 1]; - - const double yaw_vehicle = tf2::getYaw(covariance.pose.orientation); - - // To get a position in a transformed coordinate, rotate the inverse direction - Eigen::Matrix2d R_map2vehicle; - R_map2vehicle << std::cos(-yaw_vehicle), -std::sin(-yaw_vehicle), std::sin(-yaw_vehicle), - std::cos(-yaw_vehicle); - // Rotate covariance E((X, Y)^t*(X, Y)) = E(R*(x,y)*(x,y)^t*R^t) - // when Rotate point (X, Y)^t= R*(x, y)^t. - const Eigen::Matrix2d Cov_xy_vehicle = R_map2vehicle * Cov_xy_map * R_map2vehicle.transpose(); - - // The longitudinal/lateral length is represented - // in Cov_xy_vehicle(0,0), Cov_xy_vehicle(1,1) respectively. - return FootprintMargin{Cov_xy_vehicle(0, 0) * scale, Cov_xy_vehicle(1, 1) * scale}; -} - -#endif // AUTOWARE__LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp index ed44a3f777c65..45a651339cc12 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp @@ -15,15 +15,22 @@ #ifndef AUTOWARE__LANE_DEPARTURE_CHECKER__UTILS_HPP_ #define AUTOWARE__LANE_DEPARTURE_CHECKER__UTILS_HPP_ +#include +#include + #include #include +#include +#include #include namespace autoware::lane_departure_checker::utils { +using autoware::universe_utils::LinearRing2d; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; +using tier4_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; /** @@ -42,6 +49,30 @@ TrajectoryPoints cutTrajectory(const TrajectoryPoints & trajectory, const double * @note this function assumes the input trajectory is sampled dense enough */ TrajectoryPoints resampleTrajectory(const Trajectory & trajectory, const double interval); + +/** + * @brief create vehicle footprints along the trajectory with the given covariance and margin + * @param covariance vehicle pose with covariance + * @param trajectory trajectory along which the vehicle footprints are created + * @param vehicle_info vehicle information + * @param footprint_margin_scale scale of the footprint margin + * @return vehicle footprints along the trajectory + */ +std::vector createVehicleFootprints( + const geometry_msgs::msg::PoseWithCovariance & covariance, const TrajectoryPoints & trajectory, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const double footprint_margin_scale); + +/** + * @brief create vehicle footprints along the path with the given margin + * @param path path along which the vehicle footprints are created + * @param vehicle_info vehicle information + * @param footprint_extra_margin extra margin for the footprint + * @return vehicle footprints along the path + */ +std::vector createVehicleFootprints( + const PathWithLaneId & path, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const double footprint_extra_margin); } // namespace autoware::lane_departure_checker::utils #endif // AUTOWARE__LANE_DEPARTURE_CHECKER__UTILS_HPP_ diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index f775c3e32c7af..a6b91c25763c5 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -14,7 +14,6 @@ #include "autoware/lane_departure_checker/lane_departure_checker.hpp" -#include "autoware/lane_departure_checker/util/create_vehicle_footprint.hpp" #include "autoware/lane_departure_checker/utils.hpp" #include @@ -123,8 +122,9 @@ Output LaneDepartureChecker::update(const Input & input) braking_distance); output.processing_time_map["resampleTrajectory"] = stop_watch.toc(true); } - output.vehicle_footprints = - createVehicleFootprints(input.current_odom->pose, output.resampled_trajectory, param_); + output.vehicle_footprints = utils::createVehicleFootprints( + input.current_odom->pose, output.resampled_trajectory, *vehicle_info_ptr_, + param_.footprint_margin_scale); output.processing_time_map["createVehicleFootprints"] = stop_watch.toc(true); output.vehicle_passing_areas = createVehiclePassingAreas(output.vehicle_footprints); @@ -163,7 +163,8 @@ bool LaneDepartureChecker::checkPathWillLeaveLane( { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - std::vector vehicle_footprints = createVehicleFootprints(path); + std::vector vehicle_footprints = + utils::createVehicleFootprints(path, *vehicle_info_ptr_, param_.footprint_extra_margin); lanelet::ConstLanelets candidate_lanelets = getCandidateLanelets(lanelets, vehicle_footprints); return willLeaveLane(candidate_lanelets, vehicle_footprints); } @@ -177,43 +178,6 @@ PoseDeviation LaneDepartureChecker::calcTrajectoryDeviation( return autoware::universe_utils::calcPoseDeviation(trajectory.points.at(nearest_idx).pose, pose); } -std::vector LaneDepartureChecker::createVehicleFootprints( - const geometry_msgs::msg::PoseWithCovariance & covariance, const TrajectoryPoints & trajectory, - const Param & param) -{ - // Calculate longitudinal and lateral margin based on covariance - const auto margin = calcFootprintMargin(covariance, param.footprint_margin_scale); - - // Create vehicle footprint in base_link coordinate - const auto local_vehicle_footprint = vehicle_info_ptr_->createFootprint(margin.lat, margin.lon); - - // Create vehicle footprint on each TrajectoryPoint - std::vector vehicle_footprints; - for (const auto & p : trajectory) { - vehicle_footprints.push_back( - transformVector(local_vehicle_footprint, autoware::universe_utils::pose2transform(p.pose))); - } - - return vehicle_footprints; -} - -std::vector LaneDepartureChecker::createVehicleFootprints( - const PathWithLaneId & path) const -{ - // Create vehicle footprint in base_link coordinate - const auto local_vehicle_footprint = - vehicle_info_ptr_->createFootprint(param_.footprint_extra_margin); - - // Create vehicle footprint on each Path point - std::vector vehicle_footprints; - for (const auto & p : path.points) { - vehicle_footprints.push_back(transformVector( - local_vehicle_footprint, autoware::universe_utils::pose2transform(p.point.pose))); - } - - return vehicle_footprints; -} - std::vector LaneDepartureChecker::createVehiclePassingAreas( const std::vector & vehicle_footprints) { @@ -249,7 +213,8 @@ std::vector> LaneDepartureChecker::getLanele universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // Get Footprint Hull basic polygon - std::vector vehicle_footprints = createVehicleFootprints(path); + std::vector vehicle_footprints = + utils::createVehicleFootprints(path, *vehicle_info_ptr_, param_.footprint_extra_margin); LinearRing2d footprint_hull = createHullFromFootprints(vehicle_footprints); lanelet::BasicPolygon2d footprint_hull_basic_polygon = toBasicPolygon2D(footprint_hull); @@ -331,7 +296,8 @@ bool LaneDepartureChecker::checkPathWillLeaveLane( universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // check if the footprint is not fully contained within the fused lanelets polygon - const std::vector vehicle_footprints = createVehicleFootprints(path); + const std::vector vehicle_footprints = + utils::createVehicleFootprints(path, *vehicle_info_ptr_, param_.footprint_extra_margin); const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path); if (!fused_lanelets_polygon) return true; return !std::all_of( @@ -348,7 +314,8 @@ bool LaneDepartureChecker::checkPathWillLeaveLane( { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const std::vector vehicle_footprints = createVehicleFootprints(path); + const std::vector vehicle_footprints = + utils::createVehicleFootprints(path, *vehicle_info_ptr_, param_.footprint_extra_margin); auto is_all_footprints_within = [&](const auto & polygon) { return std::all_of( @@ -380,7 +347,8 @@ PathWithLaneId LaneDepartureChecker::cropPointsOutsideOfLanes( PathWithLaneId temp_path; const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path); if (path.points.empty() || !fused_lanelets_polygon) return temp_path; - const auto vehicle_footprints = createVehicleFootprints(path); + const auto vehicle_footprints = + utils::createVehicleFootprints(path, *vehicle_info_ptr_, param_.footprint_extra_margin); { universe_utils::ScopedTimeTrack st2( diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp index 1bbb06ce2f524..324936c633158 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp @@ -18,6 +18,38 @@ #include +namespace +{ +struct FootprintMargin +{ + double lon; + double lat; +}; + +FootprintMargin calcFootprintMargin( + const geometry_msgs::msg::PoseWithCovariance & covariance, const double scale) +{ + const auto Cov_in_map = covariance.covariance; + Eigen::Matrix2d Cov_xy_map; + Cov_xy_map << Cov_in_map[0 * 6 + 0], Cov_in_map[0 * 6 + 1], Cov_in_map[1 * 6 + 0], + Cov_in_map[1 * 6 + 1]; + + const double yaw_vehicle = tf2::getYaw(covariance.pose.orientation); + + // To get a position in a transformed coordinate, rotate the inverse direction + Eigen::Matrix2d R_map2vehicle; + R_map2vehicle << std::cos(-yaw_vehicle), -std::sin(-yaw_vehicle), std::sin(-yaw_vehicle), + std::cos(-yaw_vehicle); + // Rotate covariance E((X, Y)^t*(X, Y)) = E(R*(x,y)*(x,y)^t*R^t) + // when Rotate point (X, Y)^t= R*(x, y)^t. + const Eigen::Matrix2d Cov_xy_vehicle = R_map2vehicle * Cov_xy_map * R_map2vehicle.transpose(); + + // The longitudinal/lateral length is represented + // in Cov_xy_vehicle(0,0), Cov_xy_vehicle(1,1) respectively. + return FootprintMargin{Cov_xy_vehicle(0, 0) * scale, Cov_xy_vehicle(1, 1) * scale}; +} +} // namespace + namespace autoware::lane_departure_checker::utils { TrajectoryPoints cutTrajectory(const TrajectoryPoints & trajectory, const double length) @@ -91,4 +123,42 @@ TrajectoryPoints resampleTrajectory(const Trajectory & trajectory, const double return resampled; } + +std::vector createVehicleFootprints( + const geometry_msgs::msg::PoseWithCovariance & covariance, const TrajectoryPoints & trajectory, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const double footprint_margin_scale) +{ + // Calculate longitudinal and lateral margin based on covariance + const auto margin = calcFootprintMargin(covariance, footprint_margin_scale); + + // Create vehicle footprint in base_link coordinate + const auto local_vehicle_footprint = vehicle_info.createFootprint(margin.lat, margin.lon); + + // Create vehicle footprint on each TrajectoryPoint + std::vector vehicle_footprints; + for (const auto & p : trajectory) { + vehicle_footprints.push_back( + transformVector(local_vehicle_footprint, autoware::universe_utils::pose2transform(p.pose))); + } + + return vehicle_footprints; +} + +std::vector createVehicleFootprints( + const PathWithLaneId & path, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const double footprint_extra_margin) +{ + // Create vehicle footprint in base_link coordinate + const auto local_vehicle_footprint = vehicle_info.createFootprint(footprint_extra_margin); + + // Create vehicle footprint on each Path point + std::vector vehicle_footprints; + for (const auto & p : path.points) { + vehicle_footprints.push_back(transformVector( + local_vehicle_footprint, autoware::universe_utils::pose2transform(p.point.pose))); + } + + return vehicle_footprints; +} } // namespace autoware::lane_departure_checker::utils diff --git a/control/autoware_lane_departure_checker/test/test_create_vehicle_footprints.cpp b/control/autoware_lane_departure_checker/test/test_create_vehicle_footprints.cpp new file mode 100644 index 0000000000000..6189d8803a3ea --- /dev/null +++ b/control/autoware_lane_departure_checker/test/test_create_vehicle_footprints.cpp @@ -0,0 +1,280 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/lane_departure_checker/utils.hpp" + +#include + +#include + +using autoware::universe_utils::LinearRing2d; +using autoware_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::PoseWithCovariance; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; +using TrajectoryPoints = std::vector; + +namespace +{ +PoseWithCovariance create_pose_with_covariance( + const Eigen::Matrix2d & covariance_xy, const double yaw) +{ + PoseWithCovariance pose_with_covariance; + pose_with_covariance.covariance[0 * 6 + 0] = covariance_xy(0, 0); + pose_with_covariance.covariance[0 * 6 + 1] = covariance_xy(0, 1); + pose_with_covariance.covariance[1 * 6 + 0] = covariance_xy(1, 0); + pose_with_covariance.covariance[1 * 6 + 1] = covariance_xy(1, 1); + pose_with_covariance.pose.orientation.z = std::sin(yaw / 2); + pose_with_covariance.pose.orientation.w = std::cos(yaw / 2); + return pose_with_covariance; +} + +TrajectoryPoints create_trajectory_points( + const std::vector> & xy_yaws) +{ + TrajectoryPoints trajectory_points; + for (const auto & [xy, yaw] : xy_yaws) { + TrajectoryPoint p; + p.pose.position.x = xy(0); + p.pose.position.y = xy(1); + p.pose.orientation.z = std::sin(yaw / 2); + p.pose.orientation.w = std::cos(yaw / 2); + trajectory_points.push_back(p); + } + return trajectory_points; +} + +PathWithLaneId create_path(const std::vector> & xy_yaws) +{ + PathWithLaneId path; + for (const auto & [xy, yaw] : xy_yaws) { + PathPointWithLaneId p; + p.point.pose.position.x = xy(0); + p.point.pose.position.y = xy(1); + p.point.pose.orientation.z = std::sin(yaw / 2); + p.point.pose.orientation.w = std::cos(yaw / 2); + path.points.push_back(p); + } + return path; +} + +// reference: +// https://github.com/autowarefoundation/sample_vehicle_launch/blob/main/sample_vehicle_description/config/vehicle_info.param.yaml +constexpr double wheel_radius_m = 0.383; +constexpr double wheel_width_m = 0.235; +constexpr double wheel_base_m = 2.79; +constexpr double wheel_tread_m = 1.64; +constexpr double front_overhang_m = 1.0; +constexpr double rear_overhang_m = 1.1; +constexpr double left_overhang_m = 0.128; +constexpr double right_overhang_m = 0.128; +constexpr double vehicle_height_m = 2.5; +constexpr double max_steer_angle_rad = 0.70; +} // namespace + +struct CreateVehicleFootprintsAlongTrajectoryParam +{ + std::string description; + Eigen::Matrix2d covariance_xy; + double yaw; + std::vector> trajectory_points; + double footprint_margin_scale; + std::vector expected_footprints; +}; + +std::ostream & operator<<(std::ostream & os, const CreateVehicleFootprintsAlongTrajectoryParam & p) +{ + return os << p.description; +} + +struct CreateVehicleFootprintsAlongPathParam +{ + std::string description; + std::vector> path_points; + double footprint_extra_margin; + std::vector expected_footprints; +}; + +std::ostream & operator<<(std::ostream & os, const CreateVehicleFootprintsAlongPathParam & p) +{ + return os << p.description; +} + +class CreateVehicleFootprintsAlongTrajectoryTest +: public ::testing::TestWithParam +{ +protected: + void SetUp() override + { + vehicle_info = autoware::vehicle_info_utils::createVehicleInfo( + wheel_radius_m, wheel_width_m, wheel_base_m, wheel_tread_m, front_overhang_m, rear_overhang_m, + left_overhang_m, right_overhang_m, vehicle_height_m, max_steer_angle_rad); + } + + autoware::vehicle_info_utils::VehicleInfo vehicle_info; +}; + +class CreateVehicleFootprintsAlongPathTest +: public ::testing::TestWithParam +{ +protected: + void SetUp() override + { + vehicle_info = autoware::vehicle_info_utils::createVehicleInfo( + wheel_radius_m, wheel_width_m, wheel_base_m, wheel_tread_m, front_overhang_m, rear_overhang_m, + left_overhang_m, right_overhang_m, vehicle_height_m, max_steer_angle_rad); + } + + autoware::vehicle_info_utils::VehicleInfo vehicle_info; +}; + +TEST_P(CreateVehicleFootprintsAlongTrajectoryTest, test_create_vehicle_footprints) +{ + const auto p = GetParam(); + const auto pose_with_covariance = create_pose_with_covariance(p.covariance_xy, p.yaw); + const auto trajectory_points = create_trajectory_points(p.trajectory_points); + const auto footprints = autoware::lane_departure_checker::utils::createVehicleFootprints( + pose_with_covariance, trajectory_points, vehicle_info, p.footprint_margin_scale); + + ASSERT_EQ(footprints.size(), p.expected_footprints.size()); + for (size_t i = 0; i < footprints.size(); ++i) { + const auto & footprint = footprints.at(i); + const auto & expected_footprint = p.expected_footprints.at(i); + ASSERT_EQ(footprint.size(), expected_footprint.size()); + for (size_t j = 0; j < footprint.size(); ++j) { + EXPECT_DOUBLE_EQ(footprint.at(j).x(), expected_footprint.at(j).x()); + EXPECT_DOUBLE_EQ(footprint.at(j).y(), expected_footprint.at(j).y()); + } + } +} + +TEST_P(CreateVehicleFootprintsAlongPathTest, test_create_vehicle_footprints) +{ + const auto p = GetParam(); + const auto path = create_path(p.path_points); + const auto footprints = autoware::lane_departure_checker::utils::createVehicleFootprints( + path, vehicle_info, p.footprint_extra_margin); + + ASSERT_EQ(footprints.size(), p.expected_footprints.size()); + for (size_t i = 0; i < footprints.size(); ++i) { + const auto & footprint = footprints.at(i); + const auto & expected_footprint = p.expected_footprints.at(i); + ASSERT_EQ(footprint.size(), expected_footprint.size()); + for (size_t j = 0; j < footprint.size(); ++j) { + EXPECT_DOUBLE_EQ(footprint.at(j).x(), expected_footprint.at(j).x()); + EXPECT_DOUBLE_EQ(footprint.at(j).y(), expected_footprint.at(j).y()); + } + } +} + +INSTANTIATE_TEST_SUITE_P( + LaneDepartureCheckerTest, CreateVehicleFootprintsAlongTrajectoryTest, + ::testing::Values( + CreateVehicleFootprintsAlongTrajectoryParam{ + "EmptyTrajectory", Eigen::Matrix2d{{0.0, 0.0}, {0.0, 0.0}}, 0.0, {}, 0.0, {}}, + CreateVehicleFootprintsAlongTrajectoryParam{ + "SinglePointTrajectory", + Eigen::Matrix2d{{0.0, 0.0}, {0.0, 0.0}}, + 0.0, + {{{0.0, 0.0}, 0.0}}, + 0.0, + {{{front_overhang_m + wheel_base_m, wheel_tread_m / 2.0 + left_overhang_m}, + {front_overhang_m + wheel_base_m, -(wheel_tread_m / 2.0 + right_overhang_m)}, + {wheel_base_m / 2.0, -(wheel_tread_m / 2.0 + right_overhang_m)}, + {-rear_overhang_m, -(wheel_tread_m / 2.0 + right_overhang_m)}, + {-rear_overhang_m, wheel_tread_m / 2.0 + left_overhang_m}, + {wheel_base_m / 2.0, wheel_tread_m / 2.0 + left_overhang_m}, + {front_overhang_m + wheel_base_m, wheel_tread_m / 2.0 + left_overhang_m}}}}, + CreateVehicleFootprintsAlongTrajectoryParam{ + "NonZeroMargin", + Eigen::Matrix2d{{0.1, 0.0}, {0.0, 0.2}}, + 0.0, + {{{0.0, 0.0}, 0.0}, {{1.0, 0.0}, 0.0}}, + 1.0, + {{{front_overhang_m + wheel_base_m + 0.1, wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {front_overhang_m + wheel_base_m + 0.1, -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {wheel_base_m / 2.0, -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {-(rear_overhang_m + 0.1), -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {-(rear_overhang_m + 0.1), wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {wheel_base_m / 2.0, wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {front_overhang_m + wheel_base_m + 0.1, wheel_tread_m / 2.0 + left_overhang_m + 0.2}}, + {{front_overhang_m + wheel_base_m + 0.1 + 1.0, wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {front_overhang_m + wheel_base_m + 0.1 + 1.0, + -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {wheel_base_m / 2.0 + 1.0, -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {-(rear_overhang_m + 0.1) + 1.0, -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {-(rear_overhang_m + 0.1) + 1.0, wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {wheel_base_m / 2.0 + 1.0, wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {front_overhang_m + wheel_base_m + 0.1 + 1.0, + wheel_tread_m / 2.0 + left_overhang_m + 0.2}}}}, + CreateVehicleFootprintsAlongTrajectoryParam{ + "NonZeroYaw", + Eigen::Matrix2d{{0.2, 0.0}, {0.0, 0.1}}, + M_PI_2, + {{{0.0, 0.0}, 0.0}, {{1.0, 0.0}, 0.0}}, + 1.0, + {{{front_overhang_m + wheel_base_m + 0.1, wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {front_overhang_m + wheel_base_m + 0.1, -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {wheel_base_m / 2.0, -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {-(rear_overhang_m + 0.1), -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {-(rear_overhang_m + 0.1), wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {wheel_base_m / 2.0, wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {front_overhang_m + wheel_base_m + 0.1, wheel_tread_m / 2.0 + left_overhang_m + 0.2}}, + {{front_overhang_m + wheel_base_m + 0.1 + 1.0, wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {front_overhang_m + wheel_base_m + 0.1 + 1.0, + -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {wheel_base_m / 2.0 + 1.0, -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {-(rear_overhang_m + 0.1) + 1.0, -(wheel_tread_m / 2.0 + right_overhang_m + 0.2)}, + {-(rear_overhang_m + 0.1) + 1.0, wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {wheel_base_m / 2.0 + 1.0, wheel_tread_m / 2.0 + left_overhang_m + 0.2}, + {front_overhang_m + wheel_base_m + 0.1 + 1.0, + wheel_tread_m / 2.0 + left_overhang_m + 0.2}}}}), + ::testing::PrintToStringParamName()); + +INSTANTIATE_TEST_SUITE_P( + LaneDepartureCheckerTest, CreateVehicleFootprintsAlongPathTest, + ::testing::Values( + CreateVehicleFootprintsAlongPathParam{"EmptyTrajectory", {}, 0.0, {}}, + CreateVehicleFootprintsAlongPathParam{ + "SinglePointTrajectory", + {{{0.0, 0.0}, 0.0}}, + 0.0, + {{{front_overhang_m + wheel_base_m, wheel_tread_m / 2.0 + left_overhang_m}, + {front_overhang_m + wheel_base_m, -(wheel_tread_m / 2.0 + right_overhang_m)}, + {wheel_base_m / 2.0, -(wheel_tread_m / 2.0 + right_overhang_m)}, + {-rear_overhang_m, -(wheel_tread_m / 2.0 + right_overhang_m)}, + {-rear_overhang_m, wheel_tread_m / 2.0 + left_overhang_m}, + {wheel_base_m / 2.0, wheel_tread_m / 2.0 + left_overhang_m}, + {front_overhang_m + wheel_base_m, wheel_tread_m / 2.0 + left_overhang_m}}}}, + CreateVehicleFootprintsAlongPathParam{ + "NonZeroMargin", + {{{0.0, 0.0}, 0.0}, {{1.0, 0.0}, 0.0}}, + 0.1, + {{{front_overhang_m + wheel_base_m + 0.1, wheel_tread_m / 2.0 + left_overhang_m + 0.1}, + {front_overhang_m + wheel_base_m + 0.1, -(wheel_tread_m / 2.0 + right_overhang_m + 0.1)}, + {wheel_base_m / 2.0, -(wheel_tread_m / 2.0 + right_overhang_m + 0.1)}, + {-(rear_overhang_m + 0.1), -(wheel_tread_m / 2.0 + right_overhang_m + 0.1)}, + {-(rear_overhang_m + 0.1), wheel_tread_m / 2.0 + left_overhang_m + 0.1}, + {wheel_base_m / 2.0, wheel_tread_m / 2.0 + left_overhang_m + 0.1}, + {front_overhang_m + wheel_base_m + 0.1, wheel_tread_m / 2.0 + left_overhang_m + 0.1}}, + {{front_overhang_m + wheel_base_m + 0.1 + 1.0, wheel_tread_m / 2.0 + left_overhang_m + 0.1}, + {front_overhang_m + wheel_base_m + 0.1 + 1.0, + -(wheel_tread_m / 2.0 + right_overhang_m + 0.1)}, + {wheel_base_m / 2.0 + 1.0, -(wheel_tread_m / 2.0 + right_overhang_m + 0.1)}, + {-(rear_overhang_m + 0.1) + 1.0, -(wheel_tread_m / 2.0 + right_overhang_m + 0.1)}, + {-(rear_overhang_m + 0.1) + 1.0, wheel_tread_m / 2.0 + left_overhang_m + 0.1}, + {wheel_base_m / 2.0 + 1.0, wheel_tread_m / 2.0 + left_overhang_m + 0.1}, + {front_overhang_m + wheel_base_m + 0.1 + 1.0, + wheel_tread_m / 2.0 + left_overhang_m + 0.1}}}}), + ::testing::PrintToStringParamName()); diff --git a/control/autoware_lane_departure_checker/test/test_cut_trajectory.cpp b/control/autoware_lane_departure_checker/test/test_cut_trajectory.cpp index 782b986f6e556..8fa91bc8782e8 100644 --- a/control/autoware_lane_departure_checker/test/test_cut_trajectory.cpp +++ b/control/autoware_lane_departure_checker/test/test_cut_trajectory.cpp @@ -70,7 +70,7 @@ TEST_P(CutTrajectoryTest, test_cut_trajectory) } INSTANTIATE_TEST_SUITE_P( - CutTrajectoryTests, CutTrajectoryTest, + LaneDepartureCheckerTest, CutTrajectoryTest, ::testing::Values( CutTrajectoryTestParam{ "EmptyTrajectory", std::vector{}, 1.0, std::vector{}}, diff --git a/control/autoware_lane_departure_checker/test/test_resample_trajectory.cpp b/control/autoware_lane_departure_checker/test/test_resample_trajectory.cpp index ac89882a39493..67fe69323584e 100644 --- a/control/autoware_lane_departure_checker/test/test_resample_trajectory.cpp +++ b/control/autoware_lane_departure_checker/test/test_resample_trajectory.cpp @@ -71,7 +71,7 @@ TEST_P(ResampleTrajectoryTest, test_resample_trajectory) } INSTANTIATE_TEST_SUITE_P( - ResampleTrajectoryTests, ResampleTrajectoryTest, + LaneDepartureCheckerTest, ResampleTrajectoryTest, ::testing::Values( ResampleTrajectoryTestParam{"EmptyTrajectory", {}, 1.0, {}}, ResampleTrajectoryTestParam{"SinglePointTrajectory", {{1.0, 0.0, 0.0}}, 1.0, {{1.0, 0.0, 0.0}}}, From b431dcdddb59de1e8176e674d47eed8ad2ec0fdd Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Fri, 20 Sep 2024 15:07:36 +0200 Subject: [PATCH 26/95] refactor(autoware_interpolation): prefix package and namespace with autoware (#8088) Signed-off-by: Esteve Fernandez Co-authored-by: kosuke55 --- .../CMakeLists.txt | 6 +-- .../README.md | 0 .../interpolation/interpolation_utils.hpp | 10 ++-- .../interpolation/linear_interpolation.hpp | 13 +++--- .../spherical_linear_interpolation.hpp | 12 ++--- .../interpolation/spline_interpolation.hpp | 12 ++--- .../spline_interpolation_points_2d.hpp | 12 ++--- .../interpolation/zero_order_hold.hpp | 16 +++---- .../package.xml | 2 +- .../src/linear_interpolation.cpp | 10 ++-- .../src/spherical_linear_interpolation.cpp | 10 ++-- .../src/spline_interpolation.cpp | 17 +++---- .../src/spline_interpolation_points_2d.cpp | 11 ++--- .../test/src/test_interpolation.cpp | 0 .../test/src/test_interpolation_utils.cpp | 22 ++++----- .../test/src/test_linear_interpolation.cpp | 17 +++---- .../test_spherical_linear_interpolation.cpp | 6 +-- .../test/src/test_spline_interpolation.cpp | 44 +++++++++++------- .../test_spline_interpolation_points_2d.cpp | 16 ++++--- .../test/src/test_zero_order_hold.cpp | 32 ++++++++----- common/autoware_motion_utils/package.xml | 2 +- .../src/resample/resample.cpp | 33 +++++++------ .../src/trajectory/interpolation.cpp | 28 +++++------ .../src/trajectory/path_with_lane_id.cpp | 4 +- common/object_recognition_utils/package.xml | 2 +- .../src/predicted_path_utils.cpp | 12 ++--- .../package.xml | 2 +- .../src/mpc.cpp | 6 +-- .../src/mpc_utils.cpp | 10 ++-- .../longitudinal_controller_utils.hpp | 20 ++++---- .../package.xml | 2 +- .../src/longitudinal_controller_utils.cpp | 9 ++-- .../test_longitudinal_controller_utils.cpp | 12 ++--- .../package.xml | 2 +- .../include/predicted_path_checker/utils.hpp | 2 +- .../src/predicted_path_checker_node/utils.cpp | 20 ++++---- .../package.xml | 2 +- .../src/pose_covariance_modifier.cpp | 4 +- .../autoware_map_based_prediction/package.xml | 2 +- .../src/map_based_prediction_node.cpp | 2 +- .../src/path_generator.cpp | 3 +- .../package.xml | 2 +- .../optimization_based_planner.cpp | 8 ++-- .../pid_based_planner/pid_based_planner.cpp | 4 +- .../autoware/path_optimizer/mpt_optimizer.hpp | 14 +++--- .../path_optimizer/utils/geometry_utils.hpp | 6 +-- .../path_optimizer/utils/trajectory_utils.hpp | 6 +-- planning/autoware_path_optimizer/package.xml | 2 +- .../src/mpt_optimizer.cpp | 20 ++++---- planning/autoware_path_optimizer/src/node.cpp | 2 +- .../src/utils/trajectory_utils.cpp | 4 +- .../path_smoother/utils/trajectory_utils.hpp | 6 +-- planning/autoware_path_smoother/package.xml | 2 +- .../src/elastic_band_smoother.cpp | 2 +- .../src/utils/trajectory_utils.cpp | 2 +- .../package.xml | 2 +- .../src/static_centerline_generator_node.cpp | 4 +- .../autoware_velocity_smoother/package.xml | 2 +- .../velocity_planning_utils.cpp | 14 +++--- .../src/trajectory_utils.cpp | 6 +-- .../utils/data_structs.hpp | 6 +-- .../package.xml | 2 +- .../path_projection.hpp | 5 +- .../package.xml | 2 +- .../drivable_area_expansion.cpp | 2 +- .../geometric_parallel_parking.cpp | 18 ++++---- .../path_safety_checker/safety_check.cpp | 4 +- .../src/utils/path_shifter/path_shifter.cpp | 4 +- .../src/utils/path_utils.cpp | 2 +- .../package.xml | 2 +- .../src/sampling_planner_module.cpp | 2 +- .../package.xml | 2 +- .../package.xml | 2 +- .../src/scene_intersection_prepare_data.cpp | 4 +- .../package.xml | 2 +- .../src/scene_no_stopping_area.cpp | 2 +- .../package.xml | 2 +- .../src/occlusion_spot_utils.cpp | 2 +- .../package.xml | 2 +- .../utils/geometry_utils.hpp | 6 +-- .../utils/trajectory_utils.hpp | 6 +-- .../autoware_path_sampler/package.xml | 2 +- .../autoware_path_sampler/src/node.cpp | 2 +- .../src/prepare_inputs.cpp | 2 +- .../src/utils/trajectory_utils.cpp | 2 +- .../autoware_sampler_common/structures.hpp | 46 ++++++++++--------- .../autoware_sampler_common/package.xml | 2 +- .../vehicle_model/sim_model_actuation_cmd.hpp | 2 +- .../sim_model_delay_steer_map_acc_geared.hpp | 7 +-- .../vehicle_model/sim_model_actuation_cmd.cpp | 15 +++--- .../package.xml | 2 +- .../src/accel_map.cpp | 11 +++-- .../src/brake_map.cpp | 11 +++-- .../src/steer_map.cpp | 7 +-- 94 files changed, 388 insertions(+), 357 deletions(-) rename common/{interpolation => autoware_interpolation}/CMakeLists.txt (80%) rename common/{interpolation => autoware_interpolation}/README.md (100%) rename common/{interpolation/include => autoware_interpolation/include/autoware}/interpolation/interpolation_utils.hpp (93%) rename common/{interpolation/include => autoware_interpolation/include/autoware}/interpolation/linear_interpolation.hpp (75%) rename common/{interpolation/include => autoware_interpolation/include/autoware}/interpolation/spherical_linear_interpolation.hpp (79%) rename common/{interpolation/include => autoware_interpolation/include/autoware}/interpolation/spline_interpolation.hpp (91%) rename common/{interpolation/include => autoware_interpolation/include/autoware}/interpolation/spline_interpolation_points_2d.hpp (89%) rename common/{interpolation/include => autoware_interpolation/include/autoware}/interpolation/zero_order_hold.hpp (85%) rename common/{interpolation => autoware_interpolation}/package.xml (95%) rename common/{interpolation => autoware_interpolation}/src/linear_interpolation.cpp (86%) rename common/{interpolation => autoware_interpolation}/src/spherical_linear_interpolation.cpp (88%) rename common/{interpolation => autoware_interpolation}/src/spline_interpolation.cpp (94%) rename common/{interpolation => autoware_interpolation}/src/spline_interpolation_points_2d.cpp (97%) rename common/{interpolation => autoware_interpolation}/test/src/test_interpolation.cpp (100%) rename common/{interpolation => autoware_interpolation}/test/src/test_interpolation_utils.cpp (85%) rename common/{interpolation => autoware_interpolation}/test/src/test_linear_interpolation.cpp (80%) rename common/{interpolation => autoware_interpolation}/test/src/test_spherical_linear_interpolation.cpp (96%) rename common/{interpolation => autoware_interpolation}/test/src/test_spline_interpolation.cpp (84%) rename common/{interpolation => autoware_interpolation}/test/src/test_spline_interpolation_points_2d.cpp (92%) rename common/{interpolation => autoware_interpolation}/test/src/test_zero_order_hold.cpp (80%) diff --git a/common/interpolation/CMakeLists.txt b/common/autoware_interpolation/CMakeLists.txt similarity index 80% rename from common/interpolation/CMakeLists.txt rename to common/autoware_interpolation/CMakeLists.txt index cc91bed012432..09797272a2ac8 100644 --- a/common/interpolation/CMakeLists.txt +++ b/common/autoware_interpolation/CMakeLists.txt @@ -1,10 +1,10 @@ cmake_minimum_required(VERSION 3.14) -project(interpolation) +project(autoware_interpolation) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(interpolation SHARED +ament_auto_add_library(autoware_interpolation SHARED src/linear_interpolation.cpp src/spline_interpolation.cpp src/spline_interpolation_points_2d.cpp @@ -17,7 +17,7 @@ if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_interpolation ${test_files}) target_link_libraries(test_interpolation - interpolation + autoware_interpolation ) endif() diff --git a/common/interpolation/README.md b/common/autoware_interpolation/README.md similarity index 100% rename from common/interpolation/README.md rename to common/autoware_interpolation/README.md diff --git a/common/interpolation/include/interpolation/interpolation_utils.hpp b/common/autoware_interpolation/include/autoware/interpolation/interpolation_utils.hpp similarity index 93% rename from common/interpolation/include/interpolation/interpolation_utils.hpp rename to common/autoware_interpolation/include/autoware/interpolation/interpolation_utils.hpp index 9c0372f788ecb..1c0913c8847f4 100644 --- a/common/interpolation/include/interpolation/interpolation_utils.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/interpolation_utils.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef INTERPOLATION__INTERPOLATION_UTILS_HPP_ -#define INTERPOLATION__INTERPOLATION_UTILS_HPP_ +#ifndef AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_ +#define AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_ #include #include #include #include -namespace interpolation_utils +namespace autoware::interpolation { inline bool isIncreasing(const std::vector & x) { @@ -109,6 +109,6 @@ void validateKeysAndValues( throw std::invalid_argument("The size of base_keys and base_values are not the same."); } } -} // namespace interpolation_utils +} // namespace autoware::interpolation -#endif // INTERPOLATION__INTERPOLATION_UTILS_HPP_ +#endif // AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_ diff --git a/common/interpolation/include/interpolation/linear_interpolation.hpp b/common/autoware_interpolation/include/autoware/interpolation/linear_interpolation.hpp similarity index 75% rename from common/interpolation/include/interpolation/linear_interpolation.hpp rename to common/autoware_interpolation/include/autoware/interpolation/linear_interpolation.hpp index 1f8155613111e..762806b3a5c35 100644 --- a/common/interpolation/include/interpolation/linear_interpolation.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/linear_interpolation.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef INTERPOLATION__LINEAR_INTERPOLATION_HPP_ -#define INTERPOLATION__LINEAR_INTERPOLATION_HPP_ +#ifndef AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_ +#define AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_ -#include "interpolation/interpolation_utils.hpp" +#include "autoware/interpolation/interpolation_utils.hpp" #include -namespace interpolation +namespace autoware::interpolation { double lerp(const double src_val, const double dst_val, const double ratio); @@ -30,7 +30,6 @@ std::vector lerp( double lerp( const std::vector & base_keys, const std::vector & base_values, const double query_key); +} // namespace autoware::interpolation -} // namespace interpolation - -#endif // INTERPOLATION__LINEAR_INTERPOLATION_HPP_ +#endif // AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_ diff --git a/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp b/common/autoware_interpolation/include/autoware/interpolation/spherical_linear_interpolation.hpp similarity index 79% rename from common/interpolation/include/interpolation/spherical_linear_interpolation.hpp rename to common/autoware_interpolation/include/autoware/interpolation/spherical_linear_interpolation.hpp index 8c1d49fb5f607..2e4a8fbd42907 100644 --- a/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/spherical_linear_interpolation.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ -#define INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ +#ifndef AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ +#define AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ -#include "interpolation/interpolation_utils.hpp" +#include "autoware/interpolation/interpolation_utils.hpp" #include @@ -29,7 +29,7 @@ #include -namespace interpolation +namespace autoware::interpolation { geometry_msgs::msg::Quaternion slerp( const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat, @@ -43,6 +43,6 @@ std::vector slerp( geometry_msgs::msg::Quaternion lerpOrientation( const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to, const double ratio); -} // namespace interpolation +} // namespace autoware::interpolation -#endif // INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ +#endif // AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ diff --git a/common/interpolation/include/interpolation/spline_interpolation.hpp b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp similarity index 91% rename from common/interpolation/include/interpolation/spline_interpolation.hpp rename to common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp index 54603a68d37a5..f7feada78ff4f 100644 --- a/common/interpolation/include/interpolation/spline_interpolation.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef INTERPOLATION__SPLINE_INTERPOLATION_HPP_ -#define INTERPOLATION__SPLINE_INTERPOLATION_HPP_ +#ifndef AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_ +#define AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_ +#include "autoware/interpolation/interpolation_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "interpolation/interpolation_utils.hpp" #include @@ -26,7 +26,7 @@ #include #include -namespace interpolation +namespace autoware::interpolation { // static spline interpolation functions std::vector spline( @@ -35,7 +35,6 @@ std::vector spline( std::vector splineByAkima( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys); -} // namespace interpolation // non-static 1-dimensional spline interpolation // @@ -93,5 +92,6 @@ class SplineInterpolation Eigen::Index get_index(const double & key) const; }; +} // namespace autoware::interpolation -#endif // INTERPOLATION__SPLINE_INTERPOLATION_HPP_ +#endif // AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_ diff --git a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp similarity index 89% rename from common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp rename to common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp index b2170cf83bde2..c9668a4cad7eb 100644 --- a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp @@ -12,19 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_ -#define INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_ +#ifndef AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_ +#define AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_ -#include "interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" #include -namespace interpolation +namespace autoware::interpolation { template std::vector splineYawFromPoints(const std::vector & points); -} // namespace interpolation // non-static points spline interpolation // NOTE: We can calculate yaw from the x and y by interpolation derivatives. @@ -85,5 +84,6 @@ class SplineInterpolationPoints2d std::vector base_s_vec_; }; +} // namespace autoware::interpolation -#endif // INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_ +#endif // AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_ diff --git a/common/interpolation/include/interpolation/zero_order_hold.hpp b/common/autoware_interpolation/include/autoware/interpolation/zero_order_hold.hpp similarity index 85% rename from common/interpolation/include/interpolation/zero_order_hold.hpp rename to common/autoware_interpolation/include/autoware/interpolation/zero_order_hold.hpp index 966128321b470..c28f870837da7 100644 --- a/common/interpolation/include/interpolation/zero_order_hold.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/zero_order_hold.hpp @@ -12,21 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef INTERPOLATION__ZERO_ORDER_HOLD_HPP_ -#define INTERPOLATION__ZERO_ORDER_HOLD_HPP_ +#ifndef AUTOWARE__INTERPOLATION__ZERO_ORDER_HOLD_HPP_ +#define AUTOWARE__INTERPOLATION__ZERO_ORDER_HOLD_HPP_ -#include "interpolation/interpolation_utils.hpp" +#include "autoware/interpolation/interpolation_utils.hpp" #include -namespace interpolation +namespace autoware::interpolation { inline std::vector calc_closest_segment_indices( const std::vector & base_keys, const std::vector & query_keys, const double overlap_threshold = 1e-3) { // throw exception for invalid arguments - const auto validated_query_keys = interpolation_utils::validateKeys(base_keys, query_keys); + const auto validated_query_keys = validateKeys(base_keys, query_keys); std::vector closest_segment_indices(validated_query_keys.size()); size_t closest_segment_idx = 0; @@ -58,7 +58,7 @@ std::vector zero_order_hold( const std::vector & closest_segment_indices) { // throw exception for invalid arguments - interpolation_utils::validateKeysAndValues(base_keys, base_values); + validateKeysAndValues(base_keys, base_values); std::vector query_values(closest_segment_indices.size()); for (size_t i = 0; i < closest_segment_indices.size(); ++i) { @@ -76,6 +76,6 @@ std::vector zero_order_hold( return zero_order_hold( base_keys, base_values, calc_closest_segment_indices(base_keys, query_keys, overlap_threshold)); } -} // namespace interpolation +} // namespace autoware::interpolation -#endif // INTERPOLATION__ZERO_ORDER_HOLD_HPP_ +#endif // AUTOWARE__INTERPOLATION__ZERO_ORDER_HOLD_HPP_ diff --git a/common/interpolation/package.xml b/common/autoware_interpolation/package.xml similarity index 95% rename from common/interpolation/package.xml rename to common/autoware_interpolation/package.xml index 330c87df18423..d2538bd602f45 100644 --- a/common/interpolation/package.xml +++ b/common/autoware_interpolation/package.xml @@ -1,7 +1,7 @@ - interpolation + autoware_interpolation 0.1.0 The spline interpolation package Fumiya Watanabe diff --git a/common/interpolation/src/linear_interpolation.cpp b/common/autoware_interpolation/src/linear_interpolation.cpp similarity index 86% rename from common/interpolation/src/linear_interpolation.cpp rename to common/autoware_interpolation/src/linear_interpolation.cpp index f74d085dfee9e..4ba339f0e538e 100644 --- a/common/interpolation/src/linear_interpolation.cpp +++ b/common/autoware_interpolation/src/linear_interpolation.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include -namespace interpolation +namespace autoware::interpolation { double lerp(const double src_val, const double dst_val, const double ratio) { @@ -28,8 +28,8 @@ std::vector lerp( const std::vector & query_keys) { // throw exception for invalid arguments - const auto validated_query_keys = interpolation_utils::validateKeys(base_keys, query_keys); - interpolation_utils::validateKeysAndValues(base_keys, base_values); + const auto validated_query_keys = validateKeys(base_keys, query_keys); + validateKeysAndValues(base_keys, base_values); // calculate linear interpolation std::vector query_values; @@ -56,4 +56,4 @@ double lerp( { return lerp(base_keys, base_values, std::vector{query_key}).front(); } -} // namespace interpolation +} // namespace autoware::interpolation diff --git a/common/interpolation/src/spherical_linear_interpolation.cpp b/common/autoware_interpolation/src/spherical_linear_interpolation.cpp similarity index 88% rename from common/interpolation/src/spherical_linear_interpolation.cpp rename to common/autoware_interpolation/src/spherical_linear_interpolation.cpp index e44626498a80b..697dda3b06770 100644 --- a/common/interpolation/src/spherical_linear_interpolation.cpp +++ b/common/autoware_interpolation/src/spherical_linear_interpolation.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/spherical_linear_interpolation.hpp" +#include "autoware/interpolation/spherical_linear_interpolation.hpp" -namespace interpolation +namespace autoware::interpolation { geometry_msgs::msg::Quaternion slerp( const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat, @@ -34,8 +34,8 @@ std::vector slerp( const std::vector & query_keys) { // throw exception for invalid arguments - const auto validated_query_keys = interpolation_utils::validateKeys(base_keys, query_keys); - interpolation_utils::validateKeysAndValues(base_keys, base_values); + const auto validated_query_keys = validateKeys(base_keys, query_keys); + validateKeysAndValues(base_keys, base_values); // calculate linear interpolation std::vector query_values; @@ -68,4 +68,4 @@ geometry_msgs::msg::Quaternion lerpOrientation( const auto q_interpolated = q_from.slerp(q_to, ratio); return tf2::toMsg(q_interpolated); } -} // namespace interpolation +} // namespace autoware::interpolation diff --git a/common/interpolation/src/spline_interpolation.cpp b/common/autoware_interpolation/src/spline_interpolation.cpp similarity index 94% rename from common/interpolation/src/spline_interpolation.cpp rename to common/autoware_interpolation/src/spline_interpolation.cpp index 5cf8f68ca519d..78f3778ae8e21 100644 --- a/common/interpolation/src/spline_interpolation.cpp +++ b/common/autoware_interpolation/src/spline_interpolation.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" #include #include -namespace +namespace autoware::interpolation { Eigen::VectorXd solve_tridiagonal_matrix_algorithm( const Eigen::Ref & a, const Eigen::Ref & b, @@ -52,10 +52,7 @@ Eigen::VectorXd solve_tridiagonal_matrix_algorithm( return x; } -} // namespace -namespace interpolation -{ std::vector spline( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys) @@ -139,7 +136,6 @@ std::vector splineByAkima( } return res; } -} // namespace interpolation Eigen::Index SplineInterpolation::get_index(const double & key) const { @@ -153,7 +149,7 @@ void SplineInterpolation::calcSplineCoefficients( const std::vector & base_keys, const std::vector & base_values) { // throw exceptions for invalid arguments - interpolation_utils::validateKeysAndValues(base_keys, base_values); + autoware::interpolation::validateKeysAndValues(base_keys, base_values); const Eigen::VectorXd x = Eigen::Map( base_keys.data(), static_cast(base_keys.size())); const Eigen::VectorXd y = Eigen::Map( @@ -201,7 +197,7 @@ std::vector SplineInterpolation::getSplineInterpolatedValues( const std::vector & query_keys) const { // throw exceptions for invalid arguments - const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); + const auto validated_query_keys = autoware::interpolation::validateKeys(base_keys_, query_keys); std::vector interpolated_values; interpolated_values.reserve(query_keys.size()); @@ -219,7 +215,7 @@ std::vector SplineInterpolation::getSplineInterpolatedDiffValues( const std::vector & query_keys) const { // throw exceptions for invalid arguments - const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); + const auto validated_query_keys = autoware::interpolation::validateKeys(base_keys_, query_keys); std::vector interpolated_diff_values; interpolated_diff_values.reserve(query_keys.size()); @@ -236,7 +232,7 @@ std::vector SplineInterpolation::getSplineInterpolatedQuadDiffValues( const std::vector & query_keys) const { // throw exceptions for invalid arguments - const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); + const auto validated_query_keys = autoware::interpolation::validateKeys(base_keys_, query_keys); std::vector interpolated_quad_diff_values; interpolated_quad_diff_values.reserve(query_keys.size()); @@ -248,3 +244,4 @@ std::vector SplineInterpolation::getSplineInterpolatedQuadDiffValues( return interpolated_quad_diff_values; } +} // namespace autoware::interpolation diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp similarity index 97% rename from common/interpolation/src/spline_interpolation_points_2d.cpp rename to common/autoware_interpolation/src/spline_interpolation_points_2d.cpp index 95fde68b171bd..e0f55cfb24ba8 100644 --- a/common/interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/spline_interpolation_points_2d.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include -namespace +namespace autoware::interpolation { std::vector calcEuclidDist(const std::vector & x, const std::vector & y) { @@ -66,10 +66,6 @@ std::array, 4> getBaseValues( return {base_s, base_x, base_y, base_z}; } -} // namespace - -namespace interpolation -{ template std::vector splineYawFromPoints(const std::vector & points) @@ -88,8 +84,6 @@ std::vector splineYawFromPoints(const std::vector & points) template std::vector splineYawFromPoints( const std::vector & points); -} // namespace interpolation - geometry_msgs::msg::Pose SplineInterpolationPoints2d::getSplineInterpolatedPose( const size_t idx, const double s) const { @@ -215,3 +209,4 @@ void SplineInterpolationPoints2d::calcSplineCoefficientsInner( spline_y_ = SplineInterpolation(base_s_vec_, base_y_vec); spline_z_ = SplineInterpolation(base_s_vec_, base_z_vec); } +} // namespace autoware::interpolation diff --git a/common/interpolation/test/src/test_interpolation.cpp b/common/autoware_interpolation/test/src/test_interpolation.cpp similarity index 100% rename from common/interpolation/test/src/test_interpolation.cpp rename to common/autoware_interpolation/test/src/test_interpolation.cpp diff --git a/common/interpolation/test/src/test_interpolation_utils.cpp b/common/autoware_interpolation/test/src/test_interpolation_utils.cpp similarity index 85% rename from common/interpolation/test/src/test_interpolation_utils.cpp rename to common/autoware_interpolation/test/src/test_interpolation_utils.cpp index 8b3a3b9faa0c6..2aa41b6fdef00 100644 --- a/common/interpolation/test/src/test_interpolation_utils.cpp +++ b/common/autoware_interpolation/test/src/test_interpolation_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/interpolation_utils.hpp" +#include "autoware/interpolation/interpolation_utils.hpp" #include @@ -24,43 +24,43 @@ TEST(interpolation_utils, isIncreasing) { // empty const std::vector empty_vec; - EXPECT_THROW(interpolation_utils::isIncreasing(empty_vec), std::invalid_argument); + EXPECT_THROW(autoware::interpolation::isIncreasing(empty_vec), std::invalid_argument); // increase const std::vector increasing_vec{0.0, 1.5, 3.0, 4.5, 6.0}; - EXPECT_EQ(interpolation_utils::isIncreasing(increasing_vec), true); + EXPECT_EQ(autoware::interpolation::isIncreasing(increasing_vec), true); // not decrease const std::vector not_increasing_vec{0.0, 1.5, 1.5, 4.5, 6.0}; - EXPECT_EQ(interpolation_utils::isIncreasing(not_increasing_vec), false); + EXPECT_EQ(autoware::interpolation::isIncreasing(not_increasing_vec), false); // decrease const std::vector decreasing_vec{0.0, 1.5, 1.2, 4.5, 6.0}; - EXPECT_EQ(interpolation_utils::isIncreasing(decreasing_vec), false); + EXPECT_EQ(autoware::interpolation::isIncreasing(decreasing_vec), false); } TEST(interpolation_utils, isNotDecreasing) { // empty const std::vector empty_vec; - EXPECT_THROW(interpolation_utils::isNotDecreasing(empty_vec), std::invalid_argument); + EXPECT_THROW(autoware::interpolation::isNotDecreasing(empty_vec), std::invalid_argument); // increase const std::vector increasing_vec{0.0, 1.5, 3.0, 4.5, 6.0}; - EXPECT_EQ(interpolation_utils::isNotDecreasing(increasing_vec), true); + EXPECT_EQ(autoware::interpolation::isNotDecreasing(increasing_vec), true); // not decrease const std::vector not_increasing_vec{0.0, 1.5, 1.5, 4.5, 6.0}; - EXPECT_EQ(interpolation_utils::isNotDecreasing(not_increasing_vec), true); + EXPECT_EQ(autoware::interpolation::isNotDecreasing(not_increasing_vec), true); // decrease const std::vector decreasing_vec{0.0, 1.5, 1.2, 4.5, 6.0}; - EXPECT_EQ(interpolation_utils::isNotDecreasing(decreasing_vec), false); + EXPECT_EQ(autoware::interpolation::isNotDecreasing(decreasing_vec), false); } TEST(interpolation_utils, validateKeys) { - using interpolation_utils::validateKeys; + using autoware::interpolation::validateKeys; const std::vector base_keys{0.0, 1.0, 2.0, 3.0}; const std::vector query_keys{0.0, 1.0, 2.0, 3.0}; @@ -116,7 +116,7 @@ TEST(interpolation_utils, validateKeys) TEST(interpolation_utils, validateKeysAndValues) { - using interpolation_utils::validateKeysAndValues; + using autoware::interpolation::validateKeysAndValues; const std::vector base_keys{0.0, 1.0, 2.0, 3.0}; const std::vector base_values{0.0, 1.0, 2.0, 3.0}; diff --git a/common/interpolation/test/src/test_linear_interpolation.cpp b/common/autoware_interpolation/test/src/test_linear_interpolation.cpp similarity index 80% rename from common/interpolation/test/src/test_linear_interpolation.cpp rename to common/autoware_interpolation/test/src/test_linear_interpolation.cpp index 9c392943bd3c5..0fea1e514e916 100644 --- a/common/interpolation/test/src/test_linear_interpolation.cpp +++ b/common/autoware_interpolation/test/src/test_linear_interpolation.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include @@ -23,8 +23,8 @@ constexpr double epsilon = 1e-6; TEST(linear_interpolation, lerp_scalar) { - EXPECT_EQ(interpolation::lerp(0.0, 1.0, 0.3), 0.3); - EXPECT_EQ(interpolation::lerp(-0.5, 12.3, 0.3), 3.34); + EXPECT_EQ(autoware::interpolation::lerp(0.0, 1.0, 0.3), 0.3); + EXPECT_EQ(autoware::interpolation::lerp(-0.5, 12.3, 0.3), 3.34); } TEST(linear_interpolation, lerp_vector) @@ -35,7 +35,7 @@ TEST(linear_interpolation, lerp_vector) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::lerp(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::lerp(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -47,7 +47,7 @@ TEST(linear_interpolation, lerp_vector) const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; const std::vector ans{0.0, 1.05, 2.85, 6.0}; - const auto query_values = interpolation::lerp(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::lerp(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -59,7 +59,7 @@ TEST(linear_interpolation, lerp_vector) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::lerp(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::lerp(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -71,7 +71,7 @@ TEST(linear_interpolation, lerp_vector) const std::vector query_keys{0.0, 8.0, 18.0}; const std::vector ans{-0.18, 1.12, 1.4}; - const auto query_values = interpolation::lerp(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::lerp(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -87,7 +87,8 @@ TEST(linear_interpolation, lerp_scalar_query) const std::vector ans{-0.18, 1.12, 1.4}; for (size_t i = 0; i < query_keys.size(); ++i) { - const auto query_value = interpolation::lerp(base_keys, base_values, query_keys.at(i)); + const auto query_value = + autoware::interpolation::lerp(base_keys, base_values, query_keys.at(i)); EXPECT_NEAR(query_value, ans.at(i), epsilon); } } diff --git a/common/interpolation/test/src/test_spherical_linear_interpolation.cpp b/common/autoware_interpolation/test/src/test_spherical_linear_interpolation.cpp similarity index 96% rename from common/interpolation/test/src/test_spherical_linear_interpolation.cpp rename to common/autoware_interpolation/test/src/test_spherical_linear_interpolation.cpp index 0bb9ba8ef2ce8..b7ce134c680bd 100644 --- a/common/interpolation/test/src/test_spherical_linear_interpolation.cpp +++ b/common/autoware_interpolation/test/src/test_spherical_linear_interpolation.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/spherical_linear_interpolation.hpp" +#include "autoware/interpolation/spherical_linear_interpolation.hpp" #include @@ -34,7 +34,7 @@ inline geometry_msgs::msg::Quaternion createQuaternionFromRPY( TEST(slerp, spline_scalar) { - using interpolation::slerp; + using autoware::interpolation::slerp; // Same value { @@ -79,7 +79,7 @@ TEST(slerp, spline_scalar) TEST(slerp, spline_vector) { - using interpolation::slerp; + using autoware::interpolation::slerp; // query keys are same as base keys { diff --git a/common/interpolation/test/src/test_spline_interpolation.cpp b/common/autoware_interpolation/test/src/test_spline_interpolation.cpp similarity index 84% rename from common/interpolation/test/src/test_spline_interpolation.cpp rename to common/autoware_interpolation/test/src/test_spline_interpolation.cpp index 6e6a9192cf71a..766e94a47b968 100644 --- a/common/interpolation/test/src/test_spline_interpolation.cpp +++ b/common/autoware_interpolation/test/src/test_spline_interpolation.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/interpolation/spline_interpolation.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "interpolation/spline_interpolation.hpp" #include @@ -22,6 +22,8 @@ constexpr double epsilon = 1e-6; +using autoware::interpolation::SplineInterpolation; + TEST(spline_interpolation, spline) { { // straight: query_keys is same as base_keys @@ -30,7 +32,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -42,7 +44,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; const std::vector ans{0.0, 1.05, 2.85, 6.0}; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -54,7 +56,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -66,7 +68,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys{0.0, 8.0, 18.0}; const std::vector ans{-0.076114, 1.001217, 1.573640}; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -78,7 +80,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -90,7 +92,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -102,7 +104,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys{-1.0, 0.0, 4.0}; const std::vector ans{-0.808769, -0.077539, 1.035096}; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -114,7 +116,7 @@ TEST(spline_interpolation, spline) const std::vector query_keys = {0.0, 1.0, 1.5, 2.0, 3.0, 4.0}; const std::vector ans = {0.0, 0.0, 158.738293, 0.1, 0.1, 0.1}; - const auto query_values = interpolation::spline(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -129,7 +131,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -141,7 +144,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; const std::vector ans{0.0, 1.05, 2.85, 6.0}; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -153,7 +157,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -165,7 +170,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys{0.0, 8.0, 18.0}; const std::vector ans{-0.0801, 1.110749, 1.4864}; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -177,7 +183,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -189,7 +196,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -201,7 +209,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys{-1.0, 0.0, 4.0}; const std::vector ans{-0.8378, -0.0801, 0.927031}; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -213,7 +222,8 @@ TEST(spline_interpolation, splineByAkima) const std::vector query_keys = {0.0, 1.0, 1.5, 2.0, 3.0, 4.0}; const std::vector ans = {0.0, 0.0, 0.1, 0.1, 0.1, 0.1}; - const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } diff --git a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp b/common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp similarity index 92% rename from common/interpolation/test/src/test_spline_interpolation_points_2d.cpp rename to common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp index 7e41980c78bdc..974ad606b978d 100644 --- a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp +++ b/common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include @@ -23,6 +23,8 @@ constexpr double epsilon = 1e-6; +using autoware::interpolation::SplineInterpolationPoints2d; + TEST(spline_interpolation, splineYawFromPoints) { using autoware::universe_utils::createPoint; @@ -37,7 +39,7 @@ TEST(spline_interpolation, splineYawFromPoints) const std::vector ans{0.9827937, 0.9827937, 0.9827937, 0.9827937, 0.9827937}; - const auto yaws = interpolation::splineYawFromPoints(points); + const auto yaws = autoware::interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } @@ -52,7 +54,7 @@ TEST(spline_interpolation, splineYawFromPoints) points.push_back(createPoint(10.0, 12.5, 0.0)); const std::vector ans{1.368174, 0.961318, 1.086098, 0.938357, 0.278594}; - const auto yaws = interpolation::splineYawFromPoints(points); + const auto yaws = autoware::interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } @@ -62,7 +64,7 @@ TEST(spline_interpolation, splineYawFromPoints) std::vector points; points.push_back(createPoint(1.0, 0.0, 0.0)); - EXPECT_THROW(interpolation::splineYawFromPoints(points), std::logic_error); + EXPECT_THROW(autoware::interpolation::splineYawFromPoints(points), std::logic_error); } { // straight: size of base_keys is 2 (edge case in the implementation) @@ -72,7 +74,7 @@ TEST(spline_interpolation, splineYawFromPoints) const std::vector ans{0.9827937, 0.9827937}; - const auto yaws = interpolation::splineYawFromPoints(points); + const auto yaws = autoware::interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } @@ -86,7 +88,7 @@ TEST(spline_interpolation, splineYawFromPoints) const std::vector ans{0.9827937, 0.9827937, 0.9827937}; - const auto yaws = interpolation::splineYawFromPoints(points); + const auto yaws = autoware::interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } diff --git a/common/interpolation/test/src/test_zero_order_hold.cpp b/common/autoware_interpolation/test/src/test_zero_order_hold.cpp similarity index 80% rename from common/interpolation/test/src/test_zero_order_hold.cpp rename to common/autoware_interpolation/test/src/test_zero_order_hold.cpp index 541af1cf76064..c99348d66d034 100644 --- a/common/interpolation/test/src/test_zero_order_hold.cpp +++ b/common/autoware_interpolation/test/src/test_zero_order_hold.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "interpolation/zero_order_hold.hpp" +#include "autoware/interpolation/zero_order_hold.hpp" #include @@ -29,7 +29,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -41,7 +42,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation) const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; const std::vector ans{0.0, 0.0, 1.5, 6.0}; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -53,7 +55,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -65,7 +68,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation) const std::vector query_keys{0.0, 8.0, 18.0}; const std::vector ans{-1.2, 1.0, 2.0}; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -77,7 +81,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation) const std::vector query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.001}; const std::vector ans = {0.0, 1.5, 2.5, 3.5, 3.5}; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -89,7 +94,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation) const std::vector query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.0001}; const std::vector ans = {0.0, 1.5, 2.5, 3.5, 0.0}; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -104,7 +110,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation_no_double_interpolation const std::vector query_keys = base_keys; const auto ans = base_values; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_EQ(query_values.at(i), ans.at(i)); } @@ -116,7 +123,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation_no_double_interpolation const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; const std::vector ans = {true, true, false, false}; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_EQ(query_values.at(i), ans.at(i)); } @@ -128,7 +136,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation_no_double_interpolation const std::vector query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.001}; const std::vector ans = {true, true, false, true, true}; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -140,7 +149,8 @@ TEST(zero_order_hold_interpolation, vector_interpolation_no_double_interpolation const std::vector query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.0001}; const std::vector ans = base_values; - const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys); + const auto query_values = + autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } diff --git a/common/autoware_motion_utils/package.xml b/common/autoware_motion_utils/package.xml index 81fec78b04812..1fe2c4d8e1ea8 100644 --- a/common/autoware_motion_utils/package.xml +++ b/common/autoware_motion_utils/package.xml @@ -22,12 +22,12 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_interpolation autoware_planning_msgs autoware_universe_utils autoware_vehicle_msgs builtin_interfaces geometry_msgs - interpolation libboost-dev rclcpp tf2 diff --git a/common/autoware_motion_utils/src/resample/resample.cpp b/common/autoware_motion_utils/src/resample/resample.cpp index 1f29a4577e428..88ebf3f41c408 100644 --- a/common/autoware_motion_utils/src/resample/resample.cpp +++ b/common/autoware_motion_utils/src/resample/resample.cpp @@ -14,12 +14,12 @@ #include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/zero_order_hold.hpp" #include "autoware/motion_utils/resample/resample_utils.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/zero_order_hold.hpp" #include @@ -61,13 +61,13 @@ std::vector resamplePointVector( // Interpolate const auto lerp = [&](const auto & input) { - return interpolation::lerp(input_arclength, input, resampled_arclength); + return autoware::interpolation::lerp(input_arclength, input, resampled_arclength); }; const auto spline = [&](const auto & input) { - return interpolation::spline(input_arclength, input, resampled_arclength); + return autoware::interpolation::spline(input_arclength, input, resampled_arclength); }; const auto spline_by_akima = [&](const auto & input) { - return interpolation::splineByAkima(input_arclength, input, resampled_arclength); + return autoware::interpolation::splineByAkima(input_arclength, input, resampled_arclength); }; const auto interpolated_x = use_akima_spline_for_xy ? lerp(x) : spline_by_akima(x); @@ -280,14 +280,15 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath( // Interpolate const auto lerp = [&](const auto & input) { - return interpolation::lerp(input_arclength, input, resampling_arclength); + return autoware::interpolation::lerp(input_arclength, input, resampling_arclength); }; auto closest_segment_indices = - interpolation::calc_closest_segment_indices(input_arclength, resampling_arclength); + autoware::interpolation::calc_closest_segment_indices(input_arclength, resampling_arclength); const auto zoh = [&](const auto & input) { - return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); + return autoware::interpolation::zero_order_hold( + input_arclength, input, closest_segment_indices); }; const auto interpolated_pose = @@ -462,16 +463,17 @@ autoware_planning_msgs::msg::Path resamplePath( // Interpolate const auto lerp = [&](const auto & input) { - return interpolation::lerp(input_arclength, input, resampled_arclength); + return autoware::interpolation::lerp(input_arclength, input, resampled_arclength); }; std::vector closest_segment_indices; if (use_zero_order_hold_for_v) { closest_segment_indices = - interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); + autoware::interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); } const auto zoh = [&](const auto & input) { - return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); + return autoware::interpolation::zero_order_hold( + input_arclength, input, closest_segment_indices); }; const auto interpolated_pose = @@ -636,16 +638,17 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory( // Interpolate const auto lerp = [&](const auto & input) { - return interpolation::lerp(input_arclength, input, resampled_arclength); + return autoware::interpolation::lerp(input_arclength, input, resampled_arclength); }; std::vector closest_segment_indices; if (use_zero_order_hold_for_twist) { closest_segment_indices = - interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); + autoware::interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); } const auto zoh = [&](const auto & input) { - return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); + return autoware::interpolation::zero_order_hold( + input_arclength, input, closest_segment_indices); }; const auto interpolated_pose = diff --git a/common/autoware_motion_utils/src/trajectory/interpolation.cpp b/common/autoware_motion_utils/src/trajectory/interpolation.cpp index 4a9eaeca58d30..0ae9d44683f62 100644 --- a/common/autoware_motion_utils/src/trajectory/interpolation.cpp +++ b/common/autoware_motion_utils/src/trajectory/interpolation.cpp @@ -14,8 +14,8 @@ #include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "interpolation/linear_interpolation.hpp" using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -66,26 +66,26 @@ TrajectoryPoint calcInterpolatedPoint( interpolated_point.lateral_velocity_mps = curr_pt.lateral_velocity_mps; interpolated_point.acceleration_mps2 = curr_pt.acceleration_mps2; } else { - interpolated_point.longitudinal_velocity_mps = interpolation::lerp( + interpolated_point.longitudinal_velocity_mps = autoware::interpolation::lerp( curr_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, clamped_ratio); - interpolated_point.lateral_velocity_mps = interpolation::lerp( + interpolated_point.lateral_velocity_mps = autoware::interpolation::lerp( curr_pt.lateral_velocity_mps, next_pt.lateral_velocity_mps, clamped_ratio); - interpolated_point.acceleration_mps2 = - interpolation::lerp(curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); + interpolated_point.acceleration_mps2 = autoware::interpolation::lerp( + curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); } // heading rate interpolation - interpolated_point.heading_rate_rps = - interpolation::lerp(curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); + interpolated_point.heading_rate_rps = autoware::interpolation::lerp( + curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); // wheel interpolation - interpolated_point.front_wheel_angle_rad = interpolation::lerp( + interpolated_point.front_wheel_angle_rad = autoware::interpolation::lerp( curr_pt.front_wheel_angle_rad, next_pt.front_wheel_angle_rad, clamped_ratio); - interpolated_point.rear_wheel_angle_rad = - interpolation::lerp(curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); + interpolated_point.rear_wheel_angle_rad = autoware::interpolation::lerp( + curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); // time interpolation - const double interpolated_time = interpolation::lerp( + const double interpolated_time = autoware::interpolation::lerp( rclcpp::Duration(curr_pt.time_from_start).seconds(), rclcpp::Duration(next_pt.time_from_start).seconds(), clamped_ratio); interpolated_point.time_from_start = rclcpp::Duration::from_seconds(interpolated_time); @@ -134,15 +134,15 @@ PathPointWithLaneId calcInterpolatedPoint( interpolated_point.point.longitudinal_velocity_mps = curr_pt.point.longitudinal_velocity_mps; interpolated_point.point.lateral_velocity_mps = curr_pt.point.lateral_velocity_mps; } else { - interpolated_point.point.longitudinal_velocity_mps = interpolation::lerp( + interpolated_point.point.longitudinal_velocity_mps = autoware::interpolation::lerp( curr_pt.point.longitudinal_velocity_mps, next_pt.point.longitudinal_velocity_mps, clamped_ratio); - interpolated_point.point.lateral_velocity_mps = interpolation::lerp( + interpolated_point.point.lateral_velocity_mps = autoware::interpolation::lerp( curr_pt.point.lateral_velocity_mps, next_pt.point.lateral_velocity_mps, clamped_ratio); } // heading rate interpolation - interpolated_point.point.heading_rate_rps = interpolation::lerp( + interpolated_point.point.heading_rate_rps = autoware::interpolation::lerp( curr_pt.point.heading_rate_rps, next_pt.point.heading_rate_rps, clamped_ratio); return interpolated_point; diff --git a/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp b/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp index cd8de63f56c1d..55c7360a25c41 100644 --- a/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp +++ b/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp @@ -14,8 +14,8 @@ #include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include #include @@ -106,7 +106,7 @@ tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( auto cog_path = path; // calculate curvature and yaw from spline interpolation - const auto spline = SplineInterpolationPoints2d(path.points); + const auto spline = autoware::interpolation::SplineInterpolationPoints2d(path.points); const auto curvature_vec = spline.getSplineInterpolatedCurvatures(); const auto yaw_vec = spline.getSplineInterpolatedYaws(); diff --git a/common/object_recognition_utils/package.xml b/common/object_recognition_utils/package.xml index d0802bbc9b457..a3f4a99300716 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -12,10 +12,10 @@ ament_cmake_auto autoware_cmake + autoware_interpolation autoware_perception_msgs autoware_universe_utils geometry_msgs - interpolation libboost-dev pcl_conversions pcl_ros diff --git a/common/object_recognition_utils/src/predicted_path_utils.cpp b/common/object_recognition_utils/src/predicted_path_utils.cpp index e26c9e6a7e1ea..96c62d9f50323 100644 --- a/common/object_recognition_utils/src/predicted_path_utils.cpp +++ b/common/object_recognition_utils/src/predicted_path_utils.cpp @@ -14,9 +14,9 @@ #include "object_recognition_utils/predicted_path_utils.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spherical_linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spherical_linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" #include @@ -69,13 +69,13 @@ autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( } const auto lerp = [&](const auto & input) { - return interpolation::lerp(input_time, input, resampled_time); + return autoware::interpolation::lerp(input_time, input, resampled_time); }; const auto spline = [&](const auto & input) { - return interpolation::spline(input_time, input, resampled_time); + return autoware::interpolation::spline(input_time, input, resampled_time); }; const auto slerp = [&](const auto & input) { - return interpolation::slerp(input_time, input, resampled_time); + return autoware::interpolation::slerp(input_time, input, resampled_time); }; const auto interpolated_x = use_spline_for_xy ? spline(x) : lerp(x); diff --git a/control/autoware_mpc_lateral_controller/package.xml b/control/autoware_mpc_lateral_controller/package.xml index e25352797a0d0..5c5f8886d6ed3 100644 --- a/control/autoware_mpc_lateral_controller/package.xml +++ b/control/autoware_mpc_lateral_controller/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_control_msgs + autoware_interpolation autoware_motion_utils autoware_planning_msgs autoware_trajectory_follower_base @@ -28,7 +29,6 @@ diagnostic_updater eigen geometry_msgs - interpolation osqp_interface rclcpp rclcpp_components diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index b5cd0e7ba3e2a..0f350dc40ad0e 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -14,10 +14,10 @@ #include "autoware/mpc_lateral_controller/mpc.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/mpc_lateral_controller/mpc_utils.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" -#include "interpolation/linear_interpolation.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -383,8 +383,8 @@ std::pair MPC::updateStateForDelayCompensation( double k, v = 0.0; try { // NOTE: When driving backward, the curvature's sign should be reversed. - k = interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time) * sign_vx; - v = interpolation::lerp(traj.relative_time, traj.vx, mpc_curr_time); + k = autoware::interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time) * sign_vx; + v = autoware::interpolation::lerp(traj.relative_time, traj.vx, mpc_curr_time); } catch (const std::exception & e) { RCLCPP_ERROR(m_logger, "mpc resample failed at delay compensation, stop mpc: %s", e.what()); return {false, {}}; diff --git a/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp index 37eb47ab0396e..c257bada0b0b2 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp @@ -14,11 +14,11 @@ #include "autoware/mpc_lateral_controller/mpc_utils.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/normalization.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" #include #include @@ -134,10 +134,10 @@ std::pair resampleMPCTrajectoryByDistance( convertEulerAngleToMonotonic(input_yaw); const auto lerp_arc_length = [&](const auto & input_value) { - return interpolation::lerp(input_arclength, input_value, output_arclength); + return autoware::interpolation::lerp(input_arclength, input_value, output_arclength); }; const auto spline_arc_length = [&](const auto & input_value) { - return interpolation::spline(input_arclength, input_value, output_arclength); + return autoware::interpolation::spline(input_arclength, input_value, output_arclength); }; output.x = spline_arc_length(input.x); @@ -165,7 +165,7 @@ bool linearInterpMPCTrajectory( convertEulerAngleToMonotonic(in_traj_yaw); const auto lerp_arc_length = [&](const auto & input_value) { - return interpolation::lerp(in_index, input_value, out_index); + return autoware::interpolation::lerp(in_index, input_value, out_index); }; try { diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp index a36a0b4eefccd..52cab33c048ac 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -15,10 +15,10 @@ #ifndef AUTOWARE__PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ #define AUTOWARE__PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spherical_linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spherical_linear_interpolation.hpp" #include "tf2/utils.h" #include @@ -99,22 +99,22 @@ std::pair lerpTrajectoryPoint( { const size_t i = seg_idx; - interpolated_point.pose.position.x = interpolation::lerp( + interpolated_point.pose.position.x = autoware::interpolation::lerp( points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio); - interpolated_point.pose.position.y = interpolation::lerp( + interpolated_point.pose.position.y = autoware::interpolation::lerp( points.at(i).pose.position.y, points.at(i + 1).pose.position.y, interpolate_ratio); - interpolated_point.pose.position.z = interpolation::lerp( + interpolated_point.pose.position.z = autoware::interpolation::lerp( points.at(i).pose.position.z, points.at(i + 1).pose.position.z, interpolate_ratio); - interpolated_point.pose.orientation = interpolation::lerpOrientation( + interpolated_point.pose.orientation = autoware::interpolation::lerpOrientation( points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio); - interpolated_point.longitudinal_velocity_mps = interpolation::lerp( + interpolated_point.longitudinal_velocity_mps = autoware::interpolation::lerp( points.at(i).longitudinal_velocity_mps, points.at(i + 1).longitudinal_velocity_mps, interpolate_ratio); - interpolated_point.lateral_velocity_mps = interpolation::lerp( + interpolated_point.lateral_velocity_mps = autoware::interpolation::lerp( points.at(i).lateral_velocity_mps, points.at(i + 1).lateral_velocity_mps, interpolate_ratio); - interpolated_point.acceleration_mps2 = interpolation::lerp( + interpolated_point.acceleration_mps2 = autoware::interpolation::lerp( points.at(i).acceleration_mps2, points.at(i + 1).acceleration_mps2, interpolate_ratio); - interpolated_point.heading_rate_rps = interpolation::lerp( + interpolated_point.heading_rate_rps = autoware::interpolation::lerp( points.at(i).heading_rate_rps, points.at(i + 1).heading_rate_rps, interpolate_ratio); } diff --git a/control/autoware_pid_longitudinal_controller/package.xml b/control/autoware_pid_longitudinal_controller/package.xml index 91d77e454b7ff..86dab79a74559 100644 --- a/control/autoware_pid_longitudinal_controller/package.xml +++ b/control/autoware_pid_longitudinal_controller/package.xml @@ -20,6 +20,7 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_interpolation autoware_motion_utils autoware_planning_msgs autoware_trajectory_follower_base @@ -30,7 +31,6 @@ diagnostic_updater eigen geometry_msgs - interpolation rclcpp rclcpp_components std_msgs diff --git a/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 5eb6c87e063eb..134de5d1ac8bd 100644 --- a/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -176,10 +176,11 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( const auto p0 = trajectory.points.at(i).pose; const auto p1 = trajectory.points.at(i + 1).pose; p = trajectory.points.at(i).pose; - p.position.x = interpolation::lerp(p0.position.x, p1.position.x, ratio); - p.position.y = interpolation::lerp(p0.position.y, p1.position.y, ratio); - p.position.z = interpolation::lerp(p0.position.z, p1.position.z, ratio); - p.orientation = interpolation::lerpOrientation(p0.orientation, p1.orientation, ratio); + p.position.x = autoware::interpolation::lerp(p0.position.x, p1.position.x, ratio); + p.position.y = autoware::interpolation::lerp(p0.position.y, p1.position.y, ratio); + p.position.z = autoware::interpolation::lerp(p0.position.z, p1.position.z, ratio); + p.orientation = + autoware::interpolation::lerpOrientation(p0.orientation, p1.orientation, ratio); break; } remain_dist -= dist; diff --git a/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index 3c547e762bce6..a898727ded3d1 100644 --- a/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/interpolation/spherical_linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "gtest/gtest.h" -#include "interpolation/spherical_linear_interpolation.hpp" #include "tf2/LinearMath/Quaternion.h" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -303,7 +303,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) o_to.setRPY(M_PI_4, M_PI_4, M_PI_4); ratio = 0.0; - result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = autoware::interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); @@ -311,7 +311,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); ratio = 1.0; - result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = autoware::interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, M_PI_4); @@ -320,7 +320,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) ratio = 0.5; o_to.setRPY(M_PI_4, 0.0, 0.0); - result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = autoware::interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, M_PI_4 / 2); @@ -328,7 +328,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); o_to.setRPY(0.0, M_PI_4, 0.0); - result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = autoware::interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); @@ -336,7 +336,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); o_to.setRPY(0.0, 0.0, M_PI_4); - result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = autoware::interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); diff --git a/control/autoware_trajectory_follower_base/package.xml b/control/autoware_trajectory_follower_base/package.xml index fa8f5847d6c31..548129eb06f8c 100644 --- a/control/autoware_trajectory_follower_base/package.xml +++ b/control/autoware_trajectory_follower_base/package.xml @@ -21,6 +21,7 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_interpolation autoware_motion_utils autoware_planning_msgs autoware_universe_utils @@ -30,7 +31,6 @@ diagnostic_updater eigen geometry_msgs - interpolation osqp_interface rclcpp rclcpp_components diff --git a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp index 77d6df2021c8f..984584d16aa8f 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp @@ -15,10 +15,10 @@ #ifndef PREDICTED_PATH_CHECKER__UTILS_HPP_ #define PREDICTED_PATH_CHECKER__UTILS_HPP_ +#include #include #include #include -#include #include #include diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp index 48640b21b6064..4b6d0755db25f 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp @@ -119,26 +119,26 @@ TrajectoryPoint calcInterpolatedPoint( interpolated_point.lateral_velocity_mps = curr_pt.lateral_velocity_mps; interpolated_point.acceleration_mps2 = curr_pt.acceleration_mps2; } else { - interpolated_point.longitudinal_velocity_mps = interpolation::lerp( + interpolated_point.longitudinal_velocity_mps = autoware::interpolation::lerp( curr_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, clamped_ratio); - interpolated_point.lateral_velocity_mps = interpolation::lerp( + interpolated_point.lateral_velocity_mps = autoware::interpolation::lerp( curr_pt.lateral_velocity_mps, next_pt.lateral_velocity_mps, clamped_ratio); - interpolated_point.acceleration_mps2 = - interpolation::lerp(curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); + interpolated_point.acceleration_mps2 = autoware::interpolation::lerp( + curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); } // heading rate interpolation - interpolated_point.heading_rate_rps = - interpolation::lerp(curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); + interpolated_point.heading_rate_rps = autoware::interpolation::lerp( + curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); // wheel interpolation - interpolated_point.front_wheel_angle_rad = interpolation::lerp( + interpolated_point.front_wheel_angle_rad = autoware::interpolation::lerp( curr_pt.front_wheel_angle_rad, next_pt.front_wheel_angle_rad, clamped_ratio); - interpolated_point.rear_wheel_angle_rad = - interpolation::lerp(curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); + interpolated_point.rear_wheel_angle_rad = autoware::interpolation::lerp( + curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); // time interpolation - const double interpolated_time = interpolation::lerp( + const double interpolated_time = autoware::interpolation::lerp( rclcpp::Duration(curr_pt.time_from_start).seconds(), rclcpp::Duration(next_pt.time_from_start).seconds(), clamped_ratio); interpolated_point.time_from_start = rclcpp::Duration::from_seconds(interpolated_time); diff --git a/localization/autoware_pose_covariance_modifier/package.xml b/localization/autoware_pose_covariance_modifier/package.xml index c6c5a49f991dd..dc4b741edff79 100644 --- a/localization/autoware_pose_covariance_modifier/package.xml +++ b/localization/autoware_pose_covariance_modifier/package.xml @@ -12,8 +12,8 @@ ament_cmake_auto autoware_cmake + autoware_interpolation geometry_msgs - interpolation rclcpp rclcpp_components std_msgs diff --git a/localization/autoware_pose_covariance_modifier/src/pose_covariance_modifier.cpp b/localization/autoware_pose_covariance_modifier/src/pose_covariance_modifier.cpp index bb811c55d585d..53d6ecb244f3e 100644 --- a/localization/autoware_pose_covariance_modifier/src/pose_covariance_modifier.cpp +++ b/localization/autoware_pose_covariance_modifier/src/pose_covariance_modifier.cpp @@ -14,7 +14,7 @@ #include "include/pose_covariance_modifier.hpp" -#include +#include #include #include @@ -179,7 +179,7 @@ std::array PoseCovarianceModifierNode::update_ndt_covariances_from_g const double input_normalized = (x - x_min) / (x_max - x_min); // Interpolate to the output range - return interpolation::lerp(y_min, y_max, input_normalized); + return autoware::interpolation::lerp(y_min, y_max, input_normalized); }; auto ndt_variance_from_gnss_variance = [&](double ndt_variance, double gnss_variance) { diff --git a/perception/autoware_map_based_prediction/package.xml b/perception/autoware_map_based_prediction/package.xml index 9e60073de4be6..cd0b42f030eaf 100644 --- a/perception/autoware_map_based_prediction/package.xml +++ b/perception/autoware_map_based_prediction/package.xml @@ -14,12 +14,12 @@ ament_cmake autoware_cmake + autoware_interpolation autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs autoware_universe_utils glog - interpolation rclcpp rclcpp_components tf2 diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index 6427dd9c8914c..5a43a8fb25633 100644 --- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -14,6 +14,7 @@ #include "map_based_prediction/map_based_prediction_node.hpp" +#include #include #include #include @@ -24,7 +25,6 @@ #include #include #include -#include #include diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp index d8f6f85b239a7..210795d7e6b25 100644 --- a/perception/autoware_map_based_prediction/src/path_generator.cpp +++ b/perception/autoware_map_based_prediction/src/path_generator.cpp @@ -14,9 +14,10 @@ #include "map_based_prediction/path_generator.hpp" +#include +#include #include #include -#include #include diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index a781a20388106..9999be9937111 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_interpolation autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs @@ -27,7 +28,6 @@ autoware_universe_utils autoware_vehicle_info_utils geometry_msgs - interpolation nav_msgs object_recognition_utils osqp_interface diff --git a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index 899cc66cb4331..3a24cfa7dcfe0 100644 --- a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -14,6 +14,9 @@ #include "autoware/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/zero_order_hold.hpp" #include "autoware/motion_utils/marker/marker_helper.hpp" #include "autoware/motion_utils/resample/resample.hpp" #include "autoware/motion_utils/trajectory/interpolation.hpp" @@ -21,9 +24,6 @@ #include "autoware/obstacle_cruise_planner/utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/zero_order_hold.hpp" #ifdef ROS_DISTRO_GALACTIC #include @@ -237,7 +237,7 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( } // resample optimum velocity for original each position auto resampled_opt_velocity = - interpolation::lerp(opt_position, opt_velocity, resampled_opt_position); + autoware::interpolation::lerp(opt_position, opt_velocity, resampled_opt_position); for (size_t i = break_id; i < stop_traj_points.size(); ++i) { resampled_opt_velocity.push_back(stop_traj_points.at(i).longitudinal_velocity_mps); } diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 74f2002a93420..fefe7fd06007e 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -14,12 +14,12 @@ #include "autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" #include "autoware/motion_utils/marker/marker_helper.hpp" #include "autoware/obstacle_cruise_planner/utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" #include "autoware/universe_utils/ros/update_param.hpp" -#include "interpolation/spline_interpolation.hpp" #include "tier4_planning_msgs/msg/velocity_limit.hpp" @@ -609,7 +609,7 @@ std::vector PIDBasedPlanner::getAccelerationLimitedTrajectory( if (unique_s_vec.back() < sum_dist) { return unique_v_vec.back(); } - return interpolation::spline(unique_s_vec, unique_v_vec, {sum_dist}).front(); + return autoware::interpolation::spline(unique_s_vec, unique_v_vec, {sum_dist}).front(); }(); acc_limited_traj_points.at(i).longitudinal_velocity_mps = std::clamp( diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp index 3443ab663b642..3d9192c93d5b1 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp @@ -15,6 +15,8 @@ #ifndef AUTOWARE__PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_ #define AUTOWARE__PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/state_equation_generator.hpp" #include "autoware/path_optimizer/type_alias.hpp" @@ -22,8 +24,6 @@ #include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "gtest/gtest.h" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "osqp_interface/osqp_interface.hpp" #include @@ -45,9 +45,9 @@ struct Bounds static Bounds lerp(Bounds prev_bounds, Bounds next_bounds, double ratio) { const double lower_bound = - interpolation::lerp(prev_bounds.lower_bound, next_bounds.lower_bound, ratio); + autoware::interpolation::lerp(prev_bounds.lower_bound, next_bounds.lower_bound, ratio); const double upper_bound = - interpolation::lerp(prev_bounds.upper_bound, next_bounds.upper_bound, ratio); + autoware::interpolation::lerp(prev_bounds.upper_bound, next_bounds.upper_bound, ratio); return Bounds{lower_bound, upper_bound}; } @@ -249,10 +249,10 @@ class MPTOptimizer const PlannerData & planner_data, const std::vector & smoothed_points) const; void updateCurvature( std::vector & ref_points, - const SplineInterpolationPoints2d & ref_points_spline) const; + const autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline) const; void updateOrientation( std::vector & ref_points, - const SplineInterpolationPoints2d & ref_points_spline) const; + const autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline) const; void updateBounds( std::vector & ref_points, const std::vector & left_bound, @@ -266,7 +266,7 @@ class MPTOptimizer const double ego_vel) const; void updateVehicleBounds( std::vector & ref_points, - const SplineInterpolationPoints2d & ref_points_spline) const; + const autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline) const; void updateFixedPoint(std::vector & ref_points) const; void updateDeltaArcLength(std::vector & ref_points) const; void updateExtraPoints(std::vector & ref_points) const; diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp index edc91bd40d4dc..48e7d68e70e18 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp @@ -15,13 +15,13 @@ #ifndef AUTOWARE__PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ #define AUTOWARE__PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/type_alias.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp index c9900d2ce8225..ff53cfbc3c230 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp @@ -15,12 +15,12 @@ #ifndef AUTOWARE__PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ #define AUTOWARE__PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/type_alias.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include diff --git a/planning/autoware_path_optimizer/package.xml b/planning/autoware_path_optimizer/package.xml index 0830d534c7cb0..1a7869b6a87fd 100644 --- a/planning/autoware_path_optimizer/package.xml +++ b/planning/autoware_path_optimizer/package.xml @@ -14,13 +14,13 @@ ament_cmake_auto autoware_cmake + autoware_interpolation autoware_motion_utils autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils autoware_vehicle_info_utils geometry_msgs - interpolation nav_msgs osqp_interface rclcpp diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index 367fc915badba..fdffc8a926c24 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -14,13 +14,13 @@ #include "autoware/path_optimizer/mpt_optimizer.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_optimizer/utils/geometry_utils.hpp" #include "autoware/path_optimizer/utils/trajectory_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/normalization.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "tf2/utils.h" #include @@ -568,7 +568,7 @@ std::vector MPTOptimizer::calcReferencePoints( // remove repeated points ref_points = trajectory_utils::sanitizePoints(ref_points); - SplineInterpolationPoints2d ref_points_spline(ref_points); + autoware::interpolation::SplineInterpolationPoints2d ref_points_spline(ref_points); ego_seg_idx = trajectory_utils::findEgoSegmentIndex(ref_points, p.ego_pose, ego_nearest_param_); // 3. calculate orientation and curvature @@ -580,7 +580,7 @@ std::vector MPTOptimizer::calcReferencePoints( ref_points = autoware::motion_utils::cropPoints( ref_points, p.ego_pose.position, ego_seg_idx, forward_traj_length + tmp_margin, backward_traj_length); - ref_points_spline = SplineInterpolationPoints2d(ref_points); + ref_points_spline = autoware::interpolation::SplineInterpolationPoints2d(ref_points); ego_seg_idx = trajectory_utils::findEgoSegmentIndex(ref_points, p.ego_pose, ego_nearest_param_); // 5. update fixed points, and resample @@ -588,7 +588,7 @@ std::vector MPTOptimizer::calcReferencePoints( // New start point may be added and resampled. Spline calculation is required. updateFixedPoint(ref_points); ref_points = trajectory_utils::sanitizePoints(ref_points); - ref_points_spline = SplineInterpolationPoints2d(ref_points); + ref_points_spline = autoware::interpolation::SplineInterpolationPoints2d(ref_points); // 6. update bounds // NOTE: After this, resample must not be called since bounds are not interpolated. @@ -614,7 +614,7 @@ std::vector MPTOptimizer::calcReferencePoints( void MPTOptimizer::updateOrientation( std::vector & ref_points, - const SplineInterpolationPoints2d & ref_points_spline) const + const autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline) const { const auto yaw_vec = ref_points_spline.getSplineInterpolatedYaws(); for (size_t i = 0; i < ref_points.size(); ++i) { @@ -625,7 +625,7 @@ void MPTOptimizer::updateOrientation( void MPTOptimizer::updateCurvature( std::vector & ref_points, - const SplineInterpolationPoints2d & ref_points_spline) const + const autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline) const { const auto curvature_vec = ref_points_spline.getSplineInterpolatedCurvatures(); for (size_t i = 0; i < ref_points.size(); ++i) { @@ -1092,7 +1092,7 @@ void MPTOptimizer::avoidSuddenSteering( void MPTOptimizer::updateVehicleBounds( std::vector & ref_points, - const SplineInterpolationPoints2d & ref_points_spline) const + const autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline) const { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1181,10 +1181,10 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( } // for avoidance if (0 < ref_points.at(i).normalized_avoidance_cost) { - const double lat_error_weight = interpolation::lerp( + const double lat_error_weight = autoware::interpolation::lerp( mpt_param_.lat_error_weight, mpt_param_.avoidance_lat_error_weight, ref_points.at(i).normalized_avoidance_cost); - const double yaw_error_weight = interpolation::lerp( + const double yaw_error_weight = autoware::interpolation::lerp( mpt_param_.yaw_error_weight, mpt_param_.avoidance_yaw_error_weight, ref_points.at(i).normalized_avoidance_cost); return {lat_error_weight, yaw_error_weight}; @@ -1206,7 +1206,7 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( // update R std::vector> R_triplet_vec; for (size_t i = 0; i < N_ref - 1; ++i) { - const double adaptive_steer_weight = interpolation::lerp( + const double adaptive_steer_weight = autoware::interpolation::lerp( mpt_param_.steer_input_weight, mpt_param_.avoidance_steer_input_weight, ref_points.at(i).normalized_avoidance_cost); R_triplet_vec.push_back(Eigen::Triplet(D_u * i, D_u * i, adaptive_steer_weight)); diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index f0d101a046206..62a4882f8a4c1 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -14,12 +14,12 @@ #include "autoware/path_optimizer/node.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/marker/marker_helper.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/path_optimizer/debug_marker.hpp" #include "autoware/path_optimizer/utils/geometry_utils.hpp" #include "autoware/path_optimizer/utils/trajectory_utils.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "rclcpp/time.hpp" #include diff --git a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp index bcfc7e53ee67d..f5ef2ddcd2451 100644 --- a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp @@ -181,7 +181,7 @@ std::vector resampleReferencePoints( query_keys.push_back(base_keys.back() - epsilon); } - const auto query_values = interpolation::lerp(base_keys, base_values, query_keys); + const auto query_values = autoware::interpolation::lerp(base_keys, base_values, query_keys); // create output reference points by updating curvature with resampled one std::vector output_ref_points; @@ -200,7 +200,7 @@ void insertStopPoint( const double offset_to_segment = autoware::motion_utils::calcLongitudinalOffsetToSegment( traj_points, stop_seg_idx, input_stop_pose.position); - const auto traj_spline = SplineInterpolationPoints2d(traj_points); + const auto traj_spline = autoware::interpolation::SplineInterpolationPoints2d(traj_points); const auto stop_pose = traj_spline.getSplineInterpolatedPose(stop_seg_idx, offset_to_segment); if (geometry_utils::isSamePoint(traj_points.at(stop_seg_idx), stop_pose)) { diff --git a/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp index f989ab76f6952..e7f64a896bdd3 100644 --- a/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp @@ -15,12 +15,12 @@ #ifndef AUTOWARE__PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_ #define AUTOWARE__PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/path_smoother/common_structs.hpp" #include "autoware/path_smoother/type_alias.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include diff --git a/planning/autoware_path_smoother/package.xml b/planning/autoware_path_smoother/package.xml index 6cecef433fc3e..b9b79bb6ceaf6 100644 --- a/planning/autoware_path_smoother/package.xml +++ b/planning/autoware_path_smoother/package.xml @@ -14,12 +14,12 @@ ament_cmake_auto autoware_cmake + autoware_interpolation autoware_motion_utils autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils geometry_msgs - interpolation nav_msgs osqp_interface rclcpp diff --git a/planning/autoware_path_smoother/src/elastic_band_smoother.cpp b/planning/autoware_path_smoother/src/elastic_band_smoother.cpp index b92798f92728c..a3082a2c979c3 100644 --- a/planning/autoware_path_smoother/src/elastic_band_smoother.cpp +++ b/planning/autoware_path_smoother/src/elastic_band_smoother.cpp @@ -14,10 +14,10 @@ #include "autoware/path_smoother/elastic_band_smoother.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/path_smoother/utils/geometry_utils.hpp" #include "autoware/path_smoother/utils/trajectory_utils.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "rclcpp/time.hpp" #include diff --git a/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp b/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp index e533b963cf655..37c27eaae6f93 100644 --- a/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp @@ -79,7 +79,7 @@ void insertStopPoint( const double offset_to_segment = autoware::motion_utils::calcLongitudinalOffsetToSegment( traj_points, stop_seg_idx, input_stop_pose.position); - const auto traj_spline = SplineInterpolationPoints2d(traj_points); + const auto traj_spline = autoware::interpolation::SplineInterpolationPoints2d(traj_points); const auto stop_pose = traj_spline.getSplineInterpolatedPose(stop_seg_idx, offset_to_segment); if (geometry_utils::isSamePoint(traj_points.at(stop_seg_idx), stop_pose)) { diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index 931b815b176c3..2abae4af2ca67 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -17,6 +17,7 @@ autoware_behavior_path_planner_common autoware_geography_utils + autoware_interpolation autoware_lanelet2_extension autoware_map_msgs autoware_map_projection_loader @@ -31,7 +32,6 @@ autoware_vehicle_info_utils geometry_msgs global_parameter_loader - interpolation map_loader osqp_interface rclcpp diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp index 256cd2386c431..a6ba007e71485 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp @@ -14,6 +14,7 @@ #include "static_centerline_generator_node.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/map_projection_loader/load_info_from_lanelet2_map.hpp" #include "autoware/map_projection_loader/map_projection_loader.hpp" #include "autoware/motion_utils/resample/resample.hpp" @@ -25,7 +26,6 @@ #include "autoware_lanelet2_extension/utility/utilities.hpp" #include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" #include "centerline_source/bag_ego_trajectory_based_centerline.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "map_loader/lanelet2_map_loader_node.hpp" #include "type_alias.hpp" #include "utils.hpp" @@ -625,7 +625,7 @@ void StaticCenterlineGeneratorNode::validate() } // calculate curvature - SplineInterpolationPoints2d centerline_spline(centerline); + autoware::interpolation::SplineInterpolationPoints2d centerline_spline(centerline); const auto curvature_vec = centerline_spline.getSplineInterpolatedCurvatures(); const double curvature_threshold = vehicle_info_.calcCurvatureFromSteerAngle( vehicle_info_.max_steer_angle_rad - max_steer_angle_margin); diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml index f5cd3f2667c04..65684e414e90d 100644 --- a/planning/autoware_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -21,13 +21,13 @@ autoware_cmake eigen3_cmake_module + autoware_interpolation autoware_motion_utils autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils autoware_vehicle_info_utils geometry_msgs - interpolation nav_msgs osqp_interface qp_interface diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index 6e12d4a6f20fb..48f3138cfe151 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -14,7 +14,7 @@ #include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" -#include "interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include #include @@ -234,8 +234,10 @@ bool calcStopVelocityWithConstantJerkAccLimit( } if ( - !interpolation_utils::isIncreasing(xs) || !interpolation_utils::isIncreasing(distances) || - !interpolation_utils::isNotDecreasing(xs) || !interpolation_utils::isNotDecreasing(distances)) { + !autoware::interpolation::isIncreasing(xs) || + !autoware::interpolation::isIncreasing(distances) || + !autoware::interpolation::isNotDecreasing(xs) || + !autoware::interpolation::isNotDecreasing(distances)) { return false; } @@ -245,9 +247,9 @@ bool calcStopVelocityWithConstantJerkAccLimit( return false; } - const auto vel_at_wp = interpolation::lerp(xs, vs, distances); - const auto acc_at_wp = interpolation::lerp(xs, as, distances); - const auto jerk_at_wp = interpolation::lerp(xs, js, distances); + const auto vel_at_wp = autoware::interpolation::lerp(xs, vs, distances); + const auto acc_at_wp = autoware::interpolation::lerp(xs, as, distances); + const auto jerk_at_wp = autoware::interpolation::lerp(xs, js, distances); for (size_t i = 0; i < vel_at_wp.size(); ++i) { output_trajectory.at(start_index + i).longitudinal_velocity_mps = vel_at_wp.at(i); diff --git a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp index 68f05438d38c8..cf031601cfd57 100644 --- a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp @@ -14,9 +14,9 @@ #include "autoware/velocity_smoother/trajectory_utils.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "interpolation/linear_interpolation.hpp" #include #include @@ -88,10 +88,10 @@ TrajectoryPoint calcInterpolatedTrajectoryPoint( const auto & seg_pt = trajectory.at(seg_idx); const auto & next_pt = trajectory.at(seg_idx + 1); traj_p.pose = autoware::universe_utils::calcInterpolatedPose(seg_pt.pose, next_pt.pose, prop); - traj_p.longitudinal_velocity_mps = interpolation::lerp( + traj_p.longitudinal_velocity_mps = autoware::interpolation::lerp( seg_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, prop); traj_p.acceleration_mps2 = - interpolation::lerp(seg_pt.acceleration_mps2, next_pt.acceleration_mps2, prop); + autoware::interpolation::lerp(seg_pt.acceleration_mps2, next_pt.acceleration_mps2, prop); } return traj_p; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 4ba2e1aef893c..6aba4708ddca4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -18,9 +18,9 @@ #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include +#include #include #include -#include #include @@ -74,8 +74,8 @@ struct LateralAccelerationMap return std::make_pair(base_min_acc.back(), base_max_acc.back()); } - const double min_acc = interpolation::lerp(base_vel, base_min_acc, velocity); - const double max_acc = interpolation::lerp(base_vel, base_max_acc, velocity); + const double min_acc = autoware::interpolation::lerp(base_vel, base_min_acc, velocity); + const double max_acc = autoware::interpolation::lerp(base_vel, base_max_acc, velocity); return std::make_pair(min_acc, max_acc); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml index 1e52977611759..6196a4f02cff9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml @@ -40,6 +40,7 @@ autoware_behavior_path_planner_common autoware_freespace_planning_algorithms autoware_frenet_planner + autoware_interpolation autoware_lane_departure_checker autoware_lanelet2_extension autoware_motion_utils @@ -53,7 +54,6 @@ autoware_vehicle_msgs behaviortree_cpp_v3 geometry_msgs - interpolation libboost-dev libopencv-dev magic_enum diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp index 9296fd22d374b..f341fb6ba90b2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp @@ -17,7 +17,7 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" -#include +#include #include @@ -108,7 +108,8 @@ inline Point2d normal_at_distance(const Point2d & p1, const Point2d & p2, const /// @return point interpolated between a and b as per the given ratio inline Point2d lerp_point(const Point2d & a, const Point2d & b, const double ratio) { - return {interpolation::lerp(a.x(), b.x(), ratio), interpolation::lerp(a.y(), b.y(), ratio)}; + return { + interpolation::lerp(a.x(), b.x(), ratio), autoware::interpolation::lerp(a.y(), b.y(), ratio)}; } /// @brief calculate the point with distance and arc length relative to a linestring diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml index 7a189250c088d..b1d3a39379d15 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml @@ -44,6 +44,7 @@ autoware_adapi_v1_msgs autoware_freespace_planning_algorithms + autoware_interpolation autoware_lane_departure_checker autoware_lanelet2_extension autoware_motion_utils @@ -55,7 +56,6 @@ autoware_universe_utils autoware_vehicle_info_utils geometry_msgs - interpolation magic_enum object_recognition_utils rclcpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index dcab7c22a35fe..648bb0a17622c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -20,13 +20,13 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include #include #include #include #include #include #include -#include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index dc0f13270bca2..e2218e37a771b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -12,18 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" - -#include "autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" -#include "autoware/behavior_path_planner_common/utils/utils.hpp" -#include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" - +#include +#include +#include +#include +#include +#include +#include +#include #include #include -#include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index fb1da48e41875..e2ff8ce7b5195 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -15,10 +15,10 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/ros/uuid_helper.hpp" -#include "interpolation/linear_interpolation.hpp" #include #include @@ -364,7 +364,7 @@ std::optional calcInterpolatedPoseWithVelocity( const auto interpolated_pose = autoware::universe_utils::calcInterpolatedPose(prev_pt.pose, pt.pose, ratio, false); const double interpolated_velocity = - interpolation::lerp(prev_pt.velocity, pt.velocity, ratio); + autoware::interpolation::lerp(prev_pt.velocity, pt.velocity, ratio); return PoseWithVelocityStamped{relative_time, interpolated_pose, interpolated_velocity}; } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp index 8a6c899b4bd6d..69c538fa6cb54 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp @@ -16,9 +16,9 @@ #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include #include #include -#include #include #include @@ -252,7 +252,7 @@ void PathShifter::applySplineShifter(ShiftedPath * shifted_path, const bool offs query_distance.push_back(dist_from_start); } if (!query_distance.empty()) { - query_length = interpolation::spline(base_distance, base_length, query_distance); + query_length = autoware::interpolation::spline(base_distance, base_length, query_distance); } // Apply shifting. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp index 5b234198d9137..3b06e084690e3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp @@ -17,12 +17,12 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include #include #include #include #include #include -#include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml index d868025475444..5f9f4bcd12b75 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml @@ -16,6 +16,7 @@ autoware_behavior_path_planner_common autoware_bezier_sampler autoware_frenet_planner + autoware_interpolation autoware_lanelet2_extension autoware_motion_utils autoware_path_sampler @@ -28,7 +29,6 @@ autoware_vehicle_info_utils autoware_vehicle_msgs geometry_msgs - interpolation pluginlib rclcpp rclcpp_components diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp index 8fdedf38ed756..f05045f284413 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -956,7 +956,7 @@ autoware::frenet_planner::SamplingParameters SamplingPlannerModule::prepareSampl max_d -= params_.constraints.ego_width / 2.0; if (min_d < max_d) { for (auto r = 0.0; r <= 1.0; r += 1.0 / (params_.sampling.nb_target_lateral_positions - 1)) - target_lateral_positions.push_back(interpolation::lerp(min_d, max_d, r)); + target_lateral_positions.push_back(autoware::interpolation::lerp(min_d, max_d, r)); } } else { target_lateral_positions = params_.sampling.target_lateral_positions; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml index cd9c63fc0367f..be04f1f9180b9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml @@ -23,6 +23,7 @@ autoware_behavior_velocity_planner_common autoware_grid_map_utils + autoware_interpolation autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs @@ -34,7 +35,6 @@ geometry_msgs grid_map_core grid_map_ros - interpolation libboost-dev nav_msgs pcl_conversions diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml index 93e1976a251ba..1c5d23e31b2e1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml @@ -20,6 +20,7 @@ autoware_cmake autoware_behavior_velocity_planner_common + autoware_interpolation autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs @@ -31,7 +32,6 @@ autoware_vehicle_info_utils fmt geometry_msgs - interpolation libopencv-dev magic_enum nav_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index ae1d9768eaecc..b694c8aaa2e3e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -17,11 +17,11 @@ #include // for to_bg2d #include // for planning_utils:: +#include #include #include // for lanelet::autoware::RoadMarking #include #include -#include #include #include @@ -146,7 +146,7 @@ double getHighestCurvature(const lanelet::ConstLineString3d & centerline) points.push_back(*point); } - SplineInterpolationPoints2d interpolation(points); + autoware::interpolation::SplineInterpolationPoints2d interpolation(points); const std::vector curvatures = interpolation.getSplineInterpolatedCurvatures(); std::vector curvatures_positive; for (const auto & curvature : curvatures) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml index b630f988af662..21664f7596d60 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml @@ -18,6 +18,7 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common + autoware_interpolation autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs @@ -27,7 +28,6 @@ autoware_vehicle_info_utils eigen geometry_msgs - interpolation libboost-dev pluginlib rclcpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index f2bcc0b5abf44..ac58c62036e26 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -17,10 +17,10 @@ #include #include #include +#include #include #include #include -#include #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml index 22da467ba9fac..b414769e84ec2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml @@ -18,6 +18,7 @@ autoware_behavior_velocity_planner_common autoware_grid_map_utils + autoware_interpolation autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs @@ -27,7 +28,6 @@ autoware_vehicle_info_utils geometry_msgs grid_map_ros - interpolation libboost-dev libopencv-dev nav_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index 6d40c2167961a..56a5acd8c7dc4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -18,10 +18,10 @@ #include #include +#include #include #include #include -#include #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml index 0122c220e7e51..9e5e8b0edda7b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -20,6 +20,7 @@ eigen3_cmake_module autoware_adapi_v1_msgs + autoware_interpolation autoware_lanelet2_extension autoware_map_msgs autoware_motion_utils @@ -34,7 +35,6 @@ diagnostic_msgs eigen geometry_msgs - interpolation nav_msgs pcl_conversions rclcpp diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp index a27dd1576c90d..65ee862592bef 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp @@ -15,14 +15,14 @@ #ifndef AUTOWARE_PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ #define AUTOWARE_PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware_path_sampler/common_structs.hpp" #include "autoware_path_sampler/type_alias.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "eigen3/Eigen/Core" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp index 992ad95c72bb1..73a2b0c95493e 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp @@ -15,14 +15,14 @@ #ifndef AUTOWARE_PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ #define AUTOWARE_PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware_path_sampler/common_structs.hpp" #include "autoware_path_sampler/type_alias.hpp" #include "autoware_sampler_common/structures.hpp" #include "eigen3/Eigen/Core" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" diff --git a/planning/sampling_based_planner/autoware_path_sampler/package.xml b/planning/sampling_based_planner/autoware_path_sampler/package.xml index 26f85631d569f..ca137a0d06a53 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/package.xml +++ b/planning/sampling_based_planner/autoware_path_sampler/package.xml @@ -15,13 +15,13 @@ autoware_bezier_sampler autoware_frenet_planner + autoware_interpolation autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_universe_utils autoware_vehicle_info_utils geometry_msgs - interpolation nav_msgs rclcpp rclcpp_components diff --git a/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp index 15d7e883a31d2..34905f79cd364 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp @@ -14,6 +14,7 @@ #include "autoware_path_sampler/node.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" #include "autoware/motion_utils/marker/marker_helper.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware_path_sampler/path_generation.hpp" @@ -22,7 +23,6 @@ #include "autoware_path_sampler/utils/trajectory_utils.hpp" #include "autoware_sampler_common/constraints/hard_constraint.hpp" #include "autoware_sampler_common/constraints/soft_constraint.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "rclcpp/time.hpp" #include diff --git a/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp index 06e8ab9042207..127393abc5ac3 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp @@ -82,7 +82,7 @@ autoware::frenet_planner::SamplingParameters prepareSamplingParameters( max_d -= params.constraints.ego_width / 2.0; if (min_d < max_d) { for (auto r = 0.0; r <= 1.0; r += 1.0 / (params.sampling.nb_target_lateral_positions - 1)) - target_lateral_positions.push_back(interpolation::lerp(min_d, max_d, r)); + target_lateral_positions.push_back(autoware::interpolation::lerp(min_d, max_d, r)); } } else { target_lateral_positions = params.sampling.target_lateral_positions; diff --git a/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp index e8379091615f8..e6317cd21f17d 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp @@ -65,7 +65,7 @@ void insertStopPoint( const double offset_to_segment = autoware::motion_utils::calcLongitudinalOffsetToSegment( traj_points, stop_seg_idx, input_stop_pose.position); - const auto traj_spline = SplineInterpolationPoints2d(traj_points); + const auto traj_spline = autoware::interpolation::SplineInterpolationPoints2d(traj_points); const auto stop_pose = traj_spline.getSplineInterpolatedPose(stop_seg_idx, offset_to_segment); if (geometry_utils::isSamePoint(traj_points.at(stop_seg_idx), stop_pose)) { diff --git a/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp index 29b3f5a8fc145..c2cf432229b1b 100644 --- a/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp @@ -17,8 +17,8 @@ #include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include #include -#include #include @@ -269,7 +269,7 @@ struct Trajectory : Path t.lengths.reserve(new_size); for (auto i = 0lu; i < new_size; ++i) t.lengths.push_back(lengths.front() + static_cast(i) * fixed_interval); - t.times = interpolation::lerp(lengths, times, t.lengths); + t.times = autoware::interpolation::lerp(lengths, times, t.lengths); std::vector xs; std::vector ys; xs.reserve(points.size()); @@ -278,16 +278,18 @@ struct Trajectory : Path xs.push_back(p.x()); ys.push_back(p.y()); } - const auto new_xs = interpolation::lerp(times, xs, t.times); - const auto new_ys = interpolation::lerp(times, ys, t.times); + const auto new_xs = autoware::interpolation::lerp(times, xs, t.times); + const auto new_ys = autoware::interpolation::lerp(times, ys, t.times); for (auto i = 0lu; i < new_xs.size(); ++i) t.points.emplace_back(new_xs[i], new_ys[i]); - t.curvatures = interpolation::lerp(times, curvatures, t.times); - t.jerks = interpolation::lerp(times, jerks, t.times); - t.yaws = interpolation::lerp(times, yaws, t.times); - t.longitudinal_velocities = interpolation::lerp(times, longitudinal_velocities, t.times); - t.longitudinal_accelerations = interpolation::lerp(times, longitudinal_accelerations, t.times); - t.lateral_velocities = interpolation::lerp(times, lateral_velocities, t.times); - t.lateral_accelerations = interpolation::lerp(times, lateral_accelerations, t.times); + t.curvatures = autoware::interpolation::lerp(times, curvatures, t.times); + t.jerks = autoware::interpolation::lerp(times, jerks, t.times); + t.yaws = autoware::interpolation::lerp(times, yaws, t.times); + t.longitudinal_velocities = + autoware::interpolation::lerp(times, longitudinal_velocities, t.times); + t.longitudinal_accelerations = + autoware::interpolation::lerp(times, longitudinal_accelerations, t.times); + t.lateral_velocities = autoware::interpolation::lerp(times, lateral_velocities, t.times); + t.lateral_accelerations = autoware::interpolation::lerp(times, lateral_accelerations, t.times); t.constraint_results = constraint_results; t.cost = cost; return t; @@ -305,7 +307,7 @@ struct Trajectory : Path t.lengths.reserve(new_size); for (auto i = 0lu; i < new_size; ++i) t.times.push_back(static_cast(i) * fixed_interval); - t.lengths = interpolation::lerp(times, lengths, t.times); + t.lengths = autoware::interpolation::lerp(times, lengths, t.times); std::vector xs; std::vector ys; xs.reserve(points.size()); @@ -314,16 +316,18 @@ struct Trajectory : Path xs.push_back(p.x()); ys.push_back(p.y()); } - const auto new_xs = interpolation::lerp(times, xs, t.times); - const auto new_ys = interpolation::lerp(times, ys, t.times); + const auto new_xs = autoware::interpolation::lerp(times, xs, t.times); + const auto new_ys = autoware::interpolation::lerp(times, ys, t.times); for (auto i = 0lu; i < new_xs.size(); ++i) t.points.emplace_back(new_xs[i], new_ys[i]); - t.curvatures = interpolation::lerp(times, curvatures, t.times); - t.jerks = interpolation::lerp(times, jerks, t.times); - t.yaws = interpolation::lerp(times, yaws, t.times); - t.longitudinal_velocities = interpolation::lerp(times, longitudinal_velocities, t.times); - t.longitudinal_accelerations = interpolation::lerp(times, longitudinal_accelerations, t.times); - t.lateral_velocities = interpolation::lerp(times, lateral_velocities, t.times); - t.lateral_accelerations = interpolation::lerp(times, lateral_accelerations, t.times); + t.curvatures = autoware::interpolation::lerp(times, curvatures, t.times); + t.jerks = autoware::interpolation::lerp(times, jerks, t.times); + t.yaws = autoware::interpolation::lerp(times, yaws, t.times); + t.longitudinal_velocities = + autoware::interpolation::lerp(times, longitudinal_velocities, t.times); + t.longitudinal_accelerations = + autoware::interpolation::lerp(times, longitudinal_accelerations, t.times); + t.lateral_velocities = autoware::interpolation::lerp(times, lateral_velocities, t.times); + t.lateral_accelerations = autoware::interpolation::lerp(times, lateral_accelerations, t.times); t.constraint_results = constraint_results; t.cost = cost; return t; diff --git a/planning/sampling_based_planner/autoware_sampler_common/package.xml b/planning/sampling_based_planner/autoware_sampler_common/package.xml index a1462131d62ad..d1f1e3946a0dd 100644 --- a/planning/sampling_based_planner/autoware_sampler_common/package.xml +++ b/planning/sampling_based_planner/autoware_sampler_common/package.xml @@ -11,7 +11,7 @@ autoware_cmake - interpolation + autoware_interpolation ament_cmake_ros ament_lint_auto diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp index 5615f5c118165..b78ec8ccd538a 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp @@ -15,9 +15,9 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_ACTUATION_CMD_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_ACTUATION_CMD_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" -#include "interpolation/linear_interpolation.hpp" #include "simple_planning_simulator/utils/csv_loader.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp index ea0653c879727..454a35d95eb8b 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp @@ -15,9 +15,9 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_MAP_ACC_GEARED_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_MAP_ACC_GEARED_HPP_ +#include "autoware/interpolation/linear_interpolation.hpp" #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" -#include "interpolation/linear_interpolation.hpp" #include "simple_planning_simulator/utils/csv_loader.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" @@ -57,13 +57,14 @@ class AccelerationMap // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (const auto & acc_vec : acceleration_map_) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, acc_vec, clamped_vel)); + interpolated_acc_vec.push_back( + autoware::interpolation::lerp(vel_index_, acc_vec, clamped_vel)); } // calculate throttle // When the desired acceleration is smaller than the throttle area, return min acc // When the desired acceleration is greater than the throttle area, return max acc const double clamped_acc = CSVLoader::clampValue(acc_des, acc_index_); - const double acc = interpolation::lerp(acc_index_, interpolated_acc_vec, clamped_acc); + const double acc = autoware::interpolation::lerp(acc_index_, interpolated_acc_vec, clamped_acc); return acc; } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp index 19794c04750fd..069cb9e569580 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp @@ -43,11 +43,12 @@ double ActuationMap::getControlCommand(const double actuation, const double stat for (std::vector control_command_values : actuation_map_) { interpolated_control_vec.push_back( - interpolation::lerp(state_index_, control_command_values, clamped_state)); + autoware::interpolation::lerp(state_index_, control_command_values, clamped_state)); } const double clamped_actuation = CSVLoader::clampValue(actuation, actuation_index_); - return interpolation::lerp(actuation_index_, interpolated_control_vec, clamped_actuation); + return autoware::interpolation::lerp( + actuation_index_, interpolated_control_vec, clamped_actuation); } std::optional AccelMap::getThrottle(const double acc, double vel) const @@ -61,7 +62,8 @@ std::optional AccelMap::getThrottle(const double acc, double vel) const // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (std::vector accelerations : accel_map) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_indices, accelerations, clamped_vel)); + interpolated_acc_vec.push_back( + autoware::interpolation::lerp(vel_indices, accelerations, clamped_vel)); } // calculate throttle @@ -73,7 +75,7 @@ std::optional AccelMap::getThrottle(const double acc, double vel) const return throttle_indices.back(); } - return interpolation::lerp(interpolated_acc_vec, throttle_indices, acc); + return autoware::interpolation::lerp(interpolated_acc_vec, throttle_indices, acc); } double BrakeMap::getBrake(const double acc, double vel) const @@ -87,7 +89,8 @@ double BrakeMap::getBrake(const double acc, double vel) const // (brake, vel, acc) map => (brake, acc) map by fixing vel for (std::vector accelerations : brake_map) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_indices, accelerations, clamped_vel)); + interpolated_acc_vec.push_back( + autoware::interpolation::lerp(vel_indices, accelerations, clamped_vel)); } // calculate brake @@ -100,7 +103,7 @@ double BrakeMap::getBrake(const double acc, double vel) const } std::reverse(std::begin(interpolated_acc_vec), std::end(interpolated_acc_vec)); - return interpolation::lerp(interpolated_acc_vec, brake_indices, acc); + return autoware::interpolation::lerp(interpolated_acc_vec, brake_indices, acc); } // steer map sim model diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/package.xml b/vehicle/autoware_raw_vehicle_cmd_converter/package.xml index 4ed4fbee7ac81..86a76c397c728 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/package.xml +++ b/vehicle/autoware_raw_vehicle_cmd_converter/package.xml @@ -22,9 +22,9 @@ autoware_cmake autoware_control_msgs + autoware_interpolation autoware_vehicle_msgs geometry_msgs - interpolation nav_msgs rclcpp rclcpp_components diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp index 236aa7dc451c3..7365c6380900f 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp @@ -14,7 +14,7 @@ #include "autoware_raw_vehicle_cmd_converter/accel_map.hpp" -#include "interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include #include @@ -50,7 +50,8 @@ bool AccelMap::getThrottle(const double acc, double vel, double & throttle) cons const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (std::vector accelerations : accel_map_) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, accelerations, clamped_vel)); + interpolated_acc_vec.push_back( + autoware::interpolation::lerp(vel_index_, accelerations, clamped_vel)); } // calculate throttle // When the desired acceleration is smaller than the throttle area, return false => brake sequence @@ -61,7 +62,7 @@ bool AccelMap::getThrottle(const double acc, double vel, double & throttle) cons throttle = throttle_index_.back(); return true; } - throttle = interpolation::lerp(interpolated_acc_vec, throttle_index_, acc); + throttle = autoware::interpolation::lerp(interpolated_acc_vec, throttle_index_, acc); return true; } @@ -72,14 +73,14 @@ bool AccelMap::getAcceleration(const double throttle, const double vel, double & // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (const auto & acc_vec : accel_map_) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, acc_vec, clamped_vel)); + interpolated_acc_vec.push_back(autoware::interpolation::lerp(vel_index_, acc_vec, clamped_vel)); } // calculate throttle // When the desired acceleration is smaller than the throttle area, return min acc // When the desired acceleration is greater than the throttle area, return max acc const double clamped_throttle = CSVLoader::clampValue(throttle, throttle_index_, "throttle: acc"); - acc = interpolation::lerp(throttle_index_, interpolated_acc_vec, clamped_throttle); + acc = autoware::interpolation::lerp(throttle_index_, interpolated_acc_vec, clamped_throttle); return true; } diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp index ae0f8e5f41b1c..48660eccf8640 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp @@ -14,7 +14,7 @@ #include "autoware_raw_vehicle_cmd_converter/brake_map.hpp" -#include "interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include #include @@ -51,7 +51,8 @@ bool BrakeMap::getBrake(const double acc, const double vel, double & brake) // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (std::vector accelerations : brake_map_) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, accelerations, clamped_vel)); + interpolated_acc_vec.push_back( + autoware::interpolation::lerp(vel_index_, accelerations, clamped_vel)); } // calculate brake @@ -71,7 +72,7 @@ bool BrakeMap::getBrake(const double acc, const double vel, double & brake) } std::reverse(std::begin(interpolated_acc_vec), std::end(interpolated_acc_vec)); - brake = interpolation::lerp(interpolated_acc_vec, brake_index_rev_, acc); + brake = autoware::interpolation::lerp(interpolated_acc_vec, brake_index_rev_, acc); return true; } @@ -83,14 +84,14 @@ bool BrakeMap::getAcceleration(const double brake, const double vel, double & ac // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (const auto & acc_vec : brake_map_) { - interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, acc_vec, clamped_vel)); + interpolated_acc_vec.push_back(autoware::interpolation::lerp(vel_index_, acc_vec, clamped_vel)); } // calculate brake // When the desired acceleration is smaller than the brake area, return min acc // When the desired acceleration is greater than the brake area, return min acc const double clamped_brake = CSVLoader::clampValue(brake, brake_index_, "brake: acc"); - acc = interpolation::lerp(brake_index_, interpolated_acc_vec, clamped_brake); + acc = autoware::interpolation::lerp(brake_index_, interpolated_acc_vec, clamped_brake); return true; } diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp index 6abe8adfdc9e3..babefe96eb91d 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp @@ -14,7 +14,7 @@ #include "autoware_raw_vehicle_cmd_converter/steer_map.hpp" -#include "interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/linear_interpolation.hpp" #include #include @@ -46,11 +46,12 @@ void SteerMap::getSteer(const double steer_rate, const double steer, double & ou const double clamped_steer = CSVLoader::clampValue(steer, steer_index_, "steer: steer"); std::vector steer_rate_interp = {}; for (const auto & steer_rate_vec : steer_map_) { - steer_rate_interp.push_back(interpolation::lerp(steer_index_, steer_rate_vec, clamped_steer)); + steer_rate_interp.push_back( + autoware::interpolation::lerp(steer_index_, steer_rate_vec, clamped_steer)); } const double clamped_steer_rate = CSVLoader::clampValue(steer_rate, steer_rate_interp, "steer: steer_rate"); - output = interpolation::lerp(steer_rate_interp, output_index_, clamped_steer_rate); + output = autoware::interpolation::lerp(steer_rate_interp, output_index_, clamped_steer_rate); } } // namespace autoware::raw_vehicle_cmd_converter From d8dc2583d55435fc55b1e273710b1e66604dd00b Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Fri, 20 Sep 2024 22:11:54 +0900 Subject: [PATCH 27/95] chore(simple_planning_simulator): remove unnecessary lines (#8932) remove unnecessary semicolons Signed-off-by: Autumn60 --- .../vehicle_model/sim_model_delay_steer_acc_geared.cpp | 1 - .../sim_model_delay_steer_acc_geared_wo_fall_guard.cpp | 1 - 2 files changed, 2 deletions(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index aada6687baef8..88b0e6c639fd1 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -69,7 +69,6 @@ double SimModelDelaySteerAccGeared::getAx() double SimModelDelaySteerAccGeared::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; - ; } double SimModelDelaySteerAccGeared::getSteer() { diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp index 6b993b8b24409..78cfa6caeb946 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp @@ -69,7 +69,6 @@ double SimModelDelaySteerAccGearedWoFallGuard::getAx() double SimModelDelaySteerAccGearedWoFallGuard::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; - ; } double SimModelDelaySteerAccGearedWoFallGuard::getSteer() { From 1cf58eb72b037ca95c3f3bf4b5e41eced8648e3a Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 20 Sep 2024 23:58:08 +0900 Subject: [PATCH 28/95] feat(autonomous_emergency_braking): make hull markers 3d (#8930) make hull markers 3d Signed-off-by: Daniel Sanchez --- .../autonomous_emergency_braking/node.hpp | 3 ++- .../autonomous_emergency_braking/utils.hpp | 9 +++++++++ .../src/node.cpp | 18 ++++++++++++++---- .../src/utils.cpp | 17 +++++++++++++++++ 4 files changed, 42 insertions(+), 5 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index 9addc059ad0af..bc45e6049abd7 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -65,6 +65,7 @@ using sensor_msgs::msg::Imu; using sensor_msgs::msg::PointCloud2; using PointCloud = pcl::PointCloud; using autoware::universe_utils::Polygon2d; +using autoware::universe_utils::Polygon3d; using autoware::vehicle_info_utils::VehicleInfo; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; @@ -496,7 +497,7 @@ class AEB : public rclcpp::Node * @param debug_markers Marker array for debugging */ void addClusterHullMarkers( - const rclcpp::Time & current_time, const std::vector & hulls, + const rclcpp::Time & current_time, const std::vector & hulls, const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers); /** diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp index 5a2dacad2dfc9..18f3716b755ee 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp @@ -42,6 +42,7 @@ namespace autoware::motion::control::autonomous_emergency_braking::utils { using autoware::universe_utils::Polygon2d; +using autoware::universe_utils::Polygon3d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Point; @@ -105,6 +106,14 @@ std::optional getTransform( */ void fillMarkerFromPolygon( const std::vector & polygons, visualization_msgs::msg::Marker & polygon_marker); + +/** + * @brief Get the predicted object's shape as a geometry polygon + * @param polygons vector of Polygon3d + * @param polygon_marker marker to be filled with polygon points + */ +void fillMarkerFromPolygon( + const std::vector & polygons, visualization_msgs::msg::Marker & polygon_marker); } // namespace autoware::motion::control::autonomous_emergency_braking::utils #endif // AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_ diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 1f8fd6f4137a2..48d9ea4c1b458 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -59,6 +59,7 @@ namespace autoware::motion::control::autonomous_emergency_braking { using autoware::motion::control::autonomous_emergency_braking::utils::convertObjToPolygon; using autoware::universe_utils::Point2d; +using autoware::universe_utils::Point3d; using diagnostic_msgs::msg::DiagnosticStatus; namespace bg = boost::geometry; @@ -71,6 +72,16 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & bg::append(polygon.outer(), point); } +void appendPointToPolygon(Polygon3d & polygon, const geometry_msgs::msg::Point & geom_point) +{ + Point3d point; + point.x() = geom_point.x; + point.y() = geom_point.y; + point.z() = geom_point.z; + + bg::append(polygon.outer(), point); +} + Polygon2d createPolygon( const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & next_pose, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double expand_width) @@ -580,7 +591,6 @@ bool AEB::hasCollision(const double current_v, const ObjectData & closest_object // collision happens ObjectData collision_data = closest_object; collision_data.rss = rss_dist; - collision_data.distance_to_object = closest_object.distance_to_object; collision_data_keeper_.setCollisionData(collision_data); return true; } @@ -787,7 +797,7 @@ void AEB::getPointsBelongingToClusterHulls( ec.extract(cluster_idx); return cluster_idx; }); - std::vector hull_polygons; + std::vector hull_polygons; for (const auto & indices : cluster_indices) { PointCloud::Ptr cluster(new PointCloud); bool cluster_surpasses_threshold_height{false}; @@ -806,7 +816,7 @@ void AEB::getPointsBelongingToClusterHulls( std::vector polygons; PointCloud::Ptr surface_hull(new PointCloud); hull.reconstruct(*surface_hull, polygons); - Polygon2d hull_polygon; + Polygon3d hull_polygon; for (const auto & p : *surface_hull) { points_belonging_to_cluster_hulls->push_back(p); if (publish_debug_markers_) { @@ -900,7 +910,7 @@ void AEB::cropPointCloudWithEgoFootprintPath( } void AEB::addClusterHullMarkers( - const rclcpp::Time & current_time, const std::vector & hulls, + const rclcpp::Time & current_time, const std::vector & hulls, const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); diff --git a/control/autoware_autonomous_emergency_braking/src/utils.cpp b/control/autoware_autonomous_emergency_braking/src/utils.cpp index 9c8bbe388fdc5..5d9782ada5fbd 100644 --- a/control/autoware_autonomous_emergency_braking/src/utils.cpp +++ b/control/autoware_autonomous_emergency_braking/src/utils.cpp @@ -186,4 +186,21 @@ void fillMarkerFromPolygon( } } +void fillMarkerFromPolygon( + const std::vector & polygons, visualization_msgs::msg::Marker & polygon_marker) +{ + for (const auto & poly : polygons) { + for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) { + const auto & boost_cp = poly.outer().at(dp_idx); + const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); + const auto curr_point = + autoware::universe_utils::createPoint(boost_cp.x(), boost_cp.y(), boost_cp.z()); + const auto next_point = + autoware::universe_utils::createPoint(boost_np.x(), boost_np.y(), boost_np.z()); + polygon_marker.points.push_back(curr_point); + polygon_marker.points.push_back(next_point); + } + } +} + } // namespace autoware::motion::control::autonomous_emergency_braking::utils From 9f7ba86258dd33f80b03ba6bf297a2fc4e9eb042 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?= Date: Mon, 23 Sep 2024 11:50:54 +0300 Subject: [PATCH 29/95] ci(build-and-test): reset ccache stats before build (#8761) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: M. Fatih Cırıt --- .github/actions/build-and-test-differential/action.yaml | 6 ++++-- .github/workflows/build-and-test.yaml | 6 ++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/.github/actions/build-and-test-differential/action.yaml b/.github/actions/build-and-test-differential/action.yaml index 3decc3f9861b1..89893e9f55fb5 100644 --- a/.github/actions/build-and-test-differential/action.yaml +++ b/.github/actions/build-and-test-differential/action.yaml @@ -57,8 +57,10 @@ runs: restore-keys: | ccache-main-${{ runner.arch }}-${{ inputs.rosdistro }}- - - name: Show ccache stats before build - run: du -sh ${CCACHE_DIR} && ccache -s + - name: Show ccache stats before build and reset stats + run: | + du -sh ${CCACHE_DIR} && ccache -s + ccache --zero-stats shell: bash - name: Export CUDA state as a variable for adding to cache key diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index df49e5d418bc9..0f239cd328591 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -69,8 +69,10 @@ jobs: echo -e "# Set maximum cache size\nmax_size = 600MB" >> "${CCACHE_DIR}/ccache.conf" shell: bash - - name: Show ccache stats before build - run: du -sh ${CCACHE_DIR} && ccache -s + - name: Show ccache stats before build and reset stats + run: | + du -sh ${CCACHE_DIR} && ccache -s + ccache --zero-stats shell: bash - name: Export CUDA state as a variable for adding to cache key From 2965c45ac0330e3c147c5e4f548b57adffdd1610 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Tue, 24 Sep 2024 10:14:11 +0900 Subject: [PATCH 30/95] fix(autoware_compare_map_segmentation): typo bug fix (#8939) fix(compare_map_filter): typo bug fix Signed-off-by: badai-nguyen --- .../src/voxel_based_approximate_compare_map_filter/node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp index 8bc22f8c31184..3a04c3dc20902 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp @@ -86,7 +86,7 @@ VoxelBasedApproximateCompareMapFilterComponent::VoxelBasedApproximateCompareMapF distance_threshold_ = declare_parameter("distance_threshold"); bool use_dynamic_map_loading = declare_parameter("use_dynamic_map_loading"); - double downsize_ratio_z_axis = declare_parameter("downsize_ratio_z_axis"); + double downsize_ratio_z_axis = declare_parameter("downsize_ratio_z_axis"); if (downsize_ratio_z_axis <= 0.0) { RCLCPP_ERROR(this->get_logger(), "downsize_ratio_z_axis should be positive"); return; From 85733485a113b95680238680e08442424246bd34 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 24 Sep 2024 10:47:08 +0900 Subject: [PATCH 31/95] fix(autoware_image_projection_based_fusion): roi cluster fusion has no existence probability update (#8864) fix: add existence probability update, refactoring Signed-off-by: Taekjin LEE --- .../src/roi_cluster_fusion/node.cpp | 40 ++++++------------- 1 file changed, 12 insertions(+), 28 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 060b7f292c403..0a69959aecf5c 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -166,7 +166,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( int index = -1; bool associated = false; double max_iou = 0.0; - bool is_roi_label_known = + const bool is_roi_label_known = feature_obj.object.classification.front().label != ObjectClassification::UNKNOWN; for (const auto & cluster_map : m_cluster_roi) { double iou(0.0); @@ -196,34 +196,18 @@ void RoiClusterFusionNode::fuseOnSingleImage( } if (!output_cluster_msg.feature_objects.empty()) { - bool is_roi_existence_prob_higher = - output_cluster_msg.feature_objects.at(index).object.existence_probability <= - feature_obj.object.existence_probability; - if (iou_threshold_ < max_iou && is_roi_existence_prob_higher && is_roi_label_known) { - output_cluster_msg.feature_objects.at(index).object.classification = - feature_obj.object.classification; - - // Update existence_probability for fused objects - if ( - output_cluster_msg.feature_objects.at(index).object.existence_probability < - min_roi_existence_prob_) { - output_cluster_msg.feature_objects.at(index).object.existence_probability = - min_roi_existence_prob_; - } - } - - // fuse with unknown roi - - if (unknown_iou_threshold_ < max_iou && is_roi_existence_prob_higher && !is_roi_label_known) { - output_cluster_msg.feature_objects.at(index).object.classification = - feature_obj.object.classification; + auto & fused_object = output_cluster_msg.feature_objects.at(index).object; + const bool is_roi_existence_prob_higher = + fused_object.existence_probability <= feature_obj.object.existence_probability; + const bool is_roi_iou_over_threshold = + (is_roi_label_known && iou_threshold_ < max_iou) || + (!is_roi_label_known && unknown_iou_threshold_ < max_iou); + + if (is_roi_iou_over_threshold && is_roi_existence_prob_higher) { + fused_object.classification = feature_obj.object.classification; // Update existence_probability for fused objects - if ( - output_cluster_msg.feature_objects.at(index).object.existence_probability < - min_roi_existence_prob_) { - output_cluster_msg.feature_objects.at(index).object.existence_probability = - min_roi_existence_prob_; - } + fused_object.existence_probability = + std::clamp(feature_obj.object.existence_probability, min_roi_existence_prob_, 1.0f); } } if (debugger_) debugger_->image_rois_.push_back(feature_obj.feature.roi); From 81e7aba5f1e334fd6ce7259f69f3dd67b8de1afe Mon Sep 17 00:00:00 2001 From: TetsuKawa <70682030+TetsuKawa@users.noreply.github.com> Date: Tue, 24 Sep 2024 10:54:14 +0900 Subject: [PATCH 32/95] feat(system_error_monitor): remove system error monitor (#8929) * feat: delete-system-error-monitor-from-autoware Signed-off-by: TetsuKawa * feat: remove unnecessary params --------- Signed-off-by: TetsuKawa --- .github/CODEOWNERS | 1 - .../src/mrm_summary_overlay_display.cpp | 2 +- launch/tier4_system_launch/README.md | 2 - .../launch/system.launch.xml | 4 - launch/tier4_system_launch/package.xml | 1 - system/system_error_monitor/CMakeLists.txt | 18 - system/system_error_monitor/README.md | 145 ---- .../diagnostic_aggregator/_empty.param.yaml | 0 .../diagnostic_aggregator/control.param.yaml | 121 --- .../localization.param.yaml | 47 -- .../diagnostic_aggregator/map.param.yaml | 22 - .../perception.param.yaml | 15 - .../diagnostic_aggregator/planning.param.yaml | 95 --- .../diagnostic_aggregator/sensing.param.yaml | 2 - .../diagnostic_aggregator/system.param.yaml | 249 ------ .../diagnostic_aggregator/vehicle.param.yaml | 16 - .../config/system_error_monitor.param.yaml | 55 -- ...ror_monitor.planning_simulation.param.yaml | 56 -- .../diagnostics_filter.hpp | 103 --- .../system_error_monitor_core.hpp | 153 ---- .../launch/system_error_monitor.launch.xml | 55 -- .../system_error_monitor_node.launch.xml | 34 - system/system_error_monitor/package.xml | 32 - .../src/system_error_monitor_core.cpp | 722 ------------------ 24 files changed, 1 insertion(+), 1949 deletions(-) delete mode 100644 system/system_error_monitor/CMakeLists.txt delete mode 100644 system/system_error_monitor/README.md delete mode 100644 system/system_error_monitor/config/diagnostic_aggregator/_empty.param.yaml delete mode 100644 system/system_error_monitor/config/diagnostic_aggregator/control.param.yaml delete mode 100644 system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml delete mode 100644 system/system_error_monitor/config/diagnostic_aggregator/map.param.yaml delete mode 100644 system/system_error_monitor/config/diagnostic_aggregator/perception.param.yaml delete mode 100644 system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml delete mode 100644 system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml delete mode 100644 system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml delete mode 100644 system/system_error_monitor/config/diagnostic_aggregator/vehicle.param.yaml delete mode 100644 system/system_error_monitor/config/system_error_monitor.param.yaml delete mode 100644 system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml delete mode 100644 system/system_error_monitor/include/system_error_monitor/diagnostics_filter.hpp delete mode 100644 system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp delete mode 100644 system/system_error_monitor/launch/system_error_monitor.launch.xml delete mode 100644 system/system_error_monitor/launch/system_error_monitor_node.launch.xml delete mode 100644 system/system_error_monitor/package.xml delete mode 100644 system/system_error_monitor/src/system_error_monitor_core.cpp diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index b31f6c07d7426..0c0bb6955ae2a 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -236,7 +236,6 @@ system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@t system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp system/mrm_handler/** makoto.kurihara@tier4.jp ryuta.kambe@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/system_diagnostic_monitor/** isamu.takagi@tier4.jp -system/system_error_monitor/** fumihito.ito@tier4.jp system/system_monitor/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/topic_state_monitor/** ryohsuke.mitsudome@tier4.jp system/velodyne_monitor/** fumihito.ito@tier4.jp diff --git a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp index c0db8cc86450b..b0ba99c98b154 100644 --- a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp +++ b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp @@ -74,7 +74,7 @@ void insertNewlines(std::string & str, const size_t max_line_length) std::optional generateMrmMessage(diagnostic_msgs::msg::DiagnosticStatus diag_status) { - if (diag_status.hardware_id == "" || diag_status.hardware_id == "system_error_monitor") { + if (diag_status.hardware_id == "") { return std::nullopt; } else if (diag_status.level == diagnostic_msgs::msg::DiagnosticStatus::ERROR) { std::string msg = "- ERROR: " + diag_status.name + " (" + diag_status.message + ")"; diff --git a/launch/tier4_system_launch/README.md b/launch/tier4_system_launch/README.md index 76cea5a575c5d..440843bf1c3f5 100644 --- a/launch/tier4_system_launch/README.md +++ b/launch/tier4_system_launch/README.md @@ -23,5 +23,3 @@ Note that you should provide parameter paths as `PACKAGE_param_path`. The list o ... ``` - -The sensing configuration parameters used in system_error_monitor are loaded from "config/diagnostic_aggregator/sensor_kit.param.yaml" in the "`SENSOR_MODEL`\_description" package. diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 07f6ae8d4bdc2..19e5120d605e2 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -6,10 +6,6 @@ - - - - diff --git a/launch/tier4_system_launch/package.xml b/launch/tier4_system_launch/package.xml index 673596fad9972..881a65effe2c0 100644 --- a/launch/tier4_system_launch/package.xml +++ b/launch/tier4_system_launch/package.xml @@ -12,7 +12,6 @@ autoware_cmake component_state_monitor - system_error_monitor system_monitor ament_lint_auto diff --git a/system/system_error_monitor/CMakeLists.txt b/system/system_error_monitor/CMakeLists.txt deleted file mode 100644 index 91e9e20146327..0000000000000 --- a/system/system_error_monitor/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(system_error_monitor) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/system_error_monitor_core.cpp -) - -rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "AutowareErrorMonitor" - EXECUTABLE ${PROJECT_NAME}_node) - -ament_auto_package(INSTALL_TO_SHARE - launch - config -) diff --git a/system/system_error_monitor/README.md b/system/system_error_monitor/README.md deleted file mode 100644 index e765239f30f4d..0000000000000 --- a/system/system_error_monitor/README.md +++ /dev/null @@ -1,145 +0,0 @@ -# system_error_monitor - -## Purpose - -Autoware Error Monitor has two main functions. - -1. It is to judge the system hazard level from the aggregated diagnostic information of each module of Autoware. -2. It enables automatic recovery from the emergency state. - -## Inner-workings / Algorithms - -### State Transition - -```plantuml -@startuml -hide empty description - -state "Not Emergency" as N -state "Emergency" as E - -[*] --> N - -N -down-> E : found critical errors -E -up-> N : emergency status is cleared - -note right of E - If the emergency hold is enabled, a service call is required to clear the emergency status. - If not, the emergency status will automatically clear when errors recover. -end note - -@enduml -``` - -### updateEmergencyHoldingCondition Flow Chart - -```plantuml -@startuml -title updateEmergencyHoldingCondition Flow Chart - - -(*) --> "Check emergency holding condition" -note right : emergency_holding is initialized with false - -if "" then - -->[false] "emergency is not held" - else - -->[true] "Check use emergency hold condition" -endif - -if "" then - -->[true] "emergency is held" - else - -->[false] "Check emergency condition" -endif - -if "" then - -->[false] "emergency is not held" - else - -->[true] "Check auto recovery setting" -endif - -if "" then - -->[not approved auto recovery] "emergency is held" - else - -->[approved auto recovery] "Check Emergency Duration" -endif - -if "" then - -->[within recovery timeout] "emergency is not held" - else - -->[over recovery timeout] "Check Control Mode" -endif - -if "" then - -->[autonomous driving] "emergency is held" - else - -->[manual driving] "Check use emergency hold in manual driving condition" -endif - -if "" then - -->[true] "emergency is held" - else - -->[false] "emergency is not held" -endif - -@enduml -``` - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| ---------------------------- | ----------------------------------------------- | ------------------------------------------------------------------------------------------------------------------ | -| `/diagnostics_agg` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information aggregated based [diagnostic_aggregator setting](./config/diagnostic_aggregator) is used to | -| `/autoware/state` | `autoware_system_msgs::msg::AutowareState` | Required to ignore error during Route, Planning and Finalizing. | -| `/control/current_gate_mode` | `tier4_control_msgs::msg::GateMode` | Required to select the appropriate module from `autonomous_driving` or `external_control` | -| `/vehicle/control_mode` | `autoware_vehicle_msgs::msg::ControlModeReport` | Required to not hold emergency during manual driving | - -### Output - -| Name | Type | Description | -| --------------------------------- | ------------------------------------------------ | ------------------------------------------------------------------------------------ | -| `/system/emergency/hazard_status` | `autoware_system_msgs::msg::HazardStatusStamped` | HazardStatus contains system hazard level, emergency hold status and failure details | -| `/diagnostics_err` | `diagnostic_msgs::msg::DiagnosticArray` | This has the same contents as HazardStatus. This is used for visualization | - -## Parameters - -### Node Parameters - -| Name | Type | Default Value | Explanation | -| ---------------------------- | ------ | ------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `ignore_missing_diagnostics` | bool | `false` | If this parameter is turned off, it will be ignored if required modules have not been received. | -| `add_leaf_diagnostics` | bool | `true` | Required to use children diagnostics. | -| `diag_timeout_sec` | double | `1.0` (sec) | If required diagnostic is not received for a `diag_timeout_sec`, the diagnostic state become STALE state. | -| `data_ready_timeout` | double | `30.0` | If input topics required for system_error_monitor are not available for `data_ready_timeout` seconds, autoware_state will translate to emergency state. | -| `data_heartbeat_timeout` | double | `1.0` | If input topics required for system_error_monitor are not no longer subscribed for `data_heartbeat_timeout` seconds, autoware_state will translate to emergency state. | - -### Core Parameters - -| Name | Type | Default Value | Explanation | -| -------------------------------------- | ------ | ------------- | ----------------------------------------------------------------------------------------------------- | -| `hazard_recovery_timeout` | double | `5.0` | The vehicle can recovery to normal driving if emergencies disappear during `hazard_recovery_timeout`. | -| `use_emergency_hold` | bool | `false` | If it is false, the vehicle will return to normal as soon as emergencies disappear. | -| `use_emergency_hold_in_manual_driving` | bool | `false` | If this parameter is turned off, emergencies will be ignored during manual driving. | -| `emergency_hazard_level` | int | `2` | If hazard_level is more than emergency_hazard_level, autoware state will translate to emergency state | - -### YAML format for system_error_monitor - -The parameter key should be filled with the hierarchical diagnostics output by diagnostic_aggregator. Parameters prefixed with `required_modules.autonomous_driving` are for autonomous driving. Parameters with the `required_modules.remote_control` prefix are for remote control. If the value is `default`, the default value will be set. - -| Key | Type | Default Value | Explanation | -| ------------------------------------------------------------------- | ------ | ------------- | ---------------------------------------------------------------------------------------------------------- | -| `required_modules.autonomous_driving.DIAGNOSTIC_NAME.sf_at` | string | `"none"` | Diagnostic level where it becomes Safe Fault. Available options are `"none"`, `"warn"`, `"error"`. | -| `required_modules.autonomous_driving.DIAGNOSTIC_NAME.lf_at` | string | `"warn"` | Diagnostic level where it becomes Latent Fault. Available options are `"none"`, `"warn"`, `"error"`. | -| `required_modules.autonomous_driving.DIAGNOSTIC_NAME.spf_at` | string | `"error"` | Diagnostic level where it becomes Single Point Fault. Available options are `"none"`, `"warn"`, `"error"`. | -| `required_modules.autonomous_driving.DIAGNOSTIC_NAME.auto_recovery` | string | `"true"` | Determines whether the system will automatically recover when it recovers from an error. | -| `required_modules.remote_control.DIAGNOSTIC_NAME.sf_at` | string | `"none"` | Diagnostic level where it becomes Safe Fault. Available options are `"none"`, `"warn"`, `"error"`. | -| `required_modules.remote_control.DIAGNOSTIC_NAME.lf_at` | string | `"warn"` | Diagnostic level where it becomes Latent Fault. Available options are `"none"`, `"warn"`, `"error"`. | -| `required_modules.remote_control.DIAGNOSTIC_NAME.spf_at` | string | `"error"` | Diagnostic level where it becomes Single Point Fault. Available options are `"none"`, `"warn"`, `"error"`. | -| `required_modules.remote_control.DIAGNOSTIC_NAME.auto_recovery` | string | `"true"` | Determines whether the system will automatically recover when it recovers from an error. | - -## Assumptions / Known limits - -TBD. diff --git a/system/system_error_monitor/config/diagnostic_aggregator/_empty.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/_empty.param.yaml deleted file mode 100644 index e69de29bb2d1d..0000000000000 diff --git a/system/system_error_monitor/config/diagnostic_aggregator/control.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/control.param.yaml deleted file mode 100644 index 9da5b14780dd1..0000000000000 --- a/system/system_error_monitor/config/diagnostic_aggregator/control.param.yaml +++ /dev/null @@ -1,121 +0,0 @@ -/**: - ros__parameters: - control: - type: diagnostic_aggregator/AnalyzerGroup - path: control - analyzers: - autonomous_emergency_braking: - type: diagnostic_aggregator/AnalyzerGroup - path: autoware_autonomous_emergency_braking - analyzers: - performance_monitoring: - type: diagnostic_aggregator/AnalyzerGroup - path: performance_monitoring - analyzers: - emergency_stop: - type: diagnostic_aggregator/GenericAnalyzer - path: emergency_stop - contains: [": aeb_emergency_stop"] - timeout: 1.0 - - control_command_gate: - type: diagnostic_aggregator/AnalyzerGroup - path: control_command_gate - analyzers: - node_alive_monitoring: - type: diagnostic_aggregator/AnalyzerGroup - path: node_alive_monitoring - analyzers: - heartbeat: - type: diagnostic_aggregator/GenericAnalyzer - path: heartbeat - contains: ["vehicle_cmd_gate: heartbeat"] - timeout: 1.0 - - autonomous_driving: - type: diagnostic_aggregator/AnalyzerGroup - path: autonomous_driving - analyzers: - node_alive_monitoring: - type: diagnostic_aggregator/AnalyzerGroup - path: node_alive_monitoring - analyzers: - topic_status: - type: diagnostic_aggregator/GenericAnalyzer - path: topic_status - contains: [": control_topic_status"] - timeout: 1.0 - - performance_monitoring: - type: diagnostic_aggregator/AnalyzerGroup - path: performance_monitoring - analyzers: - lane_departure: - type: diagnostic_aggregator/GenericAnalyzer - path: lane_departure - contains: [": lane_departure"] - timeout: 1.0 - - trajectory_deviation: - type: diagnostic_aggregator/GenericAnalyzer - path: trajectory_deviation - contains: [": trajectory_deviation"] - timeout: 1.0 - - control_state: - type: diagnostic_aggregator/GenericAnalyzer - path: control_state - contains: [": control_state"] - timeout: 1.0 - - control_validator: - type: diagnostic_aggregator/GenericAnalyzer - path: autoware_control_validator - contains: [": control_validation_max_distance_deviation"] - timeout: 1.0 - - external_control: - type: diagnostic_aggregator/AnalyzerGroup - path: external_control - analyzers: - external_command_selector: - type: diagnostic_aggregator/AnalyzerGroup - path: external_command_selector - analyzers: - node_alive_monitoring: - type: diagnostic_aggregator/AnalyzerGroup - path: node_alive_monitoring - analyzers: - heartbeat: - type: diagnostic_aggregator/GenericAnalyzer - path: heartbeat - contains: ["external_cmd_selector: heartbeat"] - timeout: 1.0 - - local_external_control: - type: diagnostic_aggregator/AnalyzerGroup - path: local_external_control - analyzers: - device_connection: - type: diagnostic_aggregator/AnalyzerGroup - path: device_connection - analyzers: - joy_controller_connection: - type: diagnostic_aggregator/GenericAnalyzer - path: joy_controller_connection - contains: [": joy_controller_connection"] - timeout: 1.0 - - remote_external_control: - type: diagnostic_aggregator/AnalyzerGroup - path: remote_external_control - analyzers: - network_connection: - type: diagnostic_aggregator/AnalyzerGroup - path: network_connection - analyzers: - remote_control_topic_status: - type: diagnostic_aggregator/GenericAnalyzer - path: remote_control_topic_status - contains: [": remote_control_topic_status"] - timeout: 1.0 diff --git a/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml deleted file mode 100644 index ff6a85d150ee8..0000000000000 --- a/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml +++ /dev/null @@ -1,47 +0,0 @@ -/**: - ros__parameters: - localization: - type: diagnostic_aggregator/AnalyzerGroup - path: localization - analyzers: - node_alive_monitoring: - type: diagnostic_aggregator/AnalyzerGroup - path: node_alive_monitoring - analyzers: - topic_status: - type: diagnostic_aggregator/GenericAnalyzer - path: topic_status - contains: [": localization_topic_status"] - timeout: 1.0 - - performance_monitoring: - type: diagnostic_aggregator/AnalyzerGroup - path: performance_monitoring - analyzers: - scan_matching_status: - type: diagnostic_aggregator/GenericAnalyzer - path: scan_matching_status - contains: ["ndt_scan_matcher: scan_matching_status"] - timeout: 1.0 - - localization_error_ellipse: - type: diagnostic_aggregator/GenericAnalyzer - path: localization_error_ellipse - contains: ["localization_error_monitor: ellipse_error_status"] - timeout: 1.0 - - localization_stability: - type: diagnostic_aggregator/GenericAnalyzer - path: localization_stability - contains: ["localization: pose_instability_detector"] - timeout: 1.0 - - # This diagnostic should ideally be avoided in terms of Fault Tree Analysis (FTA) compatibility. - # However, we may need this since the localization accuracy is still not reliable enough and may produce - # false positives. Thus, NOTE that this diagnostic should be removed in the future when the localization accuracy - # is reliable enough. - sensor_fusion_status: - type: diagnostic_aggregator/GenericAnalyzer - path: sensor_fusion_status - contains: ["localization: ekf_localizer"] - timeout: 1.0 diff --git a/system/system_error_monitor/config/diagnostic_aggregator/map.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/map.param.yaml deleted file mode 100644 index fa218ca5f08fb..0000000000000 --- a/system/system_error_monitor/config/diagnostic_aggregator/map.param.yaml +++ /dev/null @@ -1,22 +0,0 @@ -/**: - ros__parameters: - map: - type: diagnostic_aggregator/AnalyzerGroup - path: map - analyzers: - node_alive_monitoring: - type: diagnostic_aggregator/AnalyzerGroup - path: node_alive_monitoring - analyzers: - topic_status: - type: diagnostic_aggregator/GenericAnalyzer - path: topic_status - contains: [": map_topic_status"] - timeout: 1.0 - - # TODO(Tier IV): Support this diagnostics - # route_validation: - # type: diagnostic_aggregator/GenericAnalyzer - # path: route_validation - # contains: [": route_validation"] - # timeout: 0.0 diff --git a/system/system_error_monitor/config/diagnostic_aggregator/perception.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/perception.param.yaml deleted file mode 100644 index 1f897def54f7a..0000000000000 --- a/system/system_error_monitor/config/diagnostic_aggregator/perception.param.yaml +++ /dev/null @@ -1,15 +0,0 @@ -/**: - ros__parameters: - perception: - type: diagnostic_aggregator/AnalyzerGroup - path: perception - analyzers: - node_alive_monitoring: - type: diagnostic_aggregator/AnalyzerGroup - path: node_alive_monitoring - analyzers: - topic_status: - type: diagnostic_aggregator/GenericAnalyzer - path: topic_status - contains: [": perception_topic_status"] - timeout: 1.0 diff --git a/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml deleted file mode 100644 index aad8a4ffffac4..0000000000000 --- a/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml +++ /dev/null @@ -1,95 +0,0 @@ -/**: - ros__parameters: - planning: - type: diagnostic_aggregator/AnalyzerGroup - path: planning - analyzers: - node_alive_monitoring: - type: diagnostic_aggregator/AnalyzerGroup - path: node_alive_monitoring - analyzers: - topic_status: - type: diagnostic_aggregator/GenericAnalyzer - path: topic_status - contains: [": planning_topic_status"] - timeout: 1.0 - - performance_monitoring: - type: diagnostic_aggregator/AnalyzerGroup - path: performance_monitoring - analyzers: - trajectory_validation: - type: diagnostic_aggregator/AnalyzerGroup - path: trajectory_validation - analyzers: - trajectory_size: - type: diagnostic_aggregator/GenericAnalyzer - path: trajectory_validation_size - contains: [": trajectory_validation_size"] - timeout: 1.0 - - trajectory_finite_value: - type: diagnostic_aggregator/GenericAnalyzer - path: trajectory_validation_finite - contains: [": trajectory_validation_finite"] - timeout: 1.0 - - trajectory_interval: - type: diagnostic_aggregator/GenericAnalyzer - path: trajectory_validation_interval - contains: [": trajectory_validation_interval"] - timeout: 1.0 - - trajectory_curvature: - type: diagnostic_aggregator/GenericAnalyzer - path: trajectory_validation_curvature - contains: [": trajectory_validation_curvature"] - timeout: 1.0 - - trajectory_sharp_angle: - type: diagnostic_aggregator/GenericAnalyzer - path: trajectory_validation_relative_angle - contains: [": trajectory_validation_relative_angle"] - timeout: 1.0 - - trajectory_relative_angle: - type: diagnostic_aggregator/GenericAnalyzer - path: trajectory_validation_relative_angle - contains: [": trajectory_validation_relative_angle"] - timeout: 1.0 - - trajectory_lateral_acceleration: - type: diagnostic_aggregator/GenericAnalyzer - path: trajectory_validation_lateral_acceleration - contains: [": trajectory_validation_lateral_acceleration"] - timeout: 1.0 - - trajectory_acceleration: - type: diagnostic_aggregator/GenericAnalyzer - path: trajectory_validation_acceleration - contains: [": trajectory_validation_acceleration"] - timeout: 1.0 - - trajectory_deceleration: - type: diagnostic_aggregator/GenericAnalyzer - path: trajectory_validation_deceleration - contains: [": trajectory_validation_deceleration"] - timeout: 1.0 - - trajectory_steering: - type: diagnostic_aggregator/GenericAnalyzer - path: trajectory_validation_steering - contains: [": trajectory_validation_steering"] - timeout: 1.0 - - trajectory_steering_rate: - type: diagnostic_aggregator/GenericAnalyzer - path: trajectory_validation_steering_rate - contains: [": trajectory_validation_steering_rate"] - timeout: 1.0 - - trajectory_velocity_deviation: - type: diagnostic_aggregator/GenericAnalyzer - path: trajectory_validation_velocity_deviation - contains: [": trajectory_validation_velocity_deviation"] - timeout: 1.0 diff --git a/system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml deleted file mode 100644 index 27dc4518d4f30..0000000000000 --- a/system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml +++ /dev/null @@ -1,2 +0,0 @@ -/**: - ros__parameters: {} diff --git a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml deleted file mode 100644 index f6e13012957aa..0000000000000 --- a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml +++ /dev/null @@ -1,249 +0,0 @@ -/**: - ros__parameters: - system: - type: diagnostic_aggregator/AnalyzerGroup - path: system - analyzers: - node_alive_monitoring: - type: diagnostic_aggregator/AnalyzerGroup - path: node_alive_monitoring - analyzers: - topic_status: - type: diagnostic_aggregator/GenericAnalyzer - path: topic_status - contains: [": system_topic_status"] - timeout: 1.0 - - emergency_stop_operation: - type: diagnostic_aggregator/GenericAnalyzer - path: emergency_stop_operation - contains: [": emergency_stop_operation"] - timeout: 1.0 - - service_log_checker: - type: diagnostic_aggregator/GenericAnalyzer - path: service_log_checker - contains: ["service_log_checker"] - timeout: 5.0 - - duplicated_node_checker: - type: diagnostic_aggregator/GenericAnalyzer - path: duplicated_node_checker - contains: ["duplicated_node_checker"] - timeout: 5.0 - - resource_monitoring: - type: diagnostic_aggregator/AnalyzerGroup - path: resource_monitoring - analyzers: - clock: - type: diagnostic_aggregator/AnalyzerGroup - path: clock - analyzers: - clock_offset: - type: diagnostic_aggregator/GenericAnalyzer - path: clock_offset - contains: [": NTP Offset"] - timeout: 10.0 - - cpu: - type: diagnostic_aggregator/AnalyzerGroup - path: cpu - analyzers: - temperature: - type: diagnostic_aggregator/GenericAnalyzer - path: temperature - contains: [": CPU Temperature"] - timeout: 3.0 - - usage: - type: diagnostic_aggregator/GenericAnalyzer - path: usage - contains: [": CPU Usage"] - timeout: 3.0 - - thermal_throttling: - type: diagnostic_aggregator/GenericAnalyzer - path: thermal_throttling - contains: [": CPU Thermal Throttling"] - timeout: 3.0 - - frequency: - type: diagnostic_aggregator/GenericAnalyzer - path: frequency - contains: [": CPU Frequency"] - timeout: 3.0 - - load_average: - type: diagnostic_aggregator/GenericAnalyzer - path: load_average - contains: [": CPU Load Average"] - timeout: 3.0 - - gpu: - type: diagnostic_aggregator/AnalyzerGroup - path: gpu - analyzers: - temperature: - type: diagnostic_aggregator/GenericAnalyzer - path: temperature - contains: [": GPU Temperature"] - timeout: 3.0 - - usage: - type: diagnostic_aggregator/GenericAnalyzer - path: gpu_usage - contains: [": GPU Usage"] - timeout: 3.0 - - memory_usage: - type: diagnostic_aggregator/GenericAnalyzer - path: memory_usage - contains: [": GPU Memory Usage"] - timeout: 3.0 - - thermal_throttling: - type: diagnostic_aggregator/GenericAnalyzer - path: thermal_throttling - contains: [": GPU Thermal Throttling"] - timeout: 3.0 - - frequency: - type: diagnostic_aggregator/GenericAnalyzer - path: frequency - contains: [": GPU Frequency"] - timeout: 3.0 - - memory: - type: diagnostic_aggregator/AnalyzerGroup - path: memory - analyzers: - usage: - type: diagnostic_aggregator/GenericAnalyzer - path: usage - contains: [": Memory Usage"] - timeout: 3.0 - - network: - type: diagnostic_aggregator/AnalyzerGroup - path: network - analyzers: - network_usage: - type: diagnostic_aggregator/GenericAnalyzer - path: network_usage - contains: [": Network Usage"] - timeout: 3.0 - - network_traffic: - type: diagnostic_aggregator/GenericAnalyzer - path: network_traffic - contains: [": Network Traffic"] - timeout: 3.0 - - network_crc_error: - type: diagnostic_aggregator/GenericAnalyzer - path: network_crc_error - contains: [": Network CRC Error"] - timeout: 3.0 - - ip_packet_reassembles_failed: - type: diagnostic_aggregator/GenericAnalyzer - path: ip_packet_reassembles_failed - contains: [": IP Packet Reassembles Failed"] - timeout: 3.0 - - storage: - type: diagnostic_aggregator/AnalyzerGroup - path: storage - analyzers: - temperature: - type: diagnostic_aggregator/GenericAnalyzer - path: temperature - contains: [": HDD Temperature"] - timeout: 3.0 - - recovered_error: - type: diagnostic_aggregator/GenericAnalyzer - path: recovered_error - contains: [": HDD RecoveredError"] - timeout: 3.0 - - read_data_rate: - type: diagnostic_aggregator/GenericAnalyzer - path: read_data_rate - contains: [": HDD ReadDataRate"] - timeout: 3.0 - - write_data_rate: - type: diagnostic_aggregator/GenericAnalyzer - path: write_data_rate - contains: [": HDD WriteDataRate"] - timeout: 3.0 - - read_iops: - type: diagnostic_aggregator/GenericAnalyzer - path: read_iops - contains: [": HDD ReadIOPS"] - timeout: 3.0 - - write_iops: - type: diagnostic_aggregator/GenericAnalyzer - path: write_iops - contains: [": HDD WriteIOPS"] - timeout: 3.0 - - usage: - type: diagnostic_aggregator/GenericAnalyzer - path: usage - contains: [": HDD Usage"] - timeout: 3.0 - - power_on_hours: - type: diagnostic_aggregator/GenericAnalyzer - path: power_on_hours - contains: [": HDD PowerOnHours"] - timeout: 3.0 - - total_data_written: - type: diagnostic_aggregator/GenericAnalyzer - path: total_data_written - contains: [": HDD TotalDataWritten"] - timeout: 3.0 - - connection: - type: diagnostic_aggregator/GenericAnalyzer - path: connection - contains: [": HDD Connection"] - timeout: 3.0 - - process: - type: diagnostic_aggregator/AnalyzerGroup - path: process - analyzers: - high_load: - type: diagnostic_aggregator/GenericAnalyzer - path: high_load - contains: [": High-load"] - timeout: 3.0 - - high_mem: - type: diagnostic_aggregator/GenericAnalyzer - path: high_mem - contains: [": High-mem"] - timeout: 3.0 - - tasks_summary: - type: diagnostic_aggregator/GenericAnalyzer - path: tasks_summary - contains: [": Tasks Summary"] - timeout: 3.0 - - hardware: - type: diagnostic_aggregator/AnalyzerGroup - path: voltage - analyzers: - cmos_battery: - type: diagnostic_aggregator/GenericAnalyzer - path: cmos_battery - contains: [": CMOS Battery Status"] - timeout: 3.0 diff --git a/system/system_error_monitor/config/diagnostic_aggregator/vehicle.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/vehicle.param.yaml deleted file mode 100644 index 45ede1a4de000..0000000000000 --- a/system/system_error_monitor/config/diagnostic_aggregator/vehicle.param.yaml +++ /dev/null @@ -1,16 +0,0 @@ -/**: - ros__parameters: - vehicle: - type: diagnostic_aggregator/AnalyzerGroup - path: vehicle - analyzers: - node_alive_monitoring: - type: diagnostic_aggregator/AnalyzerGroup - path: node_alive_monitoring - analyzers: - # TODO(Tier IV): Consider splitting sensor input and control command output - topic_status: - type: diagnostic_aggregator/GenericAnalyzer - path: topic_status - contains: [": vehicle_topic_status"] - timeout: 1.0 diff --git a/system/system_error_monitor/config/system_error_monitor.param.yaml b/system/system_error_monitor/config/system_error_monitor.param.yaml deleted file mode 100644 index 63fdbd0081af6..0000000000000 --- a/system/system_error_monitor/config/system_error_monitor.param.yaml +++ /dev/null @@ -1,55 +0,0 @@ -# Description: -# name: diag name -# sf_at: diag level where it becomes Safe Fault -# lf_at: diag level where it becomes Latent Fault -# spf_at: diag level where it becomes Single Point Fault -# auto_recovery: Determines whether the system will automatically recover when it recovers from an error. -# -# Note: -# empty-value for sf_at, lf_at and spf_at is "none" -# default values are: -# sf_at: "none" -# lf_at: "warn" -# spf_at: "error" -# auto_recovery: "true" ---- -/**: - ros__parameters: - required_modules: - autonomous_driving: - /autoware/control/autonomous_driving/node_alive_monitoring: default - /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default - /autoware/control/control_command_gate/node_alive_monitoring: default - /autoware/control/autoware_autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} - - /autoware/localization/node_alive_monitoring: default - /autoware/localization/performance_monitoring/scan_matching_status: { sf_at: "warn", lf_at: "none", spf_at: "none" } - /autoware/localization/performance_monitoring/localization_error_ellipse: default - - /autoware/map/node_alive_monitoring: default - - /autoware/perception/node_alive_monitoring: default - - /autoware/planning/node_alive_monitoring: default - /autoware/planning/performance_monitoring/trajectory_validation: default - - # /autoware/sensing/node_alive_monitoring: default - - /autoware/system/node_alive_monitoring: default - /autoware/system/emergency_stop_operation: default - /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } - /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "none", spf_at: "none" } - /autoware/system/duplicated_node_checker: default - - /autoware/vehicle/node_alive_monitoring: default - - external_control: - /autoware/control/control_command_gate/node_alive_monitoring: default - /autoware/control/autoware_autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} - /autoware/control/external_control/external_command_selector/node_alive_monitoring: default - - /autoware/system/node_alive_monitoring: default - /autoware/system/emergency_stop_operation: default - /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } - - /autoware/vehicle/node_alive_monitoring: default diff --git a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml deleted file mode 100644 index c9ab89a8fce96..0000000000000 --- a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml +++ /dev/null @@ -1,56 +0,0 @@ -# Description: -# name: diag name -# sf_at: diag level where it becomes Safe Fault -# lf_at: diag level where it becomes Latent Fault -# spf_at: diag level where it becomes Single Point Fault -# auto_recovery: Determines whether the system will automatically recover when it recovers from an error. -# -# Note: -# empty-value for sf_at, lf_at and spf_at is "none" -# default values are: -# sf_at: "none" -# lf_at: "warn" -# spf_at: "error" -# auto_recovery: "true" ---- -/**: - ros__parameters: - required_modules: - autonomous_driving: - /autoware/control/autonomous_driving/node_alive_monitoring: default - /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default - /autoware/control/control_command_gate/node_alive_monitoring: default - /autoware/control/autoware_autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} - - /autoware/localization/node_alive_monitoring: default - # /autoware/localization/performance_monitoring/scan_matching_status: { sf_at: "warn", lf_at: "none", spf_at: "none" } - # /autoware/localization/performance_monitoring/localization_error_ellipse: default - - /autoware/map/node_alive_monitoring: default - - /autoware/perception/node_alive_monitoring: default - - /autoware/planning/node_alive_monitoring: default - /autoware/planning/performance_monitoring/trajectory_validation: default - - # /autoware/sensing/node_alive_monitoring: default - - /autoware/system/node_alive_monitoring: default - /autoware/system/emergency_stop_operation: default - /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } - /autoware/system/duplicated_node_checker: default - # /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "error", spf_at: "none" } - # /autoware/system/duplicated_node_checker: default - - /autoware/vehicle/node_alive_monitoring: default - - external_control: - /autoware/control/control_command_gate/node_alive_monitoring: default - /autoware/control/autoware_autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} - /autoware/control/external_control/external_command_selector/node_alive_monitoring: default - - /autoware/system/node_alive_monitoring: default - /autoware/system/emergency_stop_operation: default - /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } - - /autoware/vehicle/node_alive_monitoring: default diff --git a/system/system_error_monitor/include/system_error_monitor/diagnostics_filter.hpp b/system/system_error_monitor/include/system_error_monitor/diagnostics_filter.hpp deleted file mode 100644 index cbca3ed1c477c..0000000000000 --- a/system/system_error_monitor/include/system_error_monitor/diagnostics_filter.hpp +++ /dev/null @@ -1,103 +0,0 @@ -// 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 SYSTEM_ERROR_MONITOR__DIAGNOSTICS_FILTER_HPP_ -#define SYSTEM_ERROR_MONITOR__DIAGNOSTICS_FILTER_HPP_ - -#include - -#include -#include -#include - -namespace diagnostics_filter -{ -inline std::string splitStringByLastSlash(const std::string & str) -{ - const auto last_slash = str.find_last_of("/"); - - if (last_slash == std::string::npos) { - return ""; - } - - return str.substr(0, last_slash); -} - -inline bool isChild( - const diagnostic_msgs::msg::DiagnosticStatus & child, - const diagnostic_msgs::msg::DiagnosticStatus & parent) -{ - auto name = splitStringByLastSlash(child.name); - while (name != "") { - if (name == parent.name) { - return true; - } - - name = splitStringByLastSlash(name); - } - - return false; -} - -inline bool isLeaf( - const std::unordered_set & diag_name_set, - const diagnostic_msgs::msg::DiagnosticStatus & diag) -{ - return diag_name_set.count(diag.name) == 0; -} - -inline std::unordered_set createDiagNameSet( - const std::vector & diagnostics) -{ - std::unordered_set diag_name_set; - - for (const auto & diag : diagnostics) { - diag_name_set.insert(splitStringByLastSlash(diag.name)); - } - - return diag_name_set; -} - -inline std::vector extractLeafDiagnostics( - const std::vector & diagnostics) -{ - const auto diag_name_set = createDiagNameSet(diagnostics); - - std::vector leaf_diagnostics; - for (const auto & diag : diagnostics) { - if (isLeaf(diag_name_set, diag)) { - leaf_diagnostics.emplace_back(diag); - } - } - - return leaf_diagnostics; -} - -inline std::vector extractLeafChildrenDiagnostics( - const diagnostic_msgs::msg::DiagnosticStatus & parent, - const std::vector & diagnostics) -{ - std::vector leaf_children_diagnostics; - for (const auto & diag : extractLeafDiagnostics(diagnostics)) { - if (isChild(diag, parent)) { - leaf_children_diagnostics.emplace_back(diag); - } - } - - return leaf_children_diagnostics; -} - -} // namespace diagnostics_filter - -#endif // SYSTEM_ERROR_MONITOR__DIAGNOSTICS_FILTER_HPP_ diff --git a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp deleted file mode 100644 index dbb1db027b149..0000000000000 --- a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp +++ /dev/null @@ -1,153 +0,0 @@ -// 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 SYSTEM_ERROR_MONITOR__SYSTEM_ERROR_MONITOR_CORE_HPP_ -#define SYSTEM_ERROR_MONITOR__SYSTEM_ERROR_MONITOR_CORE_HPP_ - -#include "autoware/universe_utils/ros/logger_level_configure.hpp" - -#include -#include - -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include - -struct DiagStamped -{ - std_msgs::msg::Header header; - diagnostic_msgs::msg::DiagnosticStatus status; -}; - -using DiagBuffer = std::deque; - -struct DiagConfig -{ - std::string name; - std::string sf_at; - std::string lf_at; - std::string spf_at; - bool auto_recovery; -}; - -using RequiredModules = std::vector; - -struct KeyName -{ - static constexpr const char * autonomous_driving = "autonomous_driving"; - static constexpr const char * external_control = "external_control"; -}; - -class AutowareErrorMonitor : public rclcpp::Node -{ -public: - explicit AutowareErrorMonitor(const rclcpp::NodeOptions & options); - -private: - // Parameter - struct Parameters - { - int update_rate; - bool ignore_missing_diagnostics; - bool add_leaf_diagnostics; - double data_ready_timeout; - double data_heartbeat_timeout; - double diag_timeout_sec; - double hazard_recovery_timeout; - int emergency_hazard_level; - bool use_emergency_hold; - bool use_emergency_hold_in_manual_driving; - }; - - Parameters params_{}; - - rclcpp::Time emergency_state_switch_time_; - rclcpp::Time initialized_time_; - autoware_system_msgs::msg::HazardStatus hazard_status_{}; - std::unordered_map required_modules_map_; - std::string current_mode_; - - void loadRequiredModules(const std::string & key); - - // Timer - rclcpp::TimerBase::SharedPtr timer_; - - bool isDataReady(); - bool isDataHeartbeatTimeout(); - void onTimer(); - - // Subscriber - rclcpp::Subscription::SharedPtr sub_diag_array_; - rclcpp::Subscription::SharedPtr sub_autoware_state_; - rclcpp::Subscription::SharedPtr sub_current_gate_mode_; - rclcpp::Subscription::SharedPtr sub_control_mode_; - void onAutowareState(const autoware_system_msgs::msg::AutowareState::ConstSharedPtr msg); - void onCurrentGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg); - void onControlMode(const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); - void onDiagArray(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg); - - const size_t diag_buffer_size_ = 100; - std::unordered_map diag_buffer_map_; - diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr diag_array_; - autoware_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_; - tier4_control_msgs::msg::GateMode::ConstSharedPtr current_gate_mode_; - autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; - - // Publisher - rclcpp::Publisher::SharedPtr pub_hazard_status_; - rclcpp::Publisher::SharedPtr pub_diagnostics_err_; - void publishHazardStatus(const autoware_system_msgs::msg::HazardStatus & hazard_status); - - // Service - rclcpp::Service::SharedPtr srv_clear_emergency_; - bool onClearEmergencyService( - [[maybe_unused]] std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response); - - // for Heartbeat - rclcpp::Time diag_array_stamp_; - rclcpp::Time autoware_state_stamp_; - rclcpp::Time current_gate_mode_stamp_; - rclcpp::Time control_mode_stamp_; - - // Algorithm - boost::optional getLatestDiag(const std::string & diag_name) const; - uint8_t getHazardLevel(const DiagConfig & required_module, const int diag_level) const; - void appendHazardDiag( - const DiagConfig & required_module, const diagnostic_msgs::msg::DiagnosticStatus & diag, - autoware_system_msgs::msg::HazardStatus * hazard_status) const; - autoware_system_msgs::msg::HazardStatus judgeHazardStatus() const; - void updateHazardStatus(); - void updateTimeoutHazardStatus(); - bool canAutoRecovery() const; - bool isEmergencyHoldingRequired() const; - - void loggingErrors(const autoware_system_msgs::msg::HazardStatus & diag_array); - - std::unique_ptr logger_configure_; -}; - -#endif // SYSTEM_ERROR_MONITOR__SYSTEM_ERROR_MONITOR_CORE_HPP_ diff --git a/system/system_error_monitor/launch/system_error_monitor.launch.xml b/system/system_error_monitor/launch/system_error_monitor.launch.xml deleted file mode 100644 index 9eb805b0eb2a6..0000000000000 --- a/system/system_error_monitor/launch/system_error_monitor.launch.xml +++ /dev/null @@ -1,55 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/system/system_error_monitor/launch/system_error_monitor_node.launch.xml b/system/system_error_monitor/launch/system_error_monitor_node.launch.xml deleted file mode 100644 index f5ed927c7d6ca..0000000000000 --- a/system/system_error_monitor/launch/system_error_monitor_node.launch.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/system/system_error_monitor/package.xml b/system/system_error_monitor/package.xml deleted file mode 100644 index 2dda4e462b98b..0000000000000 --- a/system/system_error_monitor/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - system_error_monitor - 0.1.1 - The system_error_monitor package in ROS 2 - Fumihito Ito - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - autoware_system_msgs - autoware_universe_utils - autoware_vehicle_msgs - diagnostic_msgs - fmt - rclcpp - rclcpp_components - std_srvs - tier4_control_msgs - - diagnostic_aggregator - rqt_robot_monitor - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp deleted file mode 100644 index ef892bd01ff50..0000000000000 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ /dev/null @@ -1,722 +0,0 @@ -// 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. - -#include -#include -#include -#include -#include -#include -#include - -#define FMT_HEADER_ONLY -#include "system_error_monitor/diagnostics_filter.hpp" -#include "system_error_monitor/system_error_monitor_core.hpp" - -#include - -namespace -{ -enum class DebugLevel { DEBUG, INFO, WARN, ERROR, FATAL }; - -template -void logThrottledNamed( - const std::string & logger_name, const rclcpp::Clock::SharedPtr clock, const double duration_ms, - const std::string & message) -{ - static std::unordered_map last_output_time; - if (last_output_time.count(logger_name) != 0) { - const auto time_from_last_output = clock->now() - last_output_time.at(logger_name); - if (time_from_last_output.seconds() * 1000.0 < duration_ms) { - return; - } - } - - last_output_time[logger_name] = clock->now(); - if constexpr (debug_level == DebugLevel::DEBUG) { - RCLCPP_DEBUG(rclcpp::get_logger(logger_name), message.c_str()); - } else if constexpr (debug_level == DebugLevel::INFO) { - RCLCPP_INFO(rclcpp::get_logger(logger_name), message.c_str()); - } else if constexpr (debug_level == DebugLevel::WARN) { - RCLCPP_WARN(rclcpp::get_logger(logger_name), message.c_str()); - } else if constexpr (debug_level == DebugLevel::ERROR) { - RCLCPP_ERROR(rclcpp::get_logger(logger_name), message.c_str()); - } else if constexpr (debug_level == DebugLevel::FATAL) { - RCLCPP_FATAL(rclcpp::get_logger(logger_name), message.c_str()); - } -} - -std::vector split(const std::string & str, const char delim) -{ - std::vector elems; - std::stringstream ss(str); - std::string item; - while (std::getline(ss, item, delim)) { - elems.push_back(item); - } - return elems; -} - -int str2level(const std::string & level_str) -{ - using diagnostic_msgs::msg::DiagnosticStatus; - using std::regex_constants::icase; - - if (std::regex_match(level_str, std::regex("warn", icase))) { - return DiagnosticStatus::WARN; - } - if (std::regex_match(level_str, std::regex("error", icase))) { - return DiagnosticStatus::ERROR; - } - if (std::regex_match(level_str, std::regex("stale", icase))) { - return DiagnosticStatus::STALE; - } - - throw std::runtime_error(fmt::format("invalid level: {}", level_str)); -} - -bool isOverLevel(const int & diag_level, const std::string & failure_level_str) -{ - if (failure_level_str == "none") { - return false; - } - - return diag_level >= str2level(failure_level_str); -} - -std::vector & getTargetDiagnosticsRef( - const int hazard_level, autoware_system_msgs::msg::HazardStatus * hazard_status) -{ - using autoware_system_msgs::msg::HazardStatus; - - if (hazard_level == HazardStatus::NO_FAULT) { - return hazard_status->diag_no_fault; - } - if (hazard_level == HazardStatus::SAFE_FAULT) { - return hazard_status->diag_safe_fault; - } - if (hazard_level == HazardStatus::LATENT_FAULT) { - return hazard_status->diag_latent_fault; - } - if (hazard_level == HazardStatus::SINGLE_POINT_FAULT) { - return hazard_status->diag_single_point_fault; - } - - throw std::runtime_error(fmt::format("invalid hazard level: {}", hazard_level)); -} - -diagnostic_msgs::msg::DiagnosticArray convertHazardStatusToDiagnosticArray( - rclcpp::Clock::SharedPtr clock, const autoware_system_msgs::msg::HazardStatus & hazard_status) -{ - using diagnostic_msgs::msg::DiagnosticStatus; - - diagnostic_msgs::msg::DiagnosticArray diag_array; - diag_array.header.stamp = clock->now(); - - const auto decorateDiag = [](const auto & hazard_diag, const std::string & label) { - auto diag = hazard_diag; - - diag.message = label + diag.message; - - return diag; - }; - - for (const auto & hazard_diag : hazard_status.diag_no_fault) { - diag_array.status.push_back(decorateDiag(hazard_diag, "[No Fault]")); - } - for (const auto & hazard_diag : hazard_status.diag_safe_fault) { - diag_array.status.push_back(decorateDiag(hazard_diag, "[Safe Fault]")); - } - for (const auto & hazard_diag : hazard_status.diag_latent_fault) { - diag_array.status.push_back(decorateDiag(hazard_diag, "[Latent Fault]")); - } - for (const auto & hazard_diag : hazard_status.diag_single_point_fault) { - diag_array.status.push_back(decorateDiag(hazard_diag, "[Single Point Fault]")); - } - - return diag_array; -} - -std::set getErrorModules( - const autoware_system_msgs::msg::HazardStatus & hazard_status, const int emergency_hazard_level) -{ - std::set error_modules; - using autoware_system_msgs::msg::HazardStatus; - if (emergency_hazard_level <= HazardStatus::SINGLE_POINT_FAULT) { - for (const auto & diag_spf : hazard_status.diag_single_point_fault) { - error_modules.insert(diag_spf.name); - } - } - if (emergency_hazard_level <= HazardStatus::LATENT_FAULT) { - for (const auto & diag_lf : hazard_status.diag_latent_fault) { - error_modules.insert(diag_lf.name); - } - } - if (emergency_hazard_level <= HazardStatus::SAFE_FAULT) { - for (const auto & diag_sf : hazard_status.diag_safe_fault) { - error_modules.insert(diag_sf.name); - } - } - - return error_modules; -} - -int isInNoFaultCondition( - const autoware_system_msgs::msg::AutowareState & autoware_state, - const tier4_control_msgs::msg::GateMode & current_gate_mode) -{ - using autoware_system_msgs::msg::AutowareState; - using tier4_control_msgs::msg::GateMode; - - const auto is_in_autonomous_ignore_state = - (autoware_state.state == AutowareState::INITIALIZING) || - (autoware_state.state == AutowareState::WAITING_FOR_ROUTE) || - (autoware_state.state == AutowareState::PLANNING) || - (autoware_state.state == AutowareState::FINALIZING); - - if (current_gate_mode.data == GateMode::AUTO && is_in_autonomous_ignore_state) { - return true; - } - - const auto is_in_external_ignore_state = (autoware_state.state == AutowareState::INITIALIZING) || - (autoware_state.state == AutowareState::FINALIZING); - - if (current_gate_mode.data == GateMode::EXTERNAL && is_in_external_ignore_state) { - return true; - } - - return false; -} -} // namespace - -AutowareErrorMonitor::AutowareErrorMonitor(const rclcpp::NodeOptions & options) -: Node( - "system_error_monitor", - rclcpp::NodeOptions(options).automatically_declare_parameters_from_overrides(true)), - diag_array_stamp_(0, 0, this->get_clock()->get_clock_type()), - autoware_state_stamp_(0, 0, this->get_clock()->get_clock_type()), - current_gate_mode_stamp_(0, 0, this->get_clock()->get_clock_type()), - control_mode_stamp_(0, 0, this->get_clock()->get_clock_type()) -{ - // Parameter - get_parameter_or("update_rate", params_.update_rate, 10); - get_parameter_or("ignore_missing_diagnostics", params_.ignore_missing_diagnostics, false); - get_parameter_or("add_leaf_diagnostics", params_.add_leaf_diagnostics, true); - get_parameter_or("data_ready_timeout", params_.data_ready_timeout, 30.0); - get_parameter_or("data_heartbeat_timeout", params_.data_heartbeat_timeout, 1.0); - get_parameter_or("diag_timeout_sec", params_.diag_timeout_sec, 1.0); - get_parameter_or("hazard_recovery_timeout", params_.hazard_recovery_timeout, 5.0); - get_parameter_or( - "emergency_hazard_level", params_.emergency_hazard_level, - autoware_system_msgs::msg::HazardStatus::LATENT_FAULT); - get_parameter_or("use_emergency_hold", params_.use_emergency_hold, false); - get_parameter_or( - "use_emergency_hold_in_manual_driving", params_.use_emergency_hold_in_manual_driving, false); - - loadRequiredModules(KeyName::autonomous_driving); - loadRequiredModules(KeyName::external_control); - - using std::placeholders::_1; - using std::placeholders::_2; - // Subscriber - sub_diag_array_ = create_subscription( - "input/diag_array", rclcpp::QoS{1}, std::bind(&AutowareErrorMonitor::onDiagArray, this, _1)); - sub_current_gate_mode_ = create_subscription( - "~/input/current_gate_mode", rclcpp::QoS{1}, - std::bind(&AutowareErrorMonitor::onCurrentGateMode, this, _1)); - sub_autoware_state_ = create_subscription( - "~/input/autoware_state", rclcpp::QoS{1}, - std::bind(&AutowareErrorMonitor::onAutowareState, this, _1)); - sub_control_mode_ = create_subscription( - "~/input/control_mode", rclcpp::QoS{1}, - std::bind(&AutowareErrorMonitor::onControlMode, this, _1)); - - // Publisher - pub_hazard_status_ = create_publisher( - "~/output/hazard_status", rclcpp::QoS{1}); - pub_diagnostics_err_ = create_publisher( - "~/output/diagnostics_err", rclcpp::QoS{1}); - - // Service - srv_clear_emergency_ = this->create_service( - "service/clear_emergency", - std::bind(&AutowareErrorMonitor::onClearEmergencyService, this, _1, _2)); - - // Initialize - autoware_vehicle_msgs::msg::ControlModeReport vehicle_state_report; - vehicle_state_report.mode = autoware_vehicle_msgs::msg::ControlModeReport::MANUAL; - control_mode_ = - std::make_shared(vehicle_state_report); - - // Timer - initialized_time_ = this->now(); - const auto period_ns = rclcpp::Rate(params_.update_rate).period(); - timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&AutowareErrorMonitor::onTimer, this)); - - logger_configure_ = std::make_unique(this); -} - -void AutowareErrorMonitor::loadRequiredModules(const std::string & key) -{ - const uint64_t depth = 3; - const auto param_names = - this->list_parameters({std::string("required_modules.") + key}, depth).names; - - if (param_names.empty()) { - throw std::runtime_error(fmt::format("no parameter found: required_modules.{}", key)); - } - - // Load module names from parameter key - std::set module_names; - RequiredModules required_modules; - - for (const auto & param_name : param_names) { - // Example of param_name: required_modules.key.module - // or required_modules.key.module.parameter - const auto split_names = split(param_name, '.'); - const auto & param_required_modules = split_names.at(0); - const auto & param_key = split_names.at(1); - const auto & param_module = split_names.at(2); - const auto module_name_with_prefix = - fmt::format("{0}.{1}.{2}", param_required_modules, param_key, param_module); - - // Skip duplicate parameter - if (module_names.count(module_name_with_prefix) != 0) { - continue; - } - module_names.insert(module_name_with_prefix); - - // Load diag level - const auto sf_key = module_name_with_prefix + std::string(".sf_at"); - std::string sf_at; - this->get_parameter_or(sf_key, sf_at, std::string("none")); - - const auto lf_key = module_name_with_prefix + std::string(".lf_at"); - std::string lf_at; - this->get_parameter_or(lf_key, lf_at, std::string("warn")); - - const auto spf_key = module_name_with_prefix + std::string(".spf_at"); - std::string spf_at; - this->get_parameter_or(spf_key, spf_at, std::string("error")); - - // auto_recovery - const auto auto_recovery_key = module_name_with_prefix + std::string(".auto_recovery"); - std::string auto_recovery_approval_str; - this->get_parameter_or(auto_recovery_key, auto_recovery_approval_str, std::string("true")); - - // Convert auto_recovery_approval_str to bool - bool auto_recovery_approval{}; - std::istringstream(auto_recovery_approval_str) >> std::boolalpha >> auto_recovery_approval; - - required_modules.push_back({param_module, sf_at, lf_at, spf_at, auto_recovery_approval}); - } - - required_modules_map_.insert(std::make_pair(key, required_modules)); -} - -void AutowareErrorMonitor::onDiagArray( - const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) -{ - diag_array_ = msg; - - const auto & header = msg->header; - - for (const auto & diag : msg->status) { - if (diag_buffer_map_.count(diag.name) == 0) { - diag_buffer_map_.insert(std::make_pair(diag.name, DiagBuffer{})); - } - - auto & diag_buffer = diag_buffer_map_.at(diag.name); - diag_buffer.push_back(DiagStamped{header, diag}); - - while (diag_buffer.size() > diag_buffer_size_) { - diag_buffer.pop_front(); - } - } - - // for Heartbeat - diag_array_stamp_ = this->now(); -} - -void AutowareErrorMonitor::onCurrentGateMode( - const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg) -{ - current_gate_mode_ = msg; - - // for Heartbeat - current_gate_mode_stamp_ = this->now(); -} - -void AutowareErrorMonitor::onAutowareState( - const autoware_system_msgs::msg::AutowareState::ConstSharedPtr msg) -{ - autoware_state_ = msg; - - // for Heartbeat - autoware_state_stamp_ = this->now(); -} - -void AutowareErrorMonitor::onControlMode( - const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) -{ - control_mode_ = msg; - - // for Heartbeat - control_mode_stamp_ = this->now(); -} - -bool AutowareErrorMonitor::isDataReady() -{ - if (!diag_array_) { - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for diag_array msg..."); - return false; - } - - if (!current_gate_mode_) { - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for current_gate_mode msg..."); - return false; - } - - if (!autoware_state_) { - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for autoware_state msg..."); - return false; - } - - if (!control_mode_) { - RCLCPP_INFO_THROTTLE( - get_logger(), *get_clock(), 5000, "waiting for vehicle_state_report msg..."); - return false; - } - return true; -} - -bool AutowareErrorMonitor::isDataHeartbeatTimeout() -{ - auto isTimeout = [this](const rclcpp::Time & last_stamp, const double threshold) { - if (last_stamp.get_clock_type() != this->now().get_clock_type()) { - RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "clock type is different..."); - return false; - } - const auto time_diff = this->now() - last_stamp; - return time_diff.seconds() > threshold; - }; - - if (isTimeout(diag_array_stamp_, params_.data_heartbeat_timeout)) { - RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "diag_array msg is timeout..."); - return true; - } - - if (isTimeout(current_gate_mode_stamp_, params_.data_heartbeat_timeout)) { - RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "current_gate_mode msg is timeout..."); - return true; - } - - if (isTimeout(autoware_state_stamp_, params_.data_heartbeat_timeout)) { - RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "autoware_state msg is timeout..."); - return true; - } - - if (isTimeout(control_mode_stamp_, params_.data_heartbeat_timeout)) { - RCLCPP_ERROR_THROTTLE( - get_logger(), *get_clock(), 5000, "vehicle_state_report msg is timeout..."); - return true; - } - - return false; -} - -void AutowareErrorMonitor::onTimer() -{ - if (!isDataReady()) { - if ((this->now() - initialized_time_).seconds() > params_.data_ready_timeout) { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(5000).count(), - "input data is timeout"); - updateTimeoutHazardStatus(); - publishHazardStatus(hazard_status_); - } - return; - } - - if (isDataHeartbeatTimeout()) { - updateTimeoutHazardStatus(); - publishHazardStatus(hazard_status_); - return; - } - - current_mode_ = current_gate_mode_->data == tier4_control_msgs::msg::GateMode::AUTO - ? KeyName::autonomous_driving - : KeyName::external_control; - - updateHazardStatus(); - publishHazardStatus(hazard_status_); -} - -boost::optional AutowareErrorMonitor::getLatestDiag( - const std::string & diag_name) const -{ - if (diag_buffer_map_.count(diag_name) == 0) { - return {}; - } - - const auto & diag_buffer = diag_buffer_map_.at(diag_name); - - if (diag_buffer.empty()) { - return {}; - } - - return diag_buffer.back(); -} - -uint8_t AutowareErrorMonitor::getHazardLevel( - const DiagConfig & required_module, const int diag_level) const -{ - using autoware_system_msgs::msg::HazardStatus; - - if (isOverLevel(diag_level, required_module.spf_at)) { - return HazardStatus::SINGLE_POINT_FAULT; - } - if (isOverLevel(diag_level, required_module.lf_at)) { - return HazardStatus::LATENT_FAULT; - } - if (isOverLevel(diag_level, required_module.sf_at)) { - return HazardStatus::SAFE_FAULT; - } - - return HazardStatus::NO_FAULT; -} - -void AutowareErrorMonitor::appendHazardDiag( - const DiagConfig & required_module, const diagnostic_msgs::msg::DiagnosticStatus & hazard_diag, - autoware_system_msgs::msg::HazardStatus * hazard_status) const -{ - const auto hazard_level = getHazardLevel(required_module, hazard_diag.level); - - auto & target_diagnostics_ref = getTargetDiagnosticsRef(hazard_level, hazard_status); - target_diagnostics_ref.push_back(hazard_diag); - - if (params_.add_leaf_diagnostics) { - for (const auto & diag : - diagnostics_filter::extractLeafChildrenDiagnostics(hazard_diag, diag_array_->status)) { - target_diagnostics_ref.push_back(diag); - } - } - - hazard_status->level = std::max(hazard_status->level, hazard_level); -} - -autoware_system_msgs::msg::HazardStatus AutowareErrorMonitor::judgeHazardStatus() const -{ - using autoware_system_msgs::msg::HazardStatus; - using diagnostic_msgs::msg::DiagnosticStatus; - - autoware_system_msgs::msg::HazardStatus hazard_status; - for (const auto & required_module : required_modules_map_.at(current_mode_)) { - const auto & diag_name = required_module.name; - const auto latest_diag = getLatestDiag(diag_name); - - // no diag found - if (!latest_diag) { - if (!params_.ignore_missing_diagnostics) { - DiagnosticStatus missing_diag; - - missing_diag.name = diag_name; - missing_diag.hardware_id = "system_error_monitor"; - missing_diag.level = DiagnosticStatus::STALE; - missing_diag.message = "no diag found"; - - appendHazardDiag(required_module, missing_diag, &hazard_status); - } - - continue; - } - - // diag level high - { - appendHazardDiag(required_module, latest_diag->status, &hazard_status); - } - - // diag timeout - { - const auto time_diff = this->now() - latest_diag->header.stamp; - if (time_diff.seconds() > params_.diag_timeout_sec) { - DiagnosticStatus timeout_diag = latest_diag->status; - timeout_diag.level = DiagnosticStatus::STALE; - timeout_diag.message = "timeout"; - - appendHazardDiag(required_module, timeout_diag, &hazard_status); - } - } - } - - // Ignore error when vehicle is not ready to start - if (isInNoFaultCondition(*autoware_state_, *current_gate_mode_)) { - hazard_status.level = autoware_system_msgs::msg::HazardStatus::NO_FAULT; - } - - return hazard_status; -} - -void AutowareErrorMonitor::updateHazardStatus() -{ - const bool prev_emergency_status = hazard_status_.emergency; - - // Create hazard status based on diagnostics - if (!hazard_status_.emergency_holding) { - const auto current_hazard_status = judgeHazardStatus(); - hazard_status_.level = current_hazard_status.level; - hazard_status_.diag_no_fault = current_hazard_status.diag_no_fault; - hazard_status_.diag_safe_fault = current_hazard_status.diag_safe_fault; - hazard_status_.diag_latent_fault = current_hazard_status.diag_latent_fault; - hazard_status_.diag_single_point_fault = current_hazard_status.diag_single_point_fault; - } - - // Update emergency status - { - hazard_status_.emergency = hazard_status_.level >= params_.emergency_hazard_level; - if (hazard_status_.emergency != prev_emergency_status) { - emergency_state_switch_time_ = this->now(); - } - } - - // Update emergency_holding condition - if (params_.use_emergency_hold) { - hazard_status_.emergency_holding = isEmergencyHoldingRequired(); - } -} - -void AutowareErrorMonitor::updateTimeoutHazardStatus() -{ - const bool prev_emergency_status = hazard_status_.emergency; - - hazard_status_.level = autoware_system_msgs::msg::HazardStatus::SINGLE_POINT_FAULT; - hazard_status_.emergency = true; - - if (hazard_status_.emergency != prev_emergency_status) { - emergency_state_switch_time_ = this->now(); - } - - hazard_status_.diag_no_fault = std::vector(); - hazard_status_.diag_safe_fault = std::vector(); - hazard_status_.diag_latent_fault = std::vector(); - - diagnostic_msgs::msg::DiagnosticStatus diag; - diag.name = "system_error_monitor/input_data_timeout"; - diag.hardware_id = "system_error_monitor"; - diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - hazard_status_.diag_single_point_fault = - std::vector{diag}; - - // Update emergency_holding condition - if (params_.use_emergency_hold) { - const auto emergency_duration = (this->now() - emergency_state_switch_time_).seconds(); - hazard_status_.emergency_holding = emergency_duration > params_.hazard_recovery_timeout; - } -} - -bool AutowareErrorMonitor::canAutoRecovery() const -{ - const auto error_modules = getErrorModules(hazard_status_, params_.emergency_hazard_level); - for (const auto & required_module : required_modules_map_.at(current_mode_)) { - if (required_module.auto_recovery) { - continue; - } - if (error_modules.count(required_module.name) != 0) { - return false; - } - } - return true; -} - -bool AutowareErrorMonitor::isEmergencyHoldingRequired() const -{ - // Does not change holding status until emergency_holding is cleared by service call - if (hazard_status_.emergency_holding) { - return true; - } - - if (!hazard_status_.emergency) { - return false; - } - - // Don't hold status if emergency duration within recovery timeout - const auto emergency_duration = (this->now() - emergency_state_switch_time_).seconds(); - const auto within_recovery_timeout = emergency_duration < params_.hazard_recovery_timeout; - if (within_recovery_timeout && canAutoRecovery()) { - return false; - } - - // Don't hold status during manual driving - const bool is_manual_driving = - (control_mode_->mode == autoware_vehicle_msgs::msg::ControlModeReport::MANUAL); - const auto no_hold_condition = - (!params_.use_emergency_hold_in_manual_driving && is_manual_driving); - if (no_hold_condition) { - return false; - } - - return true; -} - -void AutowareErrorMonitor::publishHazardStatus( - const autoware_system_msgs::msg::HazardStatus & hazard_status) -{ - autoware_system_msgs::msg::HazardStatusStamped hazard_status_stamped; - hazard_status_stamped.stamp = this->now(); - hazard_status_stamped.status = hazard_status; - pub_hazard_status_->publish(hazard_status_stamped); - - loggingErrors(hazard_status_stamped.status); - - pub_diagnostics_err_->publish( - convertHazardStatusToDiagnosticArray(this->get_clock(), hazard_status_stamped.status)); -} - -bool AutowareErrorMonitor::onClearEmergencyService( - [[maybe_unused]] std_srvs::srv::Trigger::Request::SharedPtr request, - std_srvs::srv::Trigger::Response::SharedPtr response) -{ - hazard_status_.emergency_holding = false; - updateHazardStatus(); - response->success = true; - response->message = "Emergency Holding state was cleared."; - - return true; -} - -void AutowareErrorMonitor::loggingErrors( - const autoware_system_msgs::msg::HazardStatus & hazard_status) -{ - if ( - autoware_state_ && current_gate_mode_ && - isInNoFaultCondition(*autoware_state_, *current_gate_mode_)) { - RCLCPP_DEBUG(get_logger(), "Autoware is in no-fault condition."); - return; - } - - for (const auto & hazard_diag : hazard_status.diag_latent_fault) { - const std::string logger_name = "system_error_monitor " + hazard_diag.name; - logThrottledNamed( - logger_name, get_clock(), 5000, "[Latent Fault]: " + hazard_diag.message); - } - for (const auto & hazard_diag : hazard_status.diag_single_point_fault) { - const std::string logger_name = "system_error_monitor " + hazard_diag.name; - logThrottledNamed( - logger_name, get_clock(), 5000, "[Single Point Fault]: " + hazard_diag.message); - } -} - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(AutowareErrorMonitor) From c40efa2db4859926130c49997dd3e7446a7aa4b9 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Tue, 24 Sep 2024 13:24:11 +0900 Subject: [PATCH 33/95] fix(tier4_state_rviz_plugin): fix unmatchedSuppression (#8921) fix:unmatchedSuppression Signed-off-by: kobayu858 --- common/tier4_state_rviz_plugin/src/custom_button.cpp | 1 - common/tier4_state_rviz_plugin/src/custom_container.cpp | 1 - .../tier4_state_rviz_plugin/src/custom_segmented_button.cpp | 1 - .../src/custom_segmented_button_item.cpp | 4 ---- 4 files changed, 7 deletions(-) diff --git a/common/tier4_state_rviz_plugin/src/custom_button.cpp b/common/tier4_state_rviz_plugin/src/custom_button.cpp index 1a887497a2903..a2d4b92e17621 100644 --- a/common/tier4_state_rviz_plugin/src/custom_button.cpp +++ b/common/tier4_state_rviz_plugin/src/custom_button.cpp @@ -63,7 +63,6 @@ QSize CustomElevatedButton::minimumSizeHint() const return sizeHint(); } -// cppcheck-suppress unusedFunction void CustomElevatedButton::updateStyle( const QString & text, const QColor & bgColor, const QColor & textColor, const QColor & hoverColor, const QColor & pressedColor, const QColor & checkedColor, const QColor & disabledBgColor, diff --git a/common/tier4_state_rviz_plugin/src/custom_container.cpp b/common/tier4_state_rviz_plugin/src/custom_container.cpp index 4e7bafd044897..681cebbefd492 100644 --- a/common/tier4_state_rviz_plugin/src/custom_container.cpp +++ b/common/tier4_state_rviz_plugin/src/custom_container.cpp @@ -26,7 +26,6 @@ CustomContainer::CustomContainer(QWidget * parent) : QFrame(parent), cornerRadiu setLayout(layout); } -// cppcheck-suppress unusedFunction QGridLayout * CustomContainer::getLayout() const { return layout; // Provide access to the layout diff --git a/common/tier4_state_rviz_plugin/src/custom_segmented_button.cpp b/common/tier4_state_rviz_plugin/src/custom_segmented_button.cpp index 786b58ecf1d2e..8063bdc0fbc28 100644 --- a/common/tier4_state_rviz_plugin/src/custom_segmented_button.cpp +++ b/common/tier4_state_rviz_plugin/src/custom_segmented_button.cpp @@ -40,7 +40,6 @@ CustomSegmentedButtonItem * CustomSegmentedButton::addButton(const QString & tex return button; } -// cppcheck-suppress unusedFunction QButtonGroup * CustomSegmentedButton::getButtonGroup() const { return buttonGroup; diff --git a/common/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp b/common/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp index d0ede8ec1dfe6..cdd3aa0d25263 100644 --- a/common/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp +++ b/common/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp @@ -44,14 +44,12 @@ CustomSegmentedButtonItem::CustomSegmentedButtonItem(const QString & text, QWidg // setFixedSize(width, height); // } -// cppcheck-suppress unusedFunction void CustomSegmentedButtonItem::setHovered(bool hovered) { isHovered = hovered; updateCheckableState(); } -// cppcheck-suppress unusedFunction void CustomSegmentedButtonItem::setCheckableButton(bool checkable) { setCheckable(checkable); @@ -66,14 +64,12 @@ void CustomSegmentedButtonItem::updateCheckableState() update(); } -// cppcheck-suppress unusedFunction void CustomSegmentedButtonItem::setDisabledButton(bool disabled) { isDisabled = disabled; updateCheckableState(); } -// cppcheck-suppress unusedFunction void CustomSegmentedButtonItem::setActivated(bool activated) { isActivated = activated; From 897e2a90301c047943fd602f86672abede8536fd Mon Sep 17 00:00:00 2001 From: Prakash Kannaiah <105844116+prakash-kannaiah@users.noreply.github.com> Date: Tue, 24 Sep 2024 13:10:08 +0530 Subject: [PATCH 34/95] refactor(ad_api_adaptors): rework parameter (#8796) * refactor(ad_api_adaptors): rework parameter * * ad_api_adaptors.schema.json * style(pre-commit): autofix * refactor(ad_api_adaptors): rework parameter --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../ad_api_adaptors/README.md | 4 ++ .../schema/ad_api_adaptors.schema.json | 37 +++++++++++++++++++ 2 files changed, 41 insertions(+) create mode 100644 system/default_ad_api_helpers/ad_api_adaptors/schema/ad_api_adaptors.schema.json diff --git a/system/default_ad_api_helpers/ad_api_adaptors/README.md b/system/default_ad_api_helpers/ad_api_adaptors/README.md index c04503bee70fd..74c5e6f84e80b 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/README.md +++ b/system/default_ad_api_helpers/ad_api_adaptors/README.md @@ -29,3 +29,7 @@ The clear API is called automatically before setting the route. | Client | - | /api/routing/clear_route | The route clear API. | | Client | - | /api/routing/set_route_points | The route points set API. | | Client | - | /api/routing/change_route_points | The route points change API. | + +## parameters + +{{ json_to_markdown("/system/default_ad_api_helpers/ad_api_adaptors/schema/ad_api_adaptors.schema.json") }} diff --git a/system/default_ad_api_helpers/ad_api_adaptors/schema/ad_api_adaptors.schema.json b/system/default_ad_api_helpers/ad_api_adaptors/schema/ad_api_adaptors.schema.json new file mode 100644 index 0000000000000..bdc921e31dd23 --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_adaptors/schema/ad_api_adaptors.schema.json @@ -0,0 +1,37 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "ad_api_adaptors parameter", + "type": "object", + "definitions": { + "ad_api_adaptors": { + "type": "object", + "properties": { + "initial_pose_particle_covariance": { + "type": "array", + "description": "initial_pose_particle_covariance", + "default": [ + 4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0 + ] + } + }, + "required": ["initial_pose_particle_covariance"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/initial_pose_particle_covariance" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} From d25e9a1bbedc176d4e29c70b3ed9586808bdf813 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Tue, 24 Sep 2024 18:01:03 +0900 Subject: [PATCH 35/95] chore(compare_map_segmentation): add node tests (#8907) * chore(compare_map_segmentation): add test for voxel_based_compare_map_filter Signed-off-by: badai-nguyen * feat: add test for other compare map filter Signed-off-by: badai-nguyen * style(pre-commit): autofix --------- Signed-off-by: badai-nguyen Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../CMakeLists.txt | 24 ++++ .../package.xml | 2 + ...test_distance_based_compare_map_filter.cpp | 111 +++++++++++++++++ ...l_based_approximate_compare_map_filter.cpp | 112 ++++++++++++++++++ .../test_voxel_based_compare_map_filter.cpp | 111 +++++++++++++++++ ...oxel_distance_based_compare_map_filter.cpp | 111 +++++++++++++++++ 6 files changed, 471 insertions(+) create mode 100644 perception/autoware_compare_map_segmentation/test/test_distance_based_compare_map_filter.cpp create mode 100644 perception/autoware_compare_map_segmentation/test/test_voxel_based_approximate_compare_map_filter.cpp create mode 100644 perception/autoware_compare_map_segmentation/test/test_voxel_based_compare_map_filter.cpp create mode 100644 perception/autoware_compare_map_segmentation/test/test_voxel_distance_based_compare_map_filter.cpp diff --git a/perception/autoware_compare_map_segmentation/CMakeLists.txt b/perception/autoware_compare_map_segmentation/CMakeLists.txt index 664b3f3d8066e..f4b363ea42130 100644 --- a/perception/autoware_compare_map_segmentation/CMakeLists.txt +++ b/perception/autoware_compare_map_segmentation/CMakeLists.txt @@ -89,6 +89,30 @@ install( RUNTIME DESTINATION bin ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + ament_auto_add_gtest(test_voxel_based_compare_map_filter + test/test_voxel_based_compare_map_filter.cpp + ) + target_link_libraries(test_voxel_based_compare_map_filter ${PROJECT_NAME}) + + ament_auto_add_gtest(test_distance_based_compare_map_filter + test/test_distance_based_compare_map_filter.cpp + ) + target_link_libraries(test_distance_based_compare_map_filter ${PROJECT_NAME}) + + ament_auto_add_gtest(test_voxel_based_approximate_compare_map_filter + test/test_voxel_based_approximate_compare_map_filter.cpp + ) + target_link_libraries(test_voxel_based_approximate_compare_map_filter ${PROJECT_NAME}) + + ament_auto_add_gtest(test_voxel_distance_based_compare_map_filter + test/test_voxel_distance_based_compare_map_filter.cpp + ) + target_link_libraries(test_voxel_distance_based_compare_map_filter ${PROJECT_NAME}) + +endif() ament_auto_package( INSTALL_TO_SHARE launch diff --git a/perception/autoware_compare_map_segmentation/package.xml b/perception/autoware_compare_map_segmentation/package.xml index 32d1039f36f4e..0fc34b697e366 100644 --- a/perception/autoware_compare_map_segmentation/package.xml +++ b/perception/autoware_compare_map_segmentation/package.xml @@ -18,7 +18,9 @@ autoware_cmake autoware_map_msgs + autoware_point_types autoware_pointcloud_preprocessor + autoware_test_utils autoware_universe_utils grid_map_pcl grid_map_ros diff --git a/perception/autoware_compare_map_segmentation/test/test_distance_based_compare_map_filter.cpp b/perception/autoware_compare_map_segmentation/test/test_distance_based_compare_map_filter.cpp new file mode 100644 index 0000000000000..3d4ee272d7eaf --- /dev/null +++ b/perception/autoware_compare_map_segmentation/test/test_distance_based_compare_map_filter.cpp @@ -0,0 +1,111 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/distance_based_compare_map_filter/node.hpp" + +#include +#include +#include +#include + +#include + +#include + +using autoware::compare_map_segmentation::DistanceBasedCompareMapFilterComponent; +using autoware_point_types::PointXYZIRC; +using autoware_point_types::PointXYZIRCGenerator; +using point_cloud_msg_wrapper::PointCloud2Modifier; +using sensor_msgs::msg::PointCloud2; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + return test_manager; +}; + +std::shared_ptr generateNode( + const bool use_dynamic_map_loading) +{ + auto node_options = rclcpp::NodeOptions{}; + const auto compare_map_segmentation_dir = + ament_index_cpp::get_package_share_directory("autoware_compare_map_segmentation"); + node_options.arguments( + {"--ros-args", "--params-file", + compare_map_segmentation_dir + "/config/distance_based_compare_map_filter.param.yaml"}); + + node_options.append_parameter_override("use_dynamic_map_loading", use_dynamic_map_loading); + node_options.append_parameter_override("input_frame", "map"); + node_options.append_parameter_override("publish_debug_pcd", true); + + return std::make_unique(node_options); +}; +PointCloud2 create_pointcloud(const int number_of_point) +{ + PointCloud2 pointcloud; + PointCloud2Modifier modifier(pointcloud, "map"); + modifier.resize(number_of_point); + for (int i = 0; i < number_of_point; ++i) { + modifier[i].x = static_cast(i) / 10.0F; + modifier[i].y = 0.0F; + modifier[i].z = 0.0F; + modifier[i].intensity = 0U; + modifier[i].return_type = 0U; + modifier[i].channel = 0U; + } + return pointcloud; +} + +TEST(DistanceBasedCompareMapFilterComponentTest, testEmptyInputPointcloud) +{ + rclcpp::init(0, nullptr); + const std::string input_map_topic = "/map"; + const std::string input_pc_topic = "/input"; + const std::string output_pc_topic = "/output"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(false); + + int counter = 0; + PointCloud2 output_pc_msg; + auto callback = [&counter, &output_pc_msg](const PointCloud2::ConstSharedPtr msg) { + output_pc_msg = *msg; + ++counter; + }; + rclcpp::QoS output_qos(5); + output_qos.durability_volatile(); + output_qos.keep_last(1); + output_qos.best_effort(); + test_manager->set_subscriber(output_pc_topic, callback, output_qos); + + // create and publish map pointcloud topic + PointCloud2 input_map_pointcloud = create_pointcloud(10); + rclcpp::QoS map_qos(5); + map_qos.transient_local(); + map_qos.keep_last(1); + map_qos.reliable(); + test_manager->test_pub_msg( + test_target_node, input_map_topic, input_map_pointcloud, map_qos); + + rclcpp::QoS input_qos(10); + input_qos.durability_volatile(); + input_qos.keep_last(1); + input_qos.best_effort(); + // create and publish input pointcloud topic + PointCloud2 input_pc_msg = create_pointcloud(100); + test_manager->test_pub_msg( + test_target_node, input_pc_topic, input_pc_msg, input_qos); + EXPECT_EQ(counter, 1); + EXPECT_GT(output_pc_msg.data.size(), 0); + EXPECT_GT(input_pc_msg.data.size(), output_pc_msg.data.size()); +} diff --git a/perception/autoware_compare_map_segmentation/test/test_voxel_based_approximate_compare_map_filter.cpp b/perception/autoware_compare_map_segmentation/test/test_voxel_based_approximate_compare_map_filter.cpp new file mode 100644 index 0000000000000..e165efa640d15 --- /dev/null +++ b/perception/autoware_compare_map_segmentation/test/test_voxel_based_approximate_compare_map_filter.cpp @@ -0,0 +1,112 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/voxel_based_approximate_compare_map_filter/node.hpp" + +#include +#include +#include +#include + +#include + +#include + +using autoware::compare_map_segmentation::VoxelBasedApproximateCompareMapFilterComponent; +using autoware_point_types::PointXYZIRC; +using autoware_point_types::PointXYZIRCGenerator; +using point_cloud_msg_wrapper::PointCloud2Modifier; +using sensor_msgs::msg::PointCloud2; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + return test_manager; +}; + +std::shared_ptr generateNode( + const bool use_dynamic_map_loading) +{ + auto node_options = rclcpp::NodeOptions{}; + const auto compare_map_segmentation_dir = + ament_index_cpp::get_package_share_directory("autoware_compare_map_segmentation"); + node_options.arguments( + {"--ros-args", "--params-file", + compare_map_segmentation_dir + + "/config/voxel_based_approximate_compare_map_filter.param.yaml"}); + + node_options.append_parameter_override("use_dynamic_map_loading", use_dynamic_map_loading); + node_options.append_parameter_override("input_frame", "map"); + node_options.append_parameter_override("publish_debug_pcd", true); + + return std::make_unique(node_options); +}; +PointCloud2 create_pointcloud(const int number_of_point) +{ + PointCloud2 pointcloud; + PointCloud2Modifier modifier(pointcloud, "map"); + modifier.resize(number_of_point); + for (int i = 0; i < number_of_point; ++i) { + modifier[i].x = static_cast(i) / 10.0F; + modifier[i].y = 0.0F; + modifier[i].z = 0.0F; + modifier[i].intensity = 0U; + modifier[i].return_type = 0U; + modifier[i].channel = 0U; + } + return pointcloud; +} + +TEST(VoxelBasedApproximateCompareMapFilterComponentTest, testEmptyInputPointcloud) +{ + rclcpp::init(0, nullptr); + const std::string input_map_topic = "/map"; + const std::string input_pc_topic = "/input"; + const std::string output_pc_topic = "/output"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(false); + + int counter = 0; + PointCloud2 output_pc_msg; + auto callback = [&counter, &output_pc_msg](const PointCloud2::ConstSharedPtr msg) { + output_pc_msg = *msg; + ++counter; + }; + rclcpp::QoS output_qos(5); + output_qos.durability_volatile(); + output_qos.keep_last(1); + output_qos.best_effort(); + test_manager->set_subscriber(output_pc_topic, callback, output_qos); + + // create and publish map pointcloud topic + PointCloud2 input_map_pointcloud = create_pointcloud(10); + rclcpp::QoS map_qos(5); + map_qos.transient_local(); + map_qos.keep_last(1); + map_qos.reliable(); + test_manager->test_pub_msg( + test_target_node, input_map_topic, input_map_pointcloud, map_qos); + + rclcpp::QoS input_qos(10); + input_qos.durability_volatile(); + input_qos.keep_last(1); + input_qos.best_effort(); + // create and publish input pointcloud topic + PointCloud2 input_pc_msg = create_pointcloud(100); + test_manager->test_pub_msg( + test_target_node, input_pc_topic, input_pc_msg, input_qos); + EXPECT_EQ(counter, 1); + EXPECT_GT(output_pc_msg.data.size(), 0); + EXPECT_GT(input_pc_msg.data.size(), output_pc_msg.data.size()); +} diff --git a/perception/autoware_compare_map_segmentation/test/test_voxel_based_compare_map_filter.cpp b/perception/autoware_compare_map_segmentation/test/test_voxel_based_compare_map_filter.cpp new file mode 100644 index 0000000000000..654d7443bd8d0 --- /dev/null +++ b/perception/autoware_compare_map_segmentation/test/test_voxel_based_compare_map_filter.cpp @@ -0,0 +1,111 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/voxel_based_compare_map_filter/node.hpp" + +#include +#include +#include +#include + +#include + +#include + +using autoware::compare_map_segmentation::VoxelBasedCompareMapFilterComponent; +using autoware_point_types::PointXYZIRC; +using autoware_point_types::PointXYZIRCGenerator; +using point_cloud_msg_wrapper::PointCloud2Modifier; +using sensor_msgs::msg::PointCloud2; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + return test_manager; +}; + +std::shared_ptr generateNode( + const bool use_dynamic_map_loading) +{ + auto node_options = rclcpp::NodeOptions{}; + const auto compare_map_segmentation_dir = + ament_index_cpp::get_package_share_directory("autoware_compare_map_segmentation"); + node_options.arguments( + {"--ros-args", "--params-file", + compare_map_segmentation_dir + "/config/voxel_based_compare_map_filter.param.yaml"}); + + node_options.append_parameter_override("use_dynamic_map_loading", use_dynamic_map_loading); + node_options.append_parameter_override("input_frame", "map"); + node_options.append_parameter_override("publish_debug_pcd", true); + + return std::make_unique(node_options); +}; +PointCloud2 create_pointcloud(const int number_of_point) +{ + PointCloud2 pointcloud; + PointCloud2Modifier modifier(pointcloud, "map"); + modifier.resize(number_of_point); + for (int i = 0; i < number_of_point; ++i) { + modifier[i].x = static_cast(i) / 10.0F; + modifier[i].y = 0.0F; + modifier[i].z = 0.0F; + modifier[i].intensity = 0U; + modifier[i].return_type = 0U; + modifier[i].channel = 0U; + } + return pointcloud; +} + +TEST(VoxelBasedCompareMapFilterComponentTest, testEmptyInputPointcloud) +{ + rclcpp::init(0, nullptr); + const std::string input_map_topic = "/map"; + const std::string input_pc_topic = "/input"; + const std::string output_pc_topic = "/output"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(false); + + int counter = 0; + PointCloud2 output_pc_msg; + auto callback = [&counter, &output_pc_msg](const PointCloud2::ConstSharedPtr msg) { + output_pc_msg = *msg; + ++counter; + }; + rclcpp::QoS output_qos(5); + output_qos.durability_volatile(); + output_qos.keep_last(1); + output_qos.best_effort(); + test_manager->set_subscriber(output_pc_topic, callback, output_qos); + + // create and publish map pointcloud topic + PointCloud2 input_map_pointcloud = create_pointcloud(10); + rclcpp::QoS map_qos(5); + map_qos.transient_local(); + map_qos.keep_last(1); + map_qos.reliable(); + test_manager->test_pub_msg( + test_target_node, input_map_topic, input_map_pointcloud, map_qos); + + rclcpp::QoS input_qos(10); + input_qos.durability_volatile(); + input_qos.keep_last(1); + input_qos.best_effort(); + // create and publish input pointcloud topic + PointCloud2 input_pc_msg = create_pointcloud(100); + test_manager->test_pub_msg( + test_target_node, input_pc_topic, input_pc_msg, input_qos); + EXPECT_EQ(counter, 1); + EXPECT_GT(output_pc_msg.data.size(), 0); + EXPECT_GT(input_pc_msg.data.size(), output_pc_msg.data.size()); +} diff --git a/perception/autoware_compare_map_segmentation/test/test_voxel_distance_based_compare_map_filter.cpp b/perception/autoware_compare_map_segmentation/test/test_voxel_distance_based_compare_map_filter.cpp new file mode 100644 index 0000000000000..4440a5eddc426 --- /dev/null +++ b/perception/autoware_compare_map_segmentation/test/test_voxel_distance_based_compare_map_filter.cpp @@ -0,0 +1,111 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/voxel_distance_based_compare_map_filter/node.hpp" + +#include +#include +#include +#include + +#include + +#include + +using autoware::compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent; +using autoware_point_types::PointXYZIRC; +using autoware_point_types::PointXYZIRCGenerator; +using point_cloud_msg_wrapper::PointCloud2Modifier; +using sensor_msgs::msg::PointCloud2; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + return test_manager; +}; + +std::shared_ptr generateNode( + const bool use_dynamic_map_loading) +{ + auto node_options = rclcpp::NodeOptions{}; + const auto compare_map_segmentation_dir = + ament_index_cpp::get_package_share_directory("autoware_compare_map_segmentation"); + node_options.arguments( + {"--ros-args", "--params-file", + compare_map_segmentation_dir + "/config/voxel_distance_based_compare_map_filter.param.yaml"}); + + node_options.append_parameter_override("use_dynamic_map_loading", use_dynamic_map_loading); + node_options.append_parameter_override("input_frame", "map"); + node_options.append_parameter_override("publish_debug_pcd", true); + + return std::make_unique(node_options); +}; +PointCloud2 create_pointcloud(const int number_of_point) +{ + PointCloud2 pointcloud; + PointCloud2Modifier modifier(pointcloud, "map"); + modifier.resize(number_of_point); + for (int i = 0; i < number_of_point; ++i) { + modifier[i].x = static_cast(i) / 10.0F; + modifier[i].y = 0.0F; + modifier[i].z = 0.0F; + modifier[i].intensity = 0U; + modifier[i].return_type = 0U; + modifier[i].channel = 0U; + } + return pointcloud; +} + +TEST(VoxelDistanceBasedCompareMapFilterComponentTest, testEmptyInputPointcloud) +{ + rclcpp::init(0, nullptr); + const std::string input_map_topic = "/map"; + const std::string input_pc_topic = "/input"; + const std::string output_pc_topic = "/output"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(false); + + int counter = 0; + PointCloud2 output_pc_msg; + auto callback = [&counter, &output_pc_msg](const PointCloud2::ConstSharedPtr msg) { + output_pc_msg = *msg; + ++counter; + }; + rclcpp::QoS output_qos(5); + output_qos.durability_volatile(); + output_qos.keep_last(1); + output_qos.best_effort(); + test_manager->set_subscriber(output_pc_topic, callback, output_qos); + + // create and publish map pointcloud topic + PointCloud2 input_map_pointcloud = create_pointcloud(10); + rclcpp::QoS map_qos(5); + map_qos.transient_local(); + map_qos.keep_last(1); + map_qos.reliable(); + test_manager->test_pub_msg( + test_target_node, input_map_topic, input_map_pointcloud, map_qos); + + rclcpp::QoS input_qos(10); + input_qos.durability_volatile(); + input_qos.keep_last(1); + input_qos.best_effort(); + // create and publish input pointcloud topic + PointCloud2 input_pc_msg = create_pointcloud(100); + test_manager->test_pub_msg( + test_target_node, input_pc_topic, input_pc_msg, input_qos); + EXPECT_EQ(counter, 1); + EXPECT_GT(output_pc_msg.data.size(), 0); + EXPECT_GT(input_pc_msg.data.size(), output_pc_msg.data.size()); +} From 73a9788438ac438faa44cf9855eaaa906ec1d750 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Wed, 25 Sep 2024 11:24:19 +0900 Subject: [PATCH 36/95] docs(autonomous_emergency_braking): add missing params to README (#8950) add missing params Signed-off-by: Daniel Sanchez --- control/autoware_autonomous_emergency_braking/README.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index 66d4483fbdf84..28b288093e923 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -56,7 +56,7 @@ We do not activate AEB module if it satisfies the following conditions. - Ego vehicle is not in autonomous driving state -- When the ego vehicle is not moving (Current Velocity is very low) +- When the ego vehicle is not moving (Current Velocity is below a 0.1 m/s threshold) ### 2. Generate a predicted path of the ego vehicle @@ -201,6 +201,10 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t | voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 | | voxel_grid_y | [m] | double | down sampling parameters of y-axis for voxel grid filter | 0.05 | | voxel_grid_z | [m] | double | down sampling parameters of z-axis for voxel grid filter | 100000.0 | +| cluster tolerance | [m] | double | maximum allowable distance between any two points to be considered part of the same cluster | 0.15 | +| cluster_minimum_height | [m] | double | at least one point in a cluster must be higher than this value for the cluster to be included in the set of possible collision targets | 0.1 | +| minimum_cluster_size | [-] | int | minimum required amount of points contained by a cluster for it to be considered as a possible target obstacle | 10 | +| maximum_cluster_size | [-] | int | maximum amount of points contained by a cluster for it to be considered as a possible target obstacle | 10000 | | min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | | expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 | | longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 | From 656a78e7792ddd4ea6e1c398b2506fc385566926 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Wed, 25 Sep 2024 16:22:45 +0900 Subject: [PATCH 37/95] refactor(lane_change): refactor getLaneChangePaths function (#8909) * refactor lane change utility funcions Signed-off-by: mohammad alqudah * LC utility function to get distance to next regulatory element Signed-off-by: mohammad alqudah * don't activate LC module when close to regulatory element Signed-off-by: mohammad alqudah * modify threshold distance calculation Signed-off-by: mohammad alqudah * move regulatory element check to canTransitFailureState() function Signed-off-by: mohammad alqudah * always run LC module if approaching terminal point Signed-off-by: mohammad alqudah * use max possible LC length as threshold Signed-off-by: mohammad alqudah * update LC readme Signed-off-by: mohammad alqudah * refactor implementation Signed-off-by: mohammad alqudah * update readme Signed-off-by: mohammad alqudah * refactor checking data validity Signed-off-by: mohammad alqudah * refactor sampling of prepare phase metrics and lane changing phase metrics Signed-off-by: mohammad alqudah * add route handler function to get pose from 2d arc length Signed-off-by: mohammad alqudah * refactor candidate path generation Signed-off-by: mohammad alqudah * refactor candidate path safety check Signed-off-by: mohammad alqudah * fix variable name Signed-off-by: mohammad alqudah * Update planning/autoware_route_handler/src/route_handler.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * correct parameter name Signed-off-by: mohammad alqudah * set prepare segment velocity after taking max path velocity value Signed-off-by: mohammad alqudah * update LC README Signed-off-by: mohammad alqudah * minor changes Signed-off-by: mohammad alqudah * check phase length difference with previos valid candidate path Signed-off-by: mohammad alqudah * change logger name Signed-off-by: mohammad alqudah * change functions names to snake case Signed-off-by: mohammad alqudah * use snake case for function names Signed-off-by: mohammad alqudah * add colors to flow chart in README Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --- .../autoware/route_handler/route_handler.hpp | 3 + .../src/route_handler.cpp | 27 + .../README.md | 102 ++++ .../scene.hpp | 19 +- .../utils/calculation.hpp | 33 ++ .../utils/data_structs.hpp | 40 ++ .../utils/utils.hpp | 32 +- .../src/scene.cpp | 523 +++++++----------- .../src/utils/calculation.cpp | 126 +++++ .../src/utils/utils.cpp | 50 +- 10 files changed, 559 insertions(+), 396 deletions(-) diff --git a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp index 13e13b642af1f..6202be28e7e87 100644 --- a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp +++ b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp @@ -326,6 +326,9 @@ class RouteHandler */ lanelet::ConstLanelets getShoulderLaneletsAtPose(const Pose & pose) const; + Pose get_pose_from_2d_arc_length( + const lanelet::ConstLanelets & lanelet_sequence, const double s) const; + private: // MUST lanelet::routing::RoutingGraphPtr routing_graph_ptr_; diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index 0c0d14f95c768..616b91d1bc5f9 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -47,6 +47,8 @@ namespace autoware::route_handler { namespace { +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternionFromYaw; using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::Path; using geometry_msgs::msg::Pose; @@ -2035,4 +2037,29 @@ std::optional RouteHandler::findDrivableLanePath( if (route) return route->shortestPath(); return {}; } + +Pose RouteHandler::get_pose_from_2d_arc_length( + const lanelet::ConstLanelets & lanelet_sequence, const double s) const +{ + double accumulated_distance2d = 0; + for (const auto & llt : lanelet_sequence) { + const auto & centerline = llt.centerline(); + for (auto it = centerline.begin(); std::next(it) != centerline.end(); ++it) { + const auto pt = *it; + const auto next_pt = *std::next(it); + const double distance2d = lanelet::geometry::distance2d(to2D(pt), to2D(next_pt)); + if (accumulated_distance2d + distance2d > s) { + const double ratio = (s - accumulated_distance2d) / distance2d; + const auto interpolated_pt = pt.basicPoint() * (1 - ratio) + next_pt.basicPoint() * ratio; + const auto yaw = std::atan2(next_pt.y() - pt.y(), next_pt.x() - pt.x()); + Pose pose; + pose.position = createPoint(interpolated_pt.x(), interpolated_pt.y(), interpolated_pt.z()); + pose.orientation = createQuaternionFromYaw(yaw); + return pose; + } + accumulated_distance2d += distance2d; + } + } + return Pose{}; +} } // namespace autoware::route_handler diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 5f1c3debabcf8..18a797976161c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -24,6 +24,108 @@ The lane change candidate path is divided into two phases: preparation and lane- ![lane-change-phases](./images/lane_change-lane_change_phases.png) +The following chart illustrates the process of sampling candidate paths for lane change. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +start + +if (Is current lanes, target lanes OR target neighbor lanes polygon empty?) then (yes) + #LightPink:Return prev approved path; + stop +else (no) +endif + +:Get target objects; + +:Calculate prepare phase metrics; + +:Start loop through prepare phase metrics; + +repeat + :Get prepare segment; + + if (Is LC start point outside target lanes range) then (yes) + if (Is found a valid path) then (yes) + #Orange:Return first valid path; + stop + else (no) + #LightPink:Return prev approved path; + stop + endif + else (no) + endif + + :Calculate shift phase metrics; + + :Start loop through shift phase metrics; + repeat + :Get candidate path; + note left: Details shown in the next chart + if (Is valid candidate path?) then (yes) + :Store candidate path; + if (Is candidate path safe?) then (yes) + #LightGreen:Return candidate path; + stop + else (no) + endif + else (no) + endif + repeat while (Finished looping shift phase metrics) is (FALSE) + repeat while (Is finished looping prepare phase metrics) is (FALSE) + +if (Is found a valid path) then (yes) + #Orange:Return first valid path; + stop +else (no) +endif + +#LightPink:Return prev approved path; +stop + +@enduml +``` + +While the following chart demonstrates the process of generating a valid candidate path. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +start + +:Calculate resample interval; + +:Get reference path from target lanes; + +if (Is reference path empty?) then (yes) + #LightPink:Return; + stop +else (no) +endif + +:Get LC shift line; + +:Set lane change Info; +note left: set information from sampled prepare phase and shift phase metrics\n(duration, length, velocity, and acceleration) + +:Construct candidate path; + +if (Candidate path has enough length?) then (yes) + #LightGreen:Return valid candidate path; + stop +else (no) + #LightPink:Return; + stop +endif + +@enduml +``` + ### Preparation phase The preparation trajectory is the candidate path's first and the straight portion generated along the ego vehicle's current lane. The length of the preparation trajectory is computed as follows. diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index 3f4f290663a3f..9463c4442645c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -153,18 +153,17 @@ class NormalLaneChange : public LaneChangeBase const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const; - bool hasEnoughLengthToCrosswalk( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; + bool get_lane_change_paths(LaneChangePaths & candidate_paths) const; - bool hasEnoughLengthToIntersection( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; + LaneChangePath get_candidate_path( + const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, + const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, + const Pose & lc_start_pose, const double target_lane_length, const double shift_length, + const double next_lc_buffer, const bool is_goal_in_route) const; - bool hasEnoughLengthToTrafficLight( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; - - bool getLaneChangePaths( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - Direction direction, const bool is_stuck, LaneChangePaths * candidate_paths) const; + bool check_candidate_path_safety( + const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, + const double lane_change_buffer, const bool is_stuck) const; std::optional calcTerminalLaneChangePath( const lanelet::ConstLanelets & current_lanes, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index 32b104eb23abb..1c98fc6092a31 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -24,6 +24,7 @@ using autoware::route_handler::Direction; using autoware::route_handler::RouteHandler; using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LCParamPtr; +using behavior_path_planner::lane_change::PhaseMetrics; /** * @brief Calculates the distance from the ego vehicle to the terminal point. @@ -132,6 +133,38 @@ double calc_maximum_lane_change_length( double calc_maximum_lane_change_length( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc); + +/** + * @brief Calculates the distance required during a lane change operation. + * + * Used for computing prepare or lane change length based on current and maximum velocity, + * acceleration, and duration, returning the lesser of accelerated distance or distance at max + * velocity. + * + * @param velocity The current velocity of the vehicle in meters per second (m/s). + * @param maximum_velocity The maximum velocity the vehicle can reach in meters per second (m/s). + * @param acceleration The acceleration of the vehicle in meters per second squared (m/s^2). + * @param duration The duration of the lane change in seconds (s). + * @return The calculated minimum distance in meters (m). + */ +double calc_phase_length( + const double velocity, const double maximum_velocity, const double acceleration, + const double duration); + +double calc_lane_changing_acceleration( + const double initial_lane_changing_velocity, const double max_path_velocity, + const double lane_changing_time, const double prepare_longitudinal_acc); + +std::vector calc_prepare_phase_metrics( + const CommonDataPtr & common_data_ptr, const std::vector & prepare_durations, + const std::vector & lon_accel_values, const double current_velocity, + const double min_length_threshold = 0.0, + const double max_length_threshold = std::numeric_limits::max()); + +std::vector calc_shift_phase_metrics( + const CommonDataPtr & common_data_ptr, const double shift_length, const double initial_velocity, + const double max_velocity, const double lon_accel, + const double max_length_threshold = std::numeric_limits::max()); } // namespace autoware::behavior_path_planner::utils::lane_change::calculation #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 6aba4708ddca4..351132d9f8ce5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -193,6 +193,28 @@ struct PhaseInfo } }; +struct PhaseMetrics +{ + double duration{0.0}; + double length{0.0}; + double velocity{0.0}; + double sampled_lon_accel{0.0}; + double actual_lon_accel{0.0}; + double lat_accel{0.0}; + + PhaseMetrics( + const double _duration, const double _length, const double _velocity, + const double _sampled_lon_accel, const double _actual_lon_accel, const double _lat_accel) + : duration(_duration), + length(_length), + velocity(_velocity), + sampled_lon_accel(_sampled_lon_accel), + actual_lon_accel(_actual_lon_accel), + lat_accel(_lat_accel) + { + } +}; + struct Lanes { bool current_lane_in_goal_section{false}; @@ -216,6 +238,23 @@ struct Info double lateral_acceleration{0.0}; double terminal_lane_changing_velocity{0.0}; + + Info() = default; + Info( + const PhaseMetrics & _prep_metrics, const PhaseMetrics & _lc_metrics, + const Pose & _lc_start_pose, const Pose & _lc_end_pose, const ShiftLine & _shift_line) + { + longitudinal_acceleration = + PhaseInfo{_prep_metrics.actual_lon_accel, _lc_metrics.actual_lon_accel}; + duration = PhaseInfo{_prep_metrics.duration, _lc_metrics.duration}; + velocity = PhaseInfo{_prep_metrics.velocity, _prep_metrics.velocity}; + length = PhaseInfo{_prep_metrics.length, _lc_metrics.length}; + lane_changing_start = _lc_start_pose; + lane_changing_end = _lc_end_pose; + lateral_acceleration = _lc_metrics.lat_accel; + terminal_lane_changing_velocity = _lc_metrics.velocity; + shift_line = _shift_line; + } }; template @@ -317,6 +356,7 @@ using LaneChangeModuleType = lane_change::ModuleType; using LaneChangeParameters = lane_change::Parameters; using LaneChangeStates = lane_change::States; using LaneChangePhaseInfo = lane_change::PhaseInfo; +using LaneChangePhaseMetrics = lane_change::PhaseMetrics; using LaneChangeInfo = lane_change::Info; using FilteredByLanesObjects = lane_change::LanesObjects>; using FilteredByLanesExtendedObjects = lane_change::LanesObjects; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index bf80b492d533e..c9990747f57a7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -69,10 +69,6 @@ double calcMaximumAcceleration( const double current_velocity, const double current_max_velocity, const double max_longitudinal_acc, const LaneChangeParameters & lane_change_parameters); -double calcLaneChangingAcceleration( - const double initial_lane_changing_velocity, const double max_path_velocity, - const double lane_changing_time, const double prepare_longitudinal_acc); - void setPrepareVelocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity); @@ -99,17 +95,12 @@ bool pathFootprintExceedsTargetLaneBound( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, const double margin = 0.1); -std::optional constructCandidatePath( +std::optional construct_candidate_path( const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, - const PathWithLaneId & target_lane_reference_path, + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_lane_reference_path, const std::vector> & sorted_lane_ids); -ShiftLine getLaneChangingShiftLine( - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, - const PathWithLaneId & reference_path, const double shift_length); - -ShiftLine getLaneChangingShiftLine( +ShiftLine get_lane_changing_shift_line( const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, const PathWithLaneId & reference_path, const double shift_length); @@ -260,23 +251,6 @@ bool isWithinIntersection( */ bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); -/** - * @brief Calculates the distance required during a lane change operation. - * - * Used for computing prepare or lane change length based on current and maximum velocity, - * acceleration, and duration, returning the lesser of accelerated distance or distance at max - * velocity. - * - * @param velocity The current velocity of the vehicle in meters per second (m/s). - * @param maximum_velocity The maximum velocity the vehicle can reach in meters per second (m/s). - * @param acceleration The acceleration of the vehicle in meters per second squared (m/s^2). - * @param duration The duration of the lane change in seconds (s). - * @return The calculated minimum distance in meters (m). - */ -double calcPhaseLength( - const double velocity, const double maximum_velocity, const double acceleration, - const double time); - LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr); bool is_same_lane_with_prev_iteration( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 61f84c31c88da..e8a63c09580e1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -128,9 +128,7 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) } LaneChangePaths valid_paths{}; - const bool is_stuck = isVehicleStuck(current_lanes); - bool found_safe_path = - getLaneChangePaths(current_lanes, target_lanes, direction_, is_stuck, &valid_paths); + bool found_safe_path = get_lane_change_paths(valid_paths); // if no safe path is found and ego is stuck, try to find a path with a small margin lane_change_debug_.valid_paths = valid_paths; @@ -1351,76 +1349,31 @@ bool NormalLaneChange::hasEnoughLength( return true; } -bool NormalLaneChange::hasEnoughLengthToCrosswalk( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const +bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const { - const auto current_pose = getEgoPose(); - const auto & route_handler = *getRouteHandler(); - const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); - - const double dist_to_crosswalk_from_lane_change_start_pose = - utils::getDistanceToCrosswalk(current_pose, current_lanes, *overall_graphs_ptr) - - path.info.length.prepare; - // Check lane changing section includes crosswalk - if ( - dist_to_crosswalk_from_lane_change_start_pose > 0.0 && - dist_to_crosswalk_from_lane_change_start_pose < path.info.length.lane_changing) { - return false; - } + lane_change_debug_.collision_check_objects.clear(); - return true; -} + const auto & current_lanes = get_current_lanes(); + const auto & target_lanes = get_target_lanes(); + const auto is_stuck = isVehicleStuck(current_lanes); -bool NormalLaneChange::hasEnoughLengthToIntersection( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const -{ - const auto current_pose = getEgoPose(); - const auto & route_handler = *getRouteHandler(); - const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); + const auto is_empty = [&](const auto & data, const auto & s) { + if (!data.empty()) return false; + RCLCPP_WARN(logger_, "%s is empty. Not expected.", s); + return true; + }; - const double dist_to_intersection_from_lane_change_start_pose = - utils::getDistanceToNextIntersection(current_pose, current_lanes) - path.info.length.prepare; - // Check lane changing section includes intersection + const auto target_lane_neighbors_polygon_2d = + common_data_ptr_->lanes_polygon_ptr->target_neighbor; if ( - dist_to_intersection_from_lane_change_start_pose > 0.0 && - dist_to_intersection_from_lane_change_start_pose < path.info.length.lane_changing) { + is_empty(current_lanes, "current_lanes") || is_empty(target_lanes, "target_lanes") || + is_empty(target_lane_neighbors_polygon_2d, "target_lane_neighbors_polygon_2d")) { return false; } - return true; -} - -bool NormalLaneChange::hasEnoughLengthToTrafficLight( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const -{ - const auto current_pose = getEgoPose(); - const auto dist_to_next_traffic_light = - getDistanceToNextTrafficLight(current_pose, current_lanes); - const auto dist_to_next_traffic_light_from_lc_start_pose = - dist_to_next_traffic_light - path.info.length.prepare; - - return dist_to_next_traffic_light_from_lc_start_pose <= 0.0 || - dist_to_next_traffic_light_from_lc_start_pose >= path.info.length.lane_changing; -} - -bool NormalLaneChange::getLaneChangePaths( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - Direction direction, const bool is_stuck, LaneChangePaths * candidate_paths) const -{ - lane_change_debug_.collision_check_objects.clear(); - if (current_lanes.empty() || target_lanes.empty()) { - RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); - return false; - } const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; - const auto backward_path_length = common_parameters.backward_path_length; - const auto forward_path_length = common_parameters.forward_path_length; - const auto minimum_lane_changing_velocity = - lane_change_parameters_->minimum_lane_changing_velocity; - const auto lateral_acc_sampling_num = lane_change_parameters_->lateral_acc_sampling_num; - // get velocity const auto current_velocity = getEgoVelocity(); @@ -1437,27 +1390,23 @@ bool NormalLaneChange::getLaneChangePaths( *lane_change_parameters_, route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); - const auto dist_to_end_of_current_lanes = - calculation::calc_ego_dist_to_terminal_end(common_data_ptr_); + const auto dist_to_terminal_end = calculation::calc_ego_dist_to_terminal_end(common_data_ptr_); + const auto dist_to_target_start = + calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); + const auto dist_to_terminal_start = dist_to_terminal_end - lane_change_buffer; const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); const auto sorted_lane_ids = utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); - const auto & target_neighbor_preferred_lane_poly_2d = - common_data_ptr_->lanes_polygon_ptr->target_neighbor; - if (target_neighbor_preferred_lane_poly_2d.empty()) { - RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); - return false; - } - const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); - candidate_paths->reserve( - longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num * prepare_durations.size()); + candidate_paths.reserve( + longitudinal_acc_sampling_values.size() * lane_change_parameters_->lateral_acc_sampling_num * + prepare_durations.size()); RCLCPP_DEBUG( logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", @@ -1468,270 +1417,230 @@ bool NormalLaneChange::getLaneChangePaths( utils::lane_change::get_distance_to_next_regulatory_element(common_data_ptr_, only_tl, only_tl); const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - for (const auto & prepare_duration : prepare_durations) { - for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) { - // get path on original lanes - const auto prepare_velocity = std::clamp( - current_velocity + sampled_longitudinal_acc * prepare_duration, - minimum_lane_changing_velocity, getCommonParam().max_vel); + const auto prepare_phase_metrics = calculation::calc_prepare_phase_metrics( + common_data_ptr_, prepare_durations, longitudinal_acc_sampling_values, current_velocity, + dist_to_target_start, dist_to_terminal_start); - // compute actual longitudinal acceleration - const double longitudinal_acc_on_prepare = - (prepare_duration < 1e-3) ? 0.0 - : ((prepare_velocity - current_velocity) / prepare_duration); + auto check_length_diff = + [&](const double prep_length, const double lc_length, const bool check_lc) { + if (candidate_paths.empty()) return true; - const auto prepare_length = utils::lane_change::calcPhaseLength( - current_velocity, getCommonParam().max_vel, longitudinal_acc_on_prepare, prepare_duration); + const auto prep_diff = std::abs(candidate_paths.back().info.length.prepare - prep_length); + if (prep_diff > lane_change_parameters_->skip_process_lon_diff_th_prepare) return true; - const auto ego_dist_to_terminal_start = dist_to_end_of_current_lanes - lane_change_buffer; - if (prepare_length > ego_dist_to_terminal_start) { - RCLCPP_DEBUG( - logger_, - "Reject: Prepare length exceed distance to terminal start. prep_len: %.5f, ego dist to " - "terminal start: %.5f", - prepare_length, ego_dist_to_terminal_start); - continue; - } + if (!check_lc) return false; - if (!candidate_paths->empty()) { - const auto prev_prep_diff = candidate_paths->back().info.length.prepare - prepare_length; - if (std::abs(prev_prep_diff) < lane_change_parameters_->skip_process_lon_diff_th_prepare) { - RCLCPP_DEBUG(logger_, "Skip: Change in prepare length is less than threshold."); - continue; - } + const auto lc_diff = std::abs(candidate_paths.back().info.length.lane_changing - lc_length); + if (lc_diff > lane_change_parameters_->skip_process_lon_diff_th_lane_changing) return true; + + return false; + }; + + for (const auto & prep_metric : prepare_phase_metrics) { + const auto debug_print = [&](const auto & s) { + RCLCPP_DEBUG( + logger_, "%s | prep_time: %.5f | lon_acc: %.5f | prep_len: %.5f", s, prep_metric.duration, + prep_metric.actual_lon_accel, prep_metric.length); + }; + + if (!check_length_diff(prep_metric.length, 0.0, false)) { + RCLCPP_DEBUG(logger_, "Skip: Change in prepare length is less than threshold."); + continue; + } + + auto prepare_segment = + getPrepareSegment(current_lanes, common_parameters.backward_path_length, prep_metric.length); + + if (prepare_segment.points.empty()) { + debug_print("prepare segment is empty...? Unexpected."); + continue; + } + + // TODO(Quda, Azu): Is it possible to remove these checks if we ensure prepare segment length is + // larger than distance to target lane start + if (!is_valid_start_point(common_data_ptr_, prepare_segment.points.back().point.pose)) { + debug_print( + "Reject: lane changing start point is not within the preferred lanes or its neighbors"); + continue; + } + + // lane changing start is at the end of prepare segment + const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; + const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( + current_lanes, target_lanes.front(), lane_changing_start_pose); + + // Check if the lane changing start point is not on the lanes next to target lanes, + if (target_length_from_lane_change_start_pose > 0.0) { + debug_print("lane change start is behind target lanelet!"); + break; + } + + debug_print("Prepare path satisfy constraints"); + + const auto shift_length = + lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); + + const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, lane_changing_start_pose); + + const auto max_lane_changing_length = std::invoke([&]() { + double max_length = + dist_to_terminal_start > max_prepare_length + ? std::min(dist_to_terminal_end, dist_to_next_regulatory_element) - prep_metric.length + : dist_to_terminal_end - prep_metric.length; + auto target_lane_buffer = + lane_change_parameters_->lane_change_finish_judge_buffer + next_lane_change_buffer; + if (std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction_)) > 0) { + target_lane_buffer += lane_change_parameters_->backward_length_buffer_for_end_of_lane; } - auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length); + max_length = std::min(max_length, dist_lc_start_to_end_of_lanes - target_lane_buffer); + return max_length; + }); + + const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; + const auto lane_changing_metrics = calculation::calc_shift_phase_metrics( + common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, + prep_metric.sampled_lon_accel, max_lane_changing_length); - const auto debug_print = [&](const auto & s) { + utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prep_metric.velocity); + + for (const auto & lc_metric : lane_changing_metrics) { + const auto debug_print_lat = [&](const std::string & s) { RCLCPP_DEBUG( - logger_, "%s | prep_time: %.5f | lon_acc sampled: %.5f, actual: %.5f | prep_len: %.5f", s, - prepare_duration, sampled_longitudinal_acc, longitudinal_acc_on_prepare, prepare_length); + logger_, "%s | lc_time: %.5f | lon_acc: %.5f | lat_acc: %.5f | lc_len: %.5f", s.c_str(), + lc_metric.duration, lc_metric.actual_lon_accel, lc_metric.lat_accel, lc_metric.length); }; - if (prepare_segment.points.empty()) { - debug_print("prepare segment is empty...? Unexpected."); + if (!check_length_diff(prep_metric.length, lc_metric.length, true)) { + RCLCPP_DEBUG(logger_, "Skip: Change in lane changing length is less than threshold."); continue; } - if (!is_valid_start_point(common_data_ptr_, prepare_segment.points.back().point.pose)) { - debug_print( - "Reject: lane changing start point is not within the preferred lanes or its neighbors"); + LaneChangePath candidate_path; + try { + candidate_path = get_candidate_path( + prep_metric, lc_metric, prepare_segment, sorted_lane_ids, lane_changing_start_pose, + target_lane_length, shift_length, next_lane_change_buffer, is_goal_in_route); + } catch (const std::exception & e) { + debug_print_lat(std::string("Reject: ") + e.what()); continue; } - // lane changing start getEgoPose() is at the end of prepare segment - const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; - const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( - current_lanes, target_lanes.front(), lane_changing_start_pose); + candidate_paths.push_back(candidate_path); - // Check if the lane changing start point is not on the lanes next to target lanes, - if (target_length_from_lane_change_start_pose > 0.0) { - debug_print("lane change start getEgoPose() is behind target lanelet!"); - break; + bool is_safe = false; + try { + is_safe = + check_candidate_path_safety(candidate_path, target_objects, lane_change_buffer, is_stuck); + } catch (const std::exception & e) { + debug_print_lat(std::string("Reject: ") + e.what()); + return false; } - const auto shift_length = - lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); - - const auto initial_lane_changing_velocity = prepare_velocity; - const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; - - // get lateral acceleration range - const auto [min_lateral_acc, max_lateral_acc] = - lane_change_parameters_->lane_change_lat_acc_map.find(initial_lane_changing_velocity); - const auto lateral_acc_resolution = - std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num; - - std::vector sample_lat_acc; - constexpr double eps = 0.01; - for (double a = min_lateral_acc; a < max_lateral_acc + eps; a += lateral_acc_resolution) { - sample_lat_acc.push_back(a); + if (is_safe) { + debug_print_lat("ACCEPT!!!: it is valid and safe!"); + return true; } - RCLCPP_DEBUG(logger_, " - sampling num for lat_acc: %lu", sample_lat_acc.size()); - - debug_print("Prepare path satisfy constraints"); - const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( - common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, lane_changing_start_pose); - - for (const auto & lateral_acc : sample_lat_acc) { - const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( - shift_length, lane_change_parameters_->lane_changing_lateral_jerk, lateral_acc); - const double longitudinal_acc_on_lane_changing = - utils::lane_change::calcLaneChangingAcceleration( - initial_lane_changing_velocity, max_path_velocity, lane_changing_time, - sampled_longitudinal_acc); - const auto lane_changing_length = utils::lane_change::calcPhaseLength( - initial_lane_changing_velocity, getCommonParam().max_vel, - longitudinal_acc_on_lane_changing, lane_changing_time); - - const auto debug_print_lat = [&](const auto & s) { - RCLCPP_DEBUG( - logger_, - " - %s | lc_time: %.5f | lon_acc sampled: %.5f, actual: %.5f | lc_len: %.5f", s, - lane_changing_time, sampled_longitudinal_acc, longitudinal_acc_on_lane_changing, - lane_changing_length); - }; - if (!candidate_paths->empty()) { - const auto prev_prep_diff = candidate_paths->back().info.length.prepare - prepare_length; - const auto lc_length_diff = - candidate_paths->back().info.length.lane_changing - lane_changing_length; - - // We only check lc_length_diff if and only if the current prepare_length is equal to the - // previous prepare_length. - if ( - std::abs(prev_prep_diff) < eps && - std::abs(lc_length_diff) < - lane_change_parameters_->skip_process_lon_diff_th_lane_changing) { - RCLCPP_DEBUG(logger_, "Skip: Change in lane changing length is less than threshold."); - continue; - } - } - - if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { - debug_print_lat("Reject: length of lane changing path is longer than length to goal!!"); - continue; - } - if ( - ego_dist_to_terminal_start > max_prepare_length && - lane_changing_length + prepare_length > dist_to_next_regulatory_element) { - debug_print_lat( - "Reject: length of lane changing path is longer than length to regulatory element!!"); - continue; - } + debug_print_lat("Reject: sampled path is not safe."); + } + } - // if multiple lane change is necessary, does the remaining distance is sufficient - const auto remaining_dist_in_target = std::invoke([&]() { - const auto finish_judge_buffer = lane_change_parameters_->lane_change_finish_judge_buffer; - const auto num_to_preferred_lane_from_target_lane = - std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction)); - const auto backward_len_buffer = - lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const auto backward_buffer_to_target_lane = - num_to_preferred_lane_from_target_lane == 0 ? 0.0 : backward_len_buffer; - return lane_changing_length + finish_judge_buffer + backward_buffer_to_target_lane + - next_lane_change_buffer; - }); - - if (remaining_dist_in_target > dist_lc_start_to_end_of_lanes) { - debug_print_lat("Reject: length of lane changing path is longer than length to goal!!"); - continue; - } + RCLCPP_DEBUG(logger_, "No safety path found."); + return false; +} - const auto terminal_lane_changing_velocity = std::min( - initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time, - getCommonParam().max_vel); - utils::lane_change::setPrepareVelocity( - prepare_segment, current_velocity, terminal_lane_changing_velocity); +LaneChangePath NormalLaneChange::get_candidate_path( + const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, + const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, + const Pose & lc_start_pose, const double target_lane_length, const double shift_length, + const double next_lc_buffer, const bool is_goal_in_route) const +{ + const auto & route_handler = *getRouteHandler(); + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; + const auto & forward_path_length = planner_data_->parameters.forward_path_length; - const auto target_segment = getTargetSegment( - target_lanes, lane_changing_start_pose, target_lane_length, lane_changing_length, - initial_lane_changing_velocity, next_lane_change_buffer); + const auto resample_interval = + utils::lane_change::calcLaneChangeResampleInterval(lc_metrics.length, prep_metrics.velocity); + const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( + route_handler, target_lanes, lc_start_pose, target_lane_length, lc_metrics.length, + forward_path_length, resample_interval, is_goal_in_route, next_lc_buffer); - if (target_segment.points.empty()) { - debug_print_lat("Reject: target segment is empty!! something wrong..."); - continue; - } + if (target_lane_reference_path.points.empty()) { + throw std::logic_error("target_lane_reference_path is empty!"); + } - LaneChangeInfo lane_change_info; - lane_change_info.longitudinal_acceleration = - LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing}; - lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time}; - lane_change_info.velocity = - LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity}; - lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; - lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose; - lane_change_info.lane_changing_end = target_segment.points.front().point.pose; - lane_change_info.lateral_acceleration = lateral_acc; - lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; - - const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( - lane_changing_length, initial_lane_changing_velocity); - const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( - route_handler, target_lanes, lane_changing_start_pose, target_lane_length, - lane_changing_length, forward_path_length, resample_interval, is_goal_in_route, - next_lane_change_buffer); - - if (target_lane_reference_path.points.empty()) { - debug_print_lat("Reject: target_lane_reference_path is empty!!"); - continue; - } + const auto lc_end_pose = std::invoke([&]() { + const auto dist_to_lc_start = + lanelet::utils::getArcCoordinates(target_lanes, lc_start_pose).length; + const auto dist_to_lc_end = dist_to_lc_start + lc_metrics.length; + return route_handler.get_pose_from_2d_arc_length(target_lanes, dist_to_lc_end); + }); - lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( - prepare_segment, target_segment, target_lane_reference_path, shift_length); + const auto shift_line = utils::lane_change::get_lane_changing_shift_line( + lc_start_pose, lc_end_pose, target_lane_reference_path, shift_length); - const auto candidate_path = utils::lane_change::constructCandidatePath( - common_data_ptr_, lane_change_info, prepare_segment, target_segment, - target_lane_reference_path, sorted_lane_ids); + LaneChangeInfo lane_change_info{prep_metrics, lc_metrics, lc_start_pose, lc_end_pose, shift_line}; - if (!candidate_path) { - debug_print_lat("Reject: failed to generate candidate path!!"); - continue; - } + const auto candidate_path = utils::lane_change::construct_candidate_path( + common_data_ptr_, lane_change_info, prep_segment, target_lane_reference_path, sorted_lane_ids); - if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction)) { - debug_print_lat("Reject: invalid candidate path!!"); - continue; - } + if (!candidate_path) { + throw std::logic_error("failed to generate candidate path!"); + } - candidate_paths->push_back(*candidate_path); + if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction_)) { + throw std::logic_error("invalid candidate path length!"); + } - if ( - !is_stuck && !utils::lane_change::passed_parked_objects( - common_data_ptr_, *candidate_path, filtered_objects_.target_lane_leading, - lane_change_buffer, lane_change_debug_.collision_check_objects)) { - debug_print_lat( - "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " - "lane change."); - return false; - } + return *candidate_path; +} - if ( - lane_change_parameters_->enable_target_lane_bound_check && - utils::lane_change::pathFootprintExceedsTargetLaneBound( - common_data_ptr_, candidate_path.value().shifted_path.path, - common_parameters.vehicle_info)) { - debug_print_lat("Reject: Path footprint exceeds target lane boundary. Skip lane change."); - return false; - } +bool NormalLaneChange::check_candidate_path_safety( + const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, + const double lane_change_buffer, const bool is_stuck) const +{ + if ( + !is_stuck && !utils::lane_change::passed_parked_objects( + common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading, + lane_change_buffer, lane_change_debug_.collision_check_objects)) { + throw std::logic_error( + "Ego is not stuck and parked vehicle exists in the target lane. Skip lane change."); + } - const auto is_safe = std::invoke([&]() { - constexpr size_t decel_sampling_num = 1; - const auto safety_check_with_normal_rss = isLaneChangePathSafe( - *candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params, - decel_sampling_num, lane_change_debug_.collision_check_objects); - - if (!safety_check_with_normal_rss.is_safe && is_stuck) { - const auto safety_check_with_stuck_rss = isLaneChangePathSafe( - *candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params_for_stuck, - decel_sampling_num, lane_change_debug_.collision_check_objects); - return safety_check_with_stuck_rss.is_safe; - } - - return safety_check_with_normal_rss.is_safe; - }); - - if (is_safe) { - debug_print_lat("ACCEPT!!!: it is valid and safe!"); - return true; - } + constexpr size_t decel_sampling_num = 1; + const auto safety_check_with_normal_rss = isLaneChangePathSafe( + candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params, decel_sampling_num, + lane_change_debug_.collision_check_objects); - debug_print_lat("Reject: sampled path is not safe."); - } - } + if (!safety_check_with_normal_rss.is_safe && is_stuck) { + const auto safety_check_with_stuck_rss = isLaneChangePathSafe( + candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params_for_stuck, + decel_sampling_num, lane_change_debug_.collision_check_objects); + return safety_check_with_stuck_rss.is_safe; } - RCLCPP_DEBUG(logger_, "No safety path found."); - return false; + return safety_check_with_normal_rss.is_safe; } std::optional NormalLaneChange::calcTerminalLaneChangePath( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { - if (current_lanes.empty() || target_lanes.empty()) { - RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); + const auto is_empty = [&](const auto & data, const auto & s) { + if (!data.empty()) return false; + RCLCPP_WARN(logger_, "%s is empty. Not expected.", s); + return true; + }; + + const auto target_lane_neighbors_polygon_2d = + common_data_ptr_->lanes_polygon_ptr->target_neighbor; + if ( + is_empty(current_lanes, "current_lanes") || is_empty(target_lanes, "target_lanes") || + is_empty(target_lane_neighbors_polygon_2d, "target_lane_neighbors_polygon_2d")) { return {}; } + const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; @@ -1753,13 +1662,6 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto sorted_lane_ids = utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); - const auto target_neighbor_preferred_lane_poly_2d = - common_data_ptr_->lanes_polygon_ptr->target_neighbor; - if (target_neighbor_preferred_lane_poly_2d.empty()) { - RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); - return {}; - } - // lane changing start getEgoPose() is at the end of prepare segment const auto current_lane_terminal_point = lanelet::utils::conversion::toGeomMsgPt(current_lanes.back().centerline3d().back()); @@ -1806,15 +1708,6 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( return {}; } - const lanelet::BasicPoint2d lc_start_point( - lane_changing_start_pose->position.x, lane_changing_start_pose->position.y); - - const auto & target_lane_poly_2d = common_data_ptr_->lanes_polygon_ptr->target.value(); - - const auto is_valid_start_point = - boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || - boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); - LaneChangeInfo lane_change_info; lane_change_info.longitudinal_acceleration = LaneChangePhaseInfo{0.0, 0.0}; lane_change_info.duration = LaneChangePhaseInfo{0.0, lane_changing_time}; @@ -1826,7 +1719,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( lane_change_info.lateral_acceleration = max_lateral_acc; lane_change_info.terminal_lane_changing_velocity = minimum_lane_changing_velocity; - if (!is_valid_start_point) { + if (!is_valid_start_point(common_data_ptr_, lane_changing_start_pose.value())) { RCLCPP_DEBUG( logger_, "Reject: lane changing points are not inside of the target preferred lanes or its " @@ -1846,7 +1739,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( return {}; } - lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( + lane_change_info.shift_line = utils::lane_change::get_lane_changing_shift_line( lane_changing_start_pose.value(), target_segment.points.front().point.pose, target_lane_reference_path, shift_length); @@ -1859,9 +1752,9 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( reference_segment.points.pop_back(); reference_segment.points.back().point.longitudinal_velocity_mps = minimum_lane_changing_velocity; - const auto terminal_lane_change_path = utils::lane_change::constructCandidatePath( - common_data_ptr_, lane_change_info, reference_segment, target_segment, - target_lane_reference_path, sorted_lane_ids); + const auto terminal_lane_change_path = utils::lane_change::construct_candidate_path( + common_data_ptr_, lane_change_info, reference_segment, target_lane_reference_path, + sorted_lane_ids); return terminal_lane_change_path; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index c5424f69f84e0..bda3fc4e54ea2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -20,6 +20,14 @@ namespace autoware::behavior_path_planner::utils::lane_change::calculation { + +rclcpp::Logger get_logger() +{ + constexpr const char * name{"lane_change.utils"}; + static rclcpp::Logger logger = rclcpp::get_logger(name); + return logger; +} + double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr) { const auto & lanes_ptr = common_data_ptr->lanes_ptr; @@ -231,4 +239,122 @@ double calc_maximum_lane_change_length( return calc_maximum_lane_change_length( vel, *common_data_ptr->lc_param_ptr, shift_intervals, max_acc); } + +double calc_phase_length( + const double velocity, const double maximum_velocity, const double acceleration, + const double duration) +{ + const auto length_with_acceleration = + velocity * duration + 0.5 * acceleration * std::pow(duration, 2); + const auto length_with_max_velocity = maximum_velocity * duration; + return std::min(length_with_acceleration, length_with_max_velocity); +} + +double calc_lane_changing_acceleration( + const double initial_lane_changing_velocity, const double max_path_velocity, + const double lane_changing_time, const double prepare_longitudinal_acc) +{ + if (prepare_longitudinal_acc <= 0.0) { + return 0.0; + } + + return std::clamp( + (max_path_velocity - initial_lane_changing_velocity) / lane_changing_time, 0.0, + prepare_longitudinal_acc); +} + +std::vector calc_prepare_phase_metrics( + const CommonDataPtr & common_data_ptr, const std::vector & prepare_durations, + const std::vector & lon_accel_values, const double current_velocity, + const double min_length_threshold, const double max_length_threshold) +{ + const auto & min_lc_vel = common_data_ptr->lc_param_ptr->minimum_lane_changing_velocity; + const auto & max_vel = common_data_ptr->bpp_param_ptr->max_vel; + + std::vector metrics; + + auto is_skip = [&](const double prepare_length) { + if (prepare_length > max_length_threshold || prepare_length < min_length_threshold) { + RCLCPP_DEBUG( + get_logger(), + "Skip: prepare length out of expected range. length: %.5f, threshold min: %.5f, max: %.5f", + prepare_length, min_length_threshold, max_length_threshold); + return true; + } + return false; + }; + + metrics.reserve(prepare_durations.size() * lon_accel_values.size()); + for (const auto & prepare_duration : prepare_durations) { + for (const auto & lon_accel : lon_accel_values) { + const auto prepare_velocity = + std::clamp(current_velocity + lon_accel * prepare_duration, min_lc_vel, max_vel); + + // compute actual longitudinal acceleration + const double prepare_accel = (prepare_duration < 1e-3) + ? 0.0 + : ((prepare_velocity - current_velocity) / prepare_duration); + + const auto prepare_length = + calc_phase_length(current_velocity, max_vel, prepare_accel, prepare_duration); + + if (is_skip(prepare_length)) continue; + + metrics.emplace_back( + prepare_duration, prepare_length, prepare_velocity, lon_accel, prepare_accel, 0.0); + } + } + + return metrics; +} + +std::vector calc_shift_phase_metrics( + const CommonDataPtr & common_data_ptr, const double shift_length, const double initial_velocity, + const double max_path_velocity, const double lon_accel, const double max_length_threshold) +{ + const auto & min_lc_vel = common_data_ptr->lc_param_ptr->minimum_lane_changing_velocity; + const auto & max_vel = common_data_ptr->bpp_param_ptr->max_vel; + + // get lateral acceleration range + const auto [min_lateral_acc, max_lateral_acc] = + common_data_ptr->lc_param_ptr->lane_change_lat_acc_map.find(initial_velocity); + const auto lateral_acc_resolution = std::abs(max_lateral_acc - min_lateral_acc) / + common_data_ptr->lc_param_ptr->lateral_acc_sampling_num; + + std::vector metrics; + + auto is_skip = [&](const double lane_changing_length) { + if (lane_changing_length > max_length_threshold) { + RCLCPP_DEBUG( + get_logger(), + "Skip: lane changing length exceeds maximum threshold. length: %.5f, threshold: %.5f", + lane_changing_length, max_length_threshold); + return true; + } + return false; + }; + + for (double lat_acc = min_lateral_acc; lat_acc < max_lateral_acc + eps; + lat_acc += lateral_acc_resolution) { + const auto lane_changing_duration = PathShifter::calcShiftTimeFromJerk( + shift_length, common_data_ptr->lc_param_ptr->lane_changing_lateral_jerk, lat_acc); + + const double lane_changing_accel = calc_lane_changing_acceleration( + initial_velocity, max_path_velocity, lane_changing_duration, lon_accel); + + const auto lane_changing_length = calculation::calc_phase_length( + initial_velocity, max_vel, lane_changing_accel, lane_changing_duration); + + if (is_skip(lane_changing_length)) continue; + + const auto lane_changing_velocity = std::clamp( + initial_velocity + lane_changing_accel * lane_changing_duration, min_lc_vel, max_vel); + + metrics.emplace_back( + lane_changing_duration, lane_changing_length, lane_changing_velocity, lon_accel, + lane_changing_accel, lat_acc); + } + + return metrics; +} } // namespace autoware::behavior_path_planner::utils::lane_change::calculation diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index d163c00369b12..36aedf5c99a53 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -74,7 +74,8 @@ using tier4_planning_msgs::msg::PathPointWithLaneId; rclcpp::Logger get_logger() { constexpr const char * name{"lane_change.utils"}; - return rclcpp::get_logger(name); + static rclcpp::Logger logger = rclcpp::get_logger(name); + return logger; } double calcLaneChangeResampleInterval( @@ -105,19 +106,6 @@ double calcMaximumAcceleration( return std::clamp(acc, 0.0, max_longitudinal_acc); } -double calcLaneChangingAcceleration( - const double initial_lane_changing_velocity, const double max_path_velocity, - const double lane_changing_time, const double prepare_longitudinal_acc) -{ - if (prepare_longitudinal_acc <= 0.0) { - return 0.0; - } - - return std::clamp( - (max_path_velocity - initial_lane_changing_velocity) / lane_changing_time, 0.0, - prepare_longitudinal_acc); -} - void setPrepareVelocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity) { @@ -144,7 +132,7 @@ std::vector getAccelerationValues( } if (max_acc - min_acc < std::numeric_limits::epsilon()) { - return {0.0}; + return {min_acc}; } constexpr double epsilon = 0.001; @@ -242,10 +230,9 @@ bool pathFootprintExceedsTargetLaneBound( return false; } -std::optional constructCandidatePath( +std::optional construct_candidate_path( const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, - const PathWithLaneId & target_lane_reference_path, + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_lane_reference_path, const std::vector> & sorted_lane_ids) { const auto & shift_line = lane_change_info.shift_line; @@ -297,8 +284,8 @@ std::optional constructCandidatePath( continue; } const auto nearest_idx = - autoware::motion_utils::findNearestIndex(target_segment.points, point.point.pose); - point.lane_ids = target_segment.points.at(*nearest_idx).lane_ids; + autoware::motion_utils::findNearestIndex(target_lane_reference_path.points, point.point.pose); + point.lane_ids = target_lane_reference_path.points.at(*nearest_idx).lane_ids; } // TODO(Yutaka Shimizu): remove this flag after make the isPathInLanelets faster @@ -368,18 +355,7 @@ PathWithLaneId getReferencePathFromTargetLane( lane_changing_reference_path, resample_interval, true, {0.0, lane_changing_length}); } -ShiftLine getLaneChangingShiftLine( - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, - const PathWithLaneId & reference_path, const double shift_length) -{ - const Pose & lane_changing_start_pose = prepare_segment.points.back().point.pose; - const Pose & lane_changing_end_pose = target_segment.points.front().point.pose; - - return getLaneChangingShiftLine( - lane_changing_start_pose, lane_changing_end_pose, reference_path, shift_length); -} - -ShiftLine getLaneChangingShiftLine( +ShiftLine get_lane_changing_shift_line( const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, const PathWithLaneId & reference_path, const double shift_length) { @@ -1113,16 +1089,6 @@ bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Pol polygon, utils::toPolygon2d(lanelet::utils::to2D(lanelet.polygon2d().basicPolygon()))); } -double calcPhaseLength( - const double velocity, const double maximum_velocity, const double acceleration, - const double duration) -{ - const auto length_with_acceleration = - velocity * duration + 0.5 * acceleration * std::pow(duration, 2); - const auto length_with_max_velocity = maximum_velocity * duration; - return std::min(length_with_acceleration, length_with_max_velocity); -} - LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr) { const auto & lanes = common_data_ptr->lanes_ptr; From 9ea73bb3a86d82871b3e207ee7d65339ab96aa67 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Thu, 26 Sep 2024 09:19:59 +0900 Subject: [PATCH 38/95] chore(perception_utils): add unit test (#8414) Signed-off-by: badai-nguyen --- perception/perception_utils/CMakeLists.txt | 6 ++ perception/perception_utils/package.xml | 2 + .../perception_utils/test/test_utils.cpp | 77 +++++++++++++++++++ 3 files changed, 85 insertions(+) create mode 100644 perception/perception_utils/test/test_utils.cpp diff --git a/perception/perception_utils/CMakeLists.txt b/perception/perception_utils/CMakeLists.txt index 18ef18303fb1b..e4b581928cc5e 100644 --- a/perception/perception_utils/CMakeLists.txt +++ b/perception/perception_utils/CMakeLists.txt @@ -13,4 +13,10 @@ target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS} ) +if(BUILD_TESTING) + ament_auto_add_gtest(test_utils + test/test_utils.cpp + ) +endif() + ament_auto_package() diff --git a/perception/perception_utils/package.xml b/perception/perception_utils/package.xml index b38226991eb66..94401cc68a13e 100644 --- a/perception/perception_utils/package.xml +++ b/perception/perception_utils/package.xml @@ -14,6 +14,8 @@ libopencv-dev rclcpp + ament_cmake_gtest + ament_cmake diff --git a/perception/perception_utils/test/test_utils.cpp b/perception/perception_utils/test/test_utils.cpp new file mode 100644 index 0000000000000..d28b8489971e0 --- /dev/null +++ b/perception/perception_utils/test/test_utils.cpp @@ -0,0 +1,77 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "perception_utils/run_length_encoder.hpp" + +#include + +#include + +// Test case 1: Test if the decoded image is the same as the original image +TEST(UtilsTest, runLengthEncoderDecoderTest) +{ + int height = 10; + int width = 20; + uint8_t number_cls = 16; + // Create an image as below + // 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + // 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + // 2 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + // 3 3 3 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + // 4 4 4 4 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + // 5 5 5 5 5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + // 6 6 6 6 6 6 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + // 7 7 7 7 7 7 7 0 0 0 0 0 0 0 0 0 0 0 0 0 + // 8 8 8 8 8 8 8 8 0 0 0 0 0 0 0 0 0 0 0 0 + // 9 9 9 9 9 9 9 9 9 0 0 0 0 0 0 0 0 0 0 0 + cv::Mat image = cv::Mat::zeros(10, 20, CV_8UC1); + for (int i = 0; i < height; ++i) { + for (int j = 0; j < width; ++j) { + if (j < i) { + image.at(i, j) = i % number_cls; + } else { + image.at(i, j) = 0; + } + } + } + // Compress the image + std::vector> compressed_data = perception_utils::runLengthEncoder(image); + int step = sizeof(uint8_t) + sizeof(int); + std::vector data(compressed_data.size() * step); + for (size_t i = 0; i < compressed_data.size(); ++i) { + std::memcpy(&data[i * step], &compressed_data.at(i).first, sizeof(uint8_t)); + std::memcpy(&data[i * step + sizeof(uint8_t)], &compressed_data.at(i).second, sizeof(int)); + } + // Decompress the image + cv::Mat decoded_image = perception_utils::runLengthDecoder(data, height, width); + // Compare the original image and the decoded image + ASSERT_EQ(image.rows, decoded_image.rows); + ASSERT_EQ(image.cols, decoded_image.cols); + bool image_eq = true; + for (int i = 0; i < image.rows; ++i) { + for (int j = 0; j < image.cols; ++j) { + if (image.at(i, j) != decoded_image.at(i, j)) { + image_eq = false; + break; + } + } + } + EXPECT_EQ(image_eq, true); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From ff669fd29e2e083fa6a7d3a0ee8b6abda228c821 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Thu, 26 Sep 2024 09:55:15 +0900 Subject: [PATCH 39/95] test(raw_vehicle_cmd_converter): add tests (#8951) * remove header file according to clangd warning Signed-off-by: Go Sakayori * add test Signed-off-by: Go Sakayori * fix Signed-off-by: Go Sakayori * add test for get function Signed-off-by: Go Sakayori * apply clang tidy Signed-off-by: Go Sakayori * fix test content Signed-off-by: Go Sakayori --------- Signed-off-by: Go Sakayori Signed-off-by: Go Sakayori --- .../src/accel_map.cpp | 16 ++++------ .../src/brake_map.cpp | 7 +++-- .../src/csv_loader.cpp | 6 +--- .../src/pid.cpp | 6 ++-- .../src/steer_map.cpp | 6 ++-- .../test/map_data/test_empty_map.csv | 0 ...est_autoware_raw_vehicle_cmd_converter.cpp | 29 ++++++++++++++++++- 7 files changed, 46 insertions(+), 24 deletions(-) create mode 100644 vehicle/autoware_raw_vehicle_cmd_converter/test/map_data/test_empty_map.csv diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp index 7365c6380900f..f0c64991ec169 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp @@ -16,13 +16,9 @@ #include "autoware/interpolation/linear_interpolation.hpp" -#include -#include #include #include -using namespace std::literals::chrono_literals; - namespace autoware::raw_vehicle_cmd_converter { bool AccelMap::readAccelMapFromCSV(const std::string & csv_path, const bool validation) @@ -38,10 +34,7 @@ bool AccelMap::readAccelMapFromCSV(const std::string & csv_path, const bool vali vel_index_ = CSVLoader::getRowIndex(table); throttle_index_ = CSVLoader::getColumnIndex(table); accel_map_ = CSVLoader::getMap(table); - if (validation && !CSVLoader::validateMap(accel_map_, true)) { - return false; - } - return true; + return !validation || CSVLoader::validateMap(accel_map_, true); } bool AccelMap::getThrottle(const double acc, double vel, double & throttle) const @@ -49,7 +42,8 @@ bool AccelMap::getThrottle(const double acc, double vel, double & throttle) cons std::vector interpolated_acc_vec; const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel - for (std::vector accelerations : accel_map_) { + interpolated_acc_vec.reserve(accel_map_.size()); + for (const std::vector & accelerations : accel_map_) { interpolated_acc_vec.push_back( autoware::interpolation::lerp(vel_index_, accelerations, clamped_vel)); } @@ -58,7 +52,8 @@ bool AccelMap::getThrottle(const double acc, double vel, double & throttle) cons // When the desired acceleration is greater than the throttle area, return max throttle if (acc < interpolated_acc_vec.front()) { return false; - } else if (interpolated_acc_vec.back() < acc) { + } + if (interpolated_acc_vec.back() < acc) { throttle = throttle_index_.back(); return true; } @@ -72,6 +67,7 @@ bool AccelMap::getAcceleration(const double throttle, const double vel, double & const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel + interpolated_acc_vec.reserve(accel_map_.size()); for (const auto & acc_vec : accel_map_) { interpolated_acc_vec.push_back(autoware::interpolation::lerp(vel_index_, acc_vec, clamped_vel)); } diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp index 48660eccf8640..46dd8f9bd1fe4 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp @@ -50,7 +50,8 @@ bool BrakeMap::getBrake(const double acc, const double vel, double & brake) const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "brake: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel - for (std::vector accelerations : brake_map_) { + interpolated_acc_vec.reserve(brake_map_.size()); + for (const std::vector & accelerations : brake_map_) { interpolated_acc_vec.push_back( autoware::interpolation::lerp(vel_index_, accelerations, clamped_vel)); } @@ -66,7 +67,8 @@ bool BrakeMap::getBrake(const double acc, const double vel, double & brake) acc, interpolated_acc_vec.back()); brake = brake_index_.back(); return true; - } else if (interpolated_acc_vec.front() < acc) { + } + if (interpolated_acc_vec.front() < acc) { brake = brake_index_.front(); return true; } @@ -83,6 +85,7 @@ bool BrakeMap::getAcceleration(const double brake, const double vel, double & ac const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "brake: vel"); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel + interpolated_acc_vec.reserve(brake_map_.size()); for (const auto & acc_vec : brake_map_) { interpolated_acc_vec.push_back(autoware::interpolation::lerp(vel_index_, acc_vec, clamped_vel)); } diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/csv_loader.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/csv_loader.cpp index 8044a88bc9898..9f50e025c5b36 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/csv_loader.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/csv_loader.cpp @@ -15,7 +15,6 @@ #include "autoware_raw_vehicle_cmd_converter/csv_loader.hpp" #include -#include #include #include @@ -48,10 +47,7 @@ bool CSVLoader::readCSV(Table & result, const char delim) result.push_back(tokens); } } - if (!validateData(result, csv_path_)) { - return false; - } - return true; + return validateData(result, csv_path_); } bool CSVLoader::validateMap(const Map & map, const bool is_col_decent) diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/pid.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/pid.cpp index ebc608cd61fad..6b2bffa7630f9 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/pid.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/pid.cpp @@ -14,6 +14,8 @@ #include "autoware_raw_vehicle_cmd_converter/pid.hpp" +#include + #include #include @@ -25,7 +27,7 @@ double PIDController::calculateFB( const double current_value, std::vector & pid_contributions, std::vector & errors) { const double error = target_value - current_value; - const bool enable_integration = (std::abs(reset_trigger_value) < 0.01) ? false : true; + const bool enable_integration = std::abs(reset_trigger_value) >= 0.01; return calculatePID(error, dt, enable_integration, pid_contributions, errors, false); } @@ -48,7 +50,7 @@ double PIDController::calculatePID( } double ret_i = ki_ * error_integral_; - double error_differential; + double error_differential = 0.0; if (is_first_time_) { error_differential = 0; is_first_time_ = false; diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp index babefe96eb91d..3a458c97a4374 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp @@ -35,16 +35,14 @@ bool SteerMap::readSteerMapFromCSV(const std::string & csv_path, const bool vali steer_index_ = CSVLoader::getRowIndex(table); output_index_ = CSVLoader::getColumnIndex(table); steer_map_ = CSVLoader::getMap(table); - if (validation && !CSVLoader::validateMap(steer_map_, true)) { - return false; - } - return true; + return !validation || CSVLoader::validateMap(steer_map_, true); } void SteerMap::getSteer(const double steer_rate, const double steer, double & output) const { const double clamped_steer = CSVLoader::clampValue(steer, steer_index_, "steer: steer"); std::vector steer_rate_interp = {}; + steer_rate_interp.reserve(steer_map_.size()); for (const auto & steer_rate_vec : steer_map_) { steer_rate_interp.push_back( autoware::interpolation::lerp(steer_index_, steer_rate_vec, clamped_steer)); diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/test/map_data/test_empty_map.csv b/vehicle/autoware_raw_vehicle_cmd_converter/test/map_data/test_empty_map.csv new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/test/test_autoware_raw_vehicle_cmd_converter.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/test/test_autoware_raw_vehicle_cmd_converter.cpp index 59ab3a880fabd..0d716eef3369b 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/test/test_autoware_raw_vehicle_cmd_converter.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/test/test_autoware_raw_vehicle_cmd_converter.cpp @@ -21,7 +21,6 @@ #include "gtest/gtest.h" #include -#include /* * Throttle data: (vel, throttle -> acc) @@ -124,6 +123,9 @@ TEST(ConverterTests, LoadValidPath) EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_1col_map.csv", true)); EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_inconsistent_rows_map.csv", true)); EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_not_interpolatable.csv", true)); + EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_empty_map.csv", true)); + + EXPECT_FALSE(steer_map.readSteerMapFromCSV(map_path + "test_not_interpolatable.csv", true)); } TEST(ConverterTests, AccelMapCalculation) @@ -136,6 +138,16 @@ TEST(ConverterTests, AccelMapCalculation) return output; }; + // for get function in acceleration + std::vector map_column_idx = {0.0, 5.0, 10.0}; + std::vector map_raw_idx = {0.0, 0.5, 1.0}; + std::vector> map_value = { + {0.0, -0.3, -0.5}, {1.0, 0.5, 0.0}, {3.0, 2.0, 1.5}}; + + EXPECT_EQ(accel_map.getVelIdx(), map_column_idx); + EXPECT_EQ(accel_map.getThrottleIdx(), map_raw_idx); + EXPECT_EQ(accel_map.getAccelMap(), map_value); + // case for max vel nominal acc EXPECT_DOUBLE_EQ(calcThrottle(0.0, 20.0), 0.5); @@ -148,6 +160,9 @@ TEST(ConverterTests, AccelMapCalculation) // case for interpolation EXPECT_DOUBLE_EQ(calcThrottle(2.0, 0.0), 0.75); + // case for max throttle + EXPECT_DOUBLE_EQ(calcThrottle(2.0, 10.0), 1.0); + const auto calcAcceleration = [&](double throttle, double vel) { double output; accel_map.getAcceleration(throttle, vel, output); @@ -177,6 +192,15 @@ TEST(ConverterTests, BrakeMapCalculation) return output; }; + // for get function in brake + std::vector map_column_idx = {0.0, 5.0, 10.0}; + std::vector map_raw_idx = {0.0, 0.5, 1.0}; + std::vector> map_value = { + {0.0, -0.4, -0.5}, {-1.5, -2.0, -2.0}, {-2.0, -2.5, -3.0}}; + EXPECT_EQ(brake_map.getVelIdx(), map_column_idx); + EXPECT_EQ(brake_map.getBrakeIdx(), map_raw_idx); + EXPECT_EQ(brake_map.getBrakeMap(), map_value); + // case for min vel min acc EXPECT_DOUBLE_EQ(calcBrake(-2.5, 0.0), 1.0); @@ -189,6 +213,9 @@ TEST(ConverterTests, BrakeMapCalculation) // case for interpolation EXPECT_DOUBLE_EQ(calcBrake(-2.25, 5.0), 0.75); + // case for min brake + EXPECT_DOUBLE_EQ(calcBrake(1.0, 5.0), 0.0); + const auto calcAcceleration = [&](double brake, double vel) { double output; brake_map.getAcceleration(brake, vel, output); From 9fd0daed00c95af82e59917f62cad146f8414193 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 26 Sep 2024 10:53:13 +0900 Subject: [PATCH 40/95] fix(tier4_camera_view_rviz_plugin): fix unmatchedSuppression (#8918) fix:unmatchedSuppression Signed-off-by: kobayu858 --- .../src/bird_eye_view_controller.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp index 2368e07432031..b6637a3c619a5 100644 --- a/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp +++ b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp @@ -99,7 +99,6 @@ void BirdEyeViewController::reset() y_property_->setFloat(0); } -// cppcheck-suppress unusedFunction void BirdEyeViewController::handleMouseEvent(rviz_common::ViewportMouseEvent & event) { if (event.shift()) { From dda2f192b1c1b3b3246ee3e649a7ef1d6d0b181f Mon Sep 17 00:00:00 2001 From: Mitsuhiro Sakamoto <50359861+mitukou1109@users.noreply.github.com> Date: Thu, 26 Sep 2024 10:56:17 +0900 Subject: [PATCH 41/95] feat(autoware_universe_utils): reduce dependence on Boost.Geometry (#8592) * add find_farthest() Signed-off-by: mitukou1109 * add simplify() Signed-off-by: mitukou1109 * add envelope() Signed-off-by: mitukou1109 * (WIP) add buffer() Signed-off-by: mitukou1109 * add Polygon2d class Signed-off-by: mitukou1109 * change input type of envelope() Signed-off-by: mitukou1109 * disable convexity check until correct() supports non-convex polygons Signed-off-by: mitukou1109 * add is_clockwise() Signed-off-by: mitukou1109 * make correct() support non-convex polygons Signed-off-by: mitukou1109 * fix test case Signed-off-by: mitukou1109 * Revert "(WIP) add buffer()" This reverts commit 123b0ba85ede5e558431a4336038c14023d1bef1. Signed-off-by: mitukou1109 --------- Signed-off-by: mitukou1109 --- .../universe_utils/geometry/alt_geometry.hpp | 117 ++++--- .../src/geometry/alt_geometry.cpp | 301 ++++++++++++++---- .../test/src/geometry/test_alt_geometry.cpp | 289 ++++++++++------- 3 files changed, 490 insertions(+), 217 deletions(-) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/alt_geometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/alt_geometry.hpp index 38a498f69bbae..88af38e99000a 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/alt_geometry.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/alt_geometry.hpp @@ -17,6 +17,8 @@ #include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include +#include #include #include @@ -33,6 +35,10 @@ class Vector2d Vector2d(const double x, const double y) : x_(x), y_(y) {} + explicit Vector2d(const autoware::universe_utils::Point2d & point) : x_(point.x()), y_(point.y()) + { + } + double cross(const Vector2d & other) const { return x_ * other.y() - y_ * other.x(); } double dot(const Vector2d & other) const { return x_ * other.x() + y_ * other.y(); } @@ -60,38 +66,6 @@ class Vector2d double y_; }; -// We use Vector2d to represent points, but we do not name the class Point2d directly -// as it has some vector operation functions. -using Point2d = Vector2d; -using Points2d = std::vector; - -class ConvexPolygon2d -{ -public: - explicit ConvexPolygon2d(const Points2d & vertices) - { - if (vertices.size() < 3) { - throw std::invalid_argument("At least 3 points are required for vertices."); - } - vertices_ = vertices; - } - - explicit ConvexPolygon2d(Points2d && vertices) - { - if (vertices.size() < 3) { - throw std::invalid_argument("At least 3 points are required for vertices."); - } - vertices_ = std::move(vertices); - } - - const Points2d & vertices() const { return vertices_; } - - Points2d & vertices() { return vertices_; } - -private: - Points2d vertices_; -}; - inline Vector2d operator+(const Vector2d & v1, const Vector2d & v2) { return {v1.x() + v2.x(), v1.y() + v2.y()}; @@ -112,20 +86,74 @@ inline Vector2d operator*(const double & s, const Vector2d & v) return {s * v.x(), s * v.y()}; } -Point2d from_boost(const autoware::universe_utils::Point2d & point); +// We use Vector2d to represent points, but we do not name the class Point2d directly +// as it has some vector operation functions. +using Point2d = Vector2d; +using Points2d = std::vector; +using PointList2d = std::list; + +class Polygon2d +{ +public: + static std::optional create( + const PointList2d & outer, const std::vector & inners) noexcept; -ConvexPolygon2d from_boost(const autoware::universe_utils::Polygon2d & polygon); + static std::optional create( + PointList2d && outer, std::vector && inners) noexcept; -autoware::universe_utils::Point2d to_boost(const Point2d & point); + static std::optional create( + const autoware::universe_utils::Polygon2d & polygon) noexcept; -autoware::universe_utils::Polygon2d to_boost(const ConvexPolygon2d & polygon); + const PointList2d & outer() const noexcept { return outer_; } + + PointList2d & outer() noexcept { return outer_; } + + const std::vector & inners() const noexcept { return inners_; } + + std::vector & inners() noexcept { return inners_; } + +protected: + Polygon2d(const PointList2d & outer, const std::vector & inners) + : outer_(outer), inners_(inners) + { + } + + Polygon2d(PointList2d && outer, std::vector && inners) + : outer_(std::move(outer)), inners_(std::move(inners)) + { + } + + PointList2d outer_; + + std::vector inners_; +}; + +class ConvexPolygon2d : public Polygon2d +{ +public: + static std::optional create(const PointList2d & vertices) noexcept; + + static std::optional create(PointList2d && vertices) noexcept; + + static std::optional create( + const autoware::universe_utils::Polygon2d & polygon) noexcept; + + const PointList2d & vertices() const noexcept { return outer(); } + + PointList2d & vertices() noexcept { return outer(); } + +private: + explicit ConvexPolygon2d(const PointList2d & vertices) : Polygon2d(vertices, {}) {} + + explicit ConvexPolygon2d(PointList2d && vertices) : Polygon2d(std::move(vertices), {}) {} +}; } // namespace alt double area(const alt::ConvexPolygon2d & poly); -alt::ConvexPolygon2d convex_hull(const alt::Points2d & points); +std::optional convex_hull(const alt::Points2d & points); -void correct(alt::ConvexPolygon2d & poly); +void correct(alt::Polygon2d & poly); bool covered_by(const alt::Point2d & point, const alt::ConvexPolygon2d & poly); @@ -136,9 +164,14 @@ double distance( double distance(const alt::Point2d & point, const alt::ConvexPolygon2d & poly); +std::optional envelope(const alt::Polygon2d & poly); + bool equals(const alt::Point2d & point1, const alt::Point2d & point2); -bool equals(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & poly2); +bool equals(const alt::Polygon2d & poly1, const alt::Polygon2d & poly2); + +alt::Points2d::const_iterator find_farthest( + const alt::Points2d & points, const alt::Point2d & seg_start, const alt::Point2d & seg_end); bool intersects( const alt::Point2d & seg1_start, const alt::Point2d & seg1_end, const alt::Point2d & seg2_start, @@ -149,7 +182,11 @@ bool intersects(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & bool is_above( const alt::Point2d & point, const alt::Point2d & seg_start, const alt::Point2d & seg_end); -bool is_clockwise(const alt::ConvexPolygon2d & poly); +bool is_clockwise(const alt::PointList2d & vertices); + +bool is_convex(const alt::Polygon2d & poly); + +alt::PointList2d simplify(const alt::PointList2d & line, const double max_distance); bool touches( const alt::Point2d & point, const alt::Point2d & seg_start, const alt::Point2d & seg_end); diff --git a/common/autoware_universe_utils/src/geometry/alt_geometry.cpp b/common/autoware_universe_utils/src/geometry/alt_geometry.cpp index 3271f6d299d3d..777a3f8df4c3c 100644 --- a/common/autoware_universe_utils/src/geometry/alt_geometry.cpp +++ b/common/autoware_universe_utils/src/geometry/alt_geometry.cpp @@ -19,35 +19,105 @@ namespace autoware::universe_utils // Alternatives for Boost.Geometry ---------------------------------------------------------------- namespace alt { -Point2d from_boost(const autoware::universe_utils::Point2d & point) +std::optional Polygon2d::create( + const PointList2d & outer, const std::vector & inners) noexcept { - return {point.x(), point.y()}; + Polygon2d poly(outer, inners); + correct(poly); + + if (poly.outer().size() < 4) { + return std::nullopt; + } + + for (const auto & inner : poly.inners()) { + if (inner.size() < 4) { + return std::nullopt; + } + } + + return poly; } -ConvexPolygon2d from_boost(const autoware::universe_utils::Polygon2d & polygon) +std::optional Polygon2d::create( + PointList2d && outer, std::vector && inners) noexcept { - Points2d points; + Polygon2d poly(std::move(outer), std::move(inners)); + correct(poly); + + if (poly.outer().size() < 4) { + return std::nullopt; + } + + for (const auto & inner : poly.inners()) { + if (inner.size() < 4) { + return std::nullopt; + } + } + + return poly; +} + +std::optional Polygon2d::create( + const autoware::universe_utils::Polygon2d & polygon) noexcept +{ + PointList2d outer; for (const auto & point : polygon.outer()) { - points.push_back(from_boost(point)); + outer.push_back(Point2d(point)); + } + + std::vector inners; + for (const auto & inner : polygon.inners()) { + PointList2d _inner; + for (const auto & point : inner) { + _inner.push_back(Point2d(point)); + } + inners.push_back(_inner); } - ConvexPolygon2d _polygon(points); - correct(_polygon); - return _polygon; + return Polygon2d::create(outer, inners); } -autoware::universe_utils::Point2d to_boost(const Point2d & point) +std::optional ConvexPolygon2d::create(const PointList2d & vertices) noexcept { - return {point.x(), point.y()}; + ConvexPolygon2d poly(vertices); + correct(poly); + + if (poly.vertices().size() < 4) { + return std::nullopt; + } + + if (!is_convex(poly)) { + return std::nullopt; + } + + return poly; +} + +std::optional ConvexPolygon2d::create(PointList2d && vertices) noexcept +{ + ConvexPolygon2d poly(std::move(vertices)); + correct(poly); + + if (poly.vertices().size() < 4) { + return std::nullopt; + } + + if (!is_convex(poly)) { + return std::nullopt; + } + + return poly; } -autoware::universe_utils::Polygon2d to_boost(const ConvexPolygon2d & polygon) +std::optional ConvexPolygon2d::create( + const autoware::universe_utils::Polygon2d & polygon) noexcept { - autoware::universe_utils::Polygon2d _polygon; - for (const auto & vertex : polygon.vertices()) { - _polygon.outer().push_back(to_boost(vertex)); + PointList2d vertices; + for (const auto & point : polygon.outer()) { + vertices.push_back(Point2d(point)); } - return _polygon; + + return ConvexPolygon2d::create(vertices); } } // namespace alt @@ -56,17 +126,17 @@ double area(const alt::ConvexPolygon2d & poly) const auto & vertices = poly.vertices(); double area = 0.; - for (size_t i = 1; i < vertices.size() - 1; ++i) { - area += (vertices[i + 1] - vertices.front()).cross(vertices[i] - vertices.front()) / 2; + for (auto it = std::next(vertices.cbegin()); it != std::prev(vertices.cend(), 2); ++it) { + area += (*std::next(it) - vertices.front()).cross(*it - vertices.front()) / 2; } return area; } -alt::ConvexPolygon2d convex_hull(const alt::Points2d & points) +std::optional convex_hull(const alt::Points2d & points) { if (points.size() < 3) { - throw std::invalid_argument("At least 3 points are required for calculating convex hull."); + return std::nullopt; } // QuickHull algorithm @@ -76,7 +146,7 @@ alt::ConvexPolygon2d convex_hull(const alt::Points2d & points) const auto & p_min = *p_minmax_itr.first; const auto & p_max = *p_minmax_itr.second; - alt::Points2d vertices; + alt::PointList2d vertices; if (points.size() == 3) { std::rotate_copy( @@ -89,11 +159,7 @@ alt::ConvexPolygon2d convex_hull(const alt::Points2d & points) return; } - const auto farthest = - *std::max_element(points.begin(), points.end(), [&](const auto & a, const auto & b) { - const auto seg_vec = p2 - p1; - return seg_vec.cross(a - p1) < seg_vec.cross(b - p1); - }); + const auto & farthest = *find_farthest(points, p1, p2); alt::Points2d subset1, subset2; for (const auto & point : points) { @@ -124,23 +190,35 @@ alt::ConvexPolygon2d convex_hull(const alt::Points2d & points) make_hull(make_hull, p_max, p_min, below_points); } - alt::ConvexPolygon2d hull(vertices); - correct(hull); + auto hull = alt::ConvexPolygon2d::create(vertices); + if (!hull) { + return std::nullopt; + } return hull; } -void correct(alt::ConvexPolygon2d & poly) +void correct(alt::Polygon2d & poly) { - auto & vertices = poly.vertices(); + auto correct_vertices = [](alt::PointList2d & vertices) { + // remove adjacent duplicate points + const auto it = std::unique( + vertices.begin(), vertices.end(), + [](const auto & a, const auto & b) { return equals(a, b); }); + vertices.erase(it, vertices.end()); + + if (!equals(vertices.front(), vertices.back())) { + vertices.push_back(vertices.front()); + } - // sort points in clockwise order with respect to the first point - std::sort(vertices.begin() + 1, vertices.end(), [&](const auto & a, const auto & b) { - return (a - vertices.front()).cross(b - vertices.front()) < 0; - }); + if (!is_clockwise(vertices)) { + std::reverse(std::next(vertices.begin()), std::prev(vertices.end())); + } + }; - if (equals(vertices.front(), vertices.back())) { - vertices.pop_back(); + correct_vertices(poly.outer()); + for (auto & inner : poly.inners()) { + correct_vertices(inner); } } @@ -149,19 +227,19 @@ bool covered_by(const alt::Point2d & point, const alt::ConvexPolygon2d & poly) constexpr double epsilon = 1e-6; const auto & vertices = poly.vertices(); - const auto num_of_vertices = vertices.size(); - int64_t winding_number = 0; + std::size_t winding_number = 0; const auto [y_min_vertex, y_max_vertex] = std::minmax_element( - vertices.begin(), vertices.end(), [](const auto & a, const auto & b) { return a.y() < b.y(); }); + vertices.begin(), std::prev(vertices.end()), + [](const auto & a, const auto & b) { return a.y() < b.y(); }); if (point.y() < y_min_vertex->y() || point.y() > y_max_vertex->y()) { return false; } double cross; - for (size_t i = 0; i < num_of_vertices; ++i) { - const auto & p1 = vertices[i]; - const auto & p2 = vertices[(i + 1) % num_of_vertices]; + for (auto it = vertices.cbegin(); it != std::prev(vertices.cend()); ++it) { + const auto & p1 = *it; + const auto & p2 = *std::next(it); if (p1.y() <= point.y() && p2.y() >= point.y()) { // upward edge cross = (p2 - p1).cross(point - p1); @@ -235,26 +313,61 @@ double distance(const alt::Point2d & point, const alt::ConvexPolygon2d & poly) // TODO(mitukou1109): Use plane sweep method to improve performance? const auto & vertices = poly.vertices(); double min_distance = std::numeric_limits::max(); - for (size_t i = 0; i < vertices.size(); ++i) { - min_distance = - std::min(min_distance, distance(point, vertices[i], vertices[(i + 1) % vertices.size()])); + for (auto it = vertices.cbegin(); it != std::prev(vertices.cend()); ++it) { + min_distance = std::min(min_distance, distance(point, *it, *std::next(it))); } return min_distance; } +std::optional envelope(const alt::Polygon2d & poly) +{ + const auto [x_min_vertex, x_max_vertex] = std::minmax_element( + poly.outer().begin(), std::prev(poly.outer().end()), + [](const auto & a, const auto & b) { return a.x() < b.x(); }); + + const auto [y_min_vertex, y_max_vertex] = std::minmax_element( + poly.outer().begin(), std::prev(poly.outer().end()), + [](const auto & a, const auto & b) { return a.y() < b.y(); }); + + return alt::ConvexPolygon2d::create(alt::PointList2d{ + {x_min_vertex->x(), y_min_vertex->y()}, + {x_min_vertex->x(), y_max_vertex->y()}, + {x_max_vertex->x(), y_max_vertex->y()}, + {x_max_vertex->x(), y_min_vertex->y()}}); +} + bool equals(const alt::Point2d & point1, const alt::Point2d & point2) { constexpr double epsilon = 1e-3; return std::abs(point1.x() - point2.x()) < epsilon && std::abs(point1.y() - point2.y()) < epsilon; } -bool equals(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & poly2) +bool equals(const alt::Polygon2d & poly1, const alt::Polygon2d & poly2) { - return std::all_of(poly1.vertices().begin(), poly1.vertices().end(), [&](const auto & a) { - return std::any_of(poly2.vertices().begin(), poly2.vertices().end(), [&](const auto & b) { - return equals(a, b); - }); + const auto outer_equals = std::equal( + poly1.outer().begin(), std::prev(poly1.outer().end()), poly2.outer().begin(), + std::prev(poly2.outer().end()), [](const auto & a, const auto & b) { return equals(a, b); }); + + auto inners_equal = true; + for (const auto & inner1 : poly1.inners()) { + inners_equal &= + std::any_of(poly2.inners().begin(), poly2.inners().end(), [&](const auto & inner2) { + return std::equal( + inner1.begin(), std::prev(inner1.end()), inner2.begin(), std::prev(inner2.end()), + [](const auto & a, const auto & b) { return equals(a, b); }); + }); + } + + return outer_equals && inners_equal; +} + +alt::Points2d::const_iterator find_farthest( + const alt::Points2d & points, const alt::Point2d & seg_start, const alt::Point2d & seg_end) +{ + const auto seg_vec = seg_end - seg_start; + return std::max_element(points.begin(), points.end(), [&](const auto & a, const auto & b) { + return std::abs(seg_vec.cross(a - seg_start)) < std::abs(seg_vec.cross(b - seg_start)); }); } @@ -297,7 +410,7 @@ bool intersects(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & auto find_farthest_vertex = [](const alt::ConvexPolygon2d & poly, const alt::Vector2d & direction) { return std::max_element( - poly.vertices().begin(), poly.vertices().end(), + poly.vertices().begin(), std::prev(poly.vertices().end()), [&](const auto & a, const auto & b) { return direction.dot(a) <= direction.dot(b); }); }; return *find_farthest_vertex(poly1, direction) - *find_farthest_vertex(poly2, -direction); @@ -342,9 +455,75 @@ bool is_above( return (seg_end - seg_start).cross(point - seg_start) > 0; } -bool is_clockwise(const alt::ConvexPolygon2d & poly) +bool is_clockwise(const alt::PointList2d & vertices) { - return area(poly) > 0; + double sum = 0.; + for (auto it = vertices.cbegin(); it != std::prev(vertices.cend()); ++it) { + sum += (std::next(it)->x() - it->x()) * (std::next(it)->y() + it->y()); + } + + return sum > 0; +} + +bool is_convex(const alt::Polygon2d & poly) +{ + constexpr double epsilon = 1e-6; + + if (!poly.inners().empty()) { + return false; + } + + const auto & outer = poly.outer(); + + for (auto it = std::next(outer.cbegin()); it != std::prev(outer.cend()); ++it) { + const auto & p1 = *--it; + const auto & p2 = *it; + const auto & p3 = *++it; + + if ((p2 - p1).cross(p3 - p2) > epsilon) { + return false; + } + } + + return true; +} + +alt::PointList2d simplify(const alt::PointList2d & line, const double max_distance) +{ + if (line.size() < 3) { + return line; + } + + alt::Points2d pending(std::next(line.begin()), std::prev(line.end())); + alt::PointList2d simplified; + + // Douglas-Peucker algorithm + + auto douglas_peucker = [&max_distance, &pending, &simplified]( + auto self, const alt::Point2d & seg_start, + const alt::Point2d & seg_end) { + if (pending.empty()) { + return; + } + + const auto farthest_itr = find_farthest(pending, seg_start, seg_end); + const auto farthest = *farthest_itr; + pending.erase(farthest_itr); + + if (distance(farthest, seg_start, seg_end) <= max_distance) { + return; + } + + self(self, seg_start, farthest); + simplified.push_back(farthest); + self(self, farthest, seg_end); + }; + + simplified.push_back(line.front()); + douglas_peucker(douglas_peucker, line.front(), line.back()); + simplified.push_back(line.back()); + + return simplified; } bool touches( @@ -362,17 +541,17 @@ bool touches( bool touches(const alt::Point2d & point, const alt::ConvexPolygon2d & poly) { const auto & vertices = poly.vertices(); - const auto num_of_vertices = vertices.size(); const auto [y_min_vertex, y_max_vertex] = std::minmax_element( - vertices.begin(), vertices.end(), [](const auto & a, const auto & b) { return a.y() < b.y(); }); + vertices.begin(), std::prev(vertices.end()), + [](const auto & a, const auto & b) { return a.y() < b.y(); }); if (point.y() < y_min_vertex->y() || point.y() > y_max_vertex->y()) { return false; } - for (size_t i = 0; i < num_of_vertices; ++i) { + for (auto it = vertices.cbegin(); it != std::prev(vertices.cend()); ++it) { // check if the point is on each edge of the polygon - if (touches(point, vertices[i], vertices[(i + 1) % num_of_vertices])) { + if (touches(point, *it, *std::next(it))) { return true; } } @@ -385,19 +564,19 @@ bool within(const alt::Point2d & point, const alt::ConvexPolygon2d & poly) constexpr double epsilon = 1e-6; const auto & vertices = poly.vertices(); - const auto num_of_vertices = vertices.size(); int64_t winding_number = 0; const auto [y_min_vertex, y_max_vertex] = std::minmax_element( - vertices.begin(), vertices.end(), [](const auto & a, const auto & b) { return a.y() < b.y(); }); + vertices.begin(), std::prev(vertices.end()), + [](const auto & a, const auto & b) { return a.y() < b.y(); }); if (point.y() <= y_min_vertex->y() || point.y() >= y_max_vertex->y()) { return false; } double cross; - for (size_t i = 0; i < num_of_vertices; ++i) { - const auto & p1 = vertices[i]; - const auto & p2 = vertices[(i + 1) % num_of_vertices]; + for (auto it = vertices.cbegin(); it != std::prev(vertices.cend()); ++it) { + const auto & p1 = *it; + const auto & p2 = *std::next(it); if (p1.y() < point.y() && p2.y() > point.y()) { // upward edge cross = (p2 - p1).cross(point - p1); diff --git a/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp index cc911f8848d03..b74c747fc8054 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp @@ -35,30 +35,21 @@ TEST(alt_geometry, area) using autoware::universe_utils::alt::ConvexPolygon2d; using autoware::universe_utils::alt::Point2d; - { // Clockwise + { const Point2d p1 = {0.0, 0.0}; const Point2d p2 = {0.0, 7.0}; const Point2d p3 = {4.0, 2.0}; const Point2d p4 = {2.0, 0.0}; - const auto result = area(ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = area(ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_NEAR(result, 16.0, epsilon); } - - { // Counter-clockwise - const Point2d p1 = {0.0, 0.0}; - const Point2d p2 = {2.0, 0.0}; - const Point2d p3 = {4.0, 2.0}; - const Point2d p4 = {0.0, 7.0}; - const auto result = area(ConvexPolygon2d({p1, p2, p3, p4})); - - EXPECT_NEAR(result, -16.0, epsilon); - } } TEST(alt_geometry, convexHull) { using autoware::universe_utils::convex_hull; + using autoware::universe_utils::alt::PointList2d; using autoware::universe_utils::alt::Points2d; { @@ -76,21 +67,16 @@ TEST(alt_geometry, convexHull) points.push_back({2.9, 0.7}); const auto result = convex_hull(points); - ASSERT_EQ(result.vertices().size(), 7); - EXPECT_NEAR(result.vertices().at(0).x(), 2.0, epsilon); - EXPECT_NEAR(result.vertices().at(0).y(), 1.3, epsilon); - EXPECT_NEAR(result.vertices().at(1).x(), 2.4, epsilon); - EXPECT_NEAR(result.vertices().at(1).y(), 1.7, epsilon); - EXPECT_NEAR(result.vertices().at(2).x(), 4.1, epsilon); - EXPECT_NEAR(result.vertices().at(2).y(), 3.0, epsilon); - EXPECT_NEAR(result.vertices().at(3).x(), 5.3, epsilon); - EXPECT_NEAR(result.vertices().at(3).y(), 2.6, epsilon); - EXPECT_NEAR(result.vertices().at(4).x(), 5.4, epsilon); - EXPECT_NEAR(result.vertices().at(4).y(), 1.2, epsilon); - EXPECT_NEAR(result.vertices().at(5).x(), 4.9, epsilon); - EXPECT_NEAR(result.vertices().at(5).y(), 0.8, epsilon); - EXPECT_NEAR(result.vertices().at(6).x(), 2.9, epsilon); - EXPECT_NEAR(result.vertices().at(6).y(), 0.7, epsilon); + ASSERT_TRUE(result); + PointList2d ground_truth = {{2.0, 1.3}, {2.4, 1.7}, {4.1, 3.0}, {5.3, 2.6}, + {5.4, 1.2}, {4.9, 0.8}, {2.9, 0.7}, {2.0, 1.3}}; + ASSERT_EQ(result->vertices().size(), ground_truth.size()); + auto alt_it = result->vertices().begin(); + auto ground_truth_it = ground_truth.begin(); + for (; alt_it != result->vertices().end(); ++alt_it, ++ground_truth_it) { + EXPECT_NEAR(alt_it->x(), ground_truth_it->x(), epsilon); + EXPECT_NEAR(alt_it->y(), ground_truth_it->y(), epsilon); + } } } @@ -98,46 +84,42 @@ TEST(alt_geometry, correct) { using autoware::universe_utils::correct; using autoware::universe_utils::alt::ConvexPolygon2d; - using autoware::universe_utils::alt::Points2d; + using autoware::universe_utils::alt::PointList2d; { // Correctly oriented - Points2d vertices; + PointList2d vertices; vertices.push_back({1.0, 1.0}); vertices.push_back({1.0, -1.0}); vertices.push_back({-1.0, -1.0}); vertices.push_back({-1.0, 1.0}); - ConvexPolygon2d poly(vertices); - correct(poly); - - ASSERT_EQ(poly.vertices().size(), 4); - EXPECT_NEAR(poly.vertices().at(0).x(), 1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(0).y(), 1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(1).x(), 1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(1).y(), -1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(2).x(), -1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(2).y(), -1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(3).x(), -1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(3).y(), 1.0, epsilon); + auto poly = ConvexPolygon2d::create(vertices).value(); // correct()-ed in create() + + PointList2d ground_truth = {{1.0, 1.0}, {1.0, -1.0}, {-1.0, -1.0}, {-1.0, 1.0}, {1.0, 1.0}}; + ASSERT_EQ(poly.vertices().size(), ground_truth.size()); + auto alt_it = poly.vertices().begin(); + auto ground_truth_it = ground_truth.begin(); + for (; alt_it != poly.vertices().end(); ++alt_it, ++ground_truth_it) { + EXPECT_NEAR(alt_it->x(), ground_truth_it->x(), epsilon); + EXPECT_NEAR(alt_it->y(), ground_truth_it->y(), epsilon); + } } { // Wrongly oriented - Points2d vertices; + PointList2d vertices; vertices.push_back({1.0, 1.0}); vertices.push_back({-1.0, 1.0}); - vertices.push_back({1.0, -1.0}); vertices.push_back({-1.0, -1.0}); - ConvexPolygon2d poly(vertices); - correct(poly); - - ASSERT_EQ(poly.vertices().size(), 4); - EXPECT_NEAR(poly.vertices().at(0).x(), 1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(0).y(), 1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(1).x(), 1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(1).y(), -1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(2).x(), -1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(2).y(), -1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(3).x(), -1.0, epsilon); - EXPECT_NEAR(poly.vertices().at(3).y(), 1.0, epsilon); + vertices.push_back({1.0, -1.0}); + auto poly = ConvexPolygon2d::create(vertices).value(); // correct()-ed in create() + + PointList2d ground_truth = {{1.0, 1.0}, {1.0, -1.0}, {-1.0, -1.0}, {-1.0, 1.0}, {1.0, 1.0}}; + ASSERT_EQ(poly.vertices().size(), ground_truth.size()); + auto alt_it = poly.vertices().begin(); + auto ground_truth_it = ground_truth.begin(); + for (; alt_it != poly.vertices().end(); ++alt_it, ++ground_truth_it) { + EXPECT_NEAR(alt_it->x(), ground_truth_it->x(), epsilon); + EXPECT_NEAR(alt_it->y(), ground_truth_it->y(), epsilon); + } } } @@ -153,7 +135,7 @@ TEST(alt_geometry, coveredBy) const Point2d p2 = {1.0, -1.0}; const Point2d p3 = {-1.0, -1.0}; const Point2d p4 = {-1.0, 1.0}; - const auto result = covered_by(point, ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = covered_by(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_TRUE(result); } @@ -164,7 +146,7 @@ TEST(alt_geometry, coveredBy) const Point2d p2 = {2.0, 1.0}; const Point2d p3 = {1.0, 1.0}; const Point2d p4 = {1.0, 2.0}; - const auto result = covered_by(point, ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = covered_by(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_FALSE(result); } @@ -175,7 +157,7 @@ TEST(alt_geometry, coveredBy) const Point2d p2 = {2.0, -1.0}; const Point2d p3 = {0.0, -1.0}; const Point2d p4 = {0.0, 1.0}; - const auto result = covered_by(point, ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = covered_by(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_TRUE(result); } @@ -196,8 +178,9 @@ TEST(alt_geometry, disjoint) const Point2d p6 = {4.0, 4.0}; const Point2d p7 = {6.0, 2.0}; const Point2d p8 = {4.0, 0.0}; - const auto result = - disjoint(ConvexPolygon2d({p1, p2, p3, p4}), ConvexPolygon2d({p5, p6, p7, p8})); + const auto result = disjoint( + ConvexPolygon2d::create({p1, p2, p3, p4}).value(), + ConvexPolygon2d::create({p5, p6, p7, p8}).value()); EXPECT_TRUE(result); } @@ -211,8 +194,9 @@ TEST(alt_geometry, disjoint) const Point2d p6 = {2.0, 4.0}; const Point2d p7 = {4.0, 2.0}; const Point2d p8 = {2.0, 0.0}; - const auto result = - disjoint(ConvexPolygon2d({p1, p2, p3, p4}), ConvexPolygon2d({p5, p6, p7, p8})); + const auto result = disjoint( + ConvexPolygon2d::create({p1, p2, p3, p4}).value(), + ConvexPolygon2d::create({p5, p6, p7, p8}).value()); EXPECT_FALSE(result); } @@ -298,7 +282,7 @@ TEST(alt_geometry, distance) const Point2d p2 = {3.0, -1.0}; const Point2d p3 = {1.0, -1.0}; const Point2d p4 = {1.0, 1.0}; - const auto result = distance(p, ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = distance(p, ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_NEAR(result, 1.0, epsilon); } @@ -309,12 +293,50 @@ TEST(alt_geometry, distance) const Point2d p2 = {2.0, -1.0}; const Point2d p3 = {-1.0, -1.0}; const Point2d p4 = {-1.0, 1.0}; - const auto result = distance(p, ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = distance(p, ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_NEAR(result, 0.0, epsilon); } } +TEST(geometry, envelope) +{ + using autoware::universe_utils::envelope; + using autoware::universe_utils::alt::PointList2d; + using autoware::universe_utils::alt::Polygon2d; + + { + const auto poly = Polygon2d::create( + PointList2d{ + {2.0, 1.3}, + {2.4, 1.7}, + {2.8, 1.8}, + {3.4, 1.2}, + {3.7, 1.6}, + {3.4, 2.0}, + {4.1, 3.0}, + {5.3, 2.6}, + {5.4, 1.2}, + {4.9, 0.8}, + {2.9, 0.7}, + {2.0, 1.3}}, + {PointList2d{{4.0, 2.0}, {4.2, 1.4}, {4.8, 1.9}, {4.4, 2.2}, {4.0, 2.0}}}) + .value(); + const auto result = envelope(poly); + + ASSERT_TRUE(result); + + PointList2d ground_truth = {{2.0, 0.7}, {2.0, 3.0}, {5.4, 3.0}, {5.4, 0.7}, {2.0, 0.7}}; + ASSERT_EQ(result->vertices().size(), ground_truth.size()); + auto alt_it = result->vertices().begin(); + auto ground_truth_it = ground_truth.begin(); + for (; alt_it != result->vertices().end(); ++alt_it, ++ground_truth_it) { + EXPECT_NEAR(alt_it->x(), ground_truth_it->x(), epsilon); + EXPECT_NEAR(alt_it->y(), ground_truth_it->y(), epsilon); + } + } +} + TEST(alt_geometry, intersects) { using autoware::universe_utils::intersects; @@ -420,8 +442,9 @@ TEST(alt_geometry, intersects) const Point2d p6 = {2.0, 0.0}; const Point2d p7 = {0.0, 0.0}; const Point2d p8 = {0.0, 2.0}; - const auto result = - intersects(ConvexPolygon2d({p1, p2, p3, p4}), ConvexPolygon2d({p5, p6, p7, p8})); + const auto result = intersects( + ConvexPolygon2d::create({p1, p2, p3, p4}).value(), + ConvexPolygon2d::create({p5, p6, p7, p8}).value()); EXPECT_TRUE(result); } @@ -435,8 +458,9 @@ TEST(alt_geometry, intersects) const Point2d p6 = {3.0, 2.0}; const Point2d p7 = {2.0, 2.0}; const Point2d p8 = {2.0, 3.0}; - const auto result = - intersects(ConvexPolygon2d({p1, p2, p3, p4}), ConvexPolygon2d({p5, p6, p7, p8})); + const auto result = intersects( + ConvexPolygon2d::create({p1, p2, p3, p4}).value(), + ConvexPolygon2d::create({p5, p6, p7, p8}).value()); EXPECT_FALSE(result); } @@ -450,8 +474,9 @@ TEST(alt_geometry, intersects) const Point2d p6 = {2.0, 1.0}; const Point2d p7 = {1.0, 1.0}; const Point2d p8 = {1.0, 2.0}; - const auto result = - intersects(ConvexPolygon2d({p1, p2, p3, p4}), ConvexPolygon2d({p5, p6, p7, p8})); + const auto result = intersects( + ConvexPolygon2d::create({p1, p2, p3, p4}).value(), + ConvexPolygon2d::create({p5, p6, p7, p8}).value()); EXPECT_FALSE(result); } @@ -493,30 +518,52 @@ TEST(alt_geometry, isAbove) TEST(alt_geometry, isClockwise) { using autoware::universe_utils::is_clockwise; - using autoware::universe_utils::alt::ConvexPolygon2d; - using autoware::universe_utils::alt::Point2d; + using autoware::universe_utils::alt::PointList2d; { // Clockwise - const Point2d p1 = {0.0, 0.0}; - const Point2d p2 = {0.0, 7.0}; - const Point2d p3 = {4.0, 2.0}; - const Point2d p4 = {2.0, 0.0}; - const auto result = is_clockwise(ConvexPolygon2d({p1, p2, p3, p4})); + PointList2d vertices; + vertices.push_back({0.0, 0.0}); + vertices.push_back({0.0, 7.0}); + vertices.push_back({4.0, 2.0}); + vertices.push_back({2.0, 0.0}); + const auto result = is_clockwise(vertices); EXPECT_TRUE(result); } { // Counter-clockwise - const Point2d p1 = {0.0, 0.0}; - const Point2d p2 = {2.0, 0.0}; - const Point2d p3 = {4.0, 2.0}; - const Point2d p4 = {0.0, 7.0}; - const auto result = is_clockwise(ConvexPolygon2d({p1, p2, p3, p4})); + PointList2d vertices; + vertices.push_back({0.0, 0.0}); + vertices.push_back({2.0, 0.0}); + vertices.push_back({4.0, 2.0}); + vertices.push_back({0.0, 7.0}); + const auto result = is_clockwise(vertices); EXPECT_FALSE(result); } } +TEST(geometry, simplify) +{ + using autoware::universe_utils::simplify; + using autoware::universe_utils::alt::PointList2d; + + { + const PointList2d points = {{1.1, 1.1}, {2.5, 2.1}, {3.1, 3.1}, {4.9, 1.1}, {3.1, 1.9}}; + const double max_distance = 0.5; + const auto result = simplify(points, max_distance); + + PointList2d ground_truth = {{1.1, 1.1}, {3.1, 3.1}, {4.9, 1.1}, {3.1, 1.9}}; + ASSERT_EQ(result.size(), ground_truth.size()); + auto alt_it = result.begin(); + auto ground_truth_it = ground_truth.begin(); + for (; alt_it != result.end(); ++alt_it, ++ground_truth_it) { + EXPECT_NEAR(alt_it->x(), ground_truth_it->x(), epsilon); + EXPECT_NEAR(alt_it->y(), ground_truth_it->y(), epsilon); + } + } +} + TEST(alt_geometry, touches) { using autoware::universe_utils::touches; @@ -568,7 +615,7 @@ TEST(alt_geometry, touches) const Point2d p2 = {2.0, -1.0}; const Point2d p3 = {0.0, -1.0}; const Point2d p4 = {0.0, 1.0}; - const auto result = touches(point, ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = touches(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_TRUE(result); } @@ -579,7 +626,7 @@ TEST(alt_geometry, touches) const Point2d p2 = {2.0, 1.0}; const Point2d p3 = {1.0, 1.0}; const Point2d p4 = {1.0, 2.0}; - const auto result = touches(point, ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = touches(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_FALSE(result); } @@ -597,7 +644,7 @@ TEST(alt_geometry, within) const Point2d p2 = {1.0, -1.0}; const Point2d p3 = {-1.0, -1.0}; const Point2d p4 = {-1.0, 1.0}; - const auto result = within(point, ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = within(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_TRUE(result); } @@ -608,7 +655,7 @@ TEST(alt_geometry, within) const Point2d p2 = {2.0, 1.0}; const Point2d p3 = {1.0, 1.0}; const Point2d p4 = {1.0, 2.0}; - const auto result = within(point, ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = within(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_FALSE(result); } @@ -619,7 +666,7 @@ TEST(alt_geometry, within) const Point2d p2 = {2.0, -1.0}; const Point2d p3 = {0.0, -1.0}; const Point2d p4 = {0.0, 1.0}; - const auto result = within(point, ConvexPolygon2d({p1, p2, p3, p4})); + const auto result = within(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value()); EXPECT_FALSE(result); } @@ -633,8 +680,9 @@ TEST(alt_geometry, within) const Point2d p6 = {2.0, -2.0}; const Point2d p7 = {-2.0, -2.0}; const Point2d p8 = {-2.0, 2.0}; - const auto result = - within(ConvexPolygon2d({p1, p2, p3, p4}), ConvexPolygon2d({p5, p6, p7, p8})); + const auto result = within( + ConvexPolygon2d::create({p1, p2, p3, p4}).value(), + ConvexPolygon2d::create({p5, p6, p7, p8}).value()); EXPECT_TRUE(result); } @@ -648,8 +696,9 @@ TEST(alt_geometry, within) const Point2d p6 = {3.0, 2.0}; const Point2d p7 = {2.0, 2.0}; const Point2d p8 = {2.0, 3.0}; - const auto result = - within(ConvexPolygon2d({p1, p2, p3, p4}), ConvexPolygon2d({p5, p6, p7, p8})); + const auto result = within( + ConvexPolygon2d::create({p1, p2, p3, p4}).value(), + ConvexPolygon2d::create({p5, p6, p7, p8}).value()); EXPECT_FALSE(result); } @@ -663,8 +712,9 @@ TEST(alt_geometry, within) const Point2d p6 = {1.0, -1.0}; const Point2d p7 = {-1.0, -1.0}; const Point2d p8 = {-1.0, 1.0}; - const auto result = - within(ConvexPolygon2d({p1, p2, p3, p4}), ConvexPolygon2d({p5, p6, p7, p8})); + const auto result = within( + ConvexPolygon2d::create({p1, p2, p3, p4}).value(), + ConvexPolygon2d::create({p5, p6, p7, p8}).value()); EXPECT_TRUE(result); } @@ -691,7 +741,8 @@ TEST(alt_geometry, areaRand) const auto ground_truth = boost::geometry::area(polygons[i]); ground_truth_area_ns += sw.toc(); - const auto alt_poly = autoware::universe_utils::alt::from_boost(polygons[i]); + const auto alt_poly = + autoware::universe_utils::alt::ConvexPolygon2d::create(polygons[i]).value(); sw.tic(); const auto alt = autoware::universe_utils::area(alt_poly); alt_area_ns += sw.toc(); @@ -735,21 +786,19 @@ TEST(alt_geometry, convexHullRand) boost::geometry::convex_hull(outer, ground_truth); ground_truth_hull_ns += sw.toc(); - const auto vertices = autoware::universe_utils::alt::from_boost(polygons[i]).vertices(); + const auto vertices = + autoware::universe_utils::alt::ConvexPolygon2d::create(polygons[i]).value().vertices(); sw.tic(); - const auto alt = autoware::universe_utils::convex_hull(vertices); + const auto alt = autoware::universe_utils::convex_hull({vertices.begin(), vertices.end()}); alt_hull_ns += sw.toc(); - if (ground_truth.outer().size() - 1 != alt.vertices().size()) { - std::cout << "Alt failed for the polygon: "; - std::cout << boost::geometry::wkt(polygons[i]) << std::endl; - } - ASSERT_EQ( - ground_truth.outer().size() - 1, - alt.vertices().size()); // alt::ConvexPolygon2d does not have closing point - for (size_t i = 0; i < alt.vertices().size(); ++i) { - EXPECT_NEAR(ground_truth.outer().at(i).x(), alt.vertices().at(i).x(), epsilon); - EXPECT_NEAR(ground_truth.outer().at(i).y(), alt.vertices().at(i).y(), epsilon); + ASSERT_TRUE(alt); + ASSERT_EQ(ground_truth.outer().size(), alt->vertices().size()); + auto ground_truth_it = ground_truth.outer().begin(); + auto alt_it = alt->vertices().begin(); + for (; ground_truth_it != ground_truth.outer().end(); ++ground_truth_it, ++alt_it) { + EXPECT_NEAR(ground_truth_it->x(), alt_it->x(), epsilon); + EXPECT_NEAR(ground_truth_it->y(), alt_it->y(), epsilon); } } std::printf("polygons_nb = %d, vertices = %ld\n", polygons_nb, vertices); @@ -790,8 +839,9 @@ TEST(alt_geometry, coveredByRand) ground_truth_not_covered_ns += sw.toc(); } - const auto alt_point = autoware::universe_utils::alt::from_boost(point); - const auto alt_poly = autoware::universe_utils::alt::from_boost(polygons[j]); + const auto alt_point = autoware::universe_utils::alt::Point2d(point); + const auto alt_poly = + autoware::universe_utils::alt::ConvexPolygon2d::create(polygons[j]).value(); sw.tic(); const auto alt = autoware::universe_utils::covered_by(alt_point, alt_poly); if (alt) { @@ -855,8 +905,10 @@ TEST(alt_geometry, disjointRand) ground_truth_not_disjoint_ns += sw.toc(); } - const auto alt_poly1 = autoware::universe_utils::alt::from_boost(polygons[i]); - const auto alt_poly2 = autoware::universe_utils::alt::from_boost(polygons[j]); + const auto alt_poly1 = + autoware::universe_utils::alt::ConvexPolygon2d::create(polygons[i]).value(); + const auto alt_poly2 = + autoware::universe_utils::alt::ConvexPolygon2d::create(polygons[j]).value(); sw.tic(); const auto alt = autoware::universe_utils::disjoint(alt_poly1, alt_poly2); if (alt) { @@ -919,8 +971,10 @@ TEST(alt_geometry, intersectsRand) ground_truth_no_intersect_ns += sw.toc(); } - const auto alt_poly1 = autoware::universe_utils::alt::from_boost(polygons[i]); - const auto alt_poly2 = autoware::universe_utils::alt::from_boost(polygons[j]); + const auto alt_poly1 = + autoware::universe_utils::alt::ConvexPolygon2d::create(polygons[i]).value(); + const auto alt_poly2 = + autoware::universe_utils::alt::ConvexPolygon2d::create(polygons[j]).value(); sw.tic(); const auto alt = autoware::universe_utils::intersects(alt_poly1, alt_poly2); if (alt) { @@ -984,8 +1038,9 @@ TEST(alt_geometry, touchesRand) ground_truth_not_touching_ns += sw.toc(); } - const auto alt_point = autoware::universe_utils::alt::from_boost(point); - const auto alt_poly = autoware::universe_utils::alt::from_boost(polygons[j]); + const auto alt_point = autoware::universe_utils::alt::Point2d(point); + const auto alt_poly = + autoware::universe_utils::alt::ConvexPolygon2d::create(polygons[j]).value(); sw.tic(); const auto alt = autoware::universe_utils::touches(alt_point, alt_poly); if (alt) { @@ -1049,8 +1104,10 @@ TEST(alt_geometry, withinPolygonRand) ground_truth_not_within_ns += sw.toc(); } - const auto alt_poly1 = autoware::universe_utils::alt::from_boost(polygons[i]); - const auto alt_poly2 = autoware::universe_utils::alt::from_boost(polygons[j]); + const auto alt_poly1 = + autoware::universe_utils::alt::ConvexPolygon2d::create(polygons[i]).value(); + const auto alt_poly2 = + autoware::universe_utils::alt::ConvexPolygon2d::create(polygons[j]).value(); sw.tic(); const auto alt = autoware::universe_utils::within(alt_poly1, alt_poly2); if (alt) { From 8f0e511de4c54d2856a92c0e0b9c64b28edb88e3 Mon Sep 17 00:00:00 2001 From: Ismet Atabay <56237550+ismetatabay@users.noreply.github.com> Date: Thu, 26 Sep 2024 07:35:36 +0300 Subject: [PATCH 42/95] feat(autonomous_emergency_braking): calculate the object's velocity in the search area (#8591) * refactor PR Signed-off-by: ismetatabay * WIP Signed-off-by: ismetatabay * change using polygon to lateral offset Signed-off-by: ismetatabay * improve code Signed-off-by: ismetatabay * remove redundant code Signed-off-by: ismetatabay * skip close points in MPC path generation Signed-off-by: ismetatabay * fix empty path points in short parking scenario Signed-off-by: ismetatabay * fix readme conflicts Signed-off-by: ismetatabay --------- Signed-off-by: ismetatabay --- .../README.md | 65 ++--- .../autonomous_emergency_braking.param.yaml | 1 + .../speed_calculation_expansion.drawio.svg | 234 ++++++++++++++++++ .../autonomous_emergency_braking/node.hpp | 8 +- .../package.xml | 1 + .../src/node.cpp | 160 +++++++----- .../test/test.cpp | 5 +- 7 files changed, 385 insertions(+), 89 deletions(-) create mode 100644 control/autoware_autonomous_emergency_braking/image/speed_calculation_expansion.drawio.svg diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index 28b288093e923..c3583982dde39 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -112,6 +112,14 @@ Once all target obstacles have been identified, the AEB module chooses the point ### 4. Obstacle velocity estimation +To begin calculating the target point's velocity, the point must enter the speed calculation area, +which is defined by the `speed_calculation_expansion_margin` parameter. +Depending on the operational environment, +this margin can reduce unnecessary autonomous emergency braking +caused by velocity miscalculations during the initial calculation steps. + +![speed_calculation_expansion](./image/speed_calculation_expansion.drawio.svg) + Once the position of the closest obstacle/point is determined, the AEB modules uses the history of previously detected objects to estimate the closest object relative speed using the following equations: $$ @@ -188,34 +196,35 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t ## Parameters -| Name | Unit | Type | Description | Default value | -| :-------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| publish_debug_markers | [-] | bool | flag to publish debug markers | true | -| publish_debug_pointcloud | [-] | bool | flag to publish the point cloud used for debugging | false | -| use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | -| use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | -| use_object_velocity_calculation | [-] | bool | flag to use the object velocity calculation. If set to false, object velocity is set to 0 [m/s] | true | -| check_autoware_state | [-] | bool | flag to enable or disable autoware state check. If set to false, the AEB module will run even when the ego vehicle is not in AUTONOMOUS state. | true | -| detection_range_min_height | [m] | double | minimum hight of detection range used for avoiding the ghost brake by false positive point clouds | 0.0 | -| detection_range_max_height_margin | [m] | double | margin for maximum hight of detection range used for avoiding the ghost brake by false positive point clouds. `detection_range_max_height = vehicle_height + detection_range_max_height_margin` | 0.0 | -| voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 | -| voxel_grid_y | [m] | double | down sampling parameters of y-axis for voxel grid filter | 0.05 | -| voxel_grid_z | [m] | double | down sampling parameters of z-axis for voxel grid filter | 100000.0 | -| cluster tolerance | [m] | double | maximum allowable distance between any two points to be considered part of the same cluster | 0.15 | -| cluster_minimum_height | [m] | double | at least one point in a cluster must be higher than this value for the cluster to be included in the set of possible collision targets | 0.1 | -| minimum_cluster_size | [-] | int | minimum required amount of points contained by a cluster for it to be considered as a possible target obstacle | 10 | -| maximum_cluster_size | [-] | int | maximum amount of points contained by a cluster for it to be considered as a possible target obstacle | 10000 | -| min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | -| expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 | -| longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 | -| t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 | -| a_ego_min | [m/ss] | double | maximum deceleration value of the ego vehicle | -3.0 | -| a_obj_min | [m/ss] | double | maximum deceleration value of objects | -3.0 | -| imu_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by sensors | 1.5 | -| imu_prediction_time_interval | [s] | double | time interval of the predicted path generated by sensors | 0.1 | -| mpc_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by mpc | 1.5 | -| mpc_prediction_time_interval | [s] | double | time interval of the predicted path generated by mpc | 0.1 | -| aeb_hz | [-] | double | frequency at which AEB operates per second | 10 | +| Name | Unit | Type | Description | Default value | +| :--------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| publish_debug_markers | [-] | bool | flag to publish debug markers | true | +| publish_debug_pointcloud | [-] | bool | flag to publish the point cloud used for debugging | false | +| use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | +| use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | +| use_object_velocity_calculation | [-] | bool | flag to use the object velocity calculation. If set to false, object velocity is set to 0 [m/s] | true | +| check_autoware_state | [-] | bool | flag to enable or disable autoware state check. If set to false, the AEB module will run even when the ego vehicle is not in AUTONOMOUS state. | true | +| detection_range_min_height | [m] | double | minimum hight of detection range used for avoiding the ghost brake by false positive point clouds | 0.0 | +| detection_range_max_height_margin | [m] | double | margin for maximum hight of detection range used for avoiding the ghost brake by false positive point clouds. `detection_range_max_height = vehicle_height + detection_range_max_height_margin` | 0.0 | +| voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 | +| voxel_grid_y | [m] | double | down sampling parameters of y-axis for voxel grid filter | 0.05 | +| voxel_grid_z | [m] | double | down sampling parameters of z-axis for voxel grid filter | 100000.0 | +| cluster tolerance | [m] | double | maximum allowable distance between any two points to be considered part of the same cluster | 0.15 | +| cluster_minimum_height | [m] | double | at least one point in a cluster must be higher than this value for the cluster to be included in the set of possible collision targets | 0.1 | +| minimum_cluster_size | [-] | int | minimum required amount of points contained by a cluster for it to be considered as a possible target obstacle | 10 | +| maximum_cluster_size | [-] | int | maximum amount of points contained by a cluster for it to be considered as a possible target obstacle | 10000 | +| min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | +| expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 | +| longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 | +| t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 | +| a_ego_min | [m/ss] | double | maximum deceleration value of the ego vehicle | -3.0 | +| a_obj_min | [m/ss] | double | maximum deceleration value of objects | -3.0 | +| imu_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by sensors | 1.5 | +| imu_prediction_time_interval | [s] | double | time interval of the predicted path generated by sensors | 0.1 | +| mpc_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by mpc | 1.5 | +| mpc_prediction_time_interval | [s] | double | time interval of the predicted path generated by mpc | 0.1 | +| aeb_hz | [-] | double | frequency at which AEB operates per second | 10 | +| speed_calculation_expansion_margin | [m] | double | expansion width of the ego vehicle for the beginning speed calculation | 0.1 | ## Limitations diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index d5c6356c38897..75b54fe547e32 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -27,6 +27,7 @@ # Point cloud cropping expand_width: 0.1 path_footprint_extra_margin: 4.0 + speed_calculation_expansion_margin: 0.5 # Point cloud clustering cluster_tolerance: 0.15 #[m] diff --git a/control/autoware_autonomous_emergency_braking/image/speed_calculation_expansion.drawio.svg b/control/autoware_autonomous_emergency_braking/image/speed_calculation_expansion.drawio.svg new file mode 100644 index 0000000000000..37728253370e4 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/image/speed_calculation_expansion.drawio.svg @@ -0,0 +1,234 @@ + + + + + + + + + + + + + + + + +
+
+
+ speed_calculation_expansion_margin +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ speed_calculation_expansion_margin +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Closest Point +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index bc45e6049abd7..6daa0a34dbaf4 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #include @@ -87,6 +88,7 @@ struct ObjectData double velocity{0.0}; double rss{0.0}; double distance_to_object{0.0}; + bool is_target{true}; }; /** @@ -342,6 +344,7 @@ class AEB : public rclcpp::Node rclcpp::Publisher::SharedPtr info_marker_publisher_; rclcpp::Publisher::SharedPtr debug_processing_time_detail_pub_; + rclcpp::Publisher::SharedPtr debug_rss_distance_publisher_; // timer rclcpp::TimerBase::SharedPtr timer_; mutable std::shared_ptr time_keeper_{nullptr}; @@ -436,12 +439,14 @@ class AEB : public rclcpp::Node * @brief Create object data using point cloud clusters * @param ego_path Ego vehicle path * @param ego_polys Polygons representing the ego vehicle footprint + * @param speed_calc_ego_polys Polygons representing the expanded ego vehicle footprint for speed + * calculation area * @param stamp Timestamp of the data * @param objects Vector to store the created object data * @param obstacle_points_ptr Pointer to the point cloud of obstacles */ void getClosestObjectsOnPath( - const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, + const Path & ego_path, const rclcpp::Time & stamp, const PointCloud::Ptr points_belonging_to_cluster_hulls, std::vector & objects); /** @@ -551,6 +556,7 @@ class AEB : public rclcpp::Node bool use_object_velocity_calculation_; bool check_autoware_state_; double path_footprint_extra_margin_; + double speed_calculation_expansion_margin_; double detection_range_min_height_; double detection_range_max_height_margin_; double voxel_grid_x_; diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index 173d419bf6d9a..02c8d0e6aaf83 100644 --- a/control/autoware_autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -41,6 +41,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros + tier4_debug_msgs visualization_msgs diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 48d9ea4c1b458..ec53a677cdd81 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -139,6 +139,8 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) this->create_publisher("~/debug/obstacle_pointcloud", 1); debug_marker_publisher_ = this->create_publisher("~/debug/markers", 1); info_marker_publisher_ = this->create_publisher("~/info/markers", 1); + debug_rss_distance_publisher_ = + this->create_publisher("~/debug/rss_distance", 1); } // Diagnostics { @@ -155,6 +157,8 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) use_object_velocity_calculation_ = declare_parameter("use_object_velocity_calculation"); check_autoware_state_ = declare_parameter("check_autoware_state"); path_footprint_extra_margin_ = declare_parameter("path_footprint_extra_margin"); + speed_calculation_expansion_margin_ = + declare_parameter("speed_calculation_expansion_margin"); detection_range_min_height_ = declare_parameter("detection_range_min_height"); detection_range_max_height_margin_ = declare_parameter("detection_range_max_height_margin"); @@ -215,6 +219,8 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( parameters, "use_object_velocity_calculation", use_object_velocity_calculation_); updateParam(parameters, "check_autoware_state", check_autoware_state_); updateParam(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_); + updateParam( + parameters, "speed_calculation_expansion_margin", speed_calculation_expansion_margin_); updateParam(parameters, "detection_range_min_height", detection_range_min_height_); updateParam( parameters, "detection_range_max_height_margin", detection_range_max_height_margin_); @@ -457,8 +463,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) // Crop out Pointcloud using an extra wide ego path if (use_pointcloud_data_ && !points_belonging_to_cluster_hulls->empty()) { const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp; - getClosestObjectsOnPath( - path, ego_polys, current_time, points_belonging_to_cluster_hulls, objects); + getClosestObjectsOnPath(path, current_time, points_belonging_to_cluster_hulls, objects); } if (use_predicted_object_data_) { createObjectDataUsingPredictedObjects(path, ego_polys, objects); @@ -473,39 +478,41 @@ bool AEB::checkCollision(MarkerArray & debug_markers) return objects; }; - auto check_collision = [&]( - const Path & path, const colorTuple & debug_colors, - const std::string & debug_ns, - PointCloud::Ptr points_belonging_to_cluster_hulls) { - time_keeper_->start_track("has_collision_with_" + debug_ns); - auto objects = - get_objects_on_path(path, points_belonging_to_cluster_hulls, debug_colors, debug_ns); - // Get only the closest object and calculate its speed + auto check_collision = [&](const Path & path, std::vector & objects) { + time_keeper_->start_track("has_collision"); const auto closest_object_point = std::invoke([&]() -> std::optional { - const auto closest_object_point_itr = + // Attempt to find the closest object + const auto closest_object_itr = std::min_element(objects.begin(), objects.end(), [](const auto & o1, const auto & o2) { + // target objects have priority + if (o1.is_target != o2.is_target) { + return o1.is_target; + } return o1.distance_to_object < o2.distance_to_object; }); - if (closest_object_point_itr == objects.end()) { - return std::nullopt; - } - const auto closest_object_speed = (use_object_velocity_calculation_) - ? collision_data_keeper_.calcObjectSpeedFromHistory( - *closest_object_point_itr, path, current_v) - : std::make_optional(0.0); - if (!closest_object_speed.has_value()) { - return std::nullopt; + if (closest_object_itr != objects.end()) { + // Calculate speed for the closest object + const auto closest_object_speed = (use_object_velocity_calculation_) + ? collision_data_keeper_.calcObjectSpeedFromHistory( + *closest_object_itr, path, current_v) + : std::make_optional(0.0); + + if (closest_object_speed.has_value()) { + closest_object_itr->velocity = closest_object_speed.value(); + return std::make_optional(*closest_object_itr); + } } - closest_object_point_itr->velocity = closest_object_speed.value(); - return std::make_optional(*closest_object_point_itr); + + return std::nullopt; }); - const bool has_collision = (closest_object_point.has_value()) - ? hasCollision(current_v, closest_object_point.value()) - : false; + const bool has_collision = + (closest_object_point.has_value() && closest_object_point.value().is_target) + ? hasCollision(current_v, closest_object_point.value()) + : false; - time_keeper_->end_track("has_collision_with_" + debug_ns); + time_keeper_->end_track("has_collision"); // check collision using rss distance return has_collision; }; @@ -540,27 +547,46 @@ bool AEB::checkCollision(MarkerArray & debug_markers) getPointsBelongingToClusterHulls( filtered_objects, points_belonging_to_cluster_hulls, debug_markers); - const auto has_collision_imu_path = - [&](const PointCloud::Ptr points_belonging_to_cluster_hulls) -> bool { - if (!use_imu_path_ || !angular_velocity_ptr_) return false; - constexpr colorTuple debug_color = {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999}; - const std::string ns = "imu"; - return check_collision(ego_imu_path, debug_color, ns, points_belonging_to_cluster_hulls); - }; + const auto imu_path_objects = (!use_imu_path_ || !angular_velocity_ptr_) + ? std::vector{} + : get_objects_on_path( + ego_imu_path, points_belonging_to_cluster_hulls, + {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999}, "imu"); + + const auto mpc_path_objects = + (!use_predicted_trajectory_ || !predicted_traj_ptr_ || !ego_mpc_path.has_value()) + ? std::vector{} + : get_objects_on_path( + ego_mpc_path.value(), points_belonging_to_cluster_hulls, + {0.0 / 256.0, 100.0 / 256.0, 0.0 / 256.0, 0.999}, "mpc"); + + // merge object data which comes from the ego (imu) path and predicted path + auto merge_objects = + [&](const std::vector & imu_objects, const std::vector & mpc_objects) { + std::vector merged_objects = imu_objects; + merged_objects.insert(merged_objects.end(), mpc_objects.begin(), mpc_objects.end()); + return merged_objects; + }; + + auto merged_imu_mpc_objects = merge_objects(imu_path_objects, mpc_path_objects); + if (merged_imu_mpc_objects.empty()) return false; - // step4. make function to check collision with predicted trajectory from control module - const auto has_collision_mpc_path = - [&](const PointCloud::Ptr points_belonging_to_cluster_hulls) -> bool { - if (!use_predicted_trajectory_ || !ego_mpc_path.has_value()) return false; - constexpr colorTuple debug_color = {0.0 / 256.0, 100.0 / 256.0, 0.0 / 256.0, 0.999}; - const std::string ns = "mpc"; - return check_collision( - ego_mpc_path.value(), debug_color, ns, points_belonging_to_cluster_hulls); + // merge path points for the collision checking + auto merge_paths = [&](const std::optional & mpc_path, const Path & imu_path) { + if (!mpc_path.has_value()) { + return imu_path; + } + Path merged_path = imu_path; // Start with imu_path + merged_path.insert( + merged_path.end(), mpc_path.value().begin(), mpc_path.value().end()); // Append mpc_path + return merged_path; }; - // evaluate if there is a collision for both paths - const bool has_collision = has_collision_imu_path(points_belonging_to_cluster_hulls) || - has_collision_mpc_path(points_belonging_to_cluster_hulls); + auto merge_imu_mpc_path = merge_paths(ego_mpc_path, ego_imu_path); + if (merge_imu_mpc_path.empty()) return false; + + // evaluate if there is a collision for merged (imu and mpc) paths + const bool has_collision = check_collision(merge_imu_mpc_path, merged_imu_mpc_objects); // Debug print if (!filtered_objects->empty() && publish_debug_pointcloud_) { @@ -586,6 +612,11 @@ bool AEB::hasCollision(const double current_v, const ObjectData & closest_object return ego_stopping_distance + obj_braking_distance + longitudinal_offset_; }); + tier4_debug_msgs::msg::Float32Stamped rss_distance_msg; + rss_distance_msg.stamp = get_clock()->now(); + rss_distance_msg.data = rss_dist; + debug_rss_distance_publisher_->publish(rss_distance_msg); + if (closest_object.distance_to_object > rss_dist) return false; // collision happens @@ -622,7 +653,7 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) geometry_msgs::msg::Pose current_pose; current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); - if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { + if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-2) { continue; } path.push_back(current_pose); @@ -636,7 +667,7 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) geometry_msgs::msg::Pose current_pose; current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); - if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { + if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-2) { continue; } path.push_back(current_pose); @@ -661,6 +692,12 @@ std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) for (size_t i = 0; i < predicted_traj.points.size(); ++i) { geometry_msgs::msg::Pose map_pose; tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped.value()); + + // skip points that are too close to the last point in the path + if (autoware::universe_utils::calcDistance2d(path.back(), map_pose) < 1e-2) { + continue; + } + path.push_back(map_pose); if (i * mpc_prediction_time_interval_ > mpc_prediction_time_horizon_) { @@ -668,7 +705,7 @@ std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) } } time_keeper_->end_track("createPath"); - return path; + return (!path.empty()) ? std::make_optional(path) : std::nullopt; } void AEB::generatePathFootprint( @@ -833,12 +870,12 @@ void AEB::getPointsBelongingToClusterHulls( } void AEB::getClosestObjectsOnPath( - const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, + const Path & ego_path, const rclcpp::Time & stamp, const PointCloud::Ptr points_belonging_to_cluster_hulls, std::vector & objects) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - // check if the predicted path has valid number of points - if (ego_path.size() < 2 || ego_polys.empty() || points_belonging_to_cluster_hulls->empty()) { + // check if the predicted path has a valid number of points + if (ego_path.size() < 2 || points_belonging_to_cluster_hulls->empty()) { return; } @@ -855,6 +892,19 @@ void AEB::getClosestObjectsOnPath( autoware::motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); if (std::isnan(obj_arc_length)) continue; + // calculate the lateral offset between the ego vehicle and the object + const double lateral_offset = + std::abs(autoware::motion_utils::calcLateralOffset(ego_path, obj_position)); + + if (std::isnan(lateral_offset)) continue; + + // object is outside region of interest + if ( + lateral_offset > + vehicle_info_.vehicle_width_m / 2.0 + expand_width_ + speed_calculation_expansion_margin_) { + continue; + } + // If the object is behind the ego, we need to use the backward long offset. The distance should // be a positive number in any case const bool is_object_in_front_of_ego = obj_arc_length > 0.0; @@ -867,14 +917,8 @@ void AEB::getClosestObjectsOnPath( obj.position = obj_position; obj.velocity = 0.0; obj.distance_to_object = std::abs(dist_ego_to_object); - - const Point2d obj_point(p.x, p.y); - for (const auto & ego_poly : ego_polys) { - if (bg::within(obj_point, ego_poly)) { - objects.push_back(obj); - break; - } - } + obj.is_target = (lateral_offset < vehicle_info_.vehicle_width_m / 2.0 + expand_width_); + objects.push_back(obj); } } diff --git a/control/autoware_autonomous_emergency_braking/test/test.cpp b/control/autoware_autonomous_emergency_braking/test/test.cpp index 11c2fb1ed671d..609a7a36b7d67 100644 --- a/control/autoware_autonomous_emergency_braking/test/test.cpp +++ b/control/autoware_autonomous_emergency_braking/test/test.cpp @@ -116,6 +116,8 @@ TEST_F(TestAEB, checkCollision) ObjectData object_collision; object_collision.distance_to_object = 0.5; object_collision.velocity = 0.1; + object_collision.position.x = 1.0; + object_collision.position.y = 1.0; ASSERT_TRUE(aeb_node_->hasCollision(longitudinal_velocity, object_collision)); ObjectData object_no_collision; @@ -161,8 +163,7 @@ TEST_F(TestAEB, checkImuPathGeneration) aeb_node_->getPointsBelongingToClusterHulls( obstacle_points_ptr, points_belonging_to_cluster_hulls, debug_markers); std::vector objects; - aeb_node_->getClosestObjectsOnPath( - imu_path, footprint, stamp, points_belonging_to_cluster_hulls, objects); + aeb_node_->getClosestObjectsOnPath(imu_path, stamp, points_belonging_to_cluster_hulls, objects); ASSERT_FALSE(objects.empty()); } From fed4629094b0dc47080c0ecf881ef2b17dfea640 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Thu, 26 Sep 2024 13:40:11 +0900 Subject: [PATCH 43/95] fix(static_obstacle_avoidance): remove redundant calculation (#8955) remove redundant calculation Signed-off-by: Go Sakayori --- .../src/utils.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 0b8abb96cfc1f..339ee2ed782c9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -1822,6 +1822,9 @@ void updateRoadShoulderDistance( clip_objects.push_back(object); } }); + + if (clip_objects.empty()) return; + for (auto & o : clip_objects) { const auto & vehicle_width = planner_data->parameters.vehicle_width; const auto object_type = utils::getHighestProbLabel(o.object.classification); @@ -1886,6 +1889,7 @@ void filterTargetObjects( data.other_objects.push_back(o); continue; } + o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); } else if (filtering_utils::isVehicleTypeObject(o)) { // TARGET: CAR, TRUCK, BUS, TRAILER, MOTORCYCLE From d9fda9539b14c8165a0c0e6d0676ba9e00c637dc Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 26 Sep 2024 13:47:57 +0900 Subject: [PATCH 44/95] fix(autoware_behavior_path_lane_change_module): fix unusedFunction (#8960) * fix:unusedFunction Signed-off-by: kobayu858 * fix:unusedFunction Signed-off-by: kobayu858 * fix:unusedFunction Signed-off-by: kobayu858 * fix:pre_commit Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- .../src/utils/utils.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 36aedf5c99a53..c982c0aad988f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -200,6 +200,7 @@ bool isPathInLanelets( return true; } +// cppcheck-suppress unusedFunction bool pathFootprintExceedsTargetLaneBound( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, const double margin) From bdbea2bb49a6f4124b6c28a089f72f599392248b Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 26 Sep 2024 14:14:58 +0900 Subject: [PATCH 45/95] ci(cppcheck): remove unnecessary suppressions (#8964) fix:suppressions Signed-off-by: kobayu858 --- .cppcheck_suppressions | 2 -- 1 file changed, 2 deletions(-) diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index a0e7bcce350f5..f95f6f238491b 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -3,8 +3,6 @@ checkersReport missingInclude missingIncludeSystem -unknownMacro unmatchedSuppression -unusedFunction useInitializationList useStlAlgorithm From 6bbac60aa6aac49a3d67222dd8a6c7e5df807f37 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Thu, 26 Sep 2024 14:17:29 +0900 Subject: [PATCH 46/95] chore(autoware_deteted_object_feature_remover): add code owners (#8962) Add yoshiri and kotaro as code owners of feature remover Signed-off-by: yoshiri --- .github/CODEOWNERS | 2 +- perception/autoware_detected_object_feature_remover/package.xml | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 0c0bb6955ae2a..205f6c7fb4438 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -106,7 +106,7 @@ perception/autoware_bytetrack/** manato.hirabayashi@tier4.jp yoshi.ri@tier4.jp perception/autoware_cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/autoware_compare_map_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp yukihiro.saito@tier4.jp perception/autoware_crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp -perception/autoware_detected_object_feature_remover/** tomoya.kimura@tier4.jp +perception/autoware_detected_object_feature_remover/** tomoya.kimura@tier4.jp yoshi.ri@tier4.jp kotaro.uetake@tier4.jp perception/autoware_detected_object_validation/** dai.nguyen@tier4.jp shintaro.tomie@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/autoware_detection_by_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/autoware_elevation_map_loader/** kosuke.takeuchi@tier4.jp shintaro.tomie@tier4.jp taichi.higashide@tier4.jp diff --git a/perception/autoware_detected_object_feature_remover/package.xml b/perception/autoware_detected_object_feature_remover/package.xml index a835e289cd455..eb64490df1f0f 100644 --- a/perception/autoware_detected_object_feature_remover/package.xml +++ b/perception/autoware_detected_object_feature_remover/package.xml @@ -5,6 +5,8 @@ 0.1.0 The autoware_detected_object_feature_remover package Tomoya Kimura + Yoshi Ri + Kotaro Uetake Apache License 2.0 ament_cmake From 653ce0e5410079a5fbbf82f4e37c80db5be7f089 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Thu, 26 Sep 2024 14:24:46 +0900 Subject: [PATCH 47/95] fix(crosswalk): change exceptional handling (#8956) Signed-off-by: Yuki Takagi --- .../src/scene_crosswalk.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index d7e8dbda57f3b..f56552485b5fc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -332,7 +332,7 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const double dist_default_stop = default_stop_pose_opt.has_value() ? calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose_opt->position) - : std::numeric_limits::max(); + : 0.0; updateObjectState( dist_default_stop, sparse_resample_path, crosswalk_attention_range, attention_area); From 65a36d43b618be660f6da30a3f4ce5913ca18d4d Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Thu, 26 Sep 2024 14:25:23 +0900 Subject: [PATCH 48/95] fix(simple_planning_simulator, raw_vehicle_cmd_converter): swap row index and column index for csv loader (#8963) swap row and column Signed-off-by: Go Sakayori --- .../sim_model_delay_steer_map_acc_geared.hpp | 4 ++-- .../utils/csv_loader.cpp | 4 ++-- .../vehicle_model/sim_model_actuation_cmd.cpp | 24 ++++++++++--------- .../src/accel_map.cpp | 4 ++-- .../src/brake_map.cpp | 4 ++-- .../src/csv_loader.cpp | 4 ++-- .../src/steer_map.cpp | 4 ++-- 7 files changed, 25 insertions(+), 23 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp index 454a35d95eb8b..381b6ae9da4e2 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp @@ -41,8 +41,8 @@ class AccelerationMap } vehicle_name_ = table[0][0]; - vel_index_ = CSVLoader::getRowIndex(table); - acc_index_ = CSVLoader::getColumnIndex(table); + vel_index_ = CSVLoader::getColumnIndex(table); + acc_index_ = CSVLoader::getRowIndex(table); acceleration_map_ = CSVLoader::getMap(table); std::cout << "[SimModelDelaySteerMapAccGeared]: success to read acceleration map from " diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp index 1189477a4f45b..b410f089725a2 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp @@ -119,7 +119,7 @@ Map CSVLoader::getMap(const Table & table) return map; } -std::vector CSVLoader::getRowIndex(const Table & table) +std::vector CSVLoader::getColumnIndex(const Table & table) { // NOTE: table[0][i] represents the element in the 0-th row and i-th column // This means that we are getting the index of each column in the 0-th row @@ -130,7 +130,7 @@ std::vector CSVLoader::getRowIndex(const Table & table) return index; } -std::vector CSVLoader::getColumnIndex(const Table & table) +std::vector CSVLoader::getRowIndex(const Table & table) { // NOTE: table[i][0] represents the element in the i-th row and 0-th column // This means that we are getting the index of each row in the 0-th column diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp index 069cb9e569580..74ad765687a2e 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp @@ -27,13 +27,10 @@ bool ActuationMap::readActuationMapFromCSV(const std::string & csv_path, const b return false; } - state_index_ = CSVLoader::getRowIndex(table); - actuation_index_ = CSVLoader::getColumnIndex(table); + state_index_ = CSVLoader::getColumnIndex(table); + actuation_index_ = CSVLoader::getRowIndex(table); actuation_map_ = CSVLoader::getMap(table); - if (validation && !CSVLoader::validateMap(actuation_map_, true)) { - return false; - } - return true; + return !validation || CSVLoader::validateMap(actuation_map_, true); } double ActuationMap::getControlCommand(const double actuation, const double state) const @@ -41,7 +38,8 @@ double ActuationMap::getControlCommand(const double actuation, const double stat std::vector interpolated_control_vec{}; const double clamped_state = CSVLoader::clampValue(state, state_index_); - for (std::vector control_command_values : actuation_map_) { + interpolated_control_vec.reserve(actuation_map_.size()); + for (const std::vector & control_command_values : actuation_map_) { interpolated_control_vec.push_back( autoware::interpolation::lerp(state_index_, control_command_values, clamped_state)); } @@ -61,7 +59,8 @@ std::optional AccelMap::getThrottle(const double acc, double vel) const const double clamped_vel = CSVLoader::clampValue(vel, vel_indices); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel - for (std::vector accelerations : accel_map) { + interpolated_acc_vec.reserve(accel_map.size()); + for (const std::vector & accelerations : accel_map) { interpolated_acc_vec.push_back( autoware::interpolation::lerp(vel_indices, accelerations, clamped_vel)); } @@ -71,7 +70,8 @@ std::optional AccelMap::getThrottle(const double acc, double vel) const // When the desired acceleration is greater than the throttle area, return max throttle if (acc < interpolated_acc_vec.front()) { return std::nullopt; - } else if (interpolated_acc_vec.back() < acc) { + } + if (interpolated_acc_vec.back() < acc) { return throttle_indices.back(); } @@ -88,7 +88,8 @@ double BrakeMap::getBrake(const double acc, double vel) const const double clamped_vel = CSVLoader::clampValue(vel, vel_indices); // (brake, vel, acc) map => (brake, acc) map by fixing vel - for (std::vector accelerations : brake_map) { + interpolated_acc_vec.reserve(brake_map.size()); + for (const std::vector & accelerations : brake_map) { interpolated_acc_vec.push_back( autoware::interpolation::lerp(vel_indices, accelerations, clamped_vel)); } @@ -98,7 +99,8 @@ double BrakeMap::getBrake(const double acc, double vel) const // When the desired acceleration is greater than the brake area, return min brake on the map if (acc < interpolated_acc_vec.back()) { return brake_indices.back(); - } else if (interpolated_acc_vec.front() < acc) { + } + if (interpolated_acc_vec.front() < acc) { return brake_indices.front(); } diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp index f0c64991ec169..bc012932a9b0d 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp @@ -31,8 +31,8 @@ bool AccelMap::readAccelMapFromCSV(const std::string & csv_path, const bool vali } vehicle_name_ = table[0][0]; - vel_index_ = CSVLoader::getRowIndex(table); - throttle_index_ = CSVLoader::getColumnIndex(table); + vel_index_ = CSVLoader::getColumnIndex(table); + throttle_index_ = CSVLoader::getRowIndex(table); accel_map_ = CSVLoader::getMap(table); return !validation || CSVLoader::validateMap(accel_map_, true); } diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp index 46dd8f9bd1fe4..343804d4a0ff5 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp @@ -32,8 +32,8 @@ bool BrakeMap::readBrakeMapFromCSV(const std::string & csv_path, const bool vali } vehicle_name_ = table[0][0]; - vel_index_ = CSVLoader::getRowIndex(table); - brake_index_ = CSVLoader::getColumnIndex(table); + vel_index_ = CSVLoader::getColumnIndex(table); + brake_index_ = CSVLoader::getRowIndex(table); brake_map_ = CSVLoader::getMap(table); brake_index_rev_ = brake_index_; if (validation && !CSVLoader::validateMap(brake_map_, false)) { diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/csv_loader.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/csv_loader.cpp index 9f50e025c5b36..0a31e2089e4ef 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/csv_loader.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/csv_loader.cpp @@ -115,7 +115,7 @@ Map CSVLoader::getMap(const Table & table) return map; } -std::vector CSVLoader::getRowIndex(const Table & table) +std::vector CSVLoader::getColumnIndex(const Table & table) { std::vector index = {}; for (size_t i = 1; i < table[0].size(); i++) { @@ -124,7 +124,7 @@ std::vector CSVLoader::getRowIndex(const Table & table) return index; } -std::vector CSVLoader::getColumnIndex(const Table & table) +std::vector CSVLoader::getRowIndex(const Table & table) { std::vector index = {}; for (size_t i = 1; i < table.size(); i++) { diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp index 3a458c97a4374..9777b4f24a1fa 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp @@ -32,8 +32,8 @@ bool SteerMap::readSteerMapFromCSV(const std::string & csv_path, const bool vali } vehicle_name_ = table[0][0]; - steer_index_ = CSVLoader::getRowIndex(table); - output_index_ = CSVLoader::getColumnIndex(table); + steer_index_ = CSVLoader::getColumnIndex(table); + output_index_ = CSVLoader::getRowIndex(table); steer_map_ = CSVLoader::getMap(table); return !validation || CSVLoader::validateMap(steer_map_, true); } From 17bf3ade7a4470151cbeb2369bff65cd4363d9a6 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 26 Sep 2024 14:35:39 +0900 Subject: [PATCH 49/95] feat(object_recognition_utils): implement covariance transform in the transformObjects (#8953) * feat: implement covariance transform in the transformObjects Signed-off-by: Taekjin LEE * fix: transform covariance is only the frame difference Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../include/object_recognition_utils/transform.hpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/common/object_recognition_utils/include/object_recognition_utils/transform.hpp b/common/object_recognition_utils/include/object_recognition_utils/transform.hpp index 31892853a855f..b03270133a52c 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/transform.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/transform.hpp @@ -95,10 +95,14 @@ bool transformObjects( tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world); } for (auto & object : output_msg.objects) { - tf2::fromMsg(object.kinematics.pose_with_covariance.pose, tf_objects_world2objects); + auto & pose_with_cov = object.kinematics.pose_with_covariance; + tf2::fromMsg(pose_with_cov.pose, tf_objects_world2objects); tf_target2objects = tf_target2objects_world * tf_objects_world2objects; - tf2::toMsg(tf_target2objects, object.kinematics.pose_with_covariance.pose); - // TODO(yukkysaito) transform covariance + // transform pose, frame difference and object pose + tf2::toMsg(tf_target2objects, pose_with_cov.pose); + // transform covariance, only the frame difference + pose_with_cov.covariance = + tf2::transformCovariance(pose_with_cov.covariance, tf_target2objects_world); } } return true; From 0e25a8b4e5969d260842e6ada1f017158f2f0415 Mon Sep 17 00:00:00 2001 From: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> Date: Thu, 26 Sep 2024 15:14:28 +0900 Subject: [PATCH 50/95] test(autoware_detected_object_feature_remover): add unit testing for the node (#8735) * test: add unit testing for the node Signed-off-by: ktro2828 * test: update to run node testings with `ROS_DOMAIN_ID` isolation Signed-off-by: ktro2828 --------- Signed-off-by: ktro2828 --- .../CMakeLists.txt | 14 ++ .../package.xml | 1 + ...t_detected_object_feature_remover_node.cpp | 182 ++++++++++++++++++ 3 files changed, 197 insertions(+) create mode 100644 perception/autoware_detected_object_feature_remover/test/test_detected_object_feature_remover_node.cpp diff --git a/perception/autoware_detected_object_feature_remover/CMakeLists.txt b/perception/autoware_detected_object_feature_remover/CMakeLists.txt index 54763f8bff9ab..58384613f7d3f 100644 --- a/perception/autoware_detected_object_feature_remover/CMakeLists.txt +++ b/perception/autoware_detected_object_feature_remover/CMakeLists.txt @@ -13,6 +13,20 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE detected_object_feature_remover_node ) +if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) + # run node unit testings with ROS_DOMAIN_ID isolation + ament_add_ros_isolated_gtest(test_${PROJECT_NAME}_node + test/test_detected_object_feature_remover_node.cpp + ) + target_include_directories(test_${PROJECT_NAME}_node PRIVATE src) + target_link_libraries(test_${PROJECT_NAME}_node ${${PROJECT_NAME}_LIBRARIES}) + ament_target_dependencies(test_${PROJECT_NAME}_node + ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS} + ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} + ) +endif() + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/perception/autoware_detected_object_feature_remover/package.xml b/perception/autoware_detected_object_feature_remover/package.xml index eb64490df1f0f..8726e2bf26071 100644 --- a/perception/autoware_detected_object_feature_remover/package.xml +++ b/perception/autoware_detected_object_feature_remover/package.xml @@ -21,6 +21,7 @@ ament_cmake_ros ament_lint_auto autoware_lint_common + autoware_test_utils ament_cmake diff --git a/perception/autoware_detected_object_feature_remover/test/test_detected_object_feature_remover_node.cpp b/perception/autoware_detected_object_feature_remover/test/test_detected_object_feature_remover_node.cpp new file mode 100644 index 0000000000000..e54151786278a --- /dev/null +++ b/perception/autoware_detected_object_feature_remover/test/test_detected_object_feature_remover_node.cpp @@ -0,0 +1,182 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "detected_object_feature_remover_node.hpp" + +#include +#include + +#include "tier4_perception_msgs/msg/detail/detected_object_with_feature__struct.hpp" + +#include + +namespace +{ +using autoware::detected_object_feature_remover::DetectedObjectFeatureRemover; +using autoware::test_utils::AutowareTestManager; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using tier4_perception_msgs::msg::DetectedObjectWithFeature; + +/** + * @brief Return a new `autoware::test_utils::AutowareTestManager` instance as a shared pointer. + * + * @return Shared pointer of `autoware::test_utils::AutowareTestManager`. + */ +std::shared_ptr generate_test_manager() +{ + return std::make_shared(); +} + +/** + * @brief Return a new `autoware::detected_object_feature_remover::DetectedObjectFeatureRemover` + * instance as a shared pointer. + * + * @return Shared pointer of + * `autoware::detected_object_feature_remover::DetectedObjectFeatureRemover`. + */ +std::shared_ptr generate_node( + const std::string & input_topic, const std::string & output_topic) +{ + auto node_options = rclcpp::NodeOptions{}; + node_options.arguments( + {"--ros-args", "-r", "~/input:=" + input_topic, "-r", "~/output:=" + output_topic}); + + return std::make_shared(node_options); +} + +/** + * @brief Return a new `autoware_perception_msgs::msg::DetectedObject` instance. + * + * @return New instance of `autoware_perception_msgs::msg::DetectedObject`. + */ +DetectedObject generate_detected_object() +{ + DetectedObject output; + output.kinematics.pose_with_covariance.pose.position.x = 1.0; + output.kinematics.pose_with_covariance.pose.position.y = 2.0; + output.kinematics.pose_with_covariance.pose.position.z = 3.0; + output.kinematics.pose_with_covariance.pose.orientation.x = 0.0; + output.kinematics.pose_with_covariance.pose.orientation.y = 0.0; + output.kinematics.pose_with_covariance.pose.orientation.z = 0.0; + output.kinematics.pose_with_covariance.pose.orientation.w = 1.0; + return output; +} + +/** + * @brief Return a new `tier4_perception_msgs::msg::DetectedObjectWithFeature` instance. + * @details Return a object including a single object created by `generate_detected_object`. + * + * @return New instance of `tier4_perception_msgs::msg::DetectedObjectWithFeature`. + */ +DetectedObjectWithFeature generate_feature_object() +{ + DetectedObjectWithFeature output; + output.object = generate_detected_object(); + return output; +} + +/** + * @brief Return a new `tier4_perception_msgs::msg::DetectedObjectsWithFeature` instance. + * @details If `as_emtpy=true`, returns a instance without any objects. Otherwise, returns a + * instance including a single object created by `generate_feature_object`. + * + * @param stamp Timestamp of the message. + * @param as_empty Whether to return a instance without any objects. + * @return New instance of `tier4_perception_mgs::msg::DetectedObjectsWIthFeature`. + */ +DetectedObjectsWithFeature generate_feature_objects(const rclcpp::Time & stamp, bool as_empty) +{ + DetectedObjectsWithFeature output; + output.header.frame_id = "base_link"; + output.header.stamp = stamp; + if (!as_empty) { + auto object = generate_feature_object(); + output.feature_objects.emplace_back(object); + } + return output; +} +} // namespace + +/** + * Test suite of DetectedObjectFeatureRemover. + * + * This test case checks whether the node works if the arbitrary object is input. + */ +TEST(FeatureRemoverTest, TestArbitraryObject) +{ + const std::string input_topic = "/detected_object_feature_remover/input"; + const std::string output_topic = "/detected_object_feature_remover/output"; + auto test_manager = generate_test_manager(); + auto node = generate_node(input_topic, output_topic); + + // set output subscriber + DetectedObjects output; + auto callback = [&output](const DetectedObjects::ConstSharedPtr msg) { + output.header = msg->header; + output.objects = msg->objects; + }; + test_manager->set_subscriber(output_topic, callback); + + // prepare input + auto stamp = node->get_clock()->now(); + constexpr bool as_empty = false; + auto input = generate_feature_objects(stamp, as_empty); + test_manager->test_pub_msg(node, input_topic, input); + + // Check output + auto expect = generate_detected_object(); + EXPECT_EQ(1, output.objects.size()); + EXPECT_EQ(expect, output.objects.front()); +} + +/** + * Test suite of DetectedObjectFeatureRemover. + * + * This test case checks whether the node works if the empty object is input. + */ +TEST(FeatureRemoverTest, TestEmptyObject) +{ + const std::string input_topic = "/detected_object_feature_remover/input"; + const std::string output_topic = "/detected_object_feature_remover/output"; + auto test_manager = generate_test_manager(); + auto node = generate_node(input_topic, output_topic); + + // set output subscriber + DetectedObjects output; + auto callback = [&output](const DetectedObjects::ConstSharedPtr msg) { + output.header = msg->header; + output.objects = msg->objects; + }; + test_manager->set_subscriber(output_topic, callback); + + // prepare input + auto stamp = node->get_clock()->now(); + constexpr bool as_empty = true; + auto input = generate_feature_objects(stamp, as_empty); + test_manager->test_pub_msg(node, input_topic, input); + + // Check output + EXPECT_EQ(0, output.objects.size()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} From 05093b5a9e70b3517d35bbf7bb5eb87841daab94 Mon Sep 17 00:00:00 2001 From: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> Date: Thu, 26 Sep 2024 15:17:45 +0900 Subject: [PATCH 51/95] test(autoware_shape_estimation): add unit testing for `ShapeEstimationNode` (#8740) test: add unit testing for `ShapeEstimationNode` Signed-off-by: ktro2828 --- .../autoware_shape_estimation/CMakeLists.txt | 16 +- .../autoware_shape_estimation/package.xml | 1 + .../test/test_shape_estimation_node.cpp | 194 ++++++++++++++++++ 3 files changed, 203 insertions(+), 8 deletions(-) create mode 100644 perception/autoware_shape_estimation/test/test_shape_estimation_node.cpp diff --git a/perception/autoware_shape_estimation/CMakeLists.txt b/perception/autoware_shape_estimation/CMakeLists.txt index 56f14ad9d2b48..6cf93599f9964 100644 --- a/perception/autoware_shape_estimation/CMakeLists.txt +++ b/perception/autoware_shape_estimation/CMakeLists.txt @@ -106,15 +106,15 @@ ament_auto_package(INSTALL_TO_SHARE ## Tests if(BUILD_TESTING) find_package(ament_cmake_ros REQUIRED) - list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) + set(ament_cmake_uncrustify_FOUND TRUE) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() + file(GLOB_RECURSE test_files test/*.cpp test/**/*.cpp) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files}) - file(GLOB_RECURSE test_files test/**/*.cpp) - ament_add_ros_isolated_gtest(test_shape_estimation ${test_files}) - - target_link_libraries(test_shape_estimation - ${PROJECT_NAME} + target_include_directories(test_${PROJECT_NAME} PRIVATE src) + target_link_libraries(test_${PROJECT_NAME} ${${PROJECT_NAME}_LIBRARIES}) + ament_target_dependencies(test_${PROJECT_NAME} + ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS} + ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} ) endif() diff --git a/perception/autoware_shape_estimation/package.xml b/perception/autoware_shape_estimation/package.xml index fec1009428113..644a9de661eca 100644 --- a/perception/autoware_shape_estimation/package.xml +++ b/perception/autoware_shape_estimation/package.xml @@ -35,6 +35,7 @@ ament_cmake_gtest ament_lint_auto autoware_lint_common + autoware_test_utils ament_cmake diff --git a/perception/autoware_shape_estimation/test/test_shape_estimation_node.cpp b/perception/autoware_shape_estimation/test/test_shape_estimation_node.cpp new file mode 100644 index 0000000000000..df04ade05b1ce --- /dev/null +++ b/perception/autoware_shape_estimation/test/test_shape_estimation_node.cpp @@ -0,0 +1,194 @@ +// Copyright 2024 TIER IV, inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "shape_estimation_node.hpp" + +#include +#include + +#include "tier4_perception_msgs/msg/detail/detected_objects_with_feature__struct.hpp" + +#include + +namespace +{ +using autoware::shape_estimation::ShapeEstimationNode; +using autoware::test_utils::AutowareTestManager; +using sensor_msgs::msg::PointCloud2; +using tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using tier4_perception_msgs::msg::DetectedObjectWithFeature; + +/** + * @brief Return a new `autoware::test_utils::AutowareTestManager` instance as a shared pointer. + * + * @return Shared pointer of `autoware::test_utils::AutowareTestManager`. + */ +std::shared_ptr generate_test_manager() +{ + return std::make_shared(); +} + +/** + * @brief Return a new `autoware::shape_estimation::ShapeEstimationNode` instance as a shared + * pointer. + * + * @param input_topic Input topic name. + * @param output_topic Output topic name. + * @return Shared pointer of `autoware::shape_estimation::ShapeEstimationNode`. + */ +std::shared_ptr generate_node( + const std::string & input_topic, const std::string & output_topic) +{ + const auto package_dir = + ament_index_cpp::get_package_share_directory("autoware_shape_estimation"); + + auto node_options = rclcpp::NodeOptions(); + node_options.arguments( + {"--ros-args", "--params-file", package_dir + "/config/shape_estimation.param.yaml", "-r", + "input:=" + input_topic, "-r", "objects:=" + output_topic}); + return std::make_shared(node_options); +} + +/** + * @brief Return a new `PointCloud` object shaping like L. + * + * @param length Length of L box in [m]. + * @param width Width of L box in [m]. + * @param height Height of L box in [m]. + * @param yaw Yaw rotation offset in [rad]. Defaults to 0.0. + * @param offset_x X position offset in [m]. Defaults to 0.0. + * @param offset_y Y position offset in [m]. Defaults to 0.0. + * @return Created a `PointCloud` object. + * + * @note This function is deprecated with `test/shape_estimation/test_shape_estimator.cpp`. + */ +PointCloud2 generate_cluster( + double length, double width, double height, double yaw = 0.0, double offset_x = 0.0, + double offset_y = 0.0) +{ + constexpr double x_step = 0.4; + constexpr double y_step = 0.2; + + pcl::PointCloud cluster; + for (double x = -0.5 * length; x < 0.5 * length; x += x_step) { + cluster.emplace_back(x, 0.5 * width, 0.0); + } + for (double y = -0.5 * width; y < 0.5 * width; y += y_step) { + cluster.emplace_back(-0.5 * length, y, 0.0); + } + cluster.emplace_back(0.5 * length, -0.5 * width, 0.0); + cluster.emplace_back(0.5 * length, 0.5 * width, 0.0); + cluster.emplace_back(-0.5 * length, -0.5 * width, 0.0); + cluster.emplace_back(-0.5 * length, 0.5 * width, 0.0); + cluster.emplace_back(0.0, 0.0, height); + + if (yaw != 0.0 || offset_x != 0.0 || offset_y != 0.0) { + for (auto & point : cluster) { + const double x = point.x; + const double y = point.y; + // rotate + point.x = x * cos(yaw) - y * sin(yaw); + point.y = x * sin(yaw) + y * cos(yaw); + // offset + point.x += offset_x; + point.y += offset_y; + } + } + + PointCloud2 output; + pcl::toROSMsg(cluster, output); + return output; +} + +/** + * @brief Return a new `tier4_perception_msgs::msg::DetectedObjectsWithFeature` instance. + * + * @param stamp ROS timestamp. + * @param as_empty Whether to return a object without any cluster. + * @return `tier4_perception_msgs::msg::DetectedObjectsWithFeature`. + */ +DetectedObjectsWithFeature generate_feature_objects(const rclcpp::Time & stamp, bool as_empty) +{ + constexpr double length = 4.0; + constexpr double width = 2.0; + constexpr double height = 1.0; + + DetectedObjectsWithFeature output; + output.header.frame_id = "base_link"; + output.header.stamp = stamp; + + if (!as_empty) { + DetectedObjectWithFeature object; + object.feature.cluster = generate_cluster(length, width, height); + output.feature_objects.emplace_back(object); + } + + return output; +} +} // namespace + +/** + * Test `ShapeEstimationNode`. + * + * This test case checks whether the node works with the arbitrary input. + */ +TEST(ShapeEstimationNode, testShapeEstimationNodeWithArbitraryCluster) +{ + const std::string input_topic = "/shape_estimation/input"; + const std::string output_topic = "/shape_estimation/output"; + auto test_manager = generate_test_manager(); + auto node = generate_node(input_topic, output_topic); + + // set output subscriber + int counter = 0; + auto callback = [&counter](const DetectedObjectsWithFeature::ConstSharedPtr) { ++counter; }; + test_manager->set_subscriber(output_topic, callback); + + // prepare input + auto stamp = node->get_clock()->now(); + constexpr bool as_empty = false; + auto input = generate_feature_objects(stamp, as_empty); + test_manager->test_pub_msg(node, input_topic, input); + + // check if the node worked + EXPECT_EQ(1, counter); +} + +/** + * Test `ShapeEstimationNode`. + * + * This test case checks whether the node works with the empty cluster input. + */ +TEST(ShapeEstimationNode, testShapeEstimationNodeWithEmptyCluster) +{ + const std::string input_topic = "/shape_estimation/input"; + const std::string output_topic = "/shape_estimation/output"; + auto test_manager = generate_test_manager(); + auto node = generate_node(input_topic, output_topic); + + // set output subscriber + int counter = 0; + auto callback = [&counter](const DetectedObjectsWithFeature::ConstSharedPtr) { ++counter; }; + test_manager->set_subscriber(output_topic, callback); + + // prepare input + auto stamp = node->get_clock()->now(); + constexpr bool as_empty = true; + auto input = generate_feature_objects(stamp, as_empty); + test_manager->test_pub_msg(node, input_topic, input); + + // check if the node worked + EXPECT_EQ(1, counter); +} From d10c4c2bd4be8ef04c3c668317d4f09998d08ad2 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 26 Sep 2024 19:50:57 +0900 Subject: [PATCH 52/95] feat(goal_planner): use neighboring lane of pull over lane to check goal footprint (#8716) move to utils and add tests Signed-off-by: kosuke55 --- .../util.hpp | 16 ++++ .../src/goal_searcher.cpp | 10 +-- .../src/util.cpp | 55 ++++++++++++ .../test/test_util.cpp | 83 +++++++++++++++++++ 4 files changed, 158 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index a9c9991df3c18..f68800b47f5a5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -161,6 +161,22 @@ std::string makePathPriorityDebugMessage( const std::map & path_id_to_rough_margin_map, const std::function & isSoftMargin, const std::function & isHighCurvature); +/** + * @brief combine two points + * @param points lane points + * @param points_next next lane points + * @return combined points + */ +lanelet::Points3d combineLanePoints( + const lanelet::Points3d & points, const lanelet::Points3d & points_next); +/** @brief Create a lanelet that represents the departure check area. + * @param [in] pull_over_lanes Lanelets that the vehicle will pull over to. + * @param [in] route_handler RouteHandler object. + * @return Lanelet that goal footprints should be inside. + */ +lanelet::Lanelet createDepartureCheckLanelet( + const lanelet::ConstLanelets & pull_over_lanes, const route_handler::RouteHandler & route_handler, + const bool left_side_parking); } // namespace autoware::behavior_path_planner::goal_planner_utils #endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index b9351e4588d5a..8536001c6a08e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -119,11 +119,8 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *route_handler, left_side_parking_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length); - auto lanes = utils::getExtendedCurrentLanes( - planner_data, backward_length, forward_length, - /*forward_only_in_route*/ false); - lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end()); - + const auto departure_check_lane = goal_planner_utils::createDepartureCheckLanelet( + pull_over_lanes, *route_handler, left_side_parking_); const auto goal_arc_coords = lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose_); const double s_start = std::max(0.0, goal_arc_coords.length - backward_length); @@ -194,7 +191,8 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p break; } - if (LaneDepartureChecker::isOutOfLane(lanes, transformed_vehicle_footprint)) { + if (!boost::geometry::within( + transformed_vehicle_footprint, departure_check_lane.polygon2d().basicPolygon())) { continue; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index 4749029e04ba3..beed916a31a1b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -687,4 +687,59 @@ std::string makePathPriorityDebugMessage( return ss.str(); } +lanelet::Points3d combineLanePoints( + const lanelet::Points3d & points, const lanelet::Points3d & points_next) +{ + lanelet::Points3d combined_points; + std::unordered_set point_ids; + for (const auto & point : points) { + if (point_ids.insert(point.id()).second) { + combined_points.push_back(point); + } + } + for (const auto & point : points_next) { + if (point_ids.insert(point.id()).second) { + combined_points.push_back(point); + } + } + return combined_points; +} + +lanelet::Lanelet createDepartureCheckLanelet( + const lanelet::ConstLanelets & pull_over_lanes, const route_handler::RouteHandler & route_handler, + const bool left_side_parking) +{ + const auto getBoundPoints = + [&](const lanelet::ConstLanelet & lane, const bool is_outer) -> lanelet::Points3d { + lanelet::Points3d points; + const auto & bound = left_side_parking ? (is_outer ? lane.leftBound() : lane.rightBound()) + : (is_outer ? lane.rightBound() : lane.leftBound()); + for (const auto & pt : bound) { + points.push_back(lanelet::Point3d(pt)); + } + return points; + }; + + const auto getMostInnerLane = [&](const lanelet::ConstLanelet & lane) -> lanelet::ConstLanelet { + return left_side_parking ? route_handler.getMostRightLanelet(lane, false, true) + : route_handler.getMostLeftLanelet(lane, false, true); + }; + + lanelet::Points3d outer_bound_points{}; + lanelet::Points3d inner_bound_points{}; + for (const auto & lane : pull_over_lanes) { + const auto current_outer_bound_points = getBoundPoints(lane, true); + const auto most_inner_lane = getMostInnerLane(lane); + const auto current_inner_bound_points = getBoundPoints(most_inner_lane, false); + outer_bound_points = combineLanePoints(outer_bound_points, current_outer_bound_points); + inner_bound_points = combineLanePoints(inner_bound_points, current_inner_bound_points); + } + + const auto outer_linestring = lanelet::LineString3d(lanelet::InvalId, outer_bound_points); + const auto inner_linestring = lanelet::LineString3d(lanelet::InvalId, inner_bound_points); + return lanelet::Lanelet( + lanelet::InvalId, left_side_parking ? outer_linestring : inner_linestring, + left_side_parking ? inner_linestring : outer_linestring); +} + } // namespace autoware::behavior_path_planner::goal_planner_utils diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp index ca93505fe1eae..b50ee083fddc8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp @@ -82,3 +82,86 @@ TEST_F(TestUtilWithMap, isWithinAreas) baselink_footprint, bus_stop_area_polygons), true); } + +TEST_F(TestUtilWithMap, combineLanePoints) +{ + // 1) combine points with no duplicate IDs + { + lanelet::Points3d points{ + {lanelet::Point3d(1, 0, 0), lanelet::Point3d(2, 0, 0), lanelet::Point3d(3, 0, 0)}}; + lanelet::Points3d points_next{ + {lanelet::Point3d(4, 0, 0), lanelet::Point3d(5, 0, 0), lanelet::Point3d(6, 0, 0)}}; + + const auto combined_points = + autoware::behavior_path_planner::goal_planner_utils::combineLanePoints(points, points_next); + EXPECT_EQ(combined_points.size(), 6); + } + + // 2) combine points with duplicate IDs + { + lanelet::Points3d points{ + {lanelet::Point3d(1, 0, 0), lanelet::Point3d(2, 0, 0), lanelet::Point3d(3, 0, 0)}}; + lanelet::Points3d points_next{ + {lanelet::Point3d(3, 0, 0), lanelet::Point3d(4, 0, 0), lanelet::Point3d(5, 0, 0)}}; + + const auto combined_points = + autoware::behavior_path_planner::goal_planner_utils::combineLanePoints(points, points_next); + EXPECT_EQ(combined_points.size(), 5); + } +} + +TEST_F(TestUtilWithMap, createDepartureCheckLanelet) +{ + const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); + + const geometry_msgs::msg::Pose goal_pose = + geometry_msgs::build() + .position(geometry_msgs::build() + .x(433.42254638671875) + .y(465.3381652832031) + .z(0.0)) + .orientation(geometry_msgs::build() + .x(0.0) + .y(0.0) + .z(0.306785474523741) + .w(0.9517786888879384)); + + // 1) get target shoulder lane and check it's lane id + const auto target_shoulder_lane = route_handler->getPullOverTarget(goal_pose); + EXPECT_EQ(target_shoulder_lane.has_value(), true); + EXPECT_EQ(target_shoulder_lane.value().id(), 18391); + + // 2) get shoulder lane sequence + const auto target_shoulder_lanes = + route_handler->getShoulderLaneletSequence(target_shoulder_lane.value(), goal_pose); + EXPECT_EQ(target_shoulder_lanes.size(), 3); + + // 3) check if the right bound of the departure check lane extended to the right end matches the + // right bound of the rightmost lanelet + const auto to_points3d = [](const lanelet::ConstLineString3d & bound) { + lanelet::Points3d points; + for (const auto & pt : bound) { + points.push_back(lanelet::Point3d(pt)); + } + return points; + }; + + const auto departure_check_lane = + autoware::behavior_path_planner::goal_planner_utils::createDepartureCheckLanelet( + target_shoulder_lanes, *route_handler, true); + const auto departure_check_lane_right_bound_points = + to_points3d(departure_check_lane.rightBound()); + + const std::vector most_right_lanelet_ids = {18381, 18383, 18388}; + lanelet::Points3d right_bound_points; + for (const auto & id : most_right_lanelet_ids) { + const auto lanelet = lanelet_map_ptr->laneletLayer.get(id); + right_bound_points = autoware::behavior_path_planner::goal_planner_utils::combineLanePoints( + right_bound_points, to_points3d(lanelet.rightBound())); + } + + EXPECT_EQ(departure_check_lane_right_bound_points.size(), right_bound_points.size()); + for (size_t i = 0; i < departure_check_lane_right_bound_points.size(); ++i) { + EXPECT_EQ(departure_check_lane_right_bound_points.at(i).id(), right_bound_points.at(i).id()); + } +} From 070e4ee393aafe4ae47792d85b57356146928dbc Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Thu, 26 Sep 2024 20:27:40 +0900 Subject: [PATCH 53/95] refactor(static_obstacle_avoidance): move route handler based calculation outside loop (#8968) * refactor filterTargetObjects Signed-off-by: Go Sakayori * Update planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --------- Signed-off-by: Go Sakayori Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../src/utils.cpp | 31 ++++++++++--------- 1 file changed, 17 insertions(+), 14 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 339ee2ed782c9..adea18c726ced 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -750,8 +750,8 @@ bool isObviousAvoidanceTarget( } bool isSatisfiedWithCommonCondition( - ObjectData & object, const AvoidancePlanningData & data, const double forward_detection_range, - const std::shared_ptr & planner_data, + ObjectData & object, const PathWithLaneId & path, const double forward_detection_range, + const double to_goal_distance, const Point & ego_pos, const bool is_allowed_goal_modification, const std::shared_ptr & parameters) { // Step1. filtered by target object type. @@ -767,8 +767,7 @@ bool isSatisfiedWithCommonCondition( } // Step3. filtered by longitudinal distance. - const auto & ego_pos = planner_data->self_odometry->pose.pose.position; - fillLongitudinalAndLengthByClosestEnvelopeFootprint(data.reference_path_rough, ego_pos, object); + fillLongitudinalAndLengthByClosestEnvelopeFootprint(path, ego_pos, object); if (object.longitudinal < -parameters->object_check_backward_distance) { object.info = ObjectInfo::FURTHER_THAN_THRESHOLD; @@ -783,20 +782,12 @@ bool isSatisfiedWithCommonCondition( // Step4. filtered by distance between object and goal position. // TODO(Satoshi OTA): remove following two conditions after it can execute avoidance and goal // planner module simultaneously. - const auto & rh = planner_data->route_handler; - const auto ego_idx = planner_data->findEgoIndex(data.reference_path_rough.points); - const auto to_goal_distance = - rh->isInGoalRouteSection(data.current_lanelets.back()) - ? autoware::motion_utils::calcSignedArcLength( - data.reference_path_rough.points, ego_idx, data.reference_path_rough.points.size() - 1) - : std::numeric_limits::max(); - if (object.longitudinal > to_goal_distance) { object.info = ObjectInfo::FURTHER_THAN_GOAL; return false; } - if (!utils::isAllowedGoalModification(planner_data->route_handler)) { + if (!is_allowed_goal_modification) { if ( object.longitudinal + object.length / 2 + parameters->object_check_goal_distance > to_goal_distance) { @@ -1867,9 +1858,21 @@ void filterTargetObjects( data.target_objects.push_back(object); }; + const auto & rh = planner_data->route_handler; + const auto ego_idx = planner_data->findEgoIndex(data.reference_path_rough.points); + const auto to_goal_distance = + rh->isInGoalRouteSection(data.current_lanelets.back()) + ? autoware::motion_utils::calcSignedArcLength( + data.reference_path_rough.points, ego_idx, data.reference_path_rough.points.size() - 1) + : std::numeric_limits::max(); + const auto & is_allowed_goal_modification = + utils::isAllowedGoalModification(planner_data->route_handler); + for (auto & o : objects) { if (!filtering_utils::isSatisfiedWithCommonCondition( - o, data, forward_detection_range, planner_data, parameters)) { + o, data.reference_path_rough, forward_detection_range, to_goal_distance, + planner_data->self_odometry->pose.pose.position, is_allowed_goal_modification, + parameters)) { data.other_objects.push_back(o); continue; } From efc87e3493e45dec097c7c86845c595f711b6995 Mon Sep 17 00:00:00 2001 From: Mitsuhiro Sakamoto <50359861+mitukou1109@users.noreply.github.com> Date: Fri, 27 Sep 2024 09:55:38 +0900 Subject: [PATCH 54/95] refactor(universe_utils): eliminate dependence on Boost.Geometry (#8965) * add alt::Polygon2d -> Polygon2d conversion function Signed-off-by: mitukou1109 * migrate to alt geometry Signed-off-by: mitukou1109 * invert orientation of linked list Signed-off-by: mitukou1109 * suppress cppcheck unusedFunction error Signed-off-by: mitukou1109 * fix parameter to avoid confusion Signed-off-by: mitukou1109 --------- Signed-off-by: mitukou1109 --- .../universe_utils/geometry/alt_geometry.hpp | 2 + .../universe_utils/geometry/ear_clipping.hpp | 21 +++---- .../src/geometry/alt_geometry.cpp | 19 +++++++ .../src/geometry/ear_clipping.cpp | 55 +++++++++++-------- .../src/ros/marker_helper.cpp | 4 +- 5 files changed, 68 insertions(+), 33 deletions(-) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/alt_geometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/alt_geometry.hpp index 88af38e99000a..f9ee6e1beeefa 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/alt_geometry.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/alt_geometry.hpp @@ -112,6 +112,8 @@ class Polygon2d std::vector & inners() noexcept { return inners_; } + autoware::universe_utils::Polygon2d to_boost() const; + protected: Polygon2d(const PointList2d & outer, const std::vector & inners) : outer_(outer), inners_(inners) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp index cf93740e956d8..e111504795861 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp @@ -15,26 +15,21 @@ #ifndef AUTOWARE__UNIVERSE_UTILS__GEOMETRY__EAR_CLIPPING_HPP_ #define AUTOWARE__UNIVERSE_UTILS__GEOMETRY__EAR_CLIPPING_HPP_ -#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/geometry/alt_geometry.hpp" #include #include namespace autoware::universe_utils { - -using Polygon2d = autoware::universe_utils::Polygon2d; -using Point2d = autoware::universe_utils::Point2d; -using LinearRing2d = autoware::universe_utils::LinearRing2d; - struct LinkedPoint { - explicit LinkedPoint(const Point2d & point) + explicit LinkedPoint(const alt::Point2d & point) : pt(point), steiner(false), prev_index(std::nullopt), next_index(std::nullopt) { } - Point2d pt; + alt::Point2d pt; bool steiner; std::optional prev_index; std::optional next_index; @@ -59,7 +54,7 @@ void split_ear_clipping( * @return the last index of the created linked list */ std::size_t linked_list( - const LinearRing2d & ring, const bool clockwise, std::size_t & vertices, + const alt::PointList2d & ring, const bool clockwise, std::size_t & vertices, std::vector & points); /** @@ -85,7 +80,7 @@ std::size_t eliminate_hole( * @return the updated outer_index after all holes are eliminated */ std::size_t eliminate_holes( - const std::vector & inners, std::size_t outer_index, std::size_t & vertices, + const std::vector & inners, std::size_t outer_index, std::size_t & vertices, std::vector & points); /** @@ -99,6 +94,12 @@ std::size_t eliminate_holes( * the final size of `points` vector is: `outer_points + hole_points + 2 * n_holes`. * @return A vector of convex triangles representing the triangulated polygon. */ +std::vector triangulate(const alt::Polygon2d & polygon); + +/** + * @brief Boost.Geometry version of triangulate() + * @return A vector of convex triangles representing the triangulated polygon. + */ std::vector triangulate(const Polygon2d & polygon); } // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/src/geometry/alt_geometry.cpp b/common/autoware_universe_utils/src/geometry/alt_geometry.cpp index 777a3f8df4c3c..6c060b03b4543 100644 --- a/common/autoware_universe_utils/src/geometry/alt_geometry.cpp +++ b/common/autoware_universe_utils/src/geometry/alt_geometry.cpp @@ -77,6 +77,25 @@ std::optional Polygon2d::create( return Polygon2d::create(outer, inners); } +autoware::universe_utils::Polygon2d Polygon2d::to_boost() const +{ + autoware::universe_utils::Polygon2d polygon; + + for (const auto & point : outer_) { + polygon.outer().emplace_back(point.x(), point.y()); + } + + for (const auto & inner : inners_) { + autoware::universe_utils::LinearRing2d _inner; + for (const auto & point : inner) { + _inner.emplace_back(point.x(), point.y()); + } + polygon.inners().push_back(_inner); + } + + return polygon; +} + std::optional ConvexPolygon2d::create(const PointList2d & vertices) noexcept { ConvexPolygon2d poly(vertices); diff --git a/common/autoware_universe_utils/src/geometry/ear_clipping.cpp b/common/autoware_universe_utils/src/geometry/ear_clipping.cpp index 03b39e9536b62..595c93d073ee3 100644 --- a/common/autoware_universe_utils/src/geometry/ear_clipping.cpp +++ b/common/autoware_universe_utils/src/geometry/ear_clipping.cpp @@ -211,7 +211,7 @@ bool is_valid_diagonal( } std::size_t insert_point( - const Point2d & pt, std::vector & points, + const alt::Point2d & pt, std::vector & points, const std::optional last_index) { std::size_t p_idx = points.size(); @@ -236,22 +236,19 @@ std::size_t insert_point( } std::size_t linked_list( - const LinearRing2d & ring, const bool clockwise, std::size_t & vertices, + const alt::PointList2d & ring, const bool forward, std::size_t & vertices, std::vector & points) { - double sum = 0; const std::size_t len = ring.size(); std::optional last_index = std::nullopt; - for (std::size_t i = 0, j = len > 0 ? len - 1 : 0; i < len; j = i++) { - const auto & p1 = ring[i]; - const auto & p2 = ring[j]; - sum += (p2.x() - p1.x()) * (p1.y() + p2.y()); - } - - if (clockwise == (sum > 0)) { - for (const auto & point : ring) { - last_index = insert_point(point, points, last_index); + // create forward linked list if forward is true and ring is counter-clockwise, or + // forward is false and ring is clockwise + // create reverse linked list if forward is true and ring is clockwise, or + // forward is false and ring is counter-clockwise + if (forward == !is_clockwise(ring)) { + for (auto it = ring.begin(); it != ring.end(); ++it) { + last_index = insert_point(*it, points, last_index); } } else { for (auto it = ring.rbegin(); it != ring.rend(); ++it) { @@ -417,7 +414,7 @@ std::size_t eliminate_hole( } std::size_t eliminate_holes( - const std::vector & inners, std::size_t outer_index, std::size_t & vertices, + const std::vector & inners, std::size_t outer_index, std::size_t & vertices, std::vector & points) { std::vector queue; @@ -565,7 +562,7 @@ void ear_clipping_linked( } std::vector perform_triangulation( - const Polygon2d & polygon, std::vector & indices) + const alt::Polygon2d & polygon, std::vector & indices) { indices.clear(); std::vector points; @@ -592,12 +589,12 @@ std::vector perform_triangulation( return points; } -std::vector triangulate(const Polygon2d & poly) +std::vector triangulate(const alt::Polygon2d & poly) { std::vector indices; auto points = perform_triangulation(poly, indices); - std::vector triangles; + std::vector triangles; const std::size_t num_indices = indices.size(); if (num_indices % 3 != 0) { @@ -605,16 +602,30 @@ std::vector triangulate(const Polygon2d & poly) } for (std::size_t i = 0; i < num_indices; i += 3) { - Polygon2d triangle; - triangle.outer().push_back(points[indices[i]].pt); - triangle.outer().push_back(points[indices[i + 1]].pt); - triangle.outer().push_back(points[indices[i + 2]].pt); - triangle.outer().push_back(points[indices[i]].pt); + alt::PointList2d vertices; + vertices.push_back(points[indices[i]].pt); + vertices.push_back(points[indices[i + 1]].pt); + vertices.push_back(points[indices[i + 2]].pt); + vertices.push_back(points[indices[i]].pt); - triangles.push_back(triangle); + triangles.push_back(alt::ConvexPolygon2d::create(vertices).value()); } points.clear(); return triangles; } +std::vector triangulate(const Polygon2d & poly) +{ + const auto alt_poly = alt::Polygon2d::create(poly); + if (!alt_poly.has_value()) { + return {}; + } + + const auto alt_triangles = triangulate(alt_poly.value()); + std::vector triangles; + for (const auto & alt_triangle : alt_triangles) { + triangles.push_back(alt_triangle.to_boost()); + } + return triangles; +} } // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/src/ros/marker_helper.cpp b/common/autoware_universe_utils/src/ros/marker_helper.cpp index 507be41dea7bb..378dc795a7421 100644 --- a/common/autoware_universe_utils/src/ros/marker_helper.cpp +++ b/common/autoware_universe_utils/src/ros/marker_helper.cpp @@ -16,7 +16,7 @@ namespace autoware::universe_utils { - +// cppcheck-suppress unusedFunction visualization_msgs::msg::Marker createDefaultMarker( const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id, const int32_t type, const geometry_msgs::msg::Vector3 & scale, @@ -41,6 +41,7 @@ visualization_msgs::msg::Marker createDefaultMarker( return marker; } +// cppcheck-suppress unusedFunction visualization_msgs::msg::Marker createDeletedDefaultMarker( const rclcpp::Time & now, const std::string & ns, const int32_t id) { @@ -54,6 +55,7 @@ visualization_msgs::msg::Marker createDeletedDefaultMarker( return marker; } +// cppcheck-suppress unusedFunction void appendMarkerArray( const visualization_msgs::msg::MarkerArray & additional_marker_array, visualization_msgs::msg::MarkerArray * marker_array, From f0b43d6ac887fdb9a5acc36fd894f2895d1729e7 Mon Sep 17 00:00:00 2001 From: RyuYamamoto Date: Fri, 27 Sep 2024 14:46:45 +0900 Subject: [PATCH 55/95] feat(pose_initializer): check error initial pose and gnss pose, output diagnostics (#8947) * check initial pose error use GNSS pose Signed-off-by: RyuYamamoto * add pose_error_check_enabled parameter Signed-off-by: RyuYamamoto * fixed key value name Signed-off-by: RyuYamamoto * rename diagnostics key value Signed-off-by: RyuYamamoto * update README Signed-off-by: RyuYamamoto * fixed schema json Signed-off-by: RyuYamamoto * fixed type and default in schema json Signed-off-by: RyuYamamoto * rename key value Signed-off-by: RyuYamamoto --------- Signed-off-by: RyuYamamoto --- .../autoware_pose_initializer/CMakeLists.txt | 1 + .../autoware_pose_initializer/README.md | 1 + .../config/pose_initializer.param.yaml | 2 + .../media/diagnostic_pose_reliability.png | Bin 14816 -> 22774 bytes .../schema/pose_initializer.schema.json | 13 ++++++ .../src/pose_error_check_module.cpp | 40 ++++++++++++++++++ .../src/pose_error_check_module.hpp | 38 +++++++++++++++++ .../src/pose_initializer_core.cpp | 24 ++++++++++- .../src/pose_initializer_core.hpp | 2 + 9 files changed, 120 insertions(+), 1 deletion(-) create mode 100644 localization/autoware_pose_initializer/src/pose_error_check_module.cpp create mode 100644 localization/autoware_pose_initializer/src/pose_error_check_module.hpp diff --git a/localization/autoware_pose_initializer/CMakeLists.txt b/localization/autoware_pose_initializer/CMakeLists.txt index d07858d0b704e..521b07f1d6fc9 100644 --- a/localization/autoware_pose_initializer/CMakeLists.txt +++ b/localization/autoware_pose_initializer/CMakeLists.txt @@ -9,6 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/gnss_module.cpp src/localization_module.cpp src/stop_check_module.cpp + src/pose_error_check_module.cpp src/ekf_localization_trigger_module.cpp src/ndt_localization_trigger_module.cpp ) diff --git a/localization/autoware_pose_initializer/README.md b/localization/autoware_pose_initializer/README.md index e4b9bdd28b2c5..2d8f0343f3493 100644 --- a/localization/autoware_pose_initializer/README.md +++ b/localization/autoware_pose_initializer/README.md @@ -40,6 +40,7 @@ This node depends on the map height fitter library. | ------------------------------------ | ------------------------------------------------------------ | --------------------------- | | `/localization/initialization_state` | autoware_adapi_v1_msgs::msg::LocalizationInitializationState | pose initialization state | | `/initialpose3d` | geometry_msgs::msg::PoseWithCovarianceStamped | calculated initial ego pose | +| `/diagnostics` | diagnostic_msgs::msg::DiagnosticArray | diagnostics | ## Diagnostics diff --git a/localization/autoware_pose_initializer/config/pose_initializer.param.yaml b/localization/autoware_pose_initializer/config/pose_initializer.param.yaml index 90ec78257e6cb..e403160fb55b5 100644 --- a/localization/autoware_pose_initializer/config/pose_initializer.param.yaml +++ b/localization/autoware_pose_initializer/config/pose_initializer.param.yaml @@ -6,6 +6,8 @@ gnss_pose_timeout: 3.0 # [sec] stop_check_duration: 3.0 # [sec] + pose_error_threshold: 5.0 # [m] + pose_error_check_enabled: false # check initial pose error with gnss ekf_enabled: $(var ekf_enabled) gnss_enabled: $(var gnss_enabled) yabloc_enabled: $(var yabloc_enabled) diff --git a/localization/autoware_pose_initializer/media/diagnostic_pose_reliability.png b/localization/autoware_pose_initializer/media/diagnostic_pose_reliability.png index 7e2d8dc5e632a6a2cdef186809b7df6a8b275c46..cdd38242a0f24b22b353e6b052e4a68607a99bb8 100644 GIT binary patch literal 22774 zcmdqJb8zm^`|e#^ySBB9U1Qg_cWv8!YP;R-Q`@#}+qP}*8mHfbIp@6d{^tGv&1AAx zRx-1al}vKqpX*A(739Pb;c($VKtK>BB}9}!KtS!j>-jKH-%C-0?EkDlI0;EA!+cL( zFvelu>o@>Wb-*7xQ-G_1qX~$at(}bty_1oniHWU~xgFpdyqoWPCl(83b%3y=i2=aE z&X!o&!p7vg5d?&pk(u?dwSn>XQAQXu$5C=2F%!#CY5*hKVOR(g%W-j>S&lj>2naEV zq==xhTgJtvD*#;u59-FO$T(?zFnD5%O9J!q!N)V~U|FhU#Om4!9=jslgT}Nc#M%~51j-KlEw1GBoCe?GH zuBM3X&_nsn0-GE)xFa-X1<1ukHWHyQdv?(Pz320Nswvb%!Iwtvd6WT>7(VNo zfn50+xo+|bTcOthC2l(k0y6)CV}5evZ(7eF=yMe4@xgol;$Fd{E7O|#%X%;`@y&rA z9waPidrQ>emqQxBYIa5Sh}{WzQKsRqUfYfpuIyFW8{sA#+Ow7;ZzXrtLF(u!`plgndZtpo0G%2%ok?B6h`SFp1GTS>FZzna z{nX|+#$Zor8KCV;hE?}x*2{Z}7NKwX2KmYoPB_px%gQ7d1ai3}H2@Kudq4eSG^dGN z)3+WKdTNmDb_pY6<(9ka%ZE6P>&VN~G)pf$*M@AzsycLO46!huZ{VRbycITEVvQAn z-YO_Bs2mYs$@UOMp;&)T9ZB?nN$PcxGq2gtTkr#ljyL+^?Hob$zKrC?q1Ion(myz# zfu;%H??wEI?}6p2^lgg=-);#eusa$4f{t}Yy^Xl!>Mg~Y`Y`v>I726-w~{6&r4otD z3OR`NA~5^mkmse0f9PwC!kt!}eut5A;gPmOg#zT4s>jNkNZv;? z_l@}pWE_LO5F3HEhxN$M^vF!?`37bwET^DERYYJg3)uN%--gkXqZ;F0&sRVWJYi7a zldm7PgO{;;2?cCif#AS$v4IDmU1n&_2V?i#7#64r4ZU4}J2a zAX2Wptlx3_$$HWHaU<#N*!trqbWyI;BC7q2MNad*kVN?@XR2## ziY=*!7u5pXqaSb!Dr=XX=^>hy@!VDC^WKSntM0~m^aFt_L11y}Pt7$F{I6r*l8w4$ z*TDKJE=rw*FPGL-xygP{#Y2U@L0rv7B$qCNn&+iIGzuZo>Mz}t)cdxOz$LgTpIGb~ zl7GE#spB_MDsuR6?NREPkUcec{Z`NYY|V$g?Ou41YmWE1Me=K2+Iun zq335V!YjbcPij!*71ezX@wzOhZRDo>hqIx^pU6?Uz|gq3f`UYG5OAJ~k_3TZE81Tj zrlhH3!SnzKavFdO;NCv3P>D1rN4P%QeNu$k=EwGee+(;t8grzlMj8pt+KeSL?eOir&pDmLX|p>1*{1+a15@CbRYa1N?!|Yd<1* zqs^5{Ey`*TRa2Zj+Y?wMH1P zko@Utjj6=P2r4k?|J_V|KkFYa4;Gvp8j#w_=CNckUP-#3W@5+)+xs|dW@Cc_+XWk1r>J`(G z_@A4=;J6$l+G|8YNltTcD$5e{?L^*mZsd;uz%(qoS1g05YB$`P+Ru=uC`+oH-V5i?m0Bp_4G z&OxQgX>X{x+nftgVq^GqtCfby`|q^phJ(y&`)g~1Y$Yx(j+kN3euz2vZ~JV2I%>&y$ghvwgO39B2{rrraPmU{;9w~ho?LIZ%R5;=2oNWZbca?pG ze{`F204}o<(&g~!#qlaf&SSWUC^95*@s6nw6!CVC|Azykpx9j66?Ih-ajEx=l@qQ6 z+TPSK@74k+WCj`OZZ(8?M{s&LLsPL(U3H+C3TqfY07mUlW&l47a(qFQei=HGCvAtT zf7|(!W=Sm;C7*(f+RZ`4UOf@%o+sF-#_7%_@ z`OwUGKTJqn+Sl#)?*4Fymy;xo`^n{oE0&^YyT zgcVg}Z1_e6S~GJvWV4OWPlfHUe+I;|4+X+efJ+O+CityK>+KxTDjgOD`L=_)Et`A_HjVAP`%oj7!yn6P{PJs?;PV|ZXp=4(Mk|SEaAG|98Ic4=58! z^Da<~Pn@Z5%fpAA`|8(t7r)mD%YCz=t|_|%Z>njHECY3{246VFgVM;mw~#{ z4=1gr%$ghWDPMiJ+uJ!{Qo#w=xL%sA8hLX)lboA|WSA2v0i(R6x0BD)HIDbx$`pd5 zn3F{!Mu5rpY#vz4H$#PPKYJBRwjQCX<>iJA^sP#!<7%3dRsd3wEMZ8|pP{_d$jax& zdOZ@W0z>aF(`S7h7%16y$jLwg10Hrr0&bg;h@zN(Y@|w{r<&qMTt-x|Bmz^i~bo+3^66I3P1jgNqy6hjB)ZjS9jCQ+n z*u=ec+7MD$9t;tw2&;@J0*~%=dJZs95;KW3;QasK*P8p#3=C`)0wh`~@@0wzv6&pn z#iH;7_%=QEK8CuEG9?n}FDlXfGW_<39l2P{SyPrrc7w+sUjz%P!0nRIXh;7Be}*KX zKIx^M8NLnI8(|0`#Pry?%1oeIzyxwSQA0hwukF-#O)5|{u#PFi2s?}q^J52lw~ll8 zh;~P-^>#g3N0ypB?U4OrAnd-Kkh_Rl&I7E*DfDeP{^?<_0rim8j!3BlS9~8)mf6k# zRT)(XQZL*+3FoH9OyIu~cy2w@(_aX77d)ht1Th{7RxZ<(YPH}+nYD#Ex(x5l5VKI{ zeZk6L1=WP-RhH?^e4n;R`L%ze*Q~#NVuavPPT40cq)-*We{p9b+ZjwKBRSL`?Ml6K zaRjEz8D5_{O&HM%C+7es56F0+S0#EuDX-4%%ZtV{oE?5yrU_&iCmfpD3Y@j}!J^~; zjiI;&uU{=CtW*X%6PmZ&lVl%v(`>8D@y>>hp<~tq8s6eVg{F`w5UMM?OI)gW8hoJ@ zc;?L~z9~v%u;1EhPV_*_2WSGlqTH?OB2d*%k0|0|0aH;E=+u0CK7A6gz9K-FW?~=d z!U`|N@)0jb(<80BMYbnu1TWuW(CA6_2YvIw4jBE*p|JM(>v0k{0Ec7Da0a?DClFOo z=PR^netW36lHdO}0+Tw{9os1+vcGhPFJ-9@6qVWDepe>Lr7VaT3-Ky*+n=Qr6+Z)? z_$SY7ldrhfx)+^hw=lw)62Txnv;F>o3TNX+(J@##4{#TT?C6h0uG~1$u0?m~ zb138i$3N(l4GsN;M#GMuRW-q`KdX;c(c`BD z_w&ACA0dC?4tsI!r<+E__!TFopOPoZu0zYZ!SK-6IXGL$HJp@n#3h3MwOX#<-@7uFXQi!}Z`1JeWKw`qY5NCx)G z8zM3t2+zE{nDrP72B}nUnykhDY(^z~u{5nzlr1Vx_tdfw+q?!3%^ch4t}?az{3rWQ zP4Wwe`cgsJ+8y^E(_1rXAOU>3S$&$gC})51o#rIc_BdNZLj1UzS&n+S;5_+566_0f z^NXDn(BJ5}1|z+0dC$8nID5Vlm43|BjZRBWoWo0U*9HZyiPQzfL#jxz(c^n#tNOxl zP>voL5hN9y_KB#@`zeS!Zme!}>THh~(D$DhR2C`Oh&D54wh&Jl4{cJ?ysFrZIA$#^ zm|LIdHkt-%cvbYr6)@TU z=C8UG_pJP@$xWn+(-2%5?Hr?X@OL`BrlK8V5src_3F(1aBP;VrXY8`3eYpS6T1{jG z0p4Vn2BBb_O0cE7#GGzXrZjmFZ37e~n#;R$`^ z&vRI@i#p$&pqhaA0sK#nevPSX)z~bT*zb$;Y>)3-zmZkke>YIkjB{lo3=|S_;3dpnj&KIgC{g=a+GL-jRw}{%VTxRzjTwuhV@jlN6Or|gaO>6(>$I1;#yQbBBVGhA!Qhg0IjcIl>IBNcI zk3l8_!w}kX2O)^^mpHcLs~%y6rH80YJj;IE^Jiw;Ui3w=o#=CN>eNoUgsi^_r6lUZ z$?!AtJM{E8?TR(&yVhKF_P}aT!1ZXNaul`V8ea7Vw$PXRYK!wDdDSNEGZIp; zjl*}^&++C`zdY=F-SNJe{t`qw(U*@d#H;Iq-d8`{v&xS?#lRh@Hub$Z%oOeapmilP z2bosg;S=^enr%Z;1_a&wv}_oCteUFab^ZpKbMc;!2%kVH9Xyk=r$ z-(z?>p16$a5A~tKQMR1L=Ds~okCvtENU~E%Yi$&WUW8zBI~)#qi7f;xYtf0&w^NXC?AIKEf6@Thy$_#ZL^kPG#uA}=mD7p=o)NB;r zFX#YON*J9>8*~6aocTtT=Mq8aibujI{-j6I5>XO(xBy}v*Y(pd2qKSR*Y1^|C$q-16Yu>i@(lKiisym ziE~#guz$Ld$R7~N&tp$tZvQ*LE8MiGR8+Jvke}}PTk$nZdc+$>Q9N*S>;uH!GnE7` zC2bvQI?l}1x60e%x*IC#v~h}|KN}MO%{Xh@C>DJ-TWCIGsoI&N91L|l%XqjPMe`=! zynaV zTz-r(<$0lO66A62x5=bUIXNQ2OU0BpBN+!v?LN3V>~nV?o3<6WTicq6yAt_Ay|&H? zk^G1YArW>Q#!vdwav49jAemFpP$=(4k#t|13zjqI3-V+f&ziAw!1ef@hK@C12THXm zG5E)gC0|#{7riG8=OHo)Yp52QT$U!oXN(ZDv@v+ABw2HDY_}Y=C1rRXV7VaF0<;$$puPt^+PMLUc+Lf2^7zx zQ`U2CY1<-})RG>mIG&NI8s09jLsj6Bi;wSVc)DBBSM9P!kQy&sWVds23BQVS2-U0F? zXumdeMliWBz@e{-Nf_K|Gf%I|in{vLcoIG*4#{8w6mkOAI}W)eWa2=n_4PUsJzVJ# z6L(=uk>0S4j&(pXHFM;T89CQz9Ft^|{3H8UJO@CS!Q1(f{pI3v>@6Y&IZS%cL*X4L zsXy@YG{PXs`$4@_YB`0{eFIWGZ4VK+OX0-lDftxk3E3av0^~)sylQW#l^lssCHdL0 zc+1^-)`lRrx_nA_j5+FH80j&dV6dZ!L}VyQPAzvX*$3fT{5Qi`-jIkTp=8vK@`CHZ z;tIr9)I#+^Vx3RHs#4i7n;)!Ml4v^=D+M)947>V61DAKktXNw+h+!Dydy!8^MkSyLtH-Cono zlDG-5N?>bn&yM4A&+_=8rTkSrT`Fj|y+qvx!+C)z+iMthGF#y`2K%>{Pxm#CJ*bga zEvVASpZ?(colNJQy3ZEfpjn7e2KJ%MGgsM>$X)C3f3^xBpQtQi;0UC*jK@Z&q^dpK zs*NBn?&*sG)lkR98=R$&$N@yEyEET3o~)Db{Bn}Z5kwrQ36VU;fz&bwZ6~J_Hv5lX zr2eW>k=uuT>FeQ_)aQMm1$@8_7j4ZdxfMbC@1HQiWbGA`q^W&uIy@yH)TdVVbM9ys zJcG4~3lOXoY=_LUL;3iaMU*)Op7o`Xl2hLVL2fU&Wn)(jF}wpaJL~?sg7b5RwfJMT z?E}t~IS_W)_83XhJ(_ArHD47hvFoFS%m!`SNev~Aep@4L9xAJ~gP@W^fq*?y99VEV z=UP!ZZozKB`7^|oISz-L-ArPo!!-V2du>h^jKZ7q?Zbh>)zBpMsY++Z*-6ZZHS6fP z3{yGzVze<@4kXn*V7qfyAkowFSLcUYD20V#U(qL>Sz)eSC`HGewG;3^kKuxy?-xms z4c#{=8bxk8DEK3M0dTQi@tcIMp0>baAG~c(`@L_UjOP$3mgeRy$?Z{24u(nrMAXqH zFfUCiv3el08!gz~Ax@e*IXyrds59^fEJ_Czn5Hs(<&ijnd$yEX9BKyi=H@;)Ehq$t zM6CUBo3#(F$5sCmAd;BLPrF<>?U8G%d?dYp;t!1!!CC7fixZ|jQ_(x6qcBr zf*xP{)H=UV>2&zBz5ZpPT~@-BFndVs%JR(ny#9IsQu}2X_mj_)lUoH)BtX^t%3}7TA}VRVw=W)SV;`i*AHh+z0;{&kP0KAdE)%r2EHujiRmfYS)D1* zN6snG7!=-nidEe8&y`RJ*p-b>-=hlIY;FjD23x9xj7;bn8+PJHMXX?tagFdOirNUr z^sf2^h4Js;JcsV=nv;4)>TA#`lK}l>XT)No=zT6;#oB-RbZPTwd^8Mz)JAN*CK_b< zQ@BUF%^({y=G{F#&_=7oDc~@yY*+LU5t>K7tg`Gv$vW7z7^Fb3my_Jt*lvXLI%B)u zpCHe5KUX_4(WYQ|H<*pYmt#UYuU|4YH=*hd`eYv{oV<@ZUTwclQ>h3rAOt~l%M~HR+tnCI=Sv~90U$E4>YFTJ z^uCdSO{4N*T8pr9hCw`nD7t9X(C*aV(VG5yVR)7 z#4y#d%VzpCMFRT-{>+4$1=}5e(^FM%fabt2`n9l9S%R#~N51daMQ!8!&%Nq%&gBBV z`CHvS3WVS@RtF#3e?-Wy7&TgV7DzMKitjjo+vu|OYepfs&xwgJGw^8*sqak0VLIw& z``(hmuK!o0#VXjf4=4J#$eoW#>eRG=Fb6Vgx;LyTkXoN7@<(FE?z#yiDlG9Elp%Xn zZ`V0;^+&}zo6gE#QQD_{O-v9J*zV4Vy(tyO$1veT6$Ek>gzfebvg_Dp zi?)p`xSw7pI*2t|`WP5dD75J(&S8=&c5zU`K!~8tDS80zBz)#e;QWNXoTGgAE=0Ni zQ$rl6813`zIiELpnfrm${A*v+kV{62*V@`SX8wA|@xoN8^mhH(G{%y+l=#oz)z$P&=}CnCr}`Eyqk5}XaaOe-BYqyF(QMNS-gjrk4u9^&wg zclzCpQ=`A?e^&@eHGRX`99p{x4y4t87l7xDz4b0?LN_i!+4d6D>{w1URoUW3xR$^6 z3RVXvS`k~iW(RCGPg-KTh0$0uK9ZB~UUjy&)E$dIAk;%$W%fk#dK&ElGc;dENB`}d zp@%Dm5`Bf+@o*7bS|$iOP|=z%YO1$F?gO8S4W@-_u^nvgu^Xx{;5|E$ng(feFKhTw zk~5$9VjcByxzy-Ep);X8xEX>P(mwUaxaMU=qOXHAdkJf1*LzqxC_+9RAWhjg3+M!# zItl49i;g0i>AZBTbN-a)x>Ea>WftpiXKvIb{4cez=i}zUZQ}2oP#n2LS=>qY`pt@q z!3uDf?S0p>GFa0J{`$^_(QA%L@>> z&ykrH`jj`88x-L@#df|98etdSpm2y>{#sU<7(2~@GCT-Q6O z1mw?#4}J>?_|v12nGCymSZ~$G6UYnPBT)-fe1X^t)W4m)27#l=dkIzU-r;F9%u|Pz zZUFh#6%RO;F~|^$`ddH43gDR(RTi_R8B_^%(cV5TrlE`Bj1?y+*wa?vQUxqZ0}F+> zm;tvbHDG>vou{D2&&(LhtJ@rf#*%!*m0iF0?0G|c58f9+ugeD|{n0f4u|%)drQdRb zXbD^Wo@vXR6E&3CDM@nuHw%e`~gTFB*7W z<0eCvqUsZ%3cf_rIODV*#9og?#?FpeE52kP(DHg~lEC-f4L)W?#84Ds{p`Fma+l>7 z&e8@O8MQ30S3N(w*;}ozf#{dLx+VBbm5zz|1gFn@p*h`pQV5MOmchpoqB`rYxAG5p z57_EBFz6!Y%x}pqZ5YaJ?5mBR7+rGJyuf-wf9@ZrF3%@Cw+Ah3FiU&&^#9Z$6X;$} zZB647>X}ng|6z60<&);*eZ8H#jf*fl2-~;oWIsKz$H9Pwi2frFF`%ptDwa3 za(_qGe`aE01p{%=Bzb78bkw+h3YeeYho&=2fru?AxCTMTB(6G4p$>-eDfAC^mG++A?&>lRH zKDn9w`seb6q}iD1$zyJ=_07XHmNkoDSfUco({{pR&xOc@lP85_O#o>l`43_0`1-7- zR`PuHXmA!soD4r}z>g*N3g$D$`AMG7z9tHo>c7Pc9TXC96bWNq41;9xKSpj>c=CRO z5mfA)DB^b&t{ZG&p!Il4$kMi${8`X{`Igq>yZQ%$PZSHwL34XGhZ9Q zk}5LXo)YG~Gh?-#K|*pF0P1i%c1Td+uhrcb*Y9YPUyk^`=q>8aYH( zt*6~Z2O`zbey0RT^(?{_6n76QB$$!tgX?J44a^ywP>iZpO5Qgpk{)zguAL@B(JP_UFlpMc|7dBThQB^YFp}W(r3=t<(WV zKF)BqPR|j=uUow{werZ&VEwn= z2U-o)$&{moSb}zCa7EQR`X)&?o~IoHM{86U6LS^J4$db~QVGVm(L0FeCtrKaEjuAK zFv{;c5ED(g)AB>1r91pv&(A(*=iE=tTp_3yJ*}h2RW{2PD#3NiZ{E!-1$$2fA>LnW z;&;$g#3UF$DAa?r2FJ@;9)LQTyMkA);)0y#Ko*W^k)5{={)Y zpEJx$UW^Tb&RiIC%8c~o4F3q^8_H?66aw!lxDU-F)w@e_3?WaFH84((>=TCXu*xU(I38HDURuFBu2ib-RO2(y1OELhu zh~sEyEbZ4cYevOwb+y8l3wb0?D&xp+XX9WU3NJ1@FilRWk=FGBK1-m4a5j+pR^yE30Jy|I;_7GcVrTsNXCOMoRv|JQ$o|Em>{Yfv zQPPW@gB&nx-2HuIVFgcZ@s3iW0OYNk(8^uRsf_uYN9ak%ZiLU@aLEcm?k4(=&%NsU z>-fJ$k7IB)U?)57=nEdiPW83OE7JoU#_48qg4paykV;>C9I5d$cBt+8(t^2XTGS9* zj<+R;_#$82kWfjX^7HL@qKG1(K_rq0VQ#!zhZ$fP`nJ2)MYR6WzO#SuF2L6HZlZD5 zxQ`1CSatkRY6KB#jarrutep(`zl3DQ5D~NzYY8N2ye+ zv-n~M<|S(YwEl*2mYs^{ zoh`WM#J_!kiCFfIXk-6vr?vcwi7SQ?tdI|pHhl3MR^ zeMrvGX7I@>HC!csR@A>@G@{>l?BzX!UJN!Xm*^C9{{V1<)bA46b*I&WF*A zc}5QS1oyu&b=+ILhujdSK@1~QP+%Hx9 zfEy)IadD&X?R#m{RM#0W)s(YG|1aaiTRW+RlC|2&c@eP{n18$w=2eX`X0GQV5OCvx z%4WA4%uM)2NRZwgm<4{kD{(X)C5ED_N9qnA9CtmS-<#8{usRK!q)A}SJ5GG|ak`Q` zkW8@FO>@3?eWJLA`w9NoqrZeJGS~E#8&?VUikf5^H7~^%DY@mZrGc z$napn=!3NHWcm+ky zHkC`PqQxG9LTW%jBxc$nH@=9O|NT#uYFBirb`8P&#-MyPl(K(%5h7rIupI2s zLhCR@oF{BE>ur4;7qT)yHcY}lzh3UHPnH?M3Oh3O3sr=TP7%H=FK-tA)+0Y6`jytI z-&i;Wd9Df+*9WV)IeE1aW0tzJp;8)LCsdW}UPHhGk(Qw7qvyUNVNzWPivUw{sM?ksn%aezjKLO)I(iCM-+!&|`BAXE^hz4;Lg zOuY4zpCl8j_miKv^d)-`@YmRo`?susC1r;{0KRyX8Ci3n*Vhq!cD^=)6M{Okd2%K} z$$$V{kjPNRui<|ClN)+Um#Xj^N9P&uG1rqb01V6V1 z(Yc;Ly@Vo7Fba6&5aqIgx-L1oRWrPL$+2e4J6X^080Cg1+TpM}L4a%6UgY_m(Je2{ zS65{w?}=%0y&bjo>cemZYiz+TIn{D)G1#|lwV<;TG7%Lf82nt8R~CHK6VGGr52zHJ z6()Ry{ve7Smnwd*I+^6w{uarh`A`42rPwaPF2I6RP=V3Oj`xI{WdbwWvbTUQP(;F` zEdjxhNehEQqM?*6yf~7)p1mftJit$+a4Ty{ZfT(vu)BtU#voTFhbSQVeO{xfkC+~+ zgsw>;wD7BI!jiD47#`eQn&nPY?Iw^&o7tb=5x#=ez6g~TL1o&M$=1LwQ*c8M5v)=U~zq*TZ zn6P6Yh;@Izos%ol0GAs|m1a85_OZbUQ6Km#zV*%JYKZykKzwi!09!htpl7zW>RAI= z5Pv@%*~~B+UF#vN!VN)qi3oYqlHHq@ZCL1His!f+RNndhLl?)C!ft*sHL@<|o9?Gi zhUfbqY(h?!(o(xy;oTOCq{?g$Pmh;;8<2TUh!H1t=9-cca&peOrBn1tCD9O}&?M)u zxifYx|6@LIA*VW>G2huwzTPKXcYVb8b*UX7wUH%SRCy|-21%_Ua#s%S`g8o} z26qdXGiu4r$tdC}^EXu$rx!^465oiUI}V9)3Ln4auMKY$DG6BhJ=Tdc9!rW=SAwP4 z@URy?`V;WY^*Lg3rQ)(wh6s%)C4cX#bzPPbEBE39W^Rbxr5`U^xXc1WA`x((iQUV| zx{fTquTMOV#_9$)u^)|ua`M@vY_qKKtAkvHnK&f*+P8VM`4DUh88|WGW{B$%6b_pX zT@%nKB6-;Vix!PKo^?>xfId@%dSk`3*92F>ROHD7_RuP><31&^1K7cs52`cnGo)id ze)tWeZt=VAe)Jetu`f`QTj~g{dOSnvddG;SHlCt|yn4(DRu>dB6xmfGKO5=p6^5%7 zE#)oc;d@SLPZES0y6JCMj?QJXK+ewtpKKHcC#egV?cI zl;Vzf_^O}`TOS001AE&{EF3XLkJ~RRxuN~eF7>=9aI8pc&rVjXqj0*7GATqyjDxgO z{&(=l+B@3BhST|{{FhnYqfmYHmRJrPqYigipjy3y$21@Lm|t5T_leRJZk%x-T;>|7 z4CF}jqtUiI&M)V*{IpE4q564+?PByizLYTfRvXfgd{2iPLePEK$ZAt?$vAy2SDL9* zke4Y@?T-5dxsLx@0)%X4D*?0xOg9$<%Q+LgJ?aGfkb-D9i`0yQ6VQXp+^U3P3u^%P zw6Ex)B%lCrHKtOr;2mSR3ImI__Y^|7aply*OD8i(KpC@m0?<~23d%#4=sW(S?ONn| zOs@CA_XTa=zSOXoGks@ywRFZLg+zSnKDZ9IKPB3oQOo&}1tv<8%wJ?LK9HnkJ8#2U z?J>eFezw$^QEK8Q<(Ty&*OJD97;m)TBCodbXgb>lSRM)~<_hY`d@;kVYGD~Js^HuK zty%=tlT_5dr%qU{4SLYFn)yo6ChUZ4^=ex%#^WgybSeX`v{P~Q?jY%-@zEtJ5!jFnI1`$WIcR@ldV5)hsS7htqDk!{oQ#>c}s4w+FL>nGCo|%sq^uH~=sB$Uaj<$g)}7;x?y(_j>{)cAP-vs&E`}dz*Yia#Favl*Uw(?7 ze{=imR5)2`5CZP?z~H3aRK1|}y!~zPkK^OsW5Y&p!PJ>W* zi57ZrTtwDy*WUs3B4MzZgWJz~hkI3K;}xOPMfC0WRqTFSJ<)ewMEn+1yTq%A2O11V zZpGldo{DP?l~M%=D{@8_-1{SZK}S=#y(&sxqskPC_Ed=WHE*VW(P%9VmHa}=BAOFY z)|_@=k&U!81~lhJ7MQ?QQLQDYT=r34277#eUG@yW!a z21ar}W-G<*IxybxJKy1umCKazrBdu&R;R<{68t0oq%_-8A;jow`~vemKJvdvv77nK zKWu^H2WYln@bkvCsE}Zwl*RW<*rf1&+yq8#f8oP6(VOjzgd1_aQ2R8@Q9qzrD-zeu#m?d-CO`CyP^R8{FCnDa!`#5#{YK zO7tYfEHvBYGwgPz4&JV2G{m#H_qe8aNLWoZc|${i6vBP#MJ@D+s3rF1{e$7m-$6@l zf|xv2HgOUnd_noQHH2;4E4ACGQ2*%(F`P!)%%gQ(k<32^fPm-dmr9=Moq=Y#wslNWY5la_v|`{0nHueSno{D1(-E*pf+0?Z)Q#-&+gid7I@7z+GD35=W~B- z25GkOr(1nwl3VZ6e`MUQbJ=b-C0ZRto4(btE4#bymUTEvf5dygFE+q=X7h^^9euJH zt&IHXH{4uel%)Jgb@SvxsQi0rr-!2bWjSk`v!)H)xe(gX)mAO;=PB6NT7BZU8M4{2 zgqo%Gc>PyMy4S-Pip^Fv`+b)=1r_THAFi*=36NcpO)=jVz5_4O$*!zm_K01cC)f+i za{BJ3US{wRP2AzE3Cd@YOMl{eI~N!T5#NhMA*@ZZ%@b-=n;i4)Kpy(=>C2n$o9M>T zIgLc6c#vq1M*?b5Ya;0WP1I8zDWxw9oV7btop>$b3&Es+@c}Z;fcyOyY6dK*6HAI{wcON3mQjoCijenNA+(2kK}3wFD^EPvUj$d6pUc9 zl|v5niJjlztt|FLp`U0fO@^GZNca*ra?EFG#BEBmJWpMmZlwtuJUz5mI5SE)p5Wce zWiSX4k15a509bv*Nc^b@CIga^7({zKz5o8POhI0N7Gte&zFxV)n2UGz4FeM#!EY^S zIA0S){|&Jz+_CwxW=*_AyNil%$aV#gbt4HcoBmU%w!n&9$r0GUJ+JNXwV&c$lNr2> z*J5h4(_$@ABeL`6!8-=`FuFamFh7fVBdIS7u3r8%@J7hZSffa83CIYHJmS~CJQ%7; z29cO@a~jf|FwY;TLp~?Xn+jF8*#wVJMHWBYXoqECM-ceORXO9iXSz|&gKZUkG0?2| zYkGtAXNR%rV#iOG!!h@*iFTqXxA^k5R|VK`Oj)0)QnoIUX|;OvAZQW#dxyP@jzRbNAO45#axXDnm8q2feOqsh*IGa${gZw&Jd-4C!*oPfXhg6KIFL&@}v2FNq{29`?-uwBRBPF^#po-R!Ok3U8wDkxaZDACD472|6adLil#HQ3qtH z-$zA7eKzlXtNZ~iN5Oe};iFm;`1J3~VOF}-G+8Mfp-2R;c7r4^3@QKsYm?17Wz~z< z-BkFEC92KVw?j-cM&kb}QflPfct>G5TL3ssgq;Y@?ESN~6>u_qA#B|?J-JSpVXt?A zeZ!lWL8Lt$X|6*)k)@ZgT2HvxU2n4%6_C3a#*w`hTb+#d3kcS&8TTvYr|+a*pS#7_ z?CywD#^J}}!FiJv2lT_&eQTS`4jeuVOIIy_mkoV11gRsv5Vw~Xh2jbV^rRRbUgzHx zk05um^;PS``-|#r?1=B-}cGX3QG)Q9^9Tf{wJ*m0-0^Jq;w&Lg%;P z`V|D5ozSw5q}*g1>(+-G@SA$`;l_(0_cUz&W{bMGAHQ|4XLMUV%W13$ZTF=BXt|Ac ztUAm5m*|2qvqygOoQ!k@`YLK^rp&p9Ru!}|v`SQa3!o*_xAjv;v40yD6PKVOGlHaG z3+7@B0tC(0nlQNO2cP|nRZg>Wu-!^}3hvtjNyWi7X^I_8!SuC zBFoB}G%us)Kr3kqm8BJPX+la+g%k5cP*MM&2$C{)?!JG9SuT)GTS?7rO>1gIG;eXc zW;jl{BMQ%^OeJo38Gkuf@WUaN&?%@ z-Hi`xnqV-Q;Us3RQUXOpA0j9DG;*Zl%b(LTWJ5&al}N>YNgMpP&~iG7p{vv4qx7)1 zjw0nt`sbK3=Uw|6iE4Nr*0^5z+%vUC;#@9~#VW$$=u+}|T!4yKx-RGRV*&QT zY8hMJ6Hhx}?PjK(Ur9evzkH$_GvdtsOJjF1g>36upO&0F_a6*eZKa_|NpbVZ7x1@a zEWZH_YPy&>TxL*Af*dJzQKI>e18Vd|W%p0cEOOQ;-M;1dT&Y3vI*iLLUUCGXuyLzU z82)ea!5P+bR4J;h>}Mb0sdK>sV?d)$edQDj5hv9W)PPc{6#`GREW&iJJlWV)y#$rK z(AV3q=EiLm1OXPDd~&<0Xn-yR^C5WaLzP@!#`Ei!+JX^!Ro*>xst|)^T`xJ@G6F^} zt|aj)%(U1p>5L@Bx7TCEA&pbHo-Ue^P>(v!J5eYup>M0X*8IXsuTbW|xm(?BjykA? z9P@DrRzQMAYxiuI<^o`_7aaNbyMwp_%*QLjzQHB2hRisMNX&=x=vzgY%KUbF*E!fX z!ux^A@sjhws@hl~dJ=Liy&sEZiRPFWzi@#Y0cE~X6^jct6zy<-=O4H;X+iYfPthFUk#Yu-s=yzcmS4!NFp-NriSUAak&2viCr zJCXQNZk05YNiX!4IGq!-M53)EP`HVnbMk~<2vNMCm(ZDY-dB!ix*$>qRY`Jb+D{Wl zjV5twW@n)Vp$Z)?)?ci;5e@n(!8F6Zi6TagKpiJu+j~X($)LADs?5v{UU^(B{cl5| z1fdLmT5PTmqGZ)$!b^dWY}<$lJBx+MR3tLS#R{E-Q1}iJYfEde!H_B5X`>O6WFy*5 z-Y?395T>(Vi3hwC0#$?Lh_bTEo0}`j#L^TyA)ydn{g;TdMj?bS3ccQF zcFgrhri%nyNgz3k{>v^2p+6#~Csa)qTn4NZzxz;w!6=O7MdI|aqvG(IQ6f@>kZgL1 z1sBa+L*~wHHLP>qI?-!EakURkH9K$9Nii$YPPis7{^NmXDimMzvJ*Dl=7_U(NJ~U^ z-gO)6^1M|s^hgRq&1HTV7EVy>6vi(6l_CoYdm9eyVo$k1(skg0-q^gEdl*MR6=qnWj>;1&gw zub}p)=kNyzQ-Sg1E_R)$By-z#3QeGn?ayN^9FQbOVjoSRxe81LJJ_CMytxf!yUEzA zM~E`Ez7&N{l2CZ2u=tD#jCmX}TNJz+rhInp$is+<^Q#`Gfm%XR2Qe(I1R;#%<>Z-N zRF*V4wC2vZq(!GOSk$lUQKHk;6#v!Iws&{Dm4Y*iAE04K2mbrUab)$ zL6lKkREbde6Bp};0zqkEA(sV+iy2IRusNO%&Ugm~)41C-4j4d~O1M;7hcij^zp;B? z85h4DfUk`Xolb|&#+QMsN-^Z0;Ite%k+Bf#^1LNS?tOm?o0`AJyC2Obb>|Y^9I%3= zyH>MvS|@HQq;4`@iOEVx7OOZ&Xtf$7K$wge1t>#@v3#39Gp4-HCrdVyHgh!Vzh29S z8L2$kD3M;j;SXDwH*O0{S4<>*%Q%`{_kAr|P1V1QMk64hxpMw7nJ{5s=j8wL z!ji)%8{f~Q7w)5B#X8=2dM*d9G5&TVp-?K2(4)U$=v^R$l7Ip8S@nGMH8-H7^B`Ex z29U83D|c9yQ*c&I?BFRZ`{gKm-nttt6=bjcj=eRk7pa;=6QPk1<>}0yvzLqIMdY10 zf6KFCQFNMO|DL=&XCqlTOL;U0#U))Qh$=4pU9+1lbH1CvkV$Mf_@P-JW~~ZG;i*W5H+A1#8XD=4+yoqgioaKQjQ%xQ+0i*R8`|*Ji(D#S=hWD8&5Bsj9L7ab%XX*O^At7 zaca+D2ni=V{Exq|AU8CBi&ZlJM67e(6`L8`(g!#15F(qmqsNpTR0;`gP{^NdATtLH z?9X!#1)&y&tQplD7aM1qzVzcQI8^`>d2>?;c6TQ*tQoPLhV#t{6DU1t80?5~?|XC( zbi*^a3DK<+8JBhulcXXjq=Ds5ts;$Kjl2l(^{4G?J19p-vmsBCpaZ)u&+}4yC#2F0 zUTf=vySp!Uy}65W$%*zaJi|RUV7QqL%18!}NhUyLB7fxwVmw{&@^-`C&W(tn%PFjJ z=BT0l$d|asO~Q~pkB7qDaP{`W#nv95*s*LfuNYS&mh{Uyidgw%7@lr^gf?wK`-eZ_ zpb-=f!8G);<`UEr6&r;{GLkp5FEQ;C>6S8;UDf@iDwL>jEt04peRK9`KcR8 z-?*B^AI&E1lpHgWu^g+Mx3~-*O~Y}nI7#Nd1Dq0m#PuD=qB%nds;NcEourS_S^d8e zBqzqx;hx?+oY)u@AZfH$!V@)GK&f3IEdo@WKYD;)vx;$Oat}{@u#`7C=`iY@X%yv! z=}b2J_vKRI9!>XAb6NC4bIapIN)H+~w4vleDH`u+dXHPmit({%0n`zUTCsuI!@CgS zE>Th<;Th3|;j=feVpIfbsKEvWKFM?0ymAtcbO^;mQ%+%FDQd3>+J-n#X2EN}GIS`* zeq79$B_xb`!aU34Q)Eb;H?CZaOpaov~OP| z=Hxs^CwHOqe;($=PuG)ku@1-TW$GRlWt-G0CQ)=eN1Q3U@?XOlv2tLLP$CK2q|Zgc z9bRp}Sw@1)KNmT(;(FvUsoT_YVJagzGyBnN+5z*5?%I2KZ`(x94{OGM*Q)XIc0pBg znv>_t5Xt~jhV;Pg@06k!g)DhIf$=|?57MaP#*#YnZ#=%GRHpG<+YziXt2nPBbm$V6 zKOIqbcDmL#du3$)0n~a`!HU9M4rgX!cDY6u$t7bEnt;|c^*umt{t3#I&IGsaPRi)l z8Qa74mNP+a4FeZX940fG>`(QmOwR>R8kh~bOwwxg~eC>e#joV_*4bk@D%S$G{yt7tVPJOdi zM&=)YEXE)sBO_A}x#`S1Jw5$)*&!oSCy=jsnfk&_owuwUE%R?fzUF1>3paJ%ufF=~ z?Xp8g=8lq{p3bICo8)W08o!T;Ox;9|dCSPi$kan*=Pe^6BU2BNowtmPj7&YG);HAWbu3QJAj{O6r=Lm0%bML&qL}QT(%Eq!4~K-I z^k{5mkEQ7>yOu6v!>`BiPM*ksX7VK|BV!4c&AiKY@K%B!PCgx&xb>bq#Cg^| zABU%N5J?~Gzp{JNKuP{#cK?=t?QTvh+BfDb;-#ma3kW98tbUy*pBc}) zOZHG?;s)oS2J{)c)3-Md4IR%nhi|ZV)v$-zBZl$f8#7sd%$oABGBW>8EOOqa(?5~A z^%TXYGf4gMgjtmqOR=ggV-6cu?V+IP80%N>;G!_FcS1{C{laMX+!~6i`P8f7s!>7sTC{>o9K5i`=vLT_U7|^xV37=QJM2CFZFDWzmpEFtqVS(tr)a0 zw`N70>)NVd`_vRVwG1QB%LO})8XLC|?t0{1HW&QfS}E(NKF8ZrCo=Byeb>CQSF9O? zheASdCu#JT94x(+p{j=QH|9LnnU;7 z6+Iw|_cHI9?lkqWN2_xtu;qP>U6xJx4coi^99TZCC8WI&i@%c%HqH%b{op&KpS^h> zZr#4xM1ya12Re4_NXL%xv})*xLg`6VhmKXB#kVC?_cJpdN}ySw8@3t^w(dvAh?Hc20Z-Yxv8yQES)%aJ20nZ7Kg2(=lCuJ!lL!d^%Dv7Ib={eI#f`-)R05a+&P zc)wRzvhyVPH6=RKm5Sq+QM)Sv!TIl>rc=@w7H-Q&?cWf$iX)`Yd75s?Gsvtl9wa!Q zv4}a#w{Y;(Wt2`X=qd`p-V>$aAT#i+T@Iyh- z`e*5q{0SF3P2j8bY0Mk$PsX@D3|?@`I+1Lde|`lEWJ}bNnyeEzYDY$mQgjv+SSQ6E8_m?t+PYa?pr4vxG_0e&Mp@ zO4k|tID4*;W2sNjSZUX{-`l`e`O}cbo6S<=V3EZ%0XB zyIypOZ%;z^B;qWQbQAe}_0C+*CH;@ZuRlma>$~Xr>}+1^TFwuPzqddFPe!H+>zubL zh8{^lsJYA!!@>z_ox<3Kzfx2yaZm*wt=kZ)1Ys!POyOUjiI|5FA3= zSGH0iK$Kjq21^iWs9WW$%MEDME4ratZfBVv#+wXIOib5wzC_GbGe8vREJdWg9 zu@oz5(P<1E-oBIbCNLhKM{f@Wl4L{k2|uGpP?39tlNJuS<>RVAt5X9C+!B&W^ihH# zm;DDz%(Pm2_SH%Bzp;B?85h4DfUk`Xolb|&#+QMsN-^Z0;B>8{o6oYo14zi8A|ElXjNvf{gE@@~QZv9fM(OBgnl>VK0VnAzLsu`;Exf>4! zl-1aXmE&rvq@q%Q1PX=ur@bZi)lDQ6N(B;n^f%0<6bPXtV8DDvfcqz2IZMD=;Cc zB8ZEz2b5GDs+40#&tM|Z0{g7&`fb3*(+el#kNlRUhd47gf~pBIQ7Y!{ISe5wgv&`Q zGFD>U9HwG3V_W**<{d(0^LF%@vV%$?p$!Vb|F+B=6a;qcM3`E_m@|W}0RaShyAwBY z&-DwCs&HCH+aZZ!z8=(qcAdMDG+_hP6C#v>ku-6Tz_5=u6XKb?*TQ(RC}QygKKKOs z(xA(SWEvzK+dsv?7AhpKevBQ_3>$Ht??*Jn)7^~*-Dh#wpuoGw7@lgW!m#%}ItRMp z8Qg^E)`^TuyNF3r5fst@r81JiW0DC_naE!`f*4O1yu978w{s(6=<>f58+A&0fzhq( zq3jo?bq~VT%^OcA8yo}g<-K3@fC7(*NW7F_JT{BoG0(B-&uPsTjO!mpYv&rkAjsL{T6I)lDx8+HDVMqSER9VV;boP z&i}CxsqiD^*#QLZpG@EW6Pcg7k@StLS^Uv#(rQ|rmyxN4RnA*n29KuUI9Hq`bKe0@ z2|wccj$_fBAq3r~S6Wrfi+nkI2=R?vp*Wv{N*5f}W!KO86#hImdoknhZ;79DoE;n1 zvFUI*o}sZMbd1DB0?Fk*rp$ktq^9237+vx7weSffsqqO3L~-!~mA3xG^?#f1z8XfT z62OkOum8Z>{|zL*krRgEGSt4!>Gk4bHZFLC00kKJ&NPbh!gMB^{rhsMaF3?@sJSe9 zp*boP_$1F|^U6s)(jgQNO*w^yrKr6kXdB`{*CicC%f z#YEwwtZG=>@m0QfqaU#Wj!3TV`22ZDEErdmezfY^k(dx~bmeC`RcORHv^|eZ`i}J3 zy{l%>ia-W__$kBh3Bz40;pXLu!UFs1ERNlHcj;7msn#$ir7QP#?aSEBHniy!gSQeu z!lmnU(!PC>n3MAuo!o`a|9O}fKV45w-C2p1sRu~kfB(Jc(xuBaAIi0jYoPK6h6Hxw zle10?SeehfBui4nWa={VHGe(wnAB}*xeAbxoSFUTHSK`;{AumIyq7V9?l!f1)Y`>@ zK5XuByN$Y@$~2y9JAzea(`8hI4qd|Xrz7MYrZO`BP-?w|TSZ|mhch$H7cSC8a;YAK z_3A|*#3fE0&Lp$Myz^F_E3X@yS!85n{uXN4c`Jz+`!kheYq!F#@uw~ot~b4yX=sWsT0W8yi9%Jrp{Ydj+Xhi zAz$+{^#!xeJ3c<1`1ttSZHtVIjEszh$T4pj85x;+i0r&&WMpLOA+qz9k&%(9hx{K7 Wou|1gt~EaZ0000UIo!}w3+v4u-4$I;YoJEiCzPhTby1Rei z=7)J_y1Ke+dZwq}M>_JSf+XU1yziereL|F$5>x*4>9gg>wF4~7$GJCMd-WqgyNF1u z!hRgSu%;0o_qeX&nyxAi=B^$_&Ssx1>>cdP7+g%8&CKjwEFD}gp}GY>DzRCqYPyO# zn;E%UIoOk^TG^R>6n^@|%FN1s(ALPz4bF*RW&x*{kg#xoGlMx;4X~!C>EWxeOx$xd?RSs9iq66+b{5eodA`=9%^*JeSGnY`nl_k< zqAIn&CHGqkrT6-EaHJATY+c<#Xn;hNqSO? zO7(=B$w)txSlvtn$ax#V4IIr>gpt>u&N_x}th3)HIxDMRiBe@|;zu%3`XdGSxUrrw za%1k#eU+!;cj6jG)-28_$9*vS$}A0*0G|e&oNq+1&RgZO7Kv+hBiDxut!`Fu&!Kyq zT-gzw!KI_E=-kAfWqJO{zP=ZK|yZOOnp z3T?Ki9fCjo{Dh`kPqBSI=})ySI==x+kE75_;xM7DuFkxZ*+TN`JGfUAdBy$eTz1mS zM*<^Gl9o$PlkszCSU5V`PM>Jy+k(%|esY)KTd6nOGc>CHACk7sdYvk`W$<0m?hW}$ z&LhbwMV-APo3F-`yjq?mzc6Gt zx^faX(Z|*L@oK{5_9S+NS}yI%*9*kGXy7m@}QR>SI_^VddZ$)^s1vGID#I% z8ml{)S8^Fs1lZI+6wOj+lt}32!zzNq&Nm$6yc{piU7p);-t<@>7X-IiQ^1Yy$CpLJ zsUQV>U-XWxTQ{ChK@aexgwjqS%@6+?tz#d>aWp#J(vmt-x@+fI>dFoI0vs`7HNJx@ zaqg(mM9;mjnRiO5q063OM-+X_t85mY8$zZ&NRAFGiZAZO`yBC}Tp&-U%pK3gNJ)+r z&!1wuksaAlT^7l7bwm%tuMx$6fu-yF{by7Ea%G#N#rhOH9i-9?_u4yyrR_1HTh*d8 zx9#o$pfEu!b}IAr%A;uyQ#A;?TU+*Nh7H6&w5&$|=%JOP%XM{%GuKKcegoOD<}`M` zA_uB_P@c7A)(7V@Tw?Yq7Ype)Y)h@IK3iGEWQCPxQolol36958D<&@0(hRo=GGefR zA#^v_E!k~Zs~-Airdirm?AgwQpf4t;S~Khnlkud*TmftKweN5`xOc|#GN7B@LDRm$ z7Us^hEyMSbHRwlm!+-Z3ml02pL2KFirC9S3~8 zsz|--#(gK5;l>R9hVNC$15QDU{raU6i5smtn{Y9JkbTj&d ze+}xOOPeuk#~!wJP*-%#HpHh-PTx^JPV{g!uPrjrPJmbgo}8rQE2+b=I)c7GsrY~Rw&IFI%{r$fB6AH#Mu^76|DmNsncN{0Y})*Es5Yhwb91p%3}UMYJ6tYu+?r_9twSCVKA+|fYC=(O_?DH&JkmpS8v$Fr#m6%p;m}^uEf* zYzy?Yo=IA4fJP&|lk{s2F$M2=-WpBWY^kA2PPX5c{MdDjEIou?8avV_g#)+Wk&$)61t2iY5twKuGaaq zoNaYV#z|0Z=~b^B97S2N`apPBTm3WTPE^E!u?2I_)1Yo%rWeJN%&NJkhfvkwNiumV z19a4mft(e(4v)*U%?^)ouyih9B1LozL6d2@zW$x&83XbJ&}y`ft<2!k;LKM{@*~ef z4aj%CKIUB43#V{0qPpV1N=cWTPA(b89-o+XkUjrV1fsr4w2e)#T+FsBxs$-| z8jaqbO2xEM9}Uht1)StjHL6U#Z)k4lksUPUYmB6wXPYfn8kjBU<}k@T!_mMalPp|%xbN3yYB|cPlV6sX70Am0+XjZ45h4>qe*(Br{@Ar z;%7A7TpIXZRFoEtF%E`Uvf~C67kPE2^zy;YcHWZT@lbBPil4`d<t9nB2vx6wF{dQnN^jR!D(J6CA?Co6k3OPbYI9m8{oarld7u1}K6 z(%gz!P&+5dpAdDpBz2Cej~ z7ao-oCu&`8!liXbRB2}MNyPqMYwqtm<(>1pBmfc*S3#!dG0J2~rd$`T2U??`s`q>5 z>ebkfcI2dp2sZOLS6FtLE-rPM;FW`(nQP;FOMpJS%Kg)7{%C5tlMq3QxipPU6ou?e|yo~3z6GYVF~zl?2Tiq0<&^=<8^ z#HF%qz#y!Zw|$`H>KrJ!wq|Tl#d8-UaPEfAwf3l?PtW_su=j9=!(%(X_hzBI)LAUg zKDxK`klH7~WX1OSS4#ba!6>frhFwPNoxz#y6{dPJrCCtDoQL*gD&2BJ4KOj07l8hw1QLc8rW9@SiKs%(_Re4eLMPO83AVBEr~+4ZdZ~~ zO~X_fo`WlWI4V`$I;RWsu~9SP(C_allT}a_^w^9;AvXj%oRYge(i6Zb5SO*(5J@{z zM^Hq_pCTm>F4^DxHNIaz+Pb%7)b$5;LS-$p@#~BZKA59v=?Ao=j)7V zuMrkqhHsYBURO8_vhFX0>vH{Q&}S2f45rPeQ&axfJ+E}}tP11-F6$SblMW;%pHWT zu{(^>HRn4>7}HW(JF~8D#Q6>t@_i5#t8u7Nh=LixWa$@zy$u%zY7X9gYxf5f?-W4g zLIj5Ax1J<&t_%FK0ecJI>97<;_<`a1Y~RTt=Z(Z4zI{cvJOZBdR=jX`knbqA3*0yX zc};W=394*p^PTP@%J0vDLD>@-pB?ucKV`usOG81Iu{Ds_7;$DNb+hM8vGZO3SYMLM zn50>E=0+tLW%=D-wm0tTN9pl*$dix=^V;muGuoUrnV=r3Oyx(%9jR;5F{7!LvksQj zn7_=J!=?KvlVDS~VgD|6=JO4*FWdE3GNGE0nZcW56)}|AOZr-Vtf9pD&yBsMYpZd6f>CD_g{il-?=a zlX4+1Jc$}#9AeQ{m?Bg_^35Gh6_?&tTFiR|nw~|>VMikItmnc{ymBH+cEzN5gHlsCy=+E=+mKQ>!C4-r9B6bE<9)dfapUEdF8$$8T{&F7~st zx4#U867)9-33J>KqJ zW+|@;MX7z}Jb8HsU-+g)vcdQefmeH()%iujHmpw!i_Ot}m=u;ltU$!Wzc0l10#fqq zsp(YL*F;zM(0bC~V!4iFAJ)8=%a#3;rjUAYJOvOC*P-|}(bbcC=wGs|C62U7 z;L_Ljt@pKsSbO;C#yl#GIxw$tDH|#N-;*n`U_K$;Q)0kj%S$aik!TQXRNI!}au*$l zE`|5PQ3R?op5>iSN$*wu(PN2!VelG<2^sPSC6_ zCK}%19=-1_yz;(hcxtIuFs(CAp+yg~_-kwBR!3H+AV3TD`_q_@_#Wp+vNR#Y+EY@FQAph)+1 zdp@6T^lnxYL&KEZ?>Y+jAiVLP9bwo)ul|{40#PX31G&Z7n2+}>myIhApE2>?;&nro zSr41O-~xQ{v){N965Dw(?%Ib+>!5J`s-a&y7JGv8L~^%^CXuIa+{d?8wjCs-`rL<* zUy`82y|4Ay9N70(dCd^M`Hd@$Y!6Hp+3>jP^{+4E%VSGr3!X#%9{<$tBDuZPOnqUz z%2^M<+cwWF&wX|v$~T2ARSo3}2_WvX;K)&(c1oo7`?|KM(3y}ov-gW7{w>#NTccDm z^@r}+GdF&~Va&3w-&NC^=KC;!Q^PCMnXz0wosOPhCSHiZ4bt95MRk8B{nj}Z0{nHu z4|?NU5vnKR5Ns2kra1NNR)yKVzwdlueWU|sizU%nAtwvI

B9J{CqI<85?r5j za(AUS_k+W2DPAf|bG1Gk&L#S<4&^OAsEk!_kp@hlGS|-zu5~^uD5-(H!{3k*CkADe zLWO;~90_{QgCm99Q-M_{%8e1+a95N3#v?%TmltoJkZSoaz?KrkH#h4hM zm7cO;FENX+M`3Z5Zswdq4zK8h18B5{qQN|(q)hGrG0aQAFbO1^ueOHFALI^E0MaX@ zC-Q`qcLxjVUu?(F;#62)o37D7W~iN0B#i#M$_=NQtN!OpTNGng5aQ7rOp$?MYmAUV z*=zCUv{|2qf9yZ7c;U{&yHmb{bLQNn3pW;}smbpD2!X1y3iix6BUag@XzvUdmoHCJ zco`eE>;M$bZBa^P*eXa4qk6v^f&5ouGE}SbtAc-1F&-XLEQa+^EKa!5uw~OfZB}h1 z@d>r?`Cc@<oNrK04cU-4 zu6>l!++;GVkQW1$p5?CSS!3Q}RtPgmU^|Sev<9=k8HWHinbHs}$yUwk0aR2HOC-Q$ z?c$fiGsq3$_qR}S!pi)^UeQ+ zUIjyZv{W_tZMv?mzyT-BmAuXV^~jH}j^y~Qg!-L%n6&GU;aKeyyL|teDVavTYm8!_ ziob5X5_F4Lx%g!RD0Zfz(1MR z)Ie|YRoryp-Kaw9<6_wvg(>k|GU-plN<#rJY*fmzkjBfmyk0iOJ!pcmBJNjWnVHCM zLvw|;3{Mm7!fnf4rhT^txO{&QA;|G1q9tRl`5vV6(uxEY;C~XY?h}*GEb!()=}=ip zc-pAK;xzXSnWRcDHmTbdiH9S2shUM68ghGMQ?M+LxW}mjIPBUb=H}kJEDa$Mo9|ROgi!WJE%B%E=(hr3;l-@QB7iz`B}1Cq>-OB70VzUbGZqvgtaeb&CR{aU~1K zFzTb>=&PS}vZ&c3ayt)QB;@-D&<#3+QIc0huhXm(Nf(}s-5xrKQZ=dZt{JuL%UkJyFTr0arcT$XrE%+xLmjb~Bh zFG+{0P+K5MzTLuZXUWx)Buhwc`^^ zL4#>nc)2$XOWvfa^j8T+~ zFKlbvAz&7&cjjIO(Celyiz7}&%vVKg2+R13kZyv6I(*Ecyey9HDmZ8Ao3bJXS_73D zOQ-&Jm^deS;}y$)Fw4X|$4K;CW%<*HGds`(!H8^|hQ4>|Zo!jSJR zmM9gGsBiXcgQ&DGzw79@zMII4JLg3sDcbatN3_u_yX%`>hI=nu9Ce&H&rCSSZ)p~6 z_H`_n)Ja9L`K_2$lwAIj0@9?R*kESoWQt9ARMjrTOT|@j+p!;!tpeyr+B97D7jGa$ zq>{a!JHYpm*wYP++RM$cIB4Mp)qRLM$Y1BKGUL+I~H#$itIGF-KlU*xzj^v!ztAA}X<~vvL?r#ot_T1vR_k zYIhB;&*K|b*e$p>v0jGO%5^dqSOa;x3PcomFf? ztQul_o?dZhs=2|FEDEe$}*(n}omUVmu^|E_R^C#`e2Udw-Z(gSY;9kXlz7Z$?!ia$}U|gH{w?eIo(TE}>)aTE*+EdhqNq9vHQT<)A zg0xl`GqTl`+hcJRG*yk#EY9?_X4Z=9W;`x}jF9a}rR(mY(eqw=rl}<8$n4X?OYA;U{a(gG-C!ej&0(AkSH-glVlDCpA|d1CkQP{_o|$N&q&0KBZ$ z1pciz~g2&jBngZau9GDchi!8(LKPip+HVT(Tn1 z#L*5OgL~(ywS})dI3l+UQK$LEOU&c9LB*l~qkinUR@DSgosaY=dyvgR+Ujx1bk#+4 zz{{)dwnjzw+A~YMhAm=|Pjjq^fVv^R#8)^chl#kEnjF$eU%2dQT>rAB5CJG%!DY0=h4t}5B)C*PQh5H(g}J7s+b-o5w3cU zS{Dz-b|1+^nPEL(ckOr|@EEq^@RpV@S z(#YKGgeJtohZ{NmwL9cBH84Q_1Z+HB!y!)1`S>NIKRqa>0qQcY)5<*U*_^NyTu6IU zcdi+{98S=>?9N^daqFb?SQWz>CJV@7!jPdF%Qt4QDw+?`i13MvIoLP?M+I+yxn+3z;GH<#8SI^G7 z)s?F_20`6$!b<-t#XB5FakJvedA^3MUA@2Zy>x18B2(cSDQk9%CF1L>18R*&FiV%zw0zvgH~2J6sQ?Pv3mHs$~-# zkYacVd2>-KI*5fS{?7khmck3} zxZs$;;Y!n>q&Ier2EMOx!EYD-R|uSb`u@bsE3YqE_8}48cfNf{gax}-6J_*XU{Vc@ z6x%b@EN(O{p;ksz;jY%Km<-)lxQewnCJjBgHuTtl*-AM0$8 zOLJtq*0zM|IX`z{;s9SS)}5ZKi7j=ixFLReEclbHV0w|<!3Vd_q>x{;hK)cDt+YBu0R1$YzI;nH)4v;??% zHfD<&W8H5%8Isj3F^ETW>s<5TZ^T*6{;ksSVkIIKK4sbBmR*pi;|UQRv5Gl3Y99rz z*YkBtz$c7M;hH?!EixX|E>C^f4XFO(1yC6JUaiklI}9+sYjQN+h-N=x$>SKVE_8Y5 zA#w|CkZuu#=Dl91mSlnAF63-D$U(QaH{Xl{3^a!bacx5%(r`L_^uwmy?r-;U#|Y5n z9#bn~5-td89T2bc8Y<)8Hew&n34BGld4GVHZ6Q9a@Bhl=5z%Qou&zOFKAX)y|3^rL zGp9d;3Oawlq>|{J5>Y#LZ>OL0;7Av?awhSz?7pm0y5Gx;CHXK?Ph;Dzo6d)jPH647W&XTjeoj&_%EVtNj!|uH& zT}HVpnzWbl9Kwq`L8?)zyoWq*%G2RJK%8u^aedG#K0TvB$=UakX-=?4e+D9b zC?5G=?Xftsp+6Ic2`(~V^xGif7Z^Any8Bo)oBSFufcI6v(iAb+7mVlY*rb+ z(0)6h@`%yv4)%z`AYLuW(=DsfaUR;l3Kz)4gVV7|8W*a`6L8&1puHj+g@-b^rb=mm znK&>wNAZKoQh(0h{#w_)`O&k^Y}X9xn1b;P(8UYwA^_R`S{AWldegCM{D~!vi2eg4 zsi)tT-NG%xA5k<;=O>MxQCf|2Y9#IsUjSPu?EMUAY;Th|=WC+4r^g>H^aho8U3eUQ z_>g!6|CY9D+Kg~f5s<;T6z7&Hkv%yiW*>7o+Lbom)yYgMDS2!mptt7hr`;i@NZj{XJoTH@=zD`%ti+iE5wgoTE zxO(3{sS35bqA&4HH=HGxhWd2uCG!dOim$U?r9gowmmN%ffBng%&p>~uzo~_C{k&j* zhtS2_V>(nRtJYos9h;9R!L+>EXphgQQ4P-V+&ibqd3GW6;i@v%*3S>Gd!h8=6{~g^nQXrQ86Z{ucSFkupq|tRO-N@u}XM`9}-b4qRIm ztOXnhZTk2`W=C(8y;9w@C6Ff}8FWf-<{Uy^Umr4^lv({c+_X89l2~=-q{=A~N4b2V z%QO@WV<0HnS6e^>b2L9HCU8f2dfN{B&srsuD7-UU+_fN&QeqK<+OPRtHw2`SvbZl` z#j{@%+UQAU2WC+5Bu2wu(sG>x!6Ln61<1?f6~{BD>|#j{#;hBJh(DC?Ildh{G3*;Q zP{`Dew=P);VPiCc7P|8aQ z_l@e8o|r8f;O<4aC-C#`lvBEQv3>kuo&%iz%Sb_^GSHKAo}Tg0FNU;yB%%qkd{uhQ zV3`$wLGB@MTnHoVz)_%b=YsHR^&Op(uNFV*pYAv;0>J6T5d+B;RK6Y3RioPPc}0ot zT?RPv8C42a{)I5Hl{hlk%HMdh9t(?1xKi!-LI8c>)7noSnZDY6EeX?g#o=@Qd;E?d zH~Udwf!y?7^i(roOKukJ>kNJ}HM)^$--#N5M+&JN@(yU?Zz{n;>Tka*9UAp;Nl(9a zvlk^Qk524bz#OYQ!f_(}8(+izbGKq|jZqHL_H=9+QLw;G1D?2vF79Hd#+TKQ;q&?1P6-Ji_dVN9hkOQEH0n{(z%gb=TMSj# z7L23Ah)xZ55jg$aYl=6)-mULnt+>lkL}b=xRx#kEJAO(iILT`dm&^83vOv~ez#S}{ zxPGa0o+|wD&Hu@^m$kdics=j!>y80wY^+u~@!FIf0RHHrTX@rh*HD(kzImNKQ&7JT zM&x^Us5;)Gu6yPhrM52s3$j0Jn#y^bZ0qfQ%!1--*Bw|RQ|(L)QZ2rJi>Oed%Gv@{ zcR@-tQ^BmZDoLgb(w>sR1lbRfja3Ac{yZ^-T$+RA7M~-1!DhG}&kDmieupHH%p_0z zo2FX82&u{m+f#DhN&3`q!nXNY^nf3wcUIICUp5De=fx~sQ7`#_m5B5sn;v6Ygyn{M zY&?G~J)4`-mg$}3cv2BH(Kzg4^SA>iL?i_zrz6pqF==X4Dh`)hOfXWqE7HD*Ud6th zAMCjimP(wia^Y16>y6Y2YZ3_57hmGG>bqyp8L4Af%udX#6IV*V2do#{s!;#oTM09J z%&B9V@o&EgJX=tRz}e`WyZ%i#+E-p4W)$nPQ;+`+NPXMa*=p5O4oJs#Xubd+w zu}c?q4f;d>`+t(X&{Y5zg1zYPrLN@vE?zXVk#$- zlKB67V6_dIngX2=fnS?5>?C#nJPUKt9(c~73V2R{dBn630CVa>qJqx+f*c51pu$>apfkCP#mL&HuNiL;P>?mT-Xg zbEJ{gcKxFd%)fPwO3>cp0!UJ7zcS5nZI)n-%a118g%)(6CpN=`y(Pj9##ANgC1_au+%@(rc@M5 zX!ec9H0pbtQZR4aQ3!pK4&XUm?pXZdH&@ydN~nz68f_TIkyhDs!NMo+iQLow{217> zO3hx1rbQo8rQ>aEqOQd@8X`rp7ZOoTj5C}85!me*6LaUMo2NN_yN*dpah@61sr1Y# zrOO$pBbDP~U2}Nd;a1lpfrl=lj}T8-9Ra-g3LvT!&?fU+`MYG{P0{MIERFLZ95)X~ z6JIXn*X!Q1&#D|>vNC&M_xHE{7QPf+Yz`Q$Ppo(dnG9I?LG1m|68;~~((?(^-1T2| zkWCRBcEm~5D8&bER!#NR`dX&NXwcyLVCV-zttM;oa=Tp$@A~eV_mZoRIHtl~!&Gff zUP|B}s%AC*TPr+Lv0=xI^X(t6TQqoJ(Rh%qmETSPJYJlX{EuLa`!IE?V<#l_i!dk3#aiv>sz$1IwS(%4F3KY4|Jo^Rn@ zX<;pvVBTWH+V~ z$YDtIXlS&9kvBu^{#fXcX61*R)S))n+dDmO0_U5H*&?d2H}E3dSgnc@ph2fEjOlq* z(IjS3!i#yp;?+~eZA+lX7M{9}>;lGXefBDh7nph)eYM>3LbJH^y0+GQ4TeKTrUDwG z2!dnMoo1^YL6wLtJt)C`yK_MYF@8l~FBztd@jZE`j=DA5ZP~s}>53li#>XGyNxtWd zNizJM1DBzXo|g{PS^ddN^8GF<^T?x3S z^hRc)kDT2(iQnqVSDLh$vl$=Uv8<~BTO?v`Y&{HEejNgdW!b^o8oC)<0{RT65W1#rvD z^yYs2Klt&}97yzCAD8Up#K+0)S+72go4-XCTuHrYw&y_5yFi0zxOtw(uyy0lKWi4> zTc0LK{PntG;2c0k`+;EBOkb?jqSSyfQ9<)Ea!~Bo9DbJ|av{z^*+bA+y;ddAzs3y# z_$r=g`0sG(6s&)D!EIMa+rs};1{aNkrCdgC>yLcS4 zT(IM>eZo<0r{L)yO`!uQgLE^d%E;gLO2P84N*!&U$E;L`-7>y1m-W2`#i=CKdnZv6o|H_Bl}e>(SJMHZE8|pjCg~> zob&6?U&M{=b&bTQzYH)v1@~4||30vrzLZp`eg6l?>8pts@(QgoG%@{@`%6Q|TsMl; z`XQfQr#bI{SZzQKEnK~NSM?;uqfeixtWZM&x5Krn@QWE-z1m zin`Xf9P@%!vXj@TR&y5g#EXiCqmAOa>VETMLLk)qkfmLXt}jO3C_e1ij6x?(zhd_T zfbvx&$Qv(T;J!zKVj&uiWE-vi`&W%7csDbk*h4t3!2JpJ<~(=f^`;;OuCJ5Ix%cP& zbss*yrEzw|rI1(ZY);7{c$*)|#*CKD3<$K@`4Od^xDBMd2cy5vZ8xk$0eN?qSp9`o zM=$Hl=fUU%N(9l~5!Eb{F@vUuGo=h45XQ;+%jAozWmalZ$cKwcir*=QO^YAocQ$zvMr4ZZ!g=sZ+ukyc zjXtB{SLA7t_MU1W^^>%DdhMSM?k2*?>M<>^0e-r-tO*c>ZvMhUcra_<*W*Yo_Yl%V+&f0I{}pBGU1>)&a$uR?@Y>vL@MAZ8)pT5dk)I&9^3 z{^#>3lP&%v`)%8SAk(KoD(Of7-0$NVfm;oTz=^?4$(1bPzq|)JrG@~rU^fQIGu|(F zE#S-XX*}LeBt-mXxqKlEv0IRxP@FtZ(fr9abOcyp zk@52C#HAY0K2dVWh0hjnHtC-R*?u1r<~s_JQQ`ghjpZ17&WPF4lDp0K1$w!dOwfbD zIsR$c#*^b@?8YnLumfG_X6sM($}PllwVE>E@R7VC;1f|?(;1&_NIfq9u@SYLDGQX) zIX85H9d$7gj8a}gfZ?AOzEtb6S-RGQvkIWFw? z$#{K(6vkJb#N{2(Mh=nKlMiab{I_KSL5yS0)5JA;0lZGpOXq5ocWiPznBDVa^Z;bK z3&;uc8591;43|>+a=XLL6K0<1&_MfIz=2WdDu%?~Hsnx~QmB0X#E#b(j1V3RU?|d< z^@V==HS;oD#O><85b!QfYw;*xNd~mUfst=zZ6F`ydlNp>8{a523Qi3*2GyD=X+z^vpL@MecjW2ty{C-^0PUg4Xp`y4|Y!t zW|4d!{98h|>bID?lcZLTi5rMC1I*hWkG!1yZ^Y zV4NQxEoSOGx}UgyHp(iVblCjXDqkrb#MgLqxoM=Hg)1f9{E%c1`I>c~zw2yaJM6E?y{sClUU=Z;Dis{?yzhr*-mYM`6vFB0Go0k>+s9@3w3IMibMM0} z-+vZEqTaD)+ZO)Q0o2}vh zd?c>F_s@agu6V1JqC)V?xdpo@`;Yt}_s`=L%=HsJv56=oh2!M%B!JN7> zo-d*W4Sa`Qg*Dku`kkShN=Zy-=(-S zS-B3=PZj*V>Xl5H+ELnHoN^cyd6C3NhZaI#KMYE%s$m*46(7K_Z2xBpy~<(PwLzj$ zu(|_{dOe$)KkYkaR>aHUQ`yHJ#y3P&{0|5w!+96LiXcZyB|wb$pM93n;tFEbB8I{L E0|I%-Q~&?~ diff --git a/localization/autoware_pose_initializer/schema/pose_initializer.schema.json b/localization/autoware_pose_initializer/schema/pose_initializer.schema.json index d5b92c45d3038..4602223ea69e4 100644 --- a/localization/autoware_pose_initializer/schema/pose_initializer.schema.json +++ b/localization/autoware_pose_initializer/schema/pose_initializer.schema.json @@ -29,6 +29,17 @@ "default": "3.0", "minimum": 0.0 }, + "pose_error_check_enabled": { + "type": "boolean", + "description": "If true, check error between initial pose result and GNSS pose.", + "default": "false" + }, + "pose_error_threshold": { + "type": "number", + "description": "The error threshold between GNSS and initial pose", + "default": "5.0", + "minimum": 0.0 + }, "stop_check_enabled": { "type": "string", "description": "If true, initialization is accepted only when the vehicle is stopped.", @@ -75,11 +86,13 @@ "user_defined_initial_pose", "gnss_pose_timeout", "stop_check_duration", + "pose_error_threshold", "ekf_enabled", "gnss_enabled", "yabloc_enabled", "ndt_enabled", "stop_check_enabled", + "pose_error_check_enabled", "gnss_particle_covariance", "output_pose_covariance" ], diff --git a/localization/autoware_pose_initializer/src/pose_error_check_module.cpp b/localization/autoware_pose_initializer/src/pose_error_check_module.cpp new file mode 100644 index 0000000000000..b639731372d10 --- /dev/null +++ b/localization/autoware_pose_initializer/src/pose_error_check_module.cpp @@ -0,0 +1,40 @@ +// Copyright 2022 The Autoware Contributors +// +// 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 "pose_error_check_module.hpp" + +namespace autoware::pose_initializer +{ +PoseErrorCheckModule::PoseErrorCheckModule(rclcpp::Node * node) : node_(node) +{ + pose_error_threshold_ = node_->declare_parameter("pose_error_threshold"); +} + +bool PoseErrorCheckModule::check_pose_error( + const geometry_msgs::msg::Pose & reference_pose, const geometry_msgs::msg::Pose & result_pose, + double & error_2d) +{ + const double diff_pose_x = reference_pose.position.x - result_pose.position.x; + const double diff_pose_y = reference_pose.position.y - result_pose.position.y; + error_2d = std::sqrt(std::pow(diff_pose_x, 2) + std::pow(diff_pose_y, 2)); + + if (pose_error_threshold_ <= error_2d) { + RCLCPP_INFO(node_->get_logger(), "Pose Error is Large. Error is %f", error_2d); + return false; + } + + return true; +} + +} // namespace autoware::pose_initializer diff --git a/localization/autoware_pose_initializer/src/pose_error_check_module.hpp b/localization/autoware_pose_initializer/src/pose_error_check_module.hpp new file mode 100644 index 0000000000000..358156036a8ae --- /dev/null +++ b/localization/autoware_pose_initializer/src/pose_error_check_module.hpp @@ -0,0 +1,38 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 POSE_ERROR_CHECK_MODULE_HPP_ +#define POSE_ERROR_CHECK_MODULE_HPP_ + +#include + +#include + +namespace autoware::pose_initializer +{ +class PoseErrorCheckModule +{ +public: + explicit PoseErrorCheckModule(rclcpp::Node * node); + bool check_pose_error( + const geometry_msgs::msg::Pose & reference_pose, const geometry_msgs::msg::Pose & result_pose, + double & error_2d); + +private: + rclcpp::Node * node_; + double pose_error_threshold_; +}; +} // namespace autoware::pose_initializer + +#endif // POSE_ERROR_CHECK_MODULE_HPP_ diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp index 912c6ec9b57a2..fc9c4afab2459 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp @@ -19,6 +19,7 @@ #include "gnss_module.hpp" #include "localization_module.hpp" #include "ndt_localization_trigger_module.hpp" +#include "pose_error_check_module.hpp" #include "stop_check_module.hpp" #include @@ -60,6 +61,9 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) stop_check_duration_ = declare_parameter("stop_check_duration"); stop_check_ = std::make_unique(this, stop_check_duration_ + 1.0); } + if (declare_parameter("pose_error_check_enabled")) { + pose_error_check_ = std::make_unique(this); + } logger_configure_ = std::make_unique(this); change_state(State::Message::UNINITIALIZED); @@ -167,8 +171,26 @@ void PoseInitializer::on_initialize( diagnostics_pose_reliable_->clear(); + // check pose error between gnss pose and initial pose result + if (pose_error_check_ && gnss_) { + const auto latest_gnss_pose = get_gnss_pose(); + + double gnss_error_2d; + const bool is_gnss_pose_error_small = pose_error_check_->check_pose_error( + latest_gnss_pose.pose.pose, pose.pose.pose, gnss_error_2d); + + diagnostics_pose_reliable_->add_key_value("gnss_pose_error_2d", gnss_error_2d); + diagnostics_pose_reliable_->add_key_value( + "is_gnss_pose_error_small", is_gnss_pose_error_small); + if (!is_gnss_pose_error_small) { + std::stringstream message; + message << " Large error between Initial Pose and GNSS Pose."; + diagnostics_pose_reliable_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } + } // check initial pose result and publish diagnostics - diagnostics_pose_reliable_->add_key_value("initial_pose_reliable", reliable); + diagnostics_pose_reliable_->add_key_value("is_initial_pose_reliable", reliable); if (!reliable) { std::stringstream message; message << "Initial Pose Estimation is Unstable."; diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp index a44f7f70b4b43..b9a6a66590b78 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp @@ -28,6 +28,7 @@ namespace autoware::pose_initializer { +class PoseErrorCheckModule; class StopCheckModule; class LocalizationModule; class GnssModule; @@ -56,6 +57,7 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr ndt_; std::unique_ptr yabloc_; std::unique_ptr stop_check_; + std::unique_ptr pose_error_check_; std::unique_ptr ekf_localization_trigger_; std::unique_ptr ndt_localization_trigger_; std::unique_ptr logger_configure_; From 68abc992ea08539e1f73d06630c4cfe84c42ea00 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Fri, 27 Sep 2024 14:59:11 +0900 Subject: [PATCH 56/95] feat(lane_change): modify lane change target boundary check to consider velocity (#8961) * check if candidate path footprint exceeds target lane boundary when lc velocity is above minimum Signed-off-by: mohammad alqudah * move functions to relevant module Signed-off-by: mohammad alqudah * suppress unused function cppcheck Signed-off-by: mohammad alqudah * minor change Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah --- .../src/interface.cpp | 3 + .../src/manager.cpp | 4 +- .../src/manager.hpp | 4 +- .../src/scene.cpp | 46 ++++++++++++++- .../utils/utils.hpp | 15 +---- .../src/scene.cpp | 12 ++++ .../src/utils/utils.cpp | 57 +------------------ 7 files changed, 66 insertions(+), 75 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp index fffb86767b0a8..d47f76e399b4c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -44,11 +44,14 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( { } +// cppcheck-suppress unusedFunction bool AvoidanceByLaneChangeInterface::isExecutionRequested() const { return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck() && module_type_->isValidPath(); } + +// cppcheck-suppress unusedFunction void AvoidanceByLaneChangeInterface::processOnEntry() { waitApproval(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index ef02bfaa7b0dd..ea5b853051e9a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -186,8 +186,8 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) avoidance_parameters_ = std::make_shared(p); } -std::unique_ptr -AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() +// cppcheck-suppress unusedFunction +SMIPtr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_, diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp index 95609a430ac80..44f842a67c236 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp @@ -32,6 +32,8 @@ using autoware::behavior_path_planner::LaneChangeModuleManager; using autoware::behavior_path_planner::LaneChangeModuleType; using autoware::behavior_path_planner::SceneModuleInterface; +using SMIPtr = std::unique_ptr; + class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager { public: @@ -44,7 +46,7 @@ class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager void init(rclcpp::Node * node) override; - std::unique_ptr createNewSceneModuleInstance() override; + SMIPtr createNewSceneModuleInstance() override; private: std::shared_ptr avoidance_parameters_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 25a155c324a55..3f56320af171e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -34,13 +34,55 @@ #include #include +namespace +{ +geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose) +{ + geometry_msgs::msg::Point32 p; + p.x = static_cast(pose.position.x); + p.y = static_cast(pose.position.y); + p.z = static_cast(pose.position.z); + return p; +}; + +geometry_msgs::msg::Polygon create_execution_area( + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & pose, double additional_lon_offset, double additional_lat_offset) +{ + const double & base_to_front = vehicle_info.max_longitudinal_offset_m; + const double & width = vehicle_info.vehicle_width_m; + const double & base_to_rear = vehicle_info.rear_overhang_m; + + // if stationary object, extend forward and backward by the half of lon length + const double forward_lon_offset = base_to_front + additional_lon_offset; + const double backward_lon_offset = -base_to_rear; + const double lat_offset = width / 2.0 + additional_lat_offset; + + const auto p1 = + autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0); + const auto p2 = + autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0); + const auto p3 = + autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0); + const auto p4 = + autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0); + geometry_msgs::msg::Polygon polygon; + + polygon.points.push_back(create_point32(p1)); + polygon.points.push_back(create_point32(p2)); + polygon.points.push_back(create_point32(p3)); + polygon.points.push_back(create_point32(p4)); + + return polygon; +} +} // namespace + namespace autoware::behavior_path_planner { using autoware::behavior_path_planner::Direction; using autoware::behavior_path_planner::LaneChangeModuleType; using autoware::behavior_path_planner::ObjectInfo; using autoware::behavior_path_planner::Point2d; -using autoware::behavior_path_planner::utils::lane_change::debug::createExecutionArea; AvoidanceByLaneChange::AvoidanceByLaneChange( const std::shared_ptr & parameters, @@ -84,7 +126,7 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const const auto minimum_avoid_length = calcMinAvoidanceLength(nearest_object); const auto minimum_lane_change_length = calcMinimumLaneChangeLength(); - lane_change_debug_.execution_area = createExecutionArea( + lane_change_debug_.execution_area = create_execution_area( getCommonParam().vehicle_info, getEgoPose(), std::max(minimum_lane_change_length, minimum_avoid_length), calcLateralOffset()); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index c9990747f57a7..260ef78807ff7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -91,7 +91,7 @@ bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); -bool pathFootprintExceedsTargetLaneBound( +bool path_footprint_exceeds_target_lane_bound( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, const double margin = 0.1); @@ -257,10 +257,6 @@ bool is_same_lane_with_prev_iteration( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); -Pose to_pose( - const autoware::universe_utils::Point2d & point, - const geometry_msgs::msg::Quaternion & orientation); - bool is_ahead_of_ego( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const PredictedObject & object); @@ -280,13 +276,4 @@ double get_distance_to_next_regulatory_element( const bool ignore_intersection = false); } // namespace autoware::behavior_path_planner::utils::lane_change -namespace autoware::behavior_path_planner::utils::lane_change::debug -{ -geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose); - -geometry_msgs::msg::Polygon createExecutionArea( - const VehicleInfo & vehicle_info, const Pose & pose, double additional_lon_offset, - double additional_lat_offset); -} // namespace autoware::behavior_path_planner::utils::lane_change::debug - #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index e8a63c09580e1..6a928941e71ef 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -1609,6 +1609,18 @@ bool NormalLaneChange::check_candidate_path_safety( "Ego is not stuck and parked vehicle exists in the target lane. Skip lane change."); } + const auto lc_start_velocity = candidate_path.info.velocity.prepare; + const auto min_lc_velocity = lane_change_parameters_->minimum_lane_changing_velocity; + constexpr double margin = 0.1; + // path is unsafe if it exceeds target lane boundary with a high velocity + if ( + lane_change_parameters_->enable_target_lane_bound_check && + lc_start_velocity > min_lc_velocity + margin && + utils::lane_change::path_footprint_exceeds_target_lane_bound( + common_data_ptr_, candidate_path.shifted_path.path, planner_data_->parameters.vehicle_info)) { + throw std::logic_error("Path footprint exceeds target lane boundary. Skip lane change."); + } + constexpr size_t decel_sampling_num = 1; const auto safety_check_with_normal_rss = isLaneChangePathSafe( candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params, decel_sampling_num, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index c982c0aad988f..146e704f746b3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -200,8 +200,7 @@ bool isPathInLanelets( return true; } -// cppcheck-suppress unusedFunction -bool pathFootprintExceedsTargetLaneBound( +bool path_footprint_exceeds_target_lane_bound( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, const double margin) { @@ -1145,16 +1144,6 @@ bool is_same_lane_with_prev_iteration( (prev_target_lanes.back().id() == prev_target_lanes.back().id()); } -Pose to_pose( - const autoware::universe_utils::Point2d & point, - const geometry_msgs::msg::Quaternion & orientation) -{ - Pose pose; - pose.position = autoware::universe_utils::createPoint(point.x(), point.y(), 0.0); - pose.orientation = orientation; - return pose; -} - bool is_ahead_of_ego( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const PredictedObject & object) @@ -1279,47 +1268,3 @@ double get_distance_to_next_regulatory_element( return distance; } } // namespace autoware::behavior_path_planner::utils::lane_change - -namespace autoware::behavior_path_planner::utils::lane_change::debug -{ -geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose) -{ - geometry_msgs::msg::Point32 p; - p.x = static_cast(pose.position.x); - p.y = static_cast(pose.position.y); - p.z = static_cast(pose.position.z); - return p; -}; - -geometry_msgs::msg::Polygon createExecutionArea( - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Pose & pose, - double additional_lon_offset, double additional_lat_offset) -{ - const double & base_to_front = vehicle_info.max_longitudinal_offset_m; - const double & width = vehicle_info.vehicle_width_m; - const double & base_to_rear = vehicle_info.rear_overhang_m; - - // if stationary object, extend forward and backward by the half of lon length - const double forward_lon_offset = base_to_front + additional_lon_offset; - const double backward_lon_offset = -base_to_rear; - const double lat_offset = width / 2.0 + additional_lat_offset; - - const auto p1 = - autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0); - const auto p2 = - autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0); - const auto p3 = - autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0); - const auto p4 = - autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0); - geometry_msgs::msg::Polygon polygon; - - polygon.points.push_back(create_point32(p1)); - polygon.points.push_back(create_point32(p2)); - polygon.points.push_back(create_point32(p3)); - polygon.points.push_back(create_point32(p4)); - - return polygon; -} - -} // namespace autoware::behavior_path_planner::utils::lane_change::debug From 9c7d542821ed1e85e20de47e7f402ea26b68b356 Mon Sep 17 00:00:00 2001 From: Khalil Selyan <36904941+KhalilSelyan@users.noreply.github.com> Date: Fri, 27 Sep 2024 10:55:58 +0300 Subject: [PATCH 57/95] fix(speed_display): always show speed positive and depend on gear for negatives (#8957) Signed-off-by: KhalilSelyan --- .../autoware_overlay_rviz_plugin/src/speed_display.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp index 8670f85e6ba71..d677be40dead2 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp @@ -56,7 +56,7 @@ void SpeedDisplay::updateSpeedData( // Assuming msg->state.longitudinal_velocity_mps is the field you're interested in float speed = msg->longitudinal_velocity; // we received it as a m/s value, but we want to display it in km/h - current_speed_ = (speed * 3.6); + current_speed_ = std::abs(speed * 3.6); } catch (const std::exception & e) { // Log the error std::cerr << "Error in processMessage: " << e.what() << std::endl; From 82b286cb7ab8f27e862fc566cac7ac3f99b72aa0 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 27 Sep 2024 17:31:05 +0900 Subject: [PATCH 58/95] fix(autoware_multi_object_tracker): update yaw with range-limited innovation (#8976) fix: update yaw with range-limited innovation Signed-off-by: Taekjin LEE --- .../lib/tracker/motion_model/bicycle_motion_model.cpp | 2 +- .../lib/tracker/motion_model/ctrv_motion_model.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index 2e325d18579a6..215e55cb4ac62 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -231,7 +231,7 @@ bool BicycleMotionModel::updateStatePoseHeadVel( // update state Eigen::MatrixXd Y(DIM_Y, 1); - Y << x, y, yaw, vel; + Y << x, y, fixed_yaw, vel; Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); C(0, IDX::X) = 1.0; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp index b1c5a36f9ad5b..bf5950bdd4023 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp @@ -194,7 +194,7 @@ bool CTRVMotionModel::updateStatePoseHeadVel( // update state Eigen::MatrixXd Y(DIM_Y, 1); - Y << x, y, yaw, vel; + Y << x, y, fixed_yaw, vel; Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); C(0, IDX::X) = 1.0; From 71cfaafd49c47b5c2606c53edbe26091bb56e71d Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 27 Sep 2024 19:05:55 +0900 Subject: [PATCH 59/95] refactor(lane_change): add TransientData to store commonly used lane change-related variables. (#8954) * add transient data Signed-off-by: Zulfaqar Azmi * reverted max lc dist in calcCurrentMinMax Signed-off-by: Zulfaqar Azmi * rename Signed-off-by: Zulfaqar Azmi * minor refactoring Signed-off-by: Zulfaqar Azmi * update doxygen comments Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../src/scene.cpp | 15 +- .../src/scene.hpp | 2 +- .../scene.hpp | 2 + .../utils/base_class.hpp | 2 + .../utils/calculation.hpp | 63 ++--- .../utils/data_structs.hpp | 46 +++- .../utils/utils.hpp | 4 +- .../src/interface.cpp | 1 + .../src/scene.cpp | 224 +++++++++--------- .../src/utils/calculation.cpp | 180 +++++++++----- .../src/utils/utils.cpp | 25 +- 11 files changed, 342 insertions(+), 222 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 3f56320af171e..991b5c8af656d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -124,7 +124,7 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const const auto & nearest_object = data.target_objects.front(); const auto minimum_avoid_length = calcMinAvoidanceLength(nearest_object); - const auto minimum_lane_change_length = calcMinimumLaneChangeLength(); + const auto minimum_lane_change_length = calc_minimum_dist_buffer(); lane_change_debug_.execution_area = create_execution_area( getCommonParam().vehicle_info, getEgoPose(), @@ -315,16 +315,11 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_ return avoidance_helper_->getMinAvoidanceDistance(shift_length); } -double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const +double AvoidanceByLaneChange::calc_minimum_dist_buffer() const { - const auto current_lanes = get_current_lanes(); - if (current_lanes.empty()) { - RCLCPP_DEBUG(logger_, "no empty lanes"); - return std::numeric_limits::infinity(); - } - - return utils::lane_change::calculation::calc_minimum_lane_change_length( - getRouteHandler(), current_lanes.back(), *lane_change_parameters_, direction_); + const auto [_, dist_buffer] = utils::lane_change::calculation::calc_lc_length_and_dist_buffer( + common_data_ptr_, get_current_lanes()); + return dist_buffer.min; } double AvoidanceByLaneChange::calcLateralOffset() const diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp index d6bf6bc29df97..42ae41fa380cc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp @@ -63,7 +63,7 @@ class AvoidanceByLaneChange : public NormalLaneChange void fillAvoidanceTargetObjects(AvoidancePlanningData & data, AvoidanceDebugData & debug) const; double calcMinAvoidanceLength(const ObjectData & nearest_object) const; - double calcMinimumLaneChangeLength() const; + double calc_minimum_dist_buffer() const; double calcLateralOffset() const; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index 9463c4442645c..f76d776150875 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -54,6 +54,8 @@ class NormalLaneChange : public LaneChangeBase void update_lanes(const bool is_approved) final; + void update_transient_data() final; + void update_filtered_objects() final; void updateLaneChangeStatus() override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index b237368692312..72a40caca1d6a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -67,6 +67,8 @@ class LaneChangeBase virtual void update_lanes(const bool is_approved) = 0; + virtual void update_transient_data() = 0; + virtual void update_filtered_objects() = 0; virtual void updateLaneChangeStatus() = 0; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index 1c98fc6092a31..021fa16fa6ec1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -24,25 +24,9 @@ using autoware::route_handler::Direction; using autoware::route_handler::RouteHandler; using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LCParamPtr; +using behavior_path_planner::lane_change::MinMaxValue; using behavior_path_planner::lane_change::PhaseMetrics; -/** - * @brief Calculates the distance from the ego vehicle to the terminal point. - * - * This function computes the distance from the current position of the ego vehicle - * to either the goal pose or the end of the current lane, depending on whether - * the vehicle's current lane is within the goal section. - * - * @param common_data_ptr Shared pointer to a CommonData structure, which should include: - * - Non null `lanes_ptr` that points to the current lanes data. - * - Non null `self_odometry_ptr` that contains the current pose of the ego vehicle. - * - Non null `route_handler_ptr` that contains the goal pose of the route. - * - * @return The distance to the terminal point (either the goal pose or the end of the current lane) - * in meters. - */ -double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr); - double calc_dist_from_pose_to_terminal_end( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes, const Pose & src_pose); @@ -119,13 +103,6 @@ double calc_ego_dist_to_lanes_start( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); -double calc_minimum_lane_change_length( - const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals); - -double calc_minimum_lane_change_length( - const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lane, - const LaneChangeParameters & lane_change_parameters, Direction direction); - double calc_maximum_lane_change_length( const double current_velocity, const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, const double max_acc); @@ -134,6 +111,10 @@ double calc_maximum_lane_change_length( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc); +double calc_lane_changing_acceleration( + const double initial_lane_changing_velocity, const double max_path_velocity, + const double lane_changing_time, const double prepare_longitudinal_acc); + /** * @brief Calculates the distance required during a lane change operation. * @@ -151,10 +132,6 @@ double calc_phase_length( const double velocity, const double maximum_velocity, const double acceleration, const double duration); -double calc_lane_changing_acceleration( - const double initial_lane_changing_velocity, const double max_path_velocity, - const double lane_changing_time, const double prepare_longitudinal_acc); - std::vector calc_prepare_phase_metrics( const CommonDataPtr & common_data_ptr, const std::vector & prepare_durations, const std::vector & lon_accel_values, const double current_velocity, @@ -165,6 +142,36 @@ std::vector calc_shift_phase_metrics( const CommonDataPtr & common_data_ptr, const double shift_length, const double initial_velocity, const double max_velocity, const double lon_accel, const double max_length_threshold = std::numeric_limits::max()); + +/** + * @brief Calculates the minimum and maximum lane changing lengths, along with the corresponding + * distance buffers. + * + * This function computes the minimum and maximum lane change lengths based on shift intervals and + * vehicle parameters. It only accounts for the lane changing phase itself, excluding the prepare + * distance, which should be handled separately during path generation. + * + * Additionally, the function calculates the distance buffer to ensure that the ego vehicle has + * enough space to complete the lane change, including cases where multiple lane changes may be + * necessary. + * + * @param common_data_ptr Shared pointer to a CommonData structure, which includes: + * - `lc_param_ptr`: Parameters related to lane changing, such as velocity, lateral acceleration, + * and jerk. + * - `route_handler_ptr`: Pointer to the route handler for managing routes. + * - `direction`: Lane change direction (e.g., left or right). + * - `transient_data.acc.max`: Maximum acceleration of the vehicle. + * - Other relevant data for the ego vehicle's dynamics and state. + * @param lanes The set of lanelets representing the lanes for the lane change. + * + * @return A pair of `MinMaxValue` structures where: + * - The first element contains the minimum and maximum lane changing lengths in meters. + * - The second element contains the minimum and maximum distance buffers in meters. + * If the lanes or necessary data are unavailable, returns `std::numeric_limits::max()` for + * both values. + */ +std::pair calc_lc_length_and_dist_buffer( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes); } // namespace autoware::behavior_path_planner::utils::lane_change::calculation #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 351132d9f8ce5..fc5e78e44b77f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -27,6 +27,7 @@ #include #include +#include #include #include #include @@ -218,6 +219,7 @@ struct PhaseMetrics struct Lanes { bool current_lane_in_goal_section{false}; + bool target_lane_in_goal_section{false}; lanelet::ConstLanelets current; lanelet::ConstLanelets target_neighbor; lanelet::ConstLanelets target; @@ -313,6 +315,32 @@ struct LanesPolygon std::vector preceding_target; }; +struct MinMaxValue +{ + double min{std::numeric_limits::infinity()}; + double max{std::numeric_limits::infinity()}; +}; + +struct TransientData +{ + MinMaxValue acc; // acceleration profile for accelerating lane change path + MinMaxValue lane_changing_length; // lane changing length for a single lane change + MinMaxValue + current_dist_buffer; // distance buffer computed backward from current lanes' terminal end + MinMaxValue + next_dist_buffer; // distance buffer computed backward from target lanes' terminal end + double dist_to_terminal_end{ + std::numeric_limits::min()}; // distance from ego base link to the current lanes' + // terminal end + double dist_to_terminal_start{ + std::numeric_limits::min()}; // distance from ego base link to the current lanes' + // terminal start + double max_prepare_length{ + std::numeric_limits::max()}; // maximum prepare length, starting from ego's base link + + bool is_ego_near_current_terminal_start{false}; +}; + using RouteHandlerPtr = std::shared_ptr; using BppParamPtr = std::shared_ptr; using LCParamPtr = std::shared_ptr; @@ -327,12 +355,14 @@ struct CommonData LCParamPtr lc_param_ptr; LanesPtr lanes_ptr; LanesPolygonPtr lanes_polygon_ptr; + TransientData transient_data; + PathWithLaneId current_lanes_path; ModuleType lc_type; Direction direction; - [[nodiscard]] Pose get_ego_pose() const { return self_odometry_ptr->pose.pose; } + [[nodiscard]] const Pose & get_ego_pose() const { return self_odometry_ptr->pose.pose; } - [[nodiscard]] Twist get_ego_twist() const { return self_odometry_ptr->twist.twist; } + [[nodiscard]] const Twist & get_ego_twist() const { return self_odometry_ptr->twist.twist; } [[nodiscard]] double get_ego_speed(bool use_norm = false) const { @@ -344,6 +374,18 @@ struct CommonData const auto y = get_ego_twist().linear.y; return std::hypot(x, y); } + + [[nodiscard]] bool is_data_available() const + { + return route_handler_ptr && self_odometry_ptr && bpp_param_ptr && lc_param_ptr && lanes_ptr && + lanes_polygon_ptr; + } + + [[nodiscard]] bool is_lanes_available() const + { + return lanes_ptr && !lanes_ptr->current.empty() && !lanes_ptr->target.empty() && + !lanes_ptr->target_neighbor.empty(); + } }; using CommonDataPtr = std::shared_ptr; } // namespace autoware::behavior_path_planner::lane_change diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 260ef78807ff7..469e8e97c5d9f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -122,9 +122,7 @@ std::vector generateDrivableLanes( double getLateralShift(const LaneChangePath & path); bool hasEnoughLengthToLaneChangeAfterAbort( - const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, - const Pose & curent_pose, const double abort_return_dist, - const LaneChangeParameters & lane_change_parameters, const Direction direction); + const CommonDataPtr & common_data_ptr, const double abort_return_dist); CandidateOutput assignToCandidate( const LaneChangePath & lane_change_path, const Point & ego_position); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 7ddafdbd6c935..0b647acd44268 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -76,6 +76,7 @@ void LaneChangeInterface::updateData() universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING); + module_type_->update_transient_data(); module_type_->update_filtered_objects(); module_type_->updateSpecialData(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 6a928941e71ef..5ce02395ab05e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -86,11 +86,17 @@ void NormalLaneChange::update_lanes(const bool is_approved) common_data_ptr_->lanes_ptr->target = target_lanes; const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr; + common_data_ptr_->current_lanes_path = + route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + common_data_ptr_->lanes_ptr->target_neighbor = utils::lane_change::getTargetNeighborLanes( *route_handler_ptr, current_lanes, common_data_ptr_->lc_type); common_data_ptr_->lanes_ptr->current_lane_in_goal_section = route_handler_ptr->isInGoalRouteSection(current_lanes.back()); + common_data_ptr_->lanes_ptr->target_lane_in_goal_section = + route_handler_ptr->isInGoalRouteSection(target_lanes.back()); + common_data_ptr_->lanes_ptr->preceding_target = utils::getPrecedingLanelets( *route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(), common_data_ptr_->lc_param_ptr->backward_lane_length); @@ -98,6 +104,53 @@ void NormalLaneChange::update_lanes(const bool is_approved) *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_); } +void NormalLaneChange::update_transient_data() +{ + if ( + !common_data_ptr_ || !common_data_ptr_->is_data_available() || + !common_data_ptr_->is_lanes_available()) { + return; + } + + auto & transient_data = common_data_ptr_->transient_data; + std::tie(transient_data.acc.min, transient_data.acc.max) = calcCurrentMinMaxAcceleration(); + + std::tie(transient_data.lane_changing_length, transient_data.current_dist_buffer) = + calculation::calc_lc_length_and_dist_buffer(common_data_ptr_, get_current_lanes()); + + transient_data.next_dist_buffer.min = + transient_data.current_dist_buffer.min - transient_data.lane_changing_length.min - + common_data_ptr_->lc_param_ptr->lane_change_finish_judge_buffer; + + transient_data.dist_to_terminal_end = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, common_data_ptr_->lanes_ptr->current, common_data_ptr_->get_ego_pose()); + transient_data.dist_to_terminal_start = + transient_data.dist_to_terminal_end - transient_data.current_dist_buffer.min; + + transient_data.max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); + + transient_data.is_ego_near_current_terminal_start = + transient_data.dist_to_terminal_start < transient_data.max_prepare_length; + + RCLCPP_DEBUG( + logger_, "acc - min: %.4f, max: %.4f", transient_data.acc.min, transient_data.acc.max); + RCLCPP_DEBUG( + logger_, "lane_changing_length - min: %.4f, max: %.4f", transient_data.lane_changing_length.min, + transient_data.lane_changing_length.max); + RCLCPP_DEBUG( + logger_, "current_dist_buffer - min: %.4f, max: %.4f", transient_data.current_dist_buffer.min, + transient_data.current_dist_buffer.max); + RCLCPP_DEBUG( + logger_, "next_dist_buffer - min: %.4f, max: %.4f", transient_data.next_dist_buffer.min, + transient_data.next_dist_buffer.max); + RCLCPP_DEBUG(logger_, "dist_to_terminal_start: %.4f", transient_data.dist_to_terminal_start); + RCLCPP_DEBUG(logger_, "dist_to_terminal_end: %.4f", transient_data.dist_to_terminal_end); + RCLCPP_DEBUG(logger_, "max_prepare_length: %.4f", transient_data.max_prepare_length); + RCLCPP_DEBUG( + logger_, "is_ego_near_current_terminal_start: %s", + (transient_data.is_ego_near_current_terminal_start ? "true" : "false")); +} + void NormalLaneChange::update_filtered_objects() { filtered_objects_ = filterObjects(); @@ -151,24 +204,20 @@ bool NormalLaneChange::isLaneChangeRequired() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto current_lanes = - utils::getCurrentLanesFromPath(prev_module_output_.path, planner_data_); - - if (current_lanes.empty()) { + if ( + !common_data_ptr_ || !common_data_ptr_->is_data_available() || + !common_data_ptr_->is_lanes_available()) { return false; } - const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); - - if (target_lanes.empty()) { - return false; - } + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto ego_dist_to_target_start = calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); - const auto maximum_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); + const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - if (ego_dist_to_target_start > maximum_prepare_length) { + if (ego_dist_to_target_start > max_prepare_length) { return false; } @@ -182,21 +231,10 @@ bool NormalLaneChange::isLaneChangeRequired() bool NormalLaneChange::is_near_regulatory_element() const { - const auto & current_lanes = get_current_lanes(); - - if (current_lanes.empty()) return false; - - const auto shift_intervals = - getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back()); + if (!common_data_ptr_ || !common_data_ptr_->is_data_available()) return false; - if (shift_intervals.empty()) return false; - - const auto & lc_params = *common_data_ptr_->lc_param_ptr; const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - const auto min_lc_length = - calculation::calc_minimum_lane_change_length(lc_params, shift_intervals); - const auto dist_to_terminal_start = - calculation::calc_ego_dist_to_terminal_end(common_data_ptr_) - min_lc_length; + const auto dist_to_terminal_start = common_data_ptr_->transient_data.dist_to_terminal_start; if (dist_to_terminal_start <= max_prepare_length) return false; @@ -249,14 +287,9 @@ TurnSignalInfo NormalLaneChange::get_terminal_turn_signal_info() const const auto original_turn_signal_info = prev_module_output_.turn_signal_info; - const auto shift_intervals = - getRouteHandler()->getLateralIntervalsToPreferredLane(get_current_lanes().back()); - const double next_lane_change_buffer = - calculation::calc_minimum_lane_change_length(lane_change_param, shift_intervals); - - const double buffer = next_lane_change_buffer + - lane_change_param.min_length_for_turn_signal_activation + - common_param.base_link2front; + const auto buffer = common_data_ptr_->transient_data.current_dist_buffer.min + + lane_change_param.min_length_for_turn_signal_activation + + common_param.base_link2front; const double path_length = autoware::motion_utils::calcArcLength(path.points); const auto start_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path.points, 0, std::max(path_length - buffer, 0.0)); @@ -405,9 +438,8 @@ void NormalLaneChange::insertStopPoint( return; } - const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back()); - const auto lane_change_buffer = - calculation::calc_minimum_lane_change_length(*lane_change_parameters_, shift_intervals); + const auto [_, lanes_dist_buffer] = + calculation::calc_lc_length_and_dist_buffer(common_data_ptr_, lanelets); const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); @@ -425,7 +457,7 @@ void NormalLaneChange::insertStopPoint( const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; const auto target_objects = filterObjects(); - double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + double stopping_distance = distance_to_terminal - lanes_dist_buffer.min - stop_point_buffer; const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); if (utils::isEgoWithinOriginalLane(curr_lanes_poly, getEgoPose(), planner_data_->parameters)) { @@ -486,11 +518,9 @@ void NormalLaneChange::insertStopPoint( const auto rss_dist = calcRssDistance( 0.0, lane_change_parameters_->minimum_lane_changing_velocity, lane_change_parameters_->rss_params); - const double lane_change_buffer_for_blocking_object = - calculation::calc_minimum_lane_change_length(*lane_change_parameters_, shift_intervals); const auto stopping_distance_for_obj = - distance_to_ego_lane_obj - lane_change_buffer_for_blocking_object - + distance_to_ego_lane_obj - lanes_dist_buffer.min - lane_change_parameters_->backward_length_buffer_for_blocking_object - rss_dist - getCommonParam().base_link2front; @@ -704,9 +734,8 @@ bool NormalLaneChange::isNearEndOfCurrentLanes( const auto & route_handler = getRouteHandler(); const auto & current_pose = getEgoPose(); - const auto lane_change_buffer = calculation::calc_minimum_lane_change_length( - route_handler, current_lanes.back(), *lane_change_parameters_, Direction::NONE); + // TODO(Azu) fully change to transient data const auto distance_to_lane_change_end = std::invoke([&]() { auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); @@ -716,7 +745,8 @@ bool NormalLaneChange::isNearEndOfCurrentLanes( utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)); } - return std::max(0.0, distance_to_end) - lane_change_buffer; + return std::max(0.0, distance_to_end) - + common_data_ptr_->transient_data.current_dist_buffer.min; }); lane_change_debug_.distance_to_end_of_current_lane = distance_to_lane_change_end; @@ -811,27 +841,18 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const bool NormalLaneChange::is_near_terminal() const { - const auto & current_lanes = common_data_ptr_->lanes_ptr->current; - - if (current_lanes.empty()) { + if (!common_data_ptr_ || !common_data_ptr_->is_data_available()) { return true; } - const auto & current_lanes_terminal = current_lanes.back(); + // TODO(Azu) fully change to transient data const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; - const auto direction = common_data_ptr_->direction; - const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr; - const auto min_lane_changing_distance = calculation::calc_minimum_lane_change_length( - route_handler_ptr, current_lanes_terminal, *lc_param_ptr, direction); - const auto backward_buffer = calculation::calc_stopping_distance(lc_param_ptr); - const auto min_lc_dist_with_buffer = - backward_buffer + min_lane_changing_distance + lc_param_ptr->lane_change_finish_judge_buffer; - const auto dist_from_ego_to_terminal_end = - calculation::calc_ego_dist_to_terminal_end(common_data_ptr_); + const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; + const auto min_lc_dist_with_buffer = backward_buffer + current_min_dist_buffer; - return dist_from_ego_to_terminal_end < min_lc_dist_with_buffer; + return common_data_ptr_->transient_data.dist_to_terminal_end < min_lc_dist_with_buffer; } bool NormalLaneChange::isEgoOnPreparePhase() const @@ -954,10 +975,11 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( } // calculate maximum lane change length - const double max_lane_change_length = + // TODO(Azu) Double check why it's failing with transient data + const auto current_max_dist_buffer = calculation::calc_maximum_lane_change_length(common_data_ptr_, current_lanes.back(), max_acc); - if (max_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { + if (current_max_dist_buffer > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { RCLCPP_DEBUG( logger_, "No enough distance to the end of lane. Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); @@ -979,12 +1001,12 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( // sample max acc if (route_handler.isInGoalRouteSection(target_lanes.back())) { const auto goal_pose = route_handler.getGoalPose(); - if (max_lane_change_length < utils::getSignedDistance(current_pose, goal_pose, target_lanes)) { + if (current_max_dist_buffer < utils::getSignedDistance(current_pose, goal_pose, target_lanes)) { RCLCPP_DEBUG( logger_, "Distance to goal has enough distance. Sample only max_acc: %f", max_acc); return {max_acc}; } - } else if (max_lane_change_length < utils::getDistanceToEndOfLane(current_pose, target_lanes)) { + } else if (current_max_dist_buffer < utils::getDistanceToEndOfLane(current_pose, target_lanes)) { RCLCPP_DEBUG( logger_, "Distance to end of lane has enough distance. Sample only max_acc: %f", max_acc); return {max_acc}; @@ -1313,7 +1335,7 @@ PathWithLaneId NormalLaneChange::getTargetSegment( bool NormalLaneChange::hasEnoughLength( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const Direction direction) const + const lanelet::ConstLanelets & target_lanes, [[maybe_unused]] const Direction direction) const { if (target_lanes.empty()) { return false; @@ -1323,8 +1345,7 @@ bool NormalLaneChange::hasEnoughLength( const auto & route_handler = getRouteHandler(); const auto overall_graphs_ptr = route_handler->getOverallGraphPtr(); const auto minimum_lane_change_length_to_preferred_lane = - calculation::calc_minimum_lane_change_length( - route_handler, target_lanes.back(), *lane_change_parameters_, direction); + common_data_ptr_->transient_data.next_dist_buffer.min; const double lane_change_length = path.info.length.sum(); if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { @@ -1381,19 +1402,15 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const auto longitudinal_acc_sampling_values = sampleLongitudinalAccValues(current_lanes, target_lanes); - const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); + const auto is_goal_in_route = common_data_ptr_->lanes_ptr->target_lane_in_goal_section; - const double lane_change_buffer = calculation::calc_minimum_lane_change_length( - *lane_change_parameters_, - route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); - const double next_lane_change_buffer = calculation::calc_minimum_lane_change_length( - *lane_change_parameters_, - route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); + const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; + const auto next_min_dist_buffer = common_data_ptr_->transient_data.next_dist_buffer.min; - const auto dist_to_terminal_end = calculation::calc_ego_dist_to_terminal_end(common_data_ptr_); const auto dist_to_target_start = calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); - const auto dist_to_terminal_start = dist_to_terminal_end - lane_change_buffer; + const auto dist_to_terminal_end = common_data_ptr_->transient_data.dist_to_terminal_end; + const auto dist_to_terminal_start = common_data_ptr_->transient_data.dist_to_terminal_start; const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); @@ -1431,9 +1448,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) if (!check_lc) return false; const auto lc_diff = std::abs(candidate_paths.back().info.length.lane_changing - lc_length); - if (lc_diff > lane_change_parameters_->skip_process_lon_diff_th_lane_changing) return true; - - return false; + return lc_diff > lane_change_parameters_->skip_process_lon_diff_th_lane_changing; }; for (const auto & prep_metric : prepare_phase_metrics) { @@ -1489,7 +1504,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) ? std::min(dist_to_terminal_end, dist_to_next_regulatory_element) - prep_metric.length : dist_to_terminal_end - prep_metric.length; auto target_lane_buffer = - lane_change_parameters_->lane_change_finish_judge_buffer + next_lane_change_buffer; + lane_change_parameters_->lane_change_finish_judge_buffer + next_min_dist_buffer; if (std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction_)) > 0) { target_lane_buffer += lane_change_parameters_->backward_length_buffer_for_end_of_lane; } @@ -1520,7 +1535,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) try { candidate_path = get_candidate_path( prep_metric, lc_metric, prepare_segment, sorted_lane_ids, lane_changing_start_pose, - target_lane_length, shift_length, next_lane_change_buffer, is_goal_in_route); + target_lane_length, shift_length, next_min_dist_buffer, is_goal_in_route); } catch (const std::exception & e) { debug_print_lat(std::string("Reject: ") + e.what()); continue; @@ -1530,8 +1545,8 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) bool is_safe = false; try { - is_safe = - check_candidate_path_safety(candidate_path, target_objects, lane_change_buffer, is_stuck); + is_safe = check_candidate_path_safety( + candidate_path, target_objects, current_min_dist_buffer, is_stuck); } catch (const std::exception & e) { debug_print_lat(std::string("Reject: ") + e.what()); return false; @@ -1662,12 +1677,8 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); - const double lane_change_buffer = calculation::calc_minimum_lane_change_length( - *lane_change_parameters_, - route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); - const double next_lane_change_buffer = calculation::calc_minimum_lane_change_length( - *lane_change_parameters_, - route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); + const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; + const auto next_min_dist_buffer = common_data_ptr_->transient_data.next_dist_buffer.min; const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); @@ -1686,7 +1697,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto lane_changing_start_pose = autoware::motion_utils::calcLongitudinalOffsetPose( prev_module_output_.path.points, current_lane_terminal_point, - -(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal)); + -(current_min_dist_buffer + next_min_dist_buffer + distance_to_terminal_from_goal)); if (!lane_changing_start_pose) { RCLCPP_DEBUG(logger_, "Reject: lane changing start pose not found!!!"); @@ -1712,8 +1723,8 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( shift_length, lane_change_parameters_->lane_changing_lateral_jerk, max_lateral_acc); const auto target_segment = getTargetSegment( - target_lanes, lane_changing_start_pose.value(), target_lane_length, lane_change_buffer, - minimum_lane_changing_velocity, next_lane_change_buffer); + target_lanes, lane_changing_start_pose.value(), target_lane_length, current_min_dist_buffer, + minimum_lane_changing_velocity, next_min_dist_buffer); if (target_segment.points.empty()) { RCLCPP_DEBUG(logger_, "Reject: target segment is empty!! something wrong..."); @@ -1725,7 +1736,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( lane_change_info.duration = LaneChangePhaseInfo{0.0, lane_changing_time}; lane_change_info.velocity = LaneChangePhaseInfo{minimum_lane_changing_velocity, minimum_lane_changing_velocity}; - lane_change_info.length = LaneChangePhaseInfo{0.0, lane_change_buffer}; + lane_change_info.length = LaneChangePhaseInfo{0.0, current_min_dist_buffer}; lane_change_info.lane_changing_start = lane_changing_start_pose.value(); lane_change_info.lane_changing_end = target_segment.points.front().point.pose; lane_change_info.lateral_acceleration = max_lateral_acc; @@ -1740,11 +1751,11 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( } const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( - lane_change_buffer, minimum_lane_changing_velocity); + current_min_dist_buffer, minimum_lane_changing_velocity); const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( route_handler, target_lanes, lane_changing_start_pose.value(), target_lane_length, - lane_change_buffer, forward_path_length, resample_interval, is_goal_in_route, - next_lane_change_buffer); + current_min_dist_buffer, forward_path_length, resample_interval, is_goal_in_route, + next_min_dist_buffer); if (target_lane_reference_path.points.empty()) { RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!"); @@ -1785,12 +1796,11 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const CollisionCheckDebugMap debug_data; - const auto min_lc_length = calculation::calc_minimum_lane_change_length( - *lane_change_parameters_, - common_data_ptr_->route_handler_ptr->getLateralIntervalsToPreferredLane(current_lanes.back())); + const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( - common_data_ptr_, path, filtered_objects_.target_lane_leading, min_lc_length, debug_data); + common_data_ptr_, path, filtered_objects_.target_lane_leading, current_min_dist_buffer, + debug_data); if (!has_passed_parked_objects) { RCLCPP_DEBUG(logger_, "Lane change has been delayed."); @@ -1906,16 +1916,14 @@ bool NormalLaneChange::calcAbortPath() const auto ego_nearest_dist_threshold = common_param.ego_nearest_dist_threshold; const auto ego_nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; - const auto direction = getDirection(); - const auto minimum_lane_change_length = calculation::calc_minimum_lane_change_length( - route_handler, get_current_lanes().back(), *lane_change_parameters_, direction); + const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; const auto & lane_changing_path = selected_path.path; const auto & reference_lanelets = get_current_lanes(); const auto lane_changing_end_pose_idx = std::invoke([&]() { constexpr double s_start = 0.0; const double s_end = std::max( - lanelet::utils::getLaneletLength2d(reference_lanelets) - minimum_lane_change_length, 0.0); + lanelet::utils::getLaneletLength2d(reference_lanelets) - current_min_dist_buffer, 0.0); const auto ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end); return autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( @@ -1956,8 +1964,7 @@ bool NormalLaneChange::calcAbortPath() } if (!utils::lane_change::hasEnoughLengthToLaneChangeAfterAbort( - route_handler, reference_lanelets, current_pose, abort_return_dist, - *lane_change_parameters_, direction)) { + common_data_ptr_, abort_return_dist)) { RCLCPP_ERROR(logger_, "insufficient distance to abort."); return false; } @@ -2227,10 +2234,9 @@ bool NormalLaneChange::isVehicleStuck( route_handler->isInGoalRouteSection(current_lanes.back()) ? utils::getSignedDistance(getEgoPose(), route_handler->getGoalPose(), current_lanes) : utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); - const auto lane_change_buffer = calculation::calc_minimum_lane_change_length( - route_handler, current_lanes.back(), *lane_change_parameters_, Direction::NONE); + const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const double terminal_judge_buffer = lane_change_buffer + stop_point_buffer + 1.0; + const double terminal_judge_buffer = current_min_dist_buffer + stop_point_buffer + 1.0; if (distance_to_terminal < terminal_judge_buffer) { return true; } @@ -2264,8 +2270,7 @@ bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lan } const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); - const auto max_lane_change_length = - calculation::calc_maximum_lane_change_length(common_data_ptr_, current_lanes.back(), max_acc); + const auto current_max_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.max; const auto rss_dist = calcRssDistance( 0.0, lane_change_parameters_->minimum_lane_changing_velocity, lane_change_parameters_->rss_params); @@ -2276,9 +2281,10 @@ bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lan // stopped at a traffic light. Essentially, the calculation should be based on the information of // the stop reason, but this is outside the scope of one module. I keep it as a TODO. constexpr double DETECTION_DISTANCE_MARGIN = 10.0; - const auto detection_distance = max_lane_change_length + rss_dist + + const auto detection_distance = current_max_dist_buffer + rss_dist + getCommonParam().base_link2front + DETECTION_DISTANCE_MARGIN; - RCLCPP_DEBUG(logger_, "max_lane_change_length: %f, max_acc: %f", max_lane_change_length, max_acc); + RCLCPP_DEBUG( + logger_, "current_max_dist_buffer: %f, max_acc: %f", current_max_dist_buffer, max_acc); auto is_vehicle_stuck = isVehicleStuck(current_lanes, detection_distance); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index bda3fc4e54ea2..1c4aede51074a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -28,15 +28,6 @@ rclcpp::Logger get_logger() return logger; } -double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr) -{ - const auto & lanes_ptr = common_data_ptr->lanes_ptr; - const auto & current_lanes = lanes_ptr->current; - const auto & current_pose = common_data_ptr->get_ego_pose(); - - return calc_dist_from_pose_to_terminal_end(common_data_ptr, current_lanes, current_pose); -} - double calc_dist_from_pose_to_terminal_end( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes, const Pose & src_pose) @@ -45,10 +36,10 @@ double calc_dist_from_pose_to_terminal_end( return 0.0; } - const auto & lanes_ptr = common_data_ptr->lanes_ptr; - const auto & goal_pose = common_data_ptr->route_handler_ptr->getGoalPose(); - - if (lanes_ptr->current_lane_in_goal_section) { + const auto in_goal_route_section = + common_data_ptr->route_handler_ptr->isInGoalRouteSection(lanes.back()); + if (in_goal_route_section) { + const auto & goal_pose = common_data_ptr->route_handler_ptr->getGoalPose(); return utils::getSignedDistance(src_pose, goal_pose, lanes); } return utils::getDistanceToEndOfLane(src_pose, lanes); @@ -137,8 +128,7 @@ double calc_ego_dist_to_lanes_start( return std::numeric_limits::max(); } - const auto path = - route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + const auto & path = common_data_ptr->current_lanes_path; if (path.points.empty()) { return std::numeric_limits::max(); @@ -150,44 +140,6 @@ double calc_ego_dist_to_lanes_start( return motion_utils::calcSignedArcLength(path.points, ego_position, target_front_pt); } -double calc_minimum_lane_change_length( - const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals) -{ - if (shift_intervals.empty()) { - return 0.0; - } - - const auto min_vel = lane_change_parameters.minimum_lane_changing_velocity; - const auto min_max_lat_acc = lane_change_parameters.lane_change_lat_acc_map.find(min_vel); - // const auto min_lat_acc = std::get<0>(min_max_lat_acc); - const auto max_lat_acc = std::get<1>(min_max_lat_acc); - const auto lat_jerk = lane_change_parameters.lane_changing_lateral_jerk; - const auto finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer; - - const auto calc_sum = [&](double sum, double shift_interval) { - const auto t = PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); - return sum + (min_vel * t + finish_judge_buffer); - }; - - const auto total_length = - std::accumulate(shift_intervals.begin(), shift_intervals.end(), 0.0, calc_sum); - - const auto backward_buffer = lane_change_parameters.backward_length_buffer_for_end_of_lane; - return total_length + backward_buffer * (static_cast(shift_intervals.size()) - 1.0); -} - -double calc_minimum_lane_change_length( - const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lane, - const LaneChangeParameters & lane_change_parameters, Direction direction) -{ - if (!route_handler) { - return std::numeric_limits::max(); - } - - const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lane, direction); - return calc_minimum_lane_change_length(lane_change_parameters, shift_intervals); -} - double calc_maximum_lane_change_length( const double current_velocity, const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, const double max_acc) @@ -240,6 +192,128 @@ double calc_maximum_lane_change_length( vel, *common_data_ptr->lc_param_ptr, shift_intervals, max_acc); } +std::vector calc_all_min_lc_lengths( + const LCParamPtr & lc_param_ptr, const std::vector & shift_intervals) +{ + if (shift_intervals.empty()) { + return {}; + } + + const auto min_vel = lc_param_ptr->minimum_lane_changing_velocity; + const auto min_max_lat_acc = lc_param_ptr->lane_change_lat_acc_map.find(min_vel); + const auto max_lat_acc = std::get<1>(min_max_lat_acc); + const auto lat_jerk = lc_param_ptr->lane_changing_lateral_jerk; + + std::vector min_lc_lengths{}; + min_lc_lengths.reserve(shift_intervals.size()); + + const auto min_lc_length = [&](const auto shift_interval) { + const auto t = PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); + return min_vel * t; + }; + + std::transform( + shift_intervals.cbegin(), shift_intervals.cend(), std::back_inserter(min_lc_lengths), + min_lc_length); + + return min_lc_lengths; +} + +std::vector calc_all_max_lc_lengths( + const CommonDataPtr & common_data_ptr, const std::vector & shift_intervals) +{ + if (shift_intervals.empty()) { + return {}; + } + + const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; + const auto lat_jerk = lc_param_ptr->lane_changing_lateral_jerk; + const auto t_prepare = lc_param_ptr->lane_change_prepare_duration; + const auto max_acc = common_data_ptr->transient_data.acc.max; + + const auto limit_vel = [&](const auto vel) { + const auto max_global_vel = common_data_ptr->bpp_param_ptr->max_vel; + return std::clamp(vel, lc_param_ptr->minimum_lane_changing_velocity, max_global_vel); + }; + + auto vel = limit_vel(common_data_ptr->get_ego_speed()); + + std::vector max_lc_lengths{}; + + const auto max_lc_length = [&](const auto shift_interval) { + // prepare section + vel = limit_vel(vel + max_acc * t_prepare); + const auto prepare_length = vel * t_prepare + 0.5 * max_acc * t_prepare * t_prepare; + + // lane changing section + const auto [min_lat_acc, max_lat_acc] = lc_param_ptr->lane_change_lat_acc_map.find(vel); + const auto t_lane_changing = + PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); + const auto lane_changing_length = + vel * t_lane_changing + 0.5 * max_acc * t_lane_changing * t_lane_changing; + + vel = limit_vel(vel + max_acc * t_lane_changing); + return prepare_length + lane_changing_length; + }; + + std::transform( + shift_intervals.cbegin(), shift_intervals.cend(), std::back_inserter(max_lc_lengths), + max_lc_length); + return max_lc_lengths; +} + +double calc_distance_buffer( + const LCParamPtr & lc_param_ptr, const std::vector & min_lc_lengths) +{ + if (min_lc_lengths.empty()) { + return std::numeric_limits::max(); + } + + const auto finish_judge_buffer = lc_param_ptr->lane_change_finish_judge_buffer; + const auto backward_buffer = calc_stopping_distance(lc_param_ptr); + const auto lengths_sum = std::accumulate(min_lc_lengths.begin(), min_lc_lengths.end(), 0.0); + const auto num_of_lane_changes = static_cast(min_lc_lengths.size()); + return lengths_sum + (num_of_lane_changes * finish_judge_buffer) + + ((num_of_lane_changes - 1.0) * backward_buffer); +} + +std::vector calc_shift_intervals( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes) +{ + if (!common_data_ptr || !common_data_ptr->is_data_available() || lanes.empty()) { + return {}; + } + + const auto & route_handler_ptr = common_data_ptr->route_handler_ptr; + const auto direction = common_data_ptr->direction; + + return route_handler_ptr->getLateralIntervalsToPreferredLane(lanes.back(), direction); +} + +std::pair calc_lc_length_and_dist_buffer( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes) +{ + if (!common_data_ptr || !common_data_ptr->is_data_available() || lanes.empty()) { + return {}; + } + const auto shift_intervals = calculation::calc_shift_intervals(common_data_ptr, lanes); + const auto all_min_lc_lengths = + calculation::calc_all_min_lc_lengths(common_data_ptr->lc_param_ptr, shift_intervals); + const auto min_lc_length = + !all_min_lc_lengths.empty() ? all_min_lc_lengths.front() : std::numeric_limits::max(); + const auto min_dist_buffer = + calculation::calc_distance_buffer(common_data_ptr->lc_param_ptr, all_min_lc_lengths); + + const auto all_max_lc_lengths = + calculation::calc_all_max_lc_lengths(common_data_ptr, shift_intervals); + const auto max_lc_length = + !all_max_lc_lengths.empty() ? all_max_lc_lengths.front() : std::numeric_limits::max(); + const auto max_dist_buffer = + calculation::calc_distance_buffer(common_data_ptr->lc_param_ptr, all_max_lc_lengths); + + return {{min_lc_length, max_lc_length}, {min_dist_buffer, max_dist_buffer}}; +} + double calc_phase_length( const double velocity, const double maximum_velocity, const double acceleration, const double duration) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 146e704f746b3..1fafb384d96d6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -549,28 +549,23 @@ double getLateralShift(const LaneChangePath & path) } bool hasEnoughLengthToLaneChangeAfterAbort( - const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, - const Pose & current_pose, const double abort_return_dist, - const LaneChangeParameters & lane_change_parameters, const Direction direction) + const CommonDataPtr & common_data_ptr, const double abort_return_dist) { + const auto & current_lanes = common_data_ptr->lanes_ptr->current; if (current_lanes.empty()) { return false; } - const auto minimum_lane_change_length = calculation::calc_minimum_lane_change_length( - route_handler, current_lanes.back(), lane_change_parameters, direction); - const auto abort_plus_lane_change_length = abort_return_dist + minimum_lane_change_length; + const auto & current_pose = common_data_ptr->get_ego_pose(); + const auto abort_plus_lane_change_length = + abort_return_dist + common_data_ptr->transient_data.current_dist_buffer.min; if (abort_plus_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { return false; } - if ( - abort_plus_lane_change_length > - utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)) { - return false; - } - - return true; + const auto goal_pose = common_data_ptr->route_handler_ptr->getGoalPose(); + return abort_plus_lane_change_length <= + utils::getSignedDistance(current_pose, goal_pose, current_lanes); } std::vector> getSortedLaneIds( @@ -851,9 +846,7 @@ bool passed_parked_objects( lane_change_parameters.object_check_min_road_shoulder_width; const auto & object_shiftable_ratio_threshold = lane_change_parameters.object_shiftable_ratio_threshold; - const auto & current_lanes = common_data_ptr->lanes_ptr->current; - const auto current_lane_path = - route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + const auto & current_lane_path = common_data_ptr->current_lanes_path; if (objects.empty() || lane_change_path.path.points.empty() || current_lane_path.points.empty()) { return true; From f77acfe909378565e9f961e91f9c54f2097095a4 Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Fri, 27 Sep 2024 19:31:49 +0900 Subject: [PATCH 60/95] refactor(autoware_motion_utils): refactor interpolator (#8931) * refactor interpolator Signed-off-by: Y.Hisaki * update cmake Signed-off-by: Y.Hisaki * update Signed-off-by: Y.Hisaki * rename Signed-off-by: Y.Hisaki * Update CMakeLists.txt --------- Signed-off-by: Y.Hisaki --- common/autoware_motion_utils/CMakeLists.txt | 6 +- .../examples/example_interpolator.cpp | 37 +++---- .../trajectory_container/interpolator.hpp | 1 - .../interpolator/akima_spline.hpp | 21 ++-- .../interpolator/cubic_spline.hpp | 22 ++--- ....hpp => interpolator_common_interface.hpp} | 38 ++++---- .../detail/interpolator_mixin.hpp | 96 +++++++++++++++++++ .../detail/nearest_neighbor_common_impl.hpp | 25 +++-- .../detail/stairstep_common_impl.hpp | 23 +++-- .../interpolator/interpolator.hpp | 10 +- .../interpolator/interpolator_creator.hpp | 77 --------------- .../interpolator/linear.hpp | 17 +--- .../interpolator/nearest_neighbor.hpp | 22 ----- .../interpolator/stairstep.hpp | 22 ----- .../interpolator/akima_spline.cpp | 25 ++--- .../interpolator/cubic_spline.cpp | 28 +++--- .../interpolator/linear.cpp | 20 ++-- .../test_interpolator.cpp | 16 ++-- 18 files changed, 218 insertions(+), 288 deletions(-) rename common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/{interpolator_common_impl.hpp => interpolator_common_interface.hpp} (77%) create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp delete mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator_creator.hpp diff --git a/common/autoware_motion_utils/CMakeLists.txt b/common/autoware_motion_utils/CMakeLists.txt index e7f1f56a451f4..8cae2a8ac4110 100644 --- a/common/autoware_motion_utils/CMakeLists.txt +++ b/common/autoware_motion_utils/CMakeLists.txt @@ -7,16 +7,11 @@ find_package(autoware_cmake REQUIRED) autoware_package() find_package(Boost REQUIRED) -find_package(fmt REQUIRED) ament_auto_add_library(autoware_motion_utils SHARED DIRECTORY src ) -target_link_libraries(autoware_motion_utils - fmt::fmt -) - if(BUILD_TESTING) find_package(ament_cmake_ros REQUIRED) @@ -45,6 +40,7 @@ if(BUILD_EXAMPLES) foreach(example_file ${example_files}) get_filename_component(example_name ${example_file} NAME_WE) ament_auto_add_executable(${example_name} ${example_file}) + set_source_files_properties(${example_file} PROPERTIES COMPILE_FLAGS -Wno-error -Wno-attributes -Wno-unused-parameter) target_link_libraries(${example_name} autoware_motion_utils matplotlibcpp17::matplotlibcpp17 diff --git a/common/autoware_motion_utils/examples/example_interpolator.cpp b/common/autoware_motion_utils/examples/example_interpolator.cpp index 8c98143af7e28..6ce81e4572276 100644 --- a/common/autoware_motion_utils/examples/example_interpolator.cpp +++ b/common/autoware_motion_utils/examples/example_interpolator.cpp @@ -12,7 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" #include "autoware/motion_utils/trajectory_container/interpolator/linear.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp" #include @@ -27,26 +31,25 @@ int main() auto plt = matplotlibcpp17::pyplot::import(); // create random values - std::vector axis = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0}; + std::vector bases = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0}; std::vector values; std::random_device seed_gen; std::mt19937 engine(seed_gen()); std::uniform_real_distribution<> dist(-1.0, 1.0); - for (size_t i = 0; i < axis.size(); ++i) { + for (size_t i = 0; i < bases.size(); ++i) { values.push_back(dist(engine)); } // Scatter Data - plt.scatter(Args(axis, values)); + plt.scatter(Args(bases, values)); - using autoware::motion_utils::trajectory_container::interpolator::Interpolator; - using autoware::motion_utils::trajectory_container::interpolator::InterpolatorCreator; + using autoware::motion_utils::trajectory_container::interpolator::InterpolatorInterface; // Linear Interpolator { using autoware::motion_utils::trajectory_container::interpolator::Linear; - auto interpolator = *InterpolatorCreator().set_axis(axis).set_values(values).create(); + auto interpolator = *Linear::Builder{}.set_bases(bases).set_values(values).build(); std::vector x; std::vector y; - for (double i = axis.front(); i < axis.back(); i += 0.01) { + for (double i = bases.front(); i < bases.back(); i += 0.01) { x.push_back(i); y.push_back(interpolator.compute(i)); } @@ -56,11 +59,11 @@ int main() // AkimaSpline Interpolator { using autoware::motion_utils::trajectory_container::interpolator::AkimaSpline; - auto interpolator = - *InterpolatorCreator().set_axis(axis).set_values(values).create(); + + auto interpolator = *AkimaSpline::Builder{}.set_bases(bases).set_values(values).build(); std::vector x; std::vector y; - for (double i = axis.front(); i < axis.back(); i += 0.01) { + for (double i = bases.front(); i < bases.back(); i += 0.01) { x.push_back(i); y.push_back(interpolator.compute(i)); } @@ -70,11 +73,10 @@ int main() // CubicSpline Interpolator { using autoware::motion_utils::trajectory_container::interpolator::CubicSpline; - auto interpolator = - *InterpolatorCreator().set_axis(axis).set_values(values).create(); + auto interpolator = *CubicSpline::Builder{}.set_bases(bases).set_values(values).build(); std::vector x; std::vector y; - for (double i = axis.front(); i < axis.back(); i += 0.01) { + for (double i = bases.front(); i < bases.back(); i += 0.01) { x.push_back(i); y.push_back(interpolator.compute(i)); } @@ -85,10 +87,10 @@ int main() { using autoware::motion_utils::trajectory_container::interpolator::NearestNeighbor; auto interpolator = - *InterpolatorCreator>().set_axis(axis).set_values(values).create(); + *NearestNeighbor::Builder{}.set_bases(bases).set_values(values).build(); std::vector x; std::vector y; - for (double i = axis.front(); i < axis.back(); i += 0.01) { + for (double i = bases.front(); i < bases.back(); i += 0.01) { x.push_back(i); y.push_back(interpolator.compute(i)); } @@ -98,11 +100,10 @@ int main() // Stairstep Interpolator { using autoware::motion_utils::trajectory_container::interpolator::Stairstep; - auto interpolator = - *InterpolatorCreator>().set_axis(axis).set_values(values).create(); + auto interpolator = *Stairstep::Builder{}.set_bases(bases).set_values(values).build(); std::vector x; std::vector y; - for (double i = axis.front(); i < axis.back(); i += 0.01) { + for (double i = bases.front(); i < bases.back(); i += 0.01) { x.push_back(i); y.push_back(interpolator.compute(i)); } diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp index a6542d1fc212f..a6a0c69744301 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp @@ -16,7 +16,6 @@ #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_ #include #include -#include #include #include #include diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp index 363d046534399..f9a2ac1316a4e 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp @@ -15,11 +15,10 @@ #ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_ #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_ -#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp" #include -#include #include namespace autoware::motion_utils::trajectory_container::interpolator @@ -30,7 +29,7 @@ namespace autoware::motion_utils::trajectory_container::interpolator * * This class provides methods to perform Akima spline interpolation on a set of data points. */ -class AkimaSpline : public Interpolator +class AkimaSpline : public detail::InterpolatorMixin { private: Eigen::VectorXd a_, b_, c_, d_; ///< Coefficients for the Akima spline. @@ -40,21 +39,20 @@ class AkimaSpline : public Interpolator * * This method computes the coefficients for the Akima spline. * - * @param axis The axis values. + * @param bases The bases values. * @param values The values to interpolate. */ void compute_parameters( - const Eigen::Ref & axis, + const Eigen::Ref & bases, const Eigen::Ref & values); /** * @brief Build the interpolator with the given values. * - * @param axis The axis values. + * @param bases The bases values. * @param values The values to interpolate. */ - void build_impl( - const Eigen::Ref & axis, const std::vector & values) override; + void build_impl(const std::vector & bases, const std::vector & values) override; /** * @brief Compute the interpolated value at the given point. @@ -89,13 +87,6 @@ class AkimaSpline : public Interpolator * @return The minimum number of required points. */ [[nodiscard]] size_t minimum_required_points() const override { return 5; } - - /** - * @brief Clone the interpolator. - * - * @return A shared pointer to a new instance of the interpolator. - */ - [[nodiscard]] std::shared_ptr> clone() const override; }; } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp index 50cff1dde3f35..437ddd727cc7d 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__CUBIC_SPLINE_HPP_ #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__CUBIC_SPLINE_HPP_ -#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp" #include @@ -29,33 +29,32 @@ namespace autoware::motion_utils::trajectory_container::interpolator * * This class provides methods to perform cubic spline interpolation on a set of data points. */ -class CubicSpline : public Interpolator +class CubicSpline : public detail::InterpolatorMixin { private: Eigen::VectorXd a_, b_, c_, d_; ///< Coefficients for the cubic spline. - Eigen::VectorXd h_; ///< Interval sizes between axis points. + Eigen::VectorXd h_; ///< Interval sizes between bases points. /** * @brief Compute the spline parameters. * * This method computes the coefficients for the cubic spline. * - * @param axis The axis values. + * @param bases The bases values. * @param values The values to interpolate. */ void compute_parameters( - const Eigen::Ref & axis, + const Eigen::Ref & bases, const Eigen::Ref & values); /** * @brief Build the interpolator with the given values. * - * @param axis The axis values. + * @param bases The bases values. * @param values The values to interpolate. * @return True if the interpolator was built successfully, false otherwise. */ - void build_impl( - const Eigen::Ref & axis, const std::vector & values) override; + void build_impl(const std::vector & bases, const std::vector & values) override; /** * @brief Compute the interpolated value at the given point. @@ -90,13 +89,6 @@ class CubicSpline : public Interpolator * @return The minimum number of required points. */ [[nodiscard]] size_t minimum_required_points() const override { return 4; } - - /** - * @brief Clone the interpolator. - * - * @return A shared pointer to a new instance of the interpolator. - */ - [[nodiscard]] std::shared_ptr> clone() const override; }; } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_interface.hpp similarity index 77% rename from common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_interface.hpp index f7de456b736a4..f0131e85e157c 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_interface.hpp @@ -13,15 +13,13 @@ // limitations under the License. // clang-format off -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_IMPL_HPP_ // NOLINT -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_IMPL_HPP_ // NOLINT +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_ // NOLINT +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_ // NOLINT // clang-format on #include #include -#include - #include namespace autoware::motion_utils::trajectory_container::interpolator::detail @@ -35,20 +33,20 @@ namespace autoware::motion_utils::trajectory_container::interpolator::detail * @tparam T The type of the values being interpolated. */ template -class InterpolatorCommonImpl +class InterpolatorCommonInterface { protected: - Eigen::VectorXd axis_; ///< Axis values for the interpolation. + std::vector bases_; ///< bases values for the interpolation. /** * @brief Get the start of the interpolation range. */ - [[nodiscard]] double start() const { return axis_(0); } + [[nodiscard]] double start() const { return bases_.front(); } /** * @brief Get the end of the interpolation range. */ - [[nodiscard]] double end() const { return axis_(axis_.size() - 1); } + [[nodiscard]] double end() const { return bases_.back(); } /** * @brief Compute the interpolated value at the given point. @@ -65,11 +63,10 @@ class InterpolatorCommonImpl * * This method should be overridden by subclasses to provide the specific build logic. * - * @param axis The axis values. + * @param bases The bases values. * @param values The values to interpolate. */ - virtual void build_impl( - const Eigen::Ref & axis, const std::vector & values) = 0; + virtual void build_impl(const std::vector & bases, const std::vector & values) = 0; /** * @brief Validate the input to the compute method. @@ -91,29 +88,30 @@ class InterpolatorCommonImpl [[nodiscard]] int32_t get_index(const double & s, bool end_inclusive = true) const { if (end_inclusive && s == end()) { - return static_cast(axis_.size()) - 2; + return static_cast(bases_.size()) - 2; } auto comp = [](const double & a, const double & b) { return a <= b; }; - return std::distance(axis_.begin(), std::lower_bound(axis_.begin(), axis_.end(), s, comp)) - 1; + return std::distance(bases_.begin(), std::lower_bound(bases_.begin(), bases_.end(), s, comp)) - + 1; } public: /** - * @brief Build the interpolator with the given axis and values. + * @brief Build the interpolator with the given bases and values. * - * @param axis The axis values. + * @param bases The bases values. * @param values The values to interpolate. * @return True if the interpolator was built successfully, false otherwise. */ - bool build(const Eigen::Ref & axis, const std::vector & values) + bool build(const std::vector & bases, const std::vector & values) { - if (axis.size() != static_cast(values.size())) { + if (bases.size() != values.size()) { return false; } - if (axis.size() < static_cast(minimum_required_points())) { + if (bases.size() < minimum_required_points()) { return false; } - build_impl(axis, values); + build_impl(bases, values); return true; } @@ -141,5 +139,5 @@ class InterpolatorCommonImpl } // namespace autoware::motion_utils::trajectory_container::interpolator::detail // clang-format off -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_IMPL_HPP_ // NOLINT +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_ // NOLINT // clang-format on diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp new file mode 100644 index 0000000000000..d3957567c49b5 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp @@ -0,0 +1,96 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// clang-format off +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_ // NOLINT +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_ // NOLINT +// clang-format on + +#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::motion_utils::trajectory_container::interpolator::detail +{ + +/** + * @brief Base class for interpolator implementations. + * + * This class implements the core functionality for interpolator implementations. + * + * @tparam InterpolatorType The type of the interpolator implementation. + * @tparam T The type of the values being interpolated. + */ +template +struct InterpolatorMixin : public InterpolatorInterface +{ + std::shared_ptr> clone() const override + { + return std::make_shared(static_cast(*this)); + } + + class Builder + { + private: + std::vector bases_; + std::vector values_; + + public: + [[nodiscard]] Builder & set_bases(const Eigen::Ref & bases) + { + bases_ = std::vector(bases.begin(), bases.end()); + return *this; + } + + [[nodiscard]] Builder & set_bases(const std::vector & bases) + { + bases_ = bases; + return *this; + } + + [[nodiscard]] Builder & set_values(const std::vector & values) + { + values_ = values; + return *this; + } + + [[nodiscard]] Builder & set_values(const Eigen::Ref & values) + { + values_ = std::vector(values.begin(), values.end()); + return *this; + } + + template + [[nodiscard]] std::optional build(Args &&... args) + { + auto interpolator = InterpolatorType(std::forward(args)...); + bool success = interpolator.build(bases_, values_); + if (!success) { + return std::nullopt; + } + return interpolator; + } + }; +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator::detail + +// clang-format off +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_ // NOLINT +// clang-format on diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp index 9fd451bcdcf6e..ad0e2b98ef040 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp @@ -17,11 +17,17 @@ #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_ // NOLINT // clang-format on -#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp" #include -namespace autoware::motion_utils::trajectory_container::interpolator::detail +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +template +class NearestNeighbor; + +namespace detail { /** * @brief Common Implementation of nearest neighbor. @@ -31,7 +37,7 @@ namespace autoware::motion_utils::trajectory_container::interpolator::detail * @tparam T The type of the values being interpolated. */ template -class NearestNeighborCommonImpl : public Interpolator +class NearestNeighborCommonImpl : public detail::InterpolatorMixin, T> { protected: std::vector values_; ///< Interpolation values. @@ -45,7 +51,7 @@ class NearestNeighborCommonImpl : public Interpolator [[nodiscard]] T compute_impl(const double & s) const override { const int32_t idx = this->get_index(s); - return (std::abs(s - this->axis_[idx]) <= std::abs(s - this->axis_[idx + 1])) + return (std::abs(s - this->bases_[idx]) <= std::abs(s - this->bases_[idx + 1])) ? this->values_.at(idx) : this->values_.at(idx + 1); } @@ -53,14 +59,13 @@ class NearestNeighborCommonImpl : public Interpolator /** * @brief Build the interpolator with the given values. * - * @param axis The axis values. + * @param bases The bases values. * @param values The values to interpolate. * @return True if the interpolator was built successfully, false otherwise. */ - void build_impl( - const Eigen::Ref & axis, const std::vector & values) override + void build_impl(const std::vector & bases, const std::vector & values) override { - this->axis_ = axis; + this->bases_ = bases; this->values_ = values; } @@ -73,8 +78,8 @@ class NearestNeighborCommonImpl : public Interpolator [[nodiscard]] size_t minimum_required_points() const override { return 1; } }; -} // namespace autoware::motion_utils::trajectory_container::interpolator::detail - +} // namespace detail +} // namespace autoware::motion_utils::trajectory_container::interpolator // clang-format off #endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_ // NOLINT // clang-format on diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp index 9ac5282429353..83b2275b0c121 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp @@ -17,11 +17,17 @@ #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_ // NOLINT // clang-format on -#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp" #include -namespace autoware::motion_utils::trajectory_container::interpolator::detail +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +template +class Stairstep; + +namespace detail { /** @@ -32,7 +38,7 @@ namespace autoware::motion_utils::trajectory_container::interpolator::detail * @tparam T The type of the values being interpolated. */ template -class StairstepCommonImpl : public Interpolator +class StairstepCommonImpl : public detail::InterpolatorMixin, T> { protected: std::vector values_; ///< Interpolation values. @@ -51,13 +57,12 @@ class StairstepCommonImpl : public Interpolator /** * @brief Build the interpolator with the given values. * - * @param axis The axis values. + * @param bases The bases values. * @param values The values to interpolate. */ - void build_impl( - const Eigen::Ref & axis, const std::vector & values) override + void build_impl(const std::vector & bases, const std::vector & values) override { - this->axis_ = axis; + this->bases_ = bases; this->values_ = values; } @@ -72,8 +77,8 @@ class StairstepCommonImpl : public Interpolator */ [[nodiscard]] size_t minimum_required_points() const override { return 2; } }; - -} // namespace autoware::motion_utils::trajectory_container::interpolator::detail +} // namespace detail +} // namespace autoware::motion_utils::trajectory_container::interpolator // clang-format off #endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_ // NOLINT diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp index f0b12a47819e8..03827ded59cb6 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_HPP_ #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_HPP_ -#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_interface.hpp" #include @@ -29,10 +29,10 @@ namespace autoware::motion_utils::trajectory_container::interpolator * @tparam T The type of the values being interpolated. (e.g. double, int, etc.) */ template -class Interpolator : public detail::InterpolatorCommonImpl +class InterpolatorInterface : public detail::InterpolatorCommonInterface { public: - [[nodiscard]] virtual std::shared_ptr> clone() const = 0; + [[nodiscard]] virtual std::shared_ptr> clone() const = 0; }; /** @@ -41,7 +41,7 @@ class Interpolator : public detail::InterpolatorCommonImpl * This class adds methods for computing first and second derivatives. */ template <> -class Interpolator : public detail::InterpolatorCommonImpl +class InterpolatorInterface : public detail::InterpolatorCommonInterface { protected: /** @@ -89,7 +89,7 @@ class Interpolator : public detail::InterpolatorCommonImpl return compute_second_derivative_impl(s); } - [[nodiscard]] virtual std::shared_ptr> clone() const = 0; + [[nodiscard]] virtual std::shared_ptr> clone() const = 0; }; } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator_creator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator_creator.hpp deleted file mode 100644 index 911adcb6545b6..0000000000000 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator_creator.hpp +++ /dev/null @@ -1,77 +0,0 @@ -// Copyright 2024 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_CREATOR_HPP_ -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_CREATOR_HPP_ - -#include - -#include -#include -#include - -namespace autoware::motion_utils::trajectory_container::interpolator -{ - -// Forward declaration -template -class Interpolator; - -template -class InterpolatorCreator -{ -private: - Eigen::VectorXd axis_; - std::vector values_; - -public: - [[nodiscard]] InterpolatorCreator & set_axis(const Eigen::Ref & axis) - { - axis_ = axis; - return *this; - } - - [[nodiscard]] InterpolatorCreator & set_axis(const std::vector & axis) - { - axis_ = Eigen::Map(axis.data(), static_cast(axis.size())); - return *this; - } - - [[nodiscard]] InterpolatorCreator & set_values(const std::vector & values) - { - values_ = values; - return *this; - } - - [[nodiscard]] InterpolatorCreator & set_values(const Eigen::Ref & values) - { - values_ = std::vector(values.begin(), values.end()); - return *this; - } - - template - [[nodiscard]] std::optional create(Args &&... args) - { - auto interpolator = InterpolatorType(std::forward(args)...); - bool success = interpolator.build(axis_, values_); - if (!success) { - return std::nullopt; - } - return interpolator; - } -}; - -} // namespace autoware::motion_utils::trajectory_container::interpolator - -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_CREATOR_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp index 9854b100f3742..99fcb469365b8 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp @@ -15,11 +15,10 @@ #ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__LINEAR_HPP_ #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__LINEAR_HPP_ -#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp" #include -#include #include namespace autoware::motion_utils::trajectory_container::interpolator @@ -30,7 +29,7 @@ namespace autoware::motion_utils::trajectory_container::interpolator * * This class provides methods to perform linear interpolation on a set of data points. */ -class Linear : public Interpolator +class Linear : public detail::InterpolatorMixin { private: Eigen::VectorXd values_; ///< Interpolation values. @@ -38,12 +37,11 @@ class Linear : public Interpolator /** * @brief Build the interpolator with the given values. * - * @param axis The axis values. + * @param bases The bases values. * @param values The values to interpolate. * @return True if the interpolator was built successfully, false otherwise. */ - void build_impl( - const Eigen::Ref & axis, const std::vector & values) override; + void build_impl(const std::vector & bases, const std::vector & values) override; /** * @brief Compute the interpolated value at the given point. @@ -81,13 +79,6 @@ class Linear : public Interpolator * @return The minimum number of required points. */ [[nodiscard]] size_t minimum_required_points() const override; - - /** - * @brief Clone the interpolator. - * - * @return A shared pointer to the cloned interpolator. - */ - [[nodiscard]] std::shared_ptr> clone() const override; }; } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp index 32bff2d30b41d..72b61001b05e3 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp @@ -17,8 +17,6 @@ #include "autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp" -#include - namespace autoware::motion_utils::trajectory_container::interpolator { @@ -44,16 +42,6 @@ class NearestNeighbor : public detail::NearestNeighborCommonImpl { public: NearestNeighbor() = default; - - /** - * @brief Clone the interpolator. - * - * @return A shared pointer to a new instance of the interpolator. - */ - [[nodiscard]] std::shared_ptr> clone() const override - { - return std::make_shared>(*this); - } }; /** @@ -83,16 +71,6 @@ class NearestNeighbor : public detail::NearestNeighborCommonImpl public: NearestNeighbor() = default; - - /** - * @brief Clone the interpolator. - * - * @return A shared pointer to a new instance of the interpolator. - */ - [[nodiscard]] std::shared_ptr> clone() const override - { - return std::make_shared>(*this); - } }; } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp index 21bf57b46e3b7..801b207b25afc 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp @@ -17,8 +17,6 @@ #include "autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp" -#include - namespace autoware::motion_utils::trajectory_container::interpolator { @@ -44,16 +42,6 @@ class Stairstep : public detail::StairstepCommonImpl { public: Stairstep() = default; - - /** - * @brief Clone the interpolator. - * - * @return A shared pointer to a new instance of the interpolator. - */ - [[nodiscard]] std::shared_ptr> clone() const override - { - return std::make_shared>(*this); - } }; /** @@ -83,16 +71,6 @@ class Stairstep : public detail::StairstepCommonImpl public: Stairstep() = default; - - /** - * @brief Clone the interpolator. - * - * @return A shared pointer to a new instance of the interpolator. - */ - [[nodiscard]] std::shared_ptr> clone() const override - { - return std::make_shared>(*this); - } }; } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp b/common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp index eb2fe2ab8deed..747362ac9167e 100644 --- a/common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp +++ b/common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp @@ -17,18 +17,17 @@ #include #include -#include #include namespace autoware::motion_utils::trajectory_container::interpolator { void AkimaSpline::compute_parameters( - const Eigen::Ref & axis, const Eigen::Ref & values) + const Eigen::Ref & bases, const Eigen::Ref & values) { - const auto n = static_cast(axis.size()); + const auto n = static_cast(bases.size()); - Eigen::VectorXd h = axis.tail(n - 1) - axis.head(n - 1); + Eigen::VectorXd h = bases.tail(n - 1) - bases.head(n - 1); Eigen::VectorXd m(n - 1); for (int32_t i = 0; i < n - 1; ++i) { @@ -61,39 +60,33 @@ void AkimaSpline::compute_parameters( } } -void AkimaSpline::build_impl( - const Eigen::Ref & axis, const std::vector & values) +void AkimaSpline::build_impl(const std::vector & bases, const std::vector & values) { - this->axis_ = axis; + this->bases_ = bases; compute_parameters( - this->axis_, + Eigen::Map(bases.data(), static_cast(bases.size())), Eigen::Map(values.data(), static_cast(values.size()))); } double AkimaSpline::compute_impl(const double & s) const { const int32_t i = this->get_index(s); - const double dx = s - this->axis_[i]; + const double dx = s - this->bases_[i]; return a_[i] + b_[i] * dx + c_[i] * dx * dx + d_[i] * dx * dx * dx; } double AkimaSpline::compute_first_derivative_impl(const double & s) const { const int32_t i = this->get_index(s); - const double dx = s - this->axis_[i]; + const double dx = s - this->bases_[i]; return b_[i] + 2 * c_[i] * dx + 3 * d_[i] * dx * dx; } double AkimaSpline::compute_second_derivative_impl(const double & s) const { const int32_t i = this->get_index(s); - const double dx = s - this->axis_[i]; + const double dx = s - this->bases_[i]; return 2 * c_[i] + 6 * d_[i] * dx; } -std::shared_ptr> AkimaSpline::clone() const -{ - return std::make_shared(*this); -} - } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp b/common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp index db25816d1d457..e464fc7cd0511 100644 --- a/common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp +++ b/common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp @@ -20,15 +20,15 @@ namespace autoware::motion_utils::trajectory_container::interpolator { void CubicSpline::compute_parameters( - const Eigen::Ref & axis, const Eigen::Ref & values) + const Eigen::Ref & bases, const Eigen::Ref & values) { - const int32_t n = static_cast(axis.size()) - 1; + const int32_t n = static_cast(bases.size()) - 1; - h_ = axis.tail(n) - axis.head(n); + h_ = bases.tail(n) - bases.head(n); a_ = values.transpose(); for (int32_t i = 0; i < n; ++i) { - h_(i) = axis(i + 1) - axis(i); + h_(i) = bases(i + 1) - bases(i); } Eigen::VectorXd alpha(n - 1); @@ -43,7 +43,7 @@ void CubicSpline::compute_parameters( mu(0) = z(0) = 0.0; for (int32_t i = 1; i < n; ++i) { - l(i) = 2.0 * (axis(i + 1) - axis(i - 1)) - h_(i - 1) * mu(i - 1); + l(i) = 2.0 * (bases(i + 1) - bases(i - 1)) - h_(i - 1) * mu(i - 1); mu(i) = h_(i) / l(i); z(i) = (alpha(i - 1) - h_(i - 1) * z(i - 1)) / l(i); } @@ -61,39 +61,33 @@ void CubicSpline::compute_parameters( } } -void CubicSpline::build_impl( - const Eigen::Ref & axis, const std::vector & values) +void CubicSpline::build_impl(const std::vector & bases, const std::vector & values) { - this->axis_ = axis; + this->bases_ = bases; compute_parameters( - this->axis_, + Eigen::Map(bases.data(), static_cast(bases.size())), Eigen::Map(values.data(), static_cast(values.size()))); } double CubicSpline::compute_impl(const double & s) const { const int32_t i = this->get_index(s); - const double dx = s - this->axis_(i); + const double dx = s - this->bases_.at(i); return a_(i) + b_(i) * dx + c_(i) * dx * dx + d_(i) * dx * dx * dx; } double CubicSpline::compute_first_derivative_impl(const double & s) const { const int32_t i = this->get_index(s); - const double dx = s - this->axis_(i); + const double dx = s - this->bases_.at(i); return b_(i) + 2 * c_(i) * dx + 3 * d_(i) * dx * dx; } double CubicSpline::compute_second_derivative_impl(const double & s) const { const int32_t i = this->get_index(s); - const double dx = s - this->axis_(i); + const double dx = s - this->bases_.at(i); return 2 * c_(i) + 6 * d_(i) * dx; } -std::shared_ptr> CubicSpline::clone() const -{ - return std::make_shared(*this); -} - } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp b/common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp index c3e4ec99c4357..fb714b208772c 100644 --- a/common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp +++ b/common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp @@ -16,16 +16,14 @@ #include -#include #include namespace autoware::motion_utils::trajectory_container::interpolator { -void Linear::build_impl( - const Eigen::Ref & axis, const std::vector & values) +void Linear::build_impl(const std::vector & bases, const std::vector & values) { - this->axis_ = axis; + this->bases_ = bases; this->values_ = Eigen::Map(values.data(), static_cast(values.size())); } @@ -33,8 +31,8 @@ void Linear::build_impl( double Linear::compute_impl(const double & s) const { const int32_t idx = this->get_index(s); - const double x0 = this->axis_(idx); - const double x1 = this->axis_(idx + 1); + const double x0 = this->bases_.at(idx); + const double x1 = this->bases_.at(idx + 1); const double y0 = this->values_(idx); const double y1 = this->values_(idx + 1); return y0 + (y1 - y0) * (s - x0) / (x1 - x0); @@ -43,8 +41,8 @@ double Linear::compute_impl(const double & s) const double Linear::compute_first_derivative_impl(const double & s) const { const int32_t idx = this->get_index(s); - const double x0 = this->axis_(idx); - const double x1 = this->axis_(idx + 1); + const double x0 = this->bases_.at(idx); + const double x1 = this->bases_.at(idx + 1); const double y0 = this->values_(idx); const double y1 = this->values_(idx + 1); return (y1 - y0) / (x1 - x0); @@ -59,10 +57,4 @@ size_t Linear::minimum_required_points() const { return 2; } - -std::shared_ptr> Linear::clone() const -{ - return std::make_shared(*this); -} - } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp b/common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp index 77f7af0eae93f..82c387c416436 100644 --- a/common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp +++ b/common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp @@ -15,7 +15,6 @@ #include #include -#include #include #include @@ -26,7 +25,7 @@ class TestInterpolator : public ::testing::Test { public: std::optional interpolator; - std::vector axis; + std::vector bases; std::vector values; void SetUp() override @@ -35,10 +34,10 @@ class TestInterpolator : public ::testing::Test std::random_device rd; std::mt19937 gen(rd()); std::uniform_real_distribution<> dis(-1, 1); - axis.resize(10); + bases.resize(10); values.resize(10); - for (size_t i = 0; i < axis.size(); ++i) { - axis[i] = static_cast(i); + for (size_t i = 0; i < bases.size(); ++i) { + bases[i] = static_cast(i); values[i] = dis(gen); } } @@ -55,11 +54,10 @@ TYPED_TEST_SUITE(TestInterpolator, Interpolators, ); TYPED_TEST(TestInterpolator, compute) { - using autoware::motion_utils::trajectory_container::interpolator::InterpolatorCreator; this->interpolator = - InterpolatorCreator().set_axis(this->axis).set_values(this->values).create(); - for (size_t i = 0; i < this->axis.size(); ++i) { - EXPECT_NEAR(this->values[i], this->interpolator->compute(this->axis[i]), 1e-6); + typename TypeParam::Builder().set_bases(this->bases).set_values(this->values).build(); + for (size_t i = 0; i < this->bases.size(); ++i) { + EXPECT_NEAR(this->values[i], this->interpolator->compute(this->bases[i]), 1e-6); } } From 21f547dc5904a76c8ede2e9908f0754fe0de9837 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 27 Sep 2024 20:40:37 +0900 Subject: [PATCH 61/95] fix(lane_change): fix abort distance enough check (#8979) * RT1-7991 fix abort distance enough check Signed-off-by: Zulfaqar Azmi * RT-7991 remove unused function Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../utils/utils.hpp | 3 --- .../src/scene.cpp | 8 ++++++-- .../src/utils/utils.cpp | 20 ------------------- 3 files changed, 6 insertions(+), 25 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 469e8e97c5d9f..363fa970f54c4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -121,9 +121,6 @@ std::vector generateDrivableLanes( double getLateralShift(const LaneChangePath & path); -bool hasEnoughLengthToLaneChangeAfterAbort( - const CommonDataPtr & common_data_ptr, const double abort_return_dist); - CandidateOutput assignToCandidate( const LaneChangePath & lane_change_path, const Point & ego_position); std::optional getLaneChangeTargetLane( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 5ce02395ab05e..4bde13165db0a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -1963,8 +1963,12 @@ bool NormalLaneChange::calcAbortPath() return false; } - if (!utils::lane_change::hasEnoughLengthToLaneChangeAfterAbort( - common_data_ptr_, abort_return_dist)) { + const auto enough_abort_dist = + abort_start_dist + abort_return_dist + + calculation::calc_stopping_distance(common_data_ptr_->lc_param_ptr) <= + common_data_ptr_->transient_data.dist_to_terminal_start; + + if (!enough_abort_dist) { RCLCPP_ERROR(logger_, "insufficient distance to abort."); return false; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 1fafb384d96d6..d86cb0e224625 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -548,26 +548,6 @@ double getLateralShift(const LaneChangePath & path) return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx); } -bool hasEnoughLengthToLaneChangeAfterAbort( - const CommonDataPtr & common_data_ptr, const double abort_return_dist) -{ - const auto & current_lanes = common_data_ptr->lanes_ptr->current; - if (current_lanes.empty()) { - return false; - } - - const auto & current_pose = common_data_ptr->get_ego_pose(); - const auto abort_plus_lane_change_length = - abort_return_dist + common_data_ptr->transient_data.current_dist_buffer.min; - if (abort_plus_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { - return false; - } - - const auto goal_pose = common_data_ptr->route_handler_ptr->getGoalPose(); - return abort_plus_lane_change_length <= - utils::getSignedDistance(current_pose, goal_pose, current_lanes); -} - std::vector> getSortedLaneIds( const RouteHandler & route_handler, const Pose & current_pose, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) From 92bec0dc0832fd66b77b5bffac19dc3b9685e3fb Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 30 Sep 2024 09:35:05 +0900 Subject: [PATCH 62/95] fix(autoware_map_based_prediction): adjust lateral duration when object is behind reference path (#8973) fix: adjust lateral duration when object is behind reference path Signed-off-by: Taekjin LEE --- .../src/path_generator.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp index 210795d7e6b25..a2a1b8b3d3fda 100644 --- a/perception/autoware_map_based_prediction/src/path_generator.cpp +++ b/perception/autoware_map_based_prediction/src/path_generator.cpp @@ -250,6 +250,15 @@ PredictedPath PathGenerator::generatePolynomialPath( terminal_point.d_vel = 0.0; terminal_point.d_acc = 0.0; + // if the object is behind of the reference path adjust the lateral_duration to reach the start of + // the reference path + double lateral_duration_adjusted = lateral_duration; + if (current_point.s < 0.0) { + const double distance_to_start = -current_point.s; + const double duration_to_reach = distance_to_start / terminal_point.s_vel; + lateral_duration_adjusted = std::max(lateral_duration, duration_to_reach); + } + // calculate terminal d position, based on backlash width { if (backlash_width < 0.01 /*m*/) { @@ -259,7 +268,7 @@ PredictedPath PathGenerator::generatePolynomialPath( } else { const double return_width = path_width / 2.0; // [m] const double current_momentum_d = - current_point.d + 0.5 * current_point.d_vel * lateral_duration; + current_point.d + 0.5 * current_point.d_vel * lateral_duration_adjusted; const double momentum_d_abs = std::abs(current_momentum_d); if (momentum_d_abs < backlash_width) { @@ -282,8 +291,8 @@ PredictedPath PathGenerator::generatePolynomialPath( } // Step 2. Generate Predicted Path on a Frenet coordinate - const auto frenet_predicted_path = - generateFrenetPath(current_point, terminal_point, ref_path_len, duration, lateral_duration); + const auto frenet_predicted_path = generateFrenetPath( + current_point, terminal_point, ref_path_len, duration, lateral_duration_adjusted); // Step 3. Interpolate Reference Path for converting predicted path coordinate const auto interpolated_ref_path = interpolateReferencePath(ref_path, frenet_predicted_path); From 841c331f0f13b475d5a2f4a3934429818297c18c Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 30 Sep 2024 10:01:48 +0900 Subject: [PATCH 63/95] fix(goal_planner): fix freespace planning chattering (#8981) Signed-off-by: kosuke55 --- .../src/goal_planner_module.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index ec7a8e12de573..4a59857725a5e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1329,9 +1329,12 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( return getPreviousModuleOutput(); } + const bool is_freespace = + thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE; if ( path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::NOT_DECIDED && + !is_freespace && needPathUpdate( planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, *parameters_)) { // if the final path is not decided and enough time has passed since last path update, @@ -1370,9 +1373,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( setOutput(context_data, output); // return to lane parking if it is possible - if ( - thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE && - canReturnToLaneParking(context_data)) { + if (is_freespace && canReturnToLaneParking(context_data)) { thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path()); } From 706fd9708cf3a0edf6c1d61e0a7fd4f9dc614459 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Mon, 30 Sep 2024 10:55:31 +0900 Subject: [PATCH 64/95] ci(cppcheck): add unusedFunction suppression (#8978) Signed-off-by: Ryuta Kambe --- .cppcheck_suppressions | 1 + 1 file changed, 1 insertion(+) diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index f95f6f238491b..5e1035de20c64 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -4,5 +4,6 @@ checkersReport missingInclude missingIncludeSystem unmatchedSuppression +unusedFunction useInitializationList useStlAlgorithm From 1442d1882aaff6d84d1c947f3c854678633ba5c5 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 30 Sep 2024 10:59:53 +0900 Subject: [PATCH 65/95] fix(autoware_lidar_centerpoint): convert object's velocity to follow its definition (#8980) * fix: convert object's velocity to follow its definition in box3DToDetectedObject function Signed-off-by: Taekjin LEE * Update perception/autoware_lidar_centerpoint/lib/ros_utils.cpp Co-authored-by: Kenzo Lobos Tsunekawa --------- Signed-off-by: Taekjin LEE Co-authored-by: Kenzo Lobos Tsunekawa --- perception/autoware_lidar_centerpoint/lib/ros_utils.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp index d7fc0aef204be..fc383a8d1da00 100644 --- a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp @@ -68,8 +68,10 @@ void box3DToDetectedObject( float vel_x = box3d.vel_x; float vel_y = box3d.vel_y; geometry_msgs::msg::Twist twist; - twist.linear.x = std::sqrt(std::pow(vel_x, 2) + std::pow(vel_y, 2)); - twist.angular.z = 2 * (std::atan2(vel_y, vel_x) - yaw); + twist.linear.x = std::cos(yaw) * vel_x + std::sin(yaw) * vel_y; + twist.linear.y = -std::sin(yaw) * vel_x + std::cos(yaw) * vel_y; + twist.angular.z = 0; // angular velocity is not supported + obj.kinematics.twist_with_covariance.twist = twist; obj.kinematics.has_twist = has_twist; if (has_variance) { From 35e9f4461b6d40058e111e2e187fed7793e8116b Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Mon, 30 Sep 2024 11:24:27 +0900 Subject: [PATCH 66/95] fix(interpolation): fix bug of interpolation (#8969) fix bug of interpolation Signed-off-by: Y.Hisaki --- .../src/scene_intersection_collision.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index ec6610048dc79..54af88c2f0fbb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -692,10 +692,11 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( object_info->predicted_object()); continue; } - if (!object_info->unsafe_info()) { + const auto unsafe_info_opt = object_info->unsafe_info(); + if (!unsafe_info_opt) { continue; } - const auto & unsafe_info = object_info->unsafe_info().value(); + const auto & unsafe_info = unsafe_info_opt.value(); // ========================================================================================== // if ego is over the pass judge lines, then the visualization as "too_late_objects" or // "misjudge_objects" is more important than that for "unsafe" From 72afef1df5795d00c1fd5428a3f9001d4178863a Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 30 Sep 2024 11:53:29 +0900 Subject: [PATCH 67/95] chore(codecov): ignore filename with 'debug' basename (#8984) Signed-off-by: Mamoru Sobue --- codecov.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/codecov.yaml b/codecov.yaml index 255312a29ccf7..c907c24818735 100644 --- a/codecov.yaml +++ b/codecov.yaml @@ -30,3 +30,4 @@ flag_management: ignore: - "**/test/*" - "**/test/**/*" + - "**/debug.*" From e9468b702ad7a5a0ab9b703b32746b1e1efdb303 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 30 Sep 2024 15:47:33 +0900 Subject: [PATCH 68/95] refactor(bpp): simplify ExtendedPredictedObject and add new member variables (#8889) * simplify ExtendedPredictedObject and add new member variables Signed-off-by: Zulfaqar Azmi * replace self polygon to initial polygon Signed-off-by: Zulfaqar Azmi * comment Signed-off-by: Zulfaqar Azmi * add comments to dist of ego Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../src/scene.cpp | 22 +++++------ .../src/utils/utils.cpp | 19 +++++----- .../path_safety_checker_parameters.hpp | 37 ++++++++++++------- .../src/marker_utils/utils.cpp | 2 +- .../path_safety_checker/objects_filtering.cpp | 2 +- .../path_safety_checker/safety_check.cpp | 9 ++--- .../src/start_planner_module.cpp | 2 +- .../src/scene.cpp | 10 ++--- 8 files changed, 56 insertions(+), 47 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 4bde13165db0a..a91aaba86bb1a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -483,14 +483,14 @@ void NormalLaneChange::insertStopPoint( for (const auto & object : target_objects.current_lane) { // check if stationary - const auto obj_v = std::abs(object.initial_twist.twist.linear.x); + const auto obj_v = std::abs(object.initial_twist.linear.x); if (obj_v > lane_change_parameters_->stopped_object_velocity_threshold) { continue; } // calculate distance from path front to the stationary object polygon on the ego lane. const auto polygon = - autoware::universe_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer(); + autoware::universe_utils::toPolygon2d(object.initial_pose, object.shape).outer(); for (const auto & polygon_p : polygon) { const auto p_fp = autoware::universe_utils::toMsg(polygon_p.to_3d()); const auto lateral_fp = autoware::motion_utils::calcLateralOffset(path.points, p_fp); @@ -535,21 +535,21 @@ void NormalLaneChange::insertStopPoint( const bool has_blocking_target_lane_obj = std::any_of( target_objects.target_lane_leading.begin(), target_objects.target_lane_leading.end(), [&](const auto & o) { - const auto v = std::abs(o.initial_twist.twist.linear.x); + const auto v = std::abs(o.initial_twist.linear.x); if (v > lane_change_parameters_->stopped_object_velocity_threshold) { return false; } // target_objects includes objects out of target lanes, so filter them out if (!boost::geometry::intersects( - autoware::universe_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), + autoware::universe_utils::toPolygon2d(o.initial_pose, o.shape).outer(), lanelet::utils::combineLaneletsShape(get_target_lanes()) .polygon2d() .basicPolygon())) { return false; } - const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose.pose); + const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose); return stopping_distance_for_obj < distance_to_target_lane_obj && distance_to_target_lane_obj < distance_to_ego_lane_obj; }); @@ -2121,10 +2121,10 @@ bool NormalLaneChange::has_collision_with_decel_patterns( utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); return std::any_of(objects.begin(), objects.end(), [&](const auto & obj) { - const auto selected_rss_param = (obj.initial_twist.twist.linear.x <= - lane_change_parameters_->stopped_object_velocity_threshold) - ? lane_change_parameters_->rss_params_for_parked - : rss_param; + const auto selected_rss_param = + (obj.initial_twist.linear.x <= lane_change_parameters_->stopped_object_velocity_threshold) + ? lane_change_parameters_->rss_params_for_parked + : rss_param; return is_collided( lane_change_path.path, obj, ego_predicted_path, selected_rss_param, debug_data); }); @@ -2218,10 +2218,10 @@ bool NormalLaneChange::isVehicleStuck( const auto base_distance = getArcCoordinates(current_lanes, getEgoPose()).length; for (const auto & object : lane_change_debug_.filtered_objects.current_lane) { - const auto & p = object.initial_pose.pose; // TODO(Horibe): consider footprint point + const auto & p = object.initial_pose; // TODO(Horibe): consider footprint point // Note: it needs chattering prevention. - if (std::abs(object.initial_twist.twist.linear.x) > 0.3) { // check if stationary + if (std::abs(object.initial_twist.linear.x) > 0.3) { // check if stationary continue; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index d86cb0e224625..8794d79a4d10f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -711,12 +711,12 @@ bool isParkedObject( using lanelet::geometry::toArcCoordinates; const double object_vel_norm = - std::hypot(object.initial_twist.twist.linear.x, object.initial_twist.twist.linear.y); + std::hypot(object.initial_twist.linear.x, object.initial_twist.linear.y); if (object_vel_norm > static_object_velocity_threshold) { return false; } - const auto & object_pose = object.initial_pose.pose; + const auto & object_pose = object.initial_pose; const auto object_closest_index = autoware::motion_utils::findNearestIndex(path.points, object_pose.position); const auto object_closest_pose = path.points.at(object_closest_index).point.pose; @@ -783,7 +783,7 @@ bool isParkedObject( { using lanelet::geometry::distance2d; - const auto & obj_pose = object.initial_pose.pose; + const auto & obj_pose = object.initial_pose; const auto & obj_shape = object.shape; const auto obj_poly = autoware::universe_utils::toPolygon2d(obj_pose, obj_shape); const auto obj_point = obj_pose.position; @@ -842,7 +842,7 @@ bool passed_parked_objects( const auto & leading_obj = objects.at(*leading_obj_idx); auto debug = utils::path_safety_checker::createObjectDebug(leading_obj); const auto leading_obj_poly = - autoware::universe_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape); + autoware::universe_utils::toPolygon2d(leading_obj.initial_pose, leading_obj.shape); if (leading_obj_poly.outer().empty()) { return true; } @@ -874,7 +874,7 @@ bool passed_parked_objects( const auto current_pose = common_data_ptr->get_ego_pose(); const auto dist_ego_to_obj = motion_utils::calcSignedArcLength( - current_lane_path.points, current_pose.position, leading_obj.initial_pose.pose.position); + current_lane_path.points, current_pose.position, leading_obj.initial_pose.position); if (dist_ego_to_obj < lane_change_path.info.length.lane_changing) { return true; @@ -903,12 +903,11 @@ std::optional getLeadingStaticObjectIdx( std::optional leading_obj_idx = std::nullopt; for (size_t obj_idx = 0; obj_idx < objects.size(); ++obj_idx) { const auto & obj = objects.at(obj_idx); - const auto & obj_pose = obj.initial_pose.pose; + const auto & obj_pose = obj.initial_pose; // ignore non-static object // TODO(shimizu): parametrize threshold - const double obj_vel_norm = - std::hypot(obj.initial_twist.twist.linear.x, obj.initial_twist.twist.linear.y); + const double obj_vel_norm = std::hypot(obj.initial_twist.linear.x, obj.initial_twist.linear.y); if (obj_vel_norm > 1.0) { continue; } @@ -964,8 +963,8 @@ ExtendedPredictedObject transform( const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration; const auto & velocity_threshold = lane_change_parameters.stopped_object_velocity_threshold; const auto start_time = check_at_prepare_phase ? 0.0 : prepare_duration; - const double obj_vel_norm = std::hypot( - extended_object.initial_twist.twist.linear.x, extended_object.initial_twist.twist.linear.y); + const double obj_vel_norm = + std::hypot(extended_object.initial_twist.linear.x, extended_object.initial_twist.linear.y); extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index 0e7d1cee65f02..795dc6d145190 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT #include +#include #include #include @@ -23,7 +24,7 @@ #include -#include +#include #include #include #include @@ -33,7 +34,9 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker { using autoware::universe_utils::Polygon2d; +using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::Shape; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; @@ -60,8 +63,8 @@ struct PoseWithVelocityAndPolygonStamped : public PoseWithVelocityStamped Polygon2d poly; PoseWithVelocityAndPolygonStamped( - const double time, const Pose & pose, const double velocity, const Polygon2d & poly) - : PoseWithVelocityStamped(time, pose, velocity), poly(poly) + const double time, const Pose & pose, const double velocity, Polygon2d poly) + : PoseWithVelocityStamped(time, pose, velocity), poly(std::move(poly)) { } }; @@ -75,22 +78,30 @@ struct PredictedPathWithPolygon struct ExtendedPredictedObject { unique_identifier_msgs::msg::UUID uuid; - geometry_msgs::msg::PoseWithCovariance initial_pose; - geometry_msgs::msg::TwistWithCovariance initial_twist; - geometry_msgs::msg::AccelWithCovariance initial_acceleration; - autoware_perception_msgs::msg::Shape shape; - std::vector classification; + Pose initial_pose; + Twist initial_twist; + Shape shape; + ObjectClassification classification; + Polygon2d initial_polygon; std::vector predicted_paths; + double dist_from_ego{0.0}; ///< Distance from ego to obj, can be arc length or euclidean. ExtendedPredictedObject() = default; explicit ExtendedPredictedObject(const PredictedObject & object) : uuid(object.object_id), - initial_pose(object.kinematics.initial_pose_with_covariance), - initial_twist(object.kinematics.initial_twist_with_covariance), - initial_acceleration(object.kinematics.initial_acceleration_with_covariance), - shape(object.shape), - classification(object.classification) + initial_pose(object.kinematics.initial_pose_with_covariance.pose), + initial_twist(object.kinematics.initial_twist_with_covariance.twist), + shape(object.shape) { + classification.label = std::invoke([&]() { + auto max_elem = std::max_element( + object.classification.begin(), object.classification.end(), + [](const auto & a, const auto & b) { return a.probability < b.probability; }); + + return (max_elem != object.classification.end()) ? max_elem->label + : ObjectClassification::UNKNOWN; + }); + initial_polygon = autoware::universe_utils::toPolygon2d(initial_pose, shape); } }; using ExtendedPredictedObjects = std::vector; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp index d57c2339041f4..90693aa65e661 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp @@ -608,7 +608,7 @@ MarkerArray showFilteredObjects( cube_marker = default_cube_marker(1.0, 1.0, color); cube_marker.pose = pose; }; - insert_cube_marker(obj.initial_pose.pose); + insert_cube_marker(obj.initial_pose); }); return marker_array; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 1de55dca29347..25b307ab2cc4d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -327,7 +327,7 @@ ExtendedPredictedObject transform( { ExtendedPredictedObject extended_object(object); - const auto obj_velocity = extended_object.initial_twist.twist.linear.x; + const auto obj_velocity = extended_object.initial_twist.linear.x; extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index e2ff8ce7b5195..59fc726bd10a0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -593,7 +593,7 @@ std::vector getCollidedPolygons( { debug.ego_predicted_path = predicted_ego_path; debug.obj_predicted_path = target_object_path.path; - debug.current_obj_pose = target_object.initial_pose.pose; + debug.current_obj_pose = target_object.initial_pose; } std::vector collided_polygons{}; @@ -709,11 +709,10 @@ bool checkPolygonsIntersects( CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj) { CollisionCheckDebug debug; - debug.current_obj_pose = obj.initial_pose.pose; - debug.extended_obj_polygon = - autoware::universe_utils::toPolygon2d(obj.initial_pose.pose, obj.shape); + debug.current_obj_pose = obj.initial_pose; + debug.extended_obj_polygon = autoware::universe_utils::toPolygon2d(obj.initial_pose, obj.shape); debug.obj_shape = obj.shape; - debug.current_twist = obj.initial_twist.twist; + debug.current_twist = obj.initial_twist; return {autoware::universe_utils::toBoostUUID(obj.uuid), debug}; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 4b3a76c8530dd..9df0791ed9375 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -559,7 +559,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough(const Pose & target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(), [&](const auto & o) { const auto arc_length = autoware::motion_utils::calcSignedArcLength( - centerline_path.points, ego_pose.position, o.initial_pose.pose.position); + centerline_path.points, ego_pose.position, o.initial_pose.position); if (arc_length > 0.0) return; if (std::abs(arc_length) >= std::abs(arc_length_to_closet_object)) return; arc_length_to_closet_object = arc_length; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index e3ecdff3759c0..4d13d4164d263 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -838,20 +838,20 @@ bool StaticObstacleAvoidanceModule::isSafePath( auto current_debug_data = utils::path_safety_checker::createObjectDebug(object); const auto obj_polygon = - autoware::universe_utils::toPolygon2d(object.initial_pose.pose, object.shape); + autoware::universe_utils::toPolygon2d(object.initial_pose, object.shape); const auto is_object_front = utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info); - const auto & object_twist = object.initial_twist.twist; + const auto & object_twist = object.initial_twist; const auto v_norm = std::hypot(object_twist.linear.x, object_twist.linear.y); - const auto object_type = utils::getHighestProbLabel(object.classification); - const auto object_parameter = parameters_->object_parameters.at(object_type); + const auto object_type = object.classification; + const auto object_parameter = parameters_->object_parameters.at(object_type.label); const auto is_object_moving = v_norm > object_parameter.moving_speed_threshold; const auto is_object_oncoming = is_object_moving && - utils::path_safety_checker::isTargetObjectOncoming(getEgoPose(), object.initial_pose.pose); + utils::path_safety_checker::isTargetObjectOncoming(getEgoPose(), object.initial_pose); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( object, parameters_->check_all_predicted_path); From 317df4dcad451dc8c577e87683f97783b6375af4 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Mon, 30 Sep 2024 16:37:09 +0900 Subject: [PATCH 69/95] perf(out_of_lane): use intersection with other lanes instead of difference with ego lane (#8870) Signed-off-by: Maxime CLEMENT --- .../README.md | 12 +- .../docs/ego_lane.png | Bin 17530 -> 0 bytes .../docs/other_lanes.png | Bin 0 -> 21164 bytes .../src/calculate_slowdown_points.cpp | 5 +- .../src/debug.cpp | 105 ++++++++++------ .../src/footprint.cpp | 3 + .../src/lanelets_selection.cpp | 116 +++++++++++------- .../src/lanelets_selection.hpp | 12 +- .../src/out_of_lane_collisions.cpp | 56 ++++++--- .../src/out_of_lane_collisions.hpp | 2 +- .../src/out_of_lane_module.cpp | 17 +-- .../src/types.hpp | 10 +- 12 files changed, 216 insertions(+), 122 deletions(-) delete mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ego_lane.png create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/other_lanes.png diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md index 0b6aa697fcbef..dfc88ef676838 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md @@ -27,18 +27,18 @@ The length of the trajectory used for generating the footprints is limited by th ![ego_footprints](./docs/footprints.png) -### 2. Ego lanelets +### 2. Other lanelets -In the second step, we calculate the lanelets followed by the ego trajectory. -We select all lanelets crossed by the trajectory linestring (sequence of trajectory points), as well as their preceding lanelets. +In the second step, we calculate the lanelets where collisions should be avoided. +We consider all lanelets around the ego vehicle that are not crossed by the trajectory linestring (sequence of trajectory points) or their preceding lanelets. -![ego_lane](./docs/ego_lane.png) +![other_lanes](./docs/other_lanes.png) -In the debug visualization the combination of all ego lanelets is shown as a blue polygon. +In the debug visualization, these other lanelets are shown as blue polygons. ### 3. Out of lane areas -Next, for each trajectory point, we create the corresponding out of lane areas by subtracting the ego lanelets (from step 2) from the trajectory point footprint (from step 1). +Next, for each trajectory point, we create the corresponding out of lane areas by intersection the other lanelets (from step 2) with the trajectory point footprint (from step 1). Each area is associated with the lanelets overlapped by the area and with the corresponding ego trajectory point. ![out_of_lane_areas](./docs/out_of_lane_areas.png) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ego_lane.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ego_lane.png deleted file mode 100644 index 65cb73226465ae3de6c27b35562d35c57c5906b5..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 17530 zcmbt+cRZEh|MyWTp^y_2*($>jiv)`ZhCH&b_`CIsu_%ImkmZE};1`KvZ4+g`T z#Jd9ilKS1n1pK)9Nzk3I#)}d(@2BU>3%1CK>7_U#O+ELl0v+OTNZ=T{? z@hU3ivlg=zml|mQ&9?ol)S@{e_>6CC^fS-PuLN*16d~EGZ#OngtKQ=Hj0MF;hWrj8 z@|}`Jic3-~1--eKS8uei;pR9?!%YX z$G9VQ-WEiBBxj+aa`R|#`3!&m*K6-N_!0!p!J0K3hZ$S-T&?DstxX&*<*qh-rnA3BZ+!QBzXo5 zX41=ssYH@bhT82SoStpWw6F`R-b7b-7P6_A>vR>eVbJ7KK1)|9m}Kn&^exg0$M~5F z4;=l{ja%{dx0BZTxP&D9-U>fTB_}8EVh+HLj~`Y1*v^=3ru#hAu%q{4m1OPg_TI~v zFU>R~3Kq?CwH}76j+pGvlbSG(+PWV~6>L95*461O)F94Q3l2R#9)p)$K_9h>WiN>R z5^1fRYiiFIsp-tIH9dTQ_^9>E<7YI-qSqVRYXLYb@yC&okv+G^zQqKf5JKlyNcru>rIr`-=c$$^MVTDmKVX4}Jqgzx ze}1OP{<3n|?Y1a9D;R|gFV6zIZjxMHUXG38NK!J&C28=G%04hH*v28zF2!`Z+!Wt( zZeVeeskNS50Xt4B>($p49Ua}$((;Y0vgWO>c~b8U>JCRyYhp1~` zCZ9hk9PHuJHp2J(l%`hABW-y8a_q17Hl~{}Rhx+^;e2Uf%@>*-i@mCRCsdD8F~umi~FXrH`VYp^03KCge}b#6~&vZkAd0|_AQc!t(U?y zGyHkL`+l8s;~?(7|5lpo1%A_GwIHCL6v_KnoH(4uoM^e1v`y{+v*92cJV`pbS^6`85Qu#~Y zj^xzz8K$XebL(wsDMDZG&|Lp~w5RmFE;QUtjndJ&y1MNnpDI1ZT(c^Vy}zT}gfD9; z+va+c{13K%Biu4Y=pQC!Pk+H?S6r|j;s4-rjlKd?`DmfGibA0V{r#5zjv{{7_sJqx zU~G`-t6Y~vhFbqdq1rsR7l#vAWK!XLFTh;HX@w2VKI@TOehk?#`Vs!{@6{aG&R60d zTPD`aO&7lrr$nc}Pd3|{z4k6V54UDBoAEiXnFqsY8=aSWlh`qB`?VRN?}qprj4LB` z^T$q4PYaY&$jHg>%D!XlaWle3ys0juIbFUu#X+1WAdZ&m+s@ijl(bEMzH&}2^xXNQ zDyxnk(hfhXmm>(ly-6KP;zz)RTA9mRJ1;(oUDhqxY#FxG^g)WRN8 zCyJbtp5hM$!!@I${%uVs2&^K%5p!ZpO2t~CjTLX23Nv4`yVV@zaU&^FGLx=bBl>t zw>L+SOD^@<1X_@v!6g09-@heeUc=YU>%UptgRQ6uKL2h-(Eo?TU+-ZO^H!dHGhqie zVbHQ=o8!R#e(e0C5r^sH`l3g13Qt*={GUpb{c#qlD!%3aocV9tEufSGPr{x6u`7|6 z1zTxsf=y_tQ7*7Xqh}GG#{6meV@-!y_fGupN0ET7{5vY3I@0;;sg|~`wnPfwJ@eMh zhY|6a(!5pX(&R&ua$^jpUsWiSK9|w>?2XdwER2rkaB!R|ZvV46Otu4x>MCw=xVx%} z%_R|?&0sjKzxPx4<>uv<>NiZ76QkmtR)hxz24Xf(7LEfETFWP8YPPioZ{NP{sl)^J z93gbO#cgfYs^&se(yh>-e1Byq&qHu^(y`5ca{o)=py6qMj|Fd36)m3J(OrC_JQ(fp z=N+HqBfq(mJL=!+)(tC~TYUf8lWKf-qN1Oeoh5IjV%*GgeP=WhKb`NsIb&pKNTn*Y z`rOeMrZMatgadN`P3oJzV8^4;7 zFv-r@`*GJW)&WGf&7F7l7*G9xQ?Z&atCrC+ZNVJCts~B{v0ahZd!5q7am`Cgic92V z#_isO9lAc8b(a_nuUuU^yT=6F*{%%hxyM@yGp=8{%f`I8O=a(d--7V+Z+*Q5i@INE zlU{iA?FNmw&*3NZwC~&KZaVdE2Ulm^*^{nL*4@8Vn&NI_0i^Is+F9+Mm%uir1oH@E zzrNW<+c~4SyfSs)7~gsA^1?=(sNcRVVP$t9!HeC!z(_}LA{ezfsoRr<*STXav(o4SbvR6kVwPJtg(#PVDbDPy<>6K=aN z)T+-44+qxUP_fWh=h0O)R76W+Z*PCIl<w0NY;VlJ6~1_n?A z1G;-&`{elcQbLI4( z@F=DxMO_`8fs~MnH5H?qtZ|*!VXzko=roDKxq$)yi=CV{dD5J~sh}G~f`WpF*!zyn zPTLl0s8*`;`DY|oV43B{mFw&FY!S(Zh#AhO-zMvk6exf8>}&-mqmOIHvM7Qga_C)k zrx!jA9nPrvZ6isCPBu4<+s-RRE7&qsoKIF-f*1<@6?7Gq6#zY}GTupNKv zK^~3o_gKf-a|Hjo{z-bf;{NXH$WJRe6bP8dbuBHeEiKu7BHc#}ep_do7u+c)A|&_Z z>Qb7h)M7l8H`XwfmF4AG;v(qFm1<6D9xtU_tj(UUThbrT9GrXkonJ(7rB>v|h0WWK zZ6BRJ-Z#m>{|LtBl!-qdwazIg9y(|}z@%_poGca>v*Tkhm?K7w5&q5DHm>Y!b?BP| zgVlapJuH>UgrUC?jydvtZnCEMEbr){TWqZj8qF4QgL#TpzisOCQ!Sjx3N7Xd!FJ4v z$JbM+%;@C$hIjqE8{+CL)phhbGmZXI!#s|6{eRyOa%gtnCLHP!&P3(L-6425&~h^I z`77}F0j>JKhz`H~{j2sgTg24c`)nu=2^5%sfPKEfX6q1Z?#DgJMEVfu#`$NWTd#F1 zF@y}dm1;;??b6Z1TN#{2jr<)!A<){7=R9dpPl8(VY3o~BI0aS1_x5!CckUTm#zKPV zNMP~rB}z_d^WX04ix5*7boY+vc?;dqj(rlu$K23+KGWhozq;D3qkDm@vM(cC&@~%* zVKYJ0#SG?td)>P4?BqaogfTZ(@>oG!Xo*6dvF@85NG>}I$K%lwBpD`kQDOFtnk)Wt+t>YI(=VI$(a z&R=SkYFF9NSJlsWDcjv=9rSh!s_0o3o+Zm*exB!~Y?p2~`d6UprioYc#W|W?SI}|h z)CGG@rQyR3y02PmXBkr}F`PTA+d&5Q&Gq#xg0)BUkq3^`UM=d&!^5Qu8f>IO*~JF6 z;V<0lv5SgN#4&9LGtcJ{XIonr8%w$?L+@usO<9wa%rvFj+=o zWT64$RUk+E^2Nh|fBTuw*4!J9dDjDwKKq@ClGxkX?e2Bn6a1>r2l8wQlrfdF_{_Y? zeT6#=pcD|q-gTsYQgtA+xJ0Y*jMyI`EU!+H;=6vq$@9Q_)OL@)zz2%7+$SEa+k+eV z8{riGQ+b-7joQ%oO>louko4u@XnUMV6JL28;i@8YF2&JFCo0>t))JH`Z_<70eR_!1 zi&qN(OXUBX8rIX-L6yemwn@95>hFZb_?=NeHOA&y?7gl%A7Py`7R-B*h0S@g3P&o*E% zIkpB1Gq6D~U#<=1$shT&>T%pruhQ#V-T|Iweq?wVD6M15UUJ3S=?6>w#)`E@$%0KN zWuHqrnDLIK@j1nIJ1zYBr6ZY>!l;PZGpjn>pLA4P?LK>4R$lIV&@|iPx_ZYS)GSzR zHx0t~c=zvU>FJs0?#fV)wF(XhgfNn!7ViU(rEc`M#bQvQ6{+PgD|eLYR)(7Dm5#=z z^f#XzYy!uGmHts(hP*h&lIaE)dCVT?)i*ua8b%o4 zqYJT5E_{G8fMsqnF#t7s+p~#NYw(B@-+nT?q7n}{4_Kr7=7Y@*<<}9fJH3@g_-w~! z$d7As&dSUOm?GQ3LL!P-pwKfnx?j2qZkW|g z)jQanjcs~DBk40%Kn(9R|EQ@22d)rkhVvGxA$O-dcxsvgtVd^z!+}% zjogG1XO_X;^>0BpPR~^fZTz;r-B{0fwU6dU7wVHef6L}6U&4u27=R<*&Bps}f1tk> zunUzKB85Gf%pw*Z@3cb;^;?~n$jr+$Jn6FUrwii_W>QZ?u0F)CS`~!lYdKr1W2Zc^ zW~?(MVh*eCY?8^zbO;%gWsrVHFR{BnI_otW73flT=e}J%Uuy%ImhUZ|7+m0a6^g$l z2UV4qKbovN8s45M8r*vL%I`cr_oIwf`Npp`hKKprvjeB#cS9%`t_37gdWcXwMK~q2@EP@y|yy6qNa&pH}L`U56 zMD+uHRCE~?70A*?G~i}Y;P%H#=eVM-_Vz0zzDG+b^Q)R$RaH&yn<#%&o@GzfW2xFk zv2^FHAR@`R8&?&-9o}wfzWt}rGM^Y^xy7Uh+OLUVBmAC+_d>rtr|K4Q>e8R6`Qh|Y z%is>DvMs;yjh?NkHvb$NLvGxV`WG%PnqN%J%@>aMK%G=kUao3K1_|7)F(v46*>$;ld{ghQ)E7K55V>p8DdnFh#Rr&D+2I_iwp{GA{ua zvbB!}MO7s7&zZZyy)TVz%zN3Ii@BLq26#e))WV~3M_+b7;g4!ZF&w2D<>~FssaaWB zc7ho_7QIW(c|*dYq7iE2uO*y*&oOCkbtBHJJkrBBe;j!h-n^Wc`$T1=Z^^kpLve(E zh8}Lw@s(Yij-c&yTiGuq_a&yH>%=q0r03co9laNDu|0e;GC~eh??3qSeNq`&)EBe) z^!@?NDrc>6&&$`*F}BilY-Xm;{(1Aq8|6Bsl_y$1*92sTc+(84hnzAS8bJLH$3GqY zh4D{lHksR)Z5!My+Xapy23Nn<06Y+p%Z-9Bo#Mt@>Q}5ql*=TJ8Jk=o$3RKALZnA4 zYg-2A#V*3LXU^WSlAy*UCMM=#5#2cN>+9=WZKuu(n|;T}qR5ws zH)0i#>v!{bVCM*3M5`_c8*lGvr}UoWtSoBd_+<2w>Arlnbvo;<^{n45)jqvZ9u8$c z1n^-t-yG%Is;cOwZs!_T?!;(f+=6IVh9$-vF)zvtrXRuoBzQ;E8I(h>TT;J|l6KB2N9|bGnt%cB%$a*HJfLiUVt5WYG&Q z(N;dggNV30npVsuteq4g6gxt$YeDKt`uFc~Rz$W#;YCGcq?B?PIt32KiQ>DnC6 zU`=jl?mnS1B-inm3D@SZDSH?z`joQ$`^phOFKHh!e>fEGe{a?A%5eigL=PEQ_q>GY zrhig_Kww2hLo43xGeqKjxJ74F#wuP;wYYzP3{%Y_^geNP$bFp#u$`|_86VS*jSPK+ z1~v$8u#~fAQh}h9YMFp|oa*{clFjvVFNJJZ22NVPo$$S8rKZOzxvkcY{LAv zm^id``kF$IDb^$y2i%(qK9KoSW3lZ$AbkrM=P$S#Y#1B4w&46}_S5E=nAf*7-s##w zhG;S8?$*F7AaK;!DBZv__kC2cSxS|!S=g8P%hJnf8vU)R@;%6BzS^Y^)dl&Ylfgq9qgT^jSVM`&c z=$;w!JCQY~mdC$Q+Suf6AfiB8lL!SZ?p0B;5b{;>rT8F#HL#5cx4f-U#vktNGY1Ld zSHkqaY#3n|fSH4q|Sb7piP#6%t&=}B7EceBvx!@NXk!Fy2|ZyqU~P#*ssEr zmxbRpBX4Q`QR@n2(|RaDno-z`*9zZ1jJ}18YP^49y#|x;J3R#1U(fTs+s!l7c4V)S zyh9EFZQqYRX{@@D@04A80BioWkP^Fb&;O*Zs`*sQPL(yqyLZ#iNPG}=PA5iu>Ki71 z4J6IyAR%<2KVBpi@*Hn)yTELAVI1ybvFhd8;|@$uU%Q;tIjzP&mASwo9z-n1>>aLe zO^rQwY+<;G}Gs0Fn523(c|JQ#Y{7&!f>21H$&*cQ2ZTl z?L?N6<@(_HKe73n!j<8-r33g^ums3WWQo`52Cva9(TxAalA6C4o8RH!`S=C1B0@0} z@j>h1O(rcalo&Fv$-?q}1FZ z-jBUq#}#)0D!vf1jbG(c8IGL9L*&SR2+v_+wR+FZmI4hnhPv!rgW3clcXwo)jx0r@4fB&pzJ<4AdlCV^3hUFyb+3pyr2=78VtsKs#s1ZKpWU#pyqQ+i)9n2^kDb1kuqlru;dL&O;ilQcReno-tX_t9{_3cOtm)&R1y8JKD|<7Y;V2Jo^xxEWt*V zIi7rlG4sfEl^(dy;g)Ia8w>%*#~T5wc{T`+6a;;)?cRjVSv_Av*ee^h18nyu$xw|o zf_O0cIZ~NN zjrG7!v9bQ*V{k?w6_rZ9$aekFg^3@l&J$%ubhuq3K~SA&b5|{)SWA-Dj74pru)JKG zzR}xVQ=BJn9zHcTMxlWZwO9%#0F1Gst$sKgzHCK6v%MIJi#X{cQ5(#)z?bycdMqv> z47nvU?4RfB3?Y10v_k2rD%3>+Ox`kY(=S(AOs1BWRuSzyVC{RxjG)+`$w;6zX8Y!- zDy{`R>EP2#Si2s?=gp+moY$M-c)q)srth*jBjNW6l$2W{x$BVgnQko0OXW%GE%imq z#oaRJum122s*rPw(Bg2hkJ19?p0f;o<-h0K#tP{X>a#)R>%!1tzGOdj$QSmd{>FXH zeYrJkK6wJEaty}uG>{XF3ft_~DxDVbyk6Mxh4~o5uL>0*5G643GSY^z#9Z0Mg?w?) zV9L=++XYuADfc%e(5v7#faVs^t+0zwg%v?#{hMp5I33UNl^4f(B?_%MM$G*|q$jwVtzVtY0h&<%`FaOG8c6CYG+W}qtUsGzbm#W=M0*h#=a3B~z_jgg>zFZql^#F!R1Ih+q(GaPsmew;*oHS%STq`XcKK%&Ri{r{ zF@*4>IWG^GSJ}MyWNRh}^>7Qtfyc=R%`+EKkZH(d6Z~cYy_?-J@JwBW@4j1XgVz!S zueDMzobW%n0FP&rQ!qccMERh>gZP`n^YW1NX?LK#YIWnf!QfxL1nu zPp3M?buj(cxm)J*FD?~A>KTL)3wQ1Bd*zvSxtJd6(f)0-xkLtoEVVgG>FXY(1USk4 zaklD4n_Ph;dp)50LAxWRDX2{WBj7F_?D52&K&|zDi7^-%_T#UunkMK@8bKfW(x)F*Q*;hGDK+g|X41KO<(o7P-%G^gIh%*Xln9+cFpmzGTX3ee2?fCLL10`A| zH@Lcf3HqU@jgjQfi)er#Vc+e?%``I_wMjFqu0Wq(Tyzm2wmN?$*w#u^*)T)86Hum9 zu&QVFQhG$fR&Sa!)fns}jF!H-jL$0AAW8qZ)OTq7fR(bDp?>SOyBSuv&_Ko_!n>{l_ zw+Zp-(i_xYdDncDDQ&Nzq)?ZsSH=;z68epe@H3+&v*I{ z<)!carQ^ZLgnY~^ze0V%{^~J&O~`5Ttt6-!0-xY4;$mtlYZp=saR-FkG;9>@0&O7n zfiTMqn=OU@Zp>4NJo8m#$+-|XD=4tMdYYz^O=FV!>XJA!(8FEbj-vu`Hokgta8)wV zn5Dk*f^^_ItR-)R0D9m-d;3i=5EuB)IVGwKk0FWgyqi2b=lK?*HXT3QMSLJ$b7tE-!moa{3TE?u~#w|555pT>%1 zPk`HKJru}L)lVh}30(G6hx4GekD3>R(+j6teNUdx9_!SMbHMQ@Cnvd#n_mcK%&P_7 z1lbqNLsdP|eb!fqu%n@&p{uJ4fHoQ!w+C*`bKo8088L3;WMtxlBW~0D{T{caC_+n0 z2KxI2D5F^;9w7Y6Se2^u7ArrswOyPZ?Ia+GQA9xATM-p&{#!Tv=NdPrrly96HCl`> zCNQmqYsXVlhQ!xE4PHHi`-f z9c0X&GW&6=T!FNheY5qXqNJqDj7Q_=nU`;|9wYti!NLB%-6S(TilVK0)V8Zzrt`+_ z=jCMnV|Iet+Izk^VZ8TR{)nE`wRo(j`@Y%&fV5c5&SFo(VFUMT$OqEr0!#F_w3O{x zq-=GhroLBQ#)fa+=R_rHiKER+kU(KBMXV4Hmwi`JVq3E+Ht=DsA^!7gn(3_xg1lGDpUAv65Qi2!3*)$>nO;IZ4T7Vd;DD%%IgtFr%vUs71S;MZ0lMd2#QV12}HbF|XtybrFNP3dYU(K$k3~uJp#q$w@>Bf8f?Hp&73eG#BD*R}dL= zJ;}Sx6I1q28LEoOQ8FKxWemLAvzTu;C>6B3FZu2N8(%`Fgn$3O^ta+=i(Pu5ezDe5 z@mq3`S7l>DJ4@!wPEKZUEtVujFe)Zn_Eh&hj4vqx4rD`O>nUklU@mL+G7pNd;|G6q zSgy|6u=IEd+miD=DPbseQ78@x!5#e$k1X`n<0A|Tv(V=pObKtH>?6&2g(F40MWg;O z$)G%$1Fa4pEv+rIKWGvsxqe$}tTaaK)|Icj&Q!~vyyE&T44V|y69<=Xb$nlPk%3=( z$aM2~LH%G`uKPgE@Y(`Y=q2`A@V2gnKAJrcr^gQrnDvUIiPS}^2xcr* zmaTb}>!2-ou_fWeHFDihAZ5E7YRa5oBS@JRB_$KZqdjtH7HZnJxG1FnxGlfj{;`_< z`u$<~q+IGP=PtptFDy<2=<-G{D?JJ{&v;d97OLOAc||23&!`yhs;UW>)lQoCDdtBS z=;;x2g$zjp-C;}Wu>k8Vv5_ieQgnvP`bZLxjMrriE+>%Za8V~DS&HLy^Y?djq?g!+ zR;;BDa9@~Afe03-QD{RM`oXqjnKzd;qVuD9w~)!532$l0)>^ly$)A_BWzSKe+bvOwReR7Xg7rV)42oeXvuVz%Nrk7WTU&EQ*Dllqh z6GX&W>Z`-)osEPVdg<{3r9TiOK>Gp9ua0rc=(qBT2s?b_A5JeP^Ca~HzAvb-9+$|P z%D`)VkZkak!@BK~60*oHVam0=#zGrx^lPWPEd!3 zKmn(74ES=25V$%;D8M1Q38lu^|8ZR9HgD&)t2HpCkTZn`n|^rq#d%4|M`?tAaEAk( zJCabz;Kx&W^mk8GO$Xx!!W2X}3Xw9QK$)&T8+l&Mh$|I`UZRW+J(T8tc)1fZZuevX zz}n8;_n7gRwYA5{~t&HlSwi6jjat$#d!MM_oKG5Bnf&So;@ax zAp!Y{#zVX-lXaH*uc=uGpHz<0=Yn{OuFho@eS~X2`BZY(7eV+$62mNV!I#j(SV4~( zYi|#<)R&i+cPT6`u_gX|nmiarO-+3lKLj*#7WcTJtXY9=aHpO28C*7=i7heky`}!I z-@oZd8Y*5+*11H5-6kSB`g_P4F>krubJer~7tuw5wyJ5YP>%m;C%C-4?9z}~Xw$kp z8MAOwPK%enAR@rcd1;7VEi>D`+S-8qx9l0SaK4?=Mi@AEc?HZ$3dJ6O}~Q_6NSq8Ql-ub4XFMnQmq z#zVjk#G^2e;5aEyw$yLktT!lUjX2B^AC6B;A#AYFsC$b%8E1{~nDfjQLULl(+_bd= zbs~BGe4wX-GWNdrh8y8yKMRd3Q_z;$+BFTCv$jd(WOeSZJ&=|Zq3h$QAa4EJsP&$p zeU>}#7!}1WR}c5q(hHZIJqLG0us6A-FLxXCMZKi7wFOj5F>6sgvg*048rs^d*AlR( zg=cup!ng_RfIVb0(c6pP_ITv8kEA~!tA>Px7;?J5diANLg&Kh?2W2E|UG9DjK1bVt zA-b`#0Z5`A99X&*;jwu{@!Hfq93u20PFpvrxt*V1tJcF1(~oOj%n84RDr37zOm~Tc zed8E!Ug+akU7r=;$Gg&M`66#$U!6XM{R+dW+0V) z_i6XR<%)~Rte;oQCq^c4v_3K2w%Zo=OO1N+I{?X`sKI;0uc|pZ;NBAO4zgmet^@UW z#-5$##jI2hQ*tJq-knd2(Wilj^NSsexLB#?<7^(XP`WnKkX_qh)on`?Ldb_1%Q2HW z68Ex$&d{pxsD}gAJlg_G8UnW?M}9>zI`F8Da7THyJjh96Wb;Hmp^yL~&DVmx~*pO4c`@?i=! z1CVtEWZzMD|EXd`I}=|cARy?W&6jEJdgSzue8F1G7tv1c9?FoOt)gr(6&em11=($Rasd%W}y=&xjEZfkKBoD`z@Az3$Z}k zXI0a0wX-vbh|Oiq8AMl?y#(5!#GJqe#|PBJ@RVQ@Dd75xcL29m42?z$InI;`*_&xl zgw`6^`$N|(H{pyXCFvb?57w|9UU4}Ybu*=sinVf`NG@YnBi-bsygV;k# zvd!GdSt7F3cM#0TUdELoZZqkxJ|7vpNSFQQ-nx^uSng|T0Tl1+056Qs#KB<$B%5C6 zry?Z2XXWl5v{>4ynVEBxQ0s3`;{uKFT-E^*$nWkF_W`H)X>y-OL zMGbhXQZEwt6qbrmbx2_v|7~obx}_+vNpD;;je)-#w-Y@7HOX@-`iDxb_OSX;U}1rq z@F;O;Q8fiF`x~QKe_NM=6YzQ;d+|hQ?YT*q=<5LUSYw@+YF(kAj@@^vL@rSv?jvas zOsxStWY=)~&kot5!kq{hE>ooVyJ=OVG(lh@(A-Y8MCoO!Me(fWA6{AYIH+CJpSgp` z>dt0V9Jfopz?&KHHe^Dvk|c9`tx)0yxSA*aOV)yS)!C2m9~>OyE$sP=??-gelR(9d zI3n!M^aoQ4rFgjRn|>=kyg;v(jT1kLP;oZCG?nHeLK3W7a0LBZ)8P15QI|R|#UA!_ zCd$v<{h(iH_NSY#jSX80df8c@etyzIEQXZk2lfnC%$dsC$?09pOQ~+BT-N!J_nRk9 zZLBmLKlzPx7C()yalQ=2vRB>Yybj#T>Zt9JOn!-NW{CL5!|6pMjB25g4p#Y!)YS@{A%TUJW7UaEGY)NL0}s+l5kY4PB)`i6$$d|O^c zYoa-{z1V=C{L!pt>N2Sud^(UDtW=S+j{j+UlwOGCK}fD^swY~kCU>65dc%=@6bn)- zjT>tt-A6^=4_P0uG@pR(O&8#KeXItC_NqBQD=fXA8ElxVQSAMM-x%FA3S?hBcfo=| z+>T*N7$fsN(T#qwr-v3d0(N_ss@wmDutoq1Ky|!%lqK&yzgG@Ubg71_gDfMgC5^7D z8o4BLP;$1Kgr?lJE!(KoDeWQ}n+x3_>>$3*5FW-g{o|Ie3F7(y1ZJ#ut;k%*mTAD5 z2HJ5%^$G4!SIW3jJ%90*1i%H4y|BeRiLB>`1|G8N!MxbO!!U+C-2|23(wsv-4Nr_i>cWihV&###T z5#sd1p6bkgE0+c;8evzX6h~3~1qim>^}^1}0`HRh(U6migM&Y#pPR5|+%A`X(QWc8 ze&^AVr+)I$l$NZz$8mO@70D%)d-d)$oXXR7?!}OFMn#|QTGPH1y8XY`gUfZ4lP&3o z`5#5(E~d-=$2;l*n@BM8AV#gL!>KpJ$f(Iff=Fc5L9B3RGSt(Xi&mjrj12%sI8Fkx zEMZoJTsQJqx5XVn0Ut|Zn=XhzsL9ls^O=P@?A{4kCp8w`klNW^LW8nJ@{UdVpHdPf z*}IiNBmkIr>;<@79o%-44*Zm-8v#)~=;}Yq%R#gzouZ-o@)E}PWJzcOqTbSJYj;un z_J9(Cb)d+kdh=nuW3TH*|ME-@&^?u+pGSx~-I2TH3o>V8Q;G;x85xT)!O0o2=VCn* zEgJ+iho=pulUO|EYEJR>N}N21Y%r{xO4<8%qn+C@C4UdB_11$+S&vm(!|%+wcoEbw!jm) z#Q~j_wgm{ZzV9w`n6clvcp_sLcjM+2FIU%mYAP+S$)wMVC#Ia3>?R5ZDlx6#vv28M z0s41B>1Jo5QbL<~B68P9wf!A7Itz+E_665h!Wf%hZAbkU%C0^|1QcRW9MvVh7PG*O@*rkzVr_Dd$i5oevhYYH{LVpH zw?!KEQo>yfuY1&EStD{;<1IZuJ)t|Dc`J_eoZnv1DGiYv#!@aK_yBAfuKZOv!UKPM z!NPwPceQcAdWkinokDpaTCsv7xC(&yk4uhP2Pqb^K-R7ppNL*+oDIV$f~|~EQ;E*~ z_>4O*Vfqs|L04HB^sZz3CNQ?ygB*mvBm#j1?C@Fx;%hrQJ55bZ_ZSlTEfd^XRYbe3 zrm~kCDozfu3qU@uuAi}|B9Qi$RDOKH!`_JGrq{_x($;KQ@joSdAD3_jHn{(dN$PSUi0c@LuL?l6D39tSTfkd`$C z9aRNvbM~Mnt?x3Ua7+wc zjU10!-|=Xvf}k+2!c5qqQQWy!x0v2WFyB&EQu1fVQ+0$woik%ZAUX_G#&2k;O5dFE zU4^MotBwS|UuwuMVTH?vn!aefjf^udZ(9*hQyUqLd_z03Vx zP5=DYJJBzadcrp#x< zoGAyrr}wNq4xo>;Ev`n^QL7Vcy_MYxk;Nsw2T#{X{LkzJ(G&@(`NiqEtkuJwOOjF7 zFGUc~D60+XBL3)Z5SycT!mDQf^v10@;q!T<^F{5Vpm2 z&H4^}^^=VTBV#th7LS06wM?b{I5=aCFg%FL__@>4yx=^tCvJqf$CDdIbU*tGGRoH)3eW3uj%qQ*#_wyf{bwk2oEg6?%GS~PELkA*!;m~|X; z3f;D9BXxIp4nQDFN~DPS*9$j~t{OHFEeE*MK!RwNmK>zuDf}352TO_l!U?j__Go60 zYuU+xiC=hRyY~*&;g&VywzYq~ANUt@;{|eqK-dhIgCM_EU>k7R?B$fhUz6oz2}?eb zvpgtazY=A#VK%n!4Hcy}6IfjJk-84r-4=(+Yc^O#I)qT?6s_b<* z>H=va=*>6?JEo_nb?ZxLh%s)yv+h*#+M`w@w*$W(_$VfaDVPz8p`*xWe1U_8(yZ3? zMsSjeM6o)t{|_*QLK}cUN)ZEWOmze99|RRCK=BUFRG2%iRzG7Uu1enY&PG{ zt_t5)6EL;1F%*D2wF76$OTi2&;K_B2#fooc$;NwdvttsEQV(boN)x+Nt_ zGNu$(HTv`qt!nO1P7dZEVv)B{(v4nwf1&eDbz<*=eR7d7ttBxF^w-Z5Z0g&NK+?_777UOQwcC1F&3Me?i zH4o0dzyVgQ^-xjWg7RML`I`gJZ5Tion+FnN91GApQFjDr%%j3Q=)Q|Z$74GAq>l_U z0Jve>u2ZUAd;3i1rGo>fzFGT_`AToMNLrEjpo@)H(> z0l8TCBX!^4-TTSfl^E3#Lj!}}e5rz4s3Q@=%rJ7RA z9-@Q}ojbnP>FOlcL#K$vAGTL%hJm)GWEvFccfcVC;3`suufl9xTwQ;@;3wGfH-Yccckbv%moQT zZEY=U1UNx^KNI&23;*KB-DLS>_qj}@uQqfwA_;seo3m2 z@)knrpszAwRntRp0vu6io6>P=A>fbvDqZ@$Lc2~)!vY$P8kLw1?q=VU{fh&P|DMHW z!LnzC+vnyli^8`ncfgUVpSHEY3Xw0qeERfNOdJS}jGX)kwB9Zm*}%*Jx-vb%n-*14 z67`}=YdB26;Jnq16}=7IIzSh@+HOR3td&qv=fmQx0Gt8jaXvkJHmwF0w))FTGDnED z_k}?NcfI52p?Hd&;PXn%kXOlZ0JOKA&ENZ7)(zuxz8sB{)lb2Ew8JuCl3WD z06}LE94=wY`AlT)p6g9{IlljnkpB+oYeL<@h+NPe1e9)#Vl4@A5Zo%(T0OARBcG)| z%z4gCO=WVRduV_11IPjUATKEBs-%;gwl@8QPs0;{)$gOC?}H>W$9I3c#(Hp`rF|?5 zuN^d908R(ax@_Op_$LK8-Yy;0`Lk9fCj2`$p`TN^4rqS3N8C{_&`^gn;RSNF9tO7IsqpgP zu?xR;Os5zk zZYkivOzPZjOVhVsAqOqyAkCmSM^EnkEJtSI*U31rRHh=o!tRdXR4#Bbp6}mJ+NifT zfgU^sJ|@-m5?h#q7hR{LCL-Oox4?nHtTgIr7r9mNz?aLO6N6(liJ-|(4F0ZN4^{?C z^_&iT1x%NS)J$Ijrh*GGFRutI|JOb1YjiZsfKkK<7=B>p7QaICRzg6Y$-kgcjS-E> T)*PUKflE>LsZ52mN#OqghXEdX diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/other_lanes.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/other_lanes.png new file mode 100644 index 0000000000000000000000000000000000000000..a56d6427f59e33723878bfb144665e9b57f84abe GIT binary patch literal 21164 zcmcG$1yEJd*9UqPP(&Ie1Ox#EX-VnsPU(=6R#NgJ-QB4mAl)D!UBV?qq)Vi`8!m4j z|KIm!zIor9d2imlJHrs?oW1v2d+p!)tvH0LC`n^sJjQ^*U|6y;5^6Bm-C*!ViGCOS zB~pAb9ejK6PDaNG2781Lecd?^`iy-FI{T~tEZAhf49jT)bbij2MrPpF9yfPn!gn{HooSm*DUyL zR}?pzU)A#3J-&FbYiXoM&Vo+eG`sh7HX@HSmB=m)zxzJA837jOz+kj!YNTMjFJk(^-(){( z+yggYd|`*dMAWDf@4#SjcMCCLuoyxabMVL)k^hg6n6-c*jZVOz(T#oPGjgKXfuF3}jmgFz| z&_DWEb3XP`AKpCSTlmc}Q+JV{C3%u@PMj|B{`-kb z9oPKKMW4Sz18=gVhUpjQ-%JP4fEcdJh3;cBN4!=Dxi2ol|$-V%lVQaL# z;#(k$GB+^0&)V|g_ITqACp!WY+P@FY(yu?{H~ioURA(4GQuIF9u4avhRE&(ijT6R0 z1KTOW*+Dn;GABGL-OxX*49|6K=ta)$U;YJP18om*g=V}K#mi=l#lJk89V;gDQg5Tn z{(YX0lFU+yH`44$1kVxFyh#ZO>*kTmyHFM?G_R&Sd@XUO5R)yfagbYhvT?AKEyfPz zHGRpU;?7MxJkH|2yL^47DMlqaU4qOaP~w~Fj-&Ky3ABV8Uj=+|91(9G(6yxnV@8}6*A z4(wpKa@RO9HM3kMA|gt&d2>6->c@YdtxJyT@SK}O*Pz*)XRC7qBdB_jxe2>&~cy37|r`7V56hgBqibGv^Ub;`MEmD zymFW=;_7UV0ORj+EdQ}Ng>rOi7gZ`u30tzEd-YCb#xCwJru0xDbFSlX)_Eg?X43C4 zZIXKVH`I;rTcNmDeam>e&rp0#x&k>V2uIsvEVz!Rr@3;3eNM)*XPYcD_gQv_Od_g2 zghbflaKj=nlSY}oOr;!)jwnbtU!6W0cRbnbvCQ`N_=U0V>ORmxA}inwAcsw1^3&lj zweY)_Lz(Il1=@E6dCundu7}{~fBbQ<1wRDTV48NaO=4OH9PEn$Q&rj27b!0_xC(Fm zcDkA_e09~jKv6kmvU5n1c<>g0Q`=9Q*CMSqrw9p&;|=tLp@$D2j*gB7iakx~MPC~6 z*SP09(*EeD&j~qGX0W{>N!Str8i2Q4*$D?GW{S;Oi@c-h>Dt#uH=(pQ@V;!q(`lRX zkwrA{?8niX6&knJ4G1lzQWqL1m@*v}TEv{6^~HCv7`1rq&Ndi7JUkD8))4wrpRXEa zyt=vh=Jxp)XZxWg*4AtGkJ)5Y&jE_Q64gx_a%jC_A3->@Tqv7gDBnwg4=(k7jgB9v zEAOqudIqLY%31w&u~TpK{PGOkRE@OcI3)3c9&SjJUlW3Udj)>;Etg+wF$cfs0^Pv2 zM^DI{oGQ<(Xre!(fFs{Fp`GpOH8r4EDfI;9J`l6bCV8;A#9>v=Z24tP06_Y;rM&4= zm#cd|XUfXiKCQ*JK0CD&F}=9=zzROaui7HpagZrH4~bUoHGm?MSqu(ayOOAWl7h>B z-VRQEuXEiQ=>uudK^J zYHnuZIeT*DXty(6sJ>Te*Aep8#ALB{!tjV@iY)?1_!>7B^XX@;d$3p72CbFfJ~V54 zg(eGHMy!^isThAsru9D$z1 z7%0|WRe~gh&rZ|nAmj7$aIW?AOG*vW)0=n++qbuF_i({>`O@m@D=;(PaCA=U85woY zys5ubzebx7LYttj*3&<47m$#2Qi1yfiw{<@Fkmc9@_%6$5i_s8u;qqd9;eEgJJ{_! z?3npm*`;y)KK6RIY46_fX7Jua-U#rRLaX@O2Zfk1&QrF?j=*Gw#O8*Eg79vv@61n> z1Gh=SR z7Jj}Moay${%qA_{V{?$-O({|CDDRibsYQLjT|QvOzE3+n>KGFR70EaT4Q}mTde!)r z7r(XQiJ48!nVp{e_J}DhxRIwxr^44zPj74gVE`L25Ahe;d<1xxk35~t7Hro(F4Mek zp7O`AFv#YNpkA2fKvdP$*N39V&H~BX-W|!Wc{E$hj8|S+v?uqOY4@<{m%9XiTpmhW zvtVWMQC77-nCO-rJ#J`_L=XG^T_G#iBDN*ipy6N)tsjnQK z#pW~InK8C?O!AtVEnMV?s*ta|rr(-qD0yRMNoUYX)Gk_gXL%(h5D16CL4$4hNJ){7 z<{-Z6XSQ5=NAC2Z2^;7AeqQ{JS6V>!kwrI~z1^fI0_R+xr4Q&cn+$al2ckN0Xx-|n zs>Gb_RGO#qtq3tV^i3J^zNlePeD~ROY2y7d<896Rk#L1*M9<3x0>9i`LpO#;;UTtq z8R?!a8q>dP`c6&^{mqV@u}z=9it*Yu+;WZk{+5B{*GZ##`9xSZ-B?jDu^JrkvY;mq zO=s-i?W8J1y-EMW5z*n+-1E3EqHN)O*UjO#b?X{y+U6~r6aU9(H!NzkvF}BzIz1kmqjI;=~S=l+EjFpQrOITxU-SGGDR{*pf)* zNVc4aG)23Kw!gGO6bXFTe~83b$5JCTPC*SPK@{QQz4?~M=KY)bggGuX5&8&||7PPL zcpkxnr9ZKREo!{9>mRiVkOfS0VWwwAe_-9@{<5mnq3 zCgdoB!p$tKH!&wVXc^*C$Hvm-(oFVu?(<{?LjPr((T5EM)2Oca>^E3Ir=C9h+*gE8qjj+Hw;KR?loCSDFL8l z@!8uP!=h$LTun}1*ld1lv9<0+9~FNO+)R_>L_50ZcyYLbKsLw4k@iLWJ99)#X}Luy zs^oMKrIu=sjUA|POx5h$UX75AGBc{+Y0~yG^}fmMTx;_G`>GGFaQ=mfp+@CDOe^wn74X?z#!T3Q^>cCYe&J%{J? zT^tomwqIVZvPrS$(%1h@xkh^c0uNu+cV^t#LV{U+UqZj?mI}JR;VL(HLfzhH1P*$rxbc1{%?aUyuI@@rjM(6N&1S5uOf@m z%z50&4Sm>><&Ff)68)AT|BS9{WMndxFGVFiH%>+wCG!}cAKs@@@1zxTjatP$K{@;1QS9^P~2Z72p;wt%>JU7tHv@a z%2v&9{sSaV-(BQ!fZr-;(;Vvq40XegW4zLKH?a$E0;~fApR0&D!Xl}l; z-xqHVJc+CCe_d5=D^*%m&K78YG!Q&2lGEpf0&A-{2#yDEK0x1JDz=DKcl-PDk{?j7 zQkkAo<}K=l-&mvi%ju17-y<96KhRv>xeJtr*~}*YIVUcFJclLst411~_4~cBB}u$4 zXz|iAKG-+w(pIkfeRQ>*mLKT2gO#@7A2hBXI1`?mKMq>tc~YpsaG+RmM zoI`^5J5WRIjEoF@!YP1vVjU1~>_AplKhs_`szEN?y;8hvjaRcUmu>HBsgg4R| zEFp1;6s(e_RO9tPdGO`aQXm1{bt?-ADaBugxC!yk6`Kz4-rKgT9MHL&%)8tVbF{JF zr7qq9F_?uVjg0wB%ai9pdQuK+(P(s@l4PoTg|nV_Uka|`639LtQ7z|q^?+|#U4D_V zz>wifw`8OPwXPxeMLsTHD+jl!=EJ4iVh5z9O9NqG)7YQxN)2y-1#}sDhE?6>m5pvQ4Gu4yuduQTTx~ye+s$~hvmMz2Y~nJZ zxivUq?GH0OZl4M2RVCKax}!VbkQ1#f)9ev9ek6Nr?_)fpDf4@Ab3zDY zEF&0<&-lU3{-Tc<|BwS`F9(^Aebk!qXRmWiaudhH^2wh*M>XYvfoQ!Ke{py^!%EP6 zy;&^W;65b+|R|{w%3&l zpZrb-_=-_%t7BWp*;-NYZe5Lp9y;Q8;9Lnzx1nVV+WOaqZ0W5hrL7X&ZWS%ap~FKx zLJF0BU2LK>)bzJHrhE0uX$(xx5`7HvN(s@}sW2yu5X3 zIF6A}iA-x|JOCmB)0 z&zj(T{8t`FOk2hc5y~9@oR6|@q(sbPfss+xaLs?n(NhRBCs9@C7F={kJ71^dV#aE=5ukv zlV0Eg!TeD+Du@zgSh#a$cN7!SEk8zjjgW8GT0G3g&KBb%~k&&R6k3_{sGx(d=M-p3X6>Sf|!iUo-;>q`~QTmua>novyy{A>(3v^Ve;NhP( zXtS`q&!KNe!UNy;7q&=$`^1#0;aLJg))(w`S7?RGspOLZ!1mtrHUy7o!xwY?O|b3OS7! z1__#%%2|J%^ffeecTPO{H4N5gkNk5#Ruc@btf*L6)Sr#6lAs4*Xcz>uY7B!m(}Q1E zKj2W@y5&Vinjg6%oduvxCNy9Ud9#-yv3y^lR#(tj@>RR>cul&K)9vW*-=vy@hZAVW z)hbb6meMpkrGwtb4y>hC}UX8ZP|h zKQJ;fg7wA&sK$*oiSTKdy>8Dle{%Nkcz&E4m7O?A3Unkl*-lTpgb!s6-gF9IlTlwD zM3;X*#K;ty<4(D1c--I0u{1mDx#(1x%!Yl4Fdx;oeeCfBZ_q@+A7KhH>Cit5a#ItB z6dbyZLhe6}Zjf8$1@rD}e0Yz-mDi5S-%dwg6kjzx?$7omVvnP_{Zmy{rAA*;)pFC< zw>77=DLcOFk1#$;0fDp#H6aF?>wdA(4a1?Y)yDNjvfol~+Ue43!;7A1xYx>jGsoGk z0sO|r8hf=xvdU!E%*}6;yzf2y-NDsqIONoWg%bI{m&L{ODnyJn-GOzO-ce^FT3dxa zx-BkJ#SN^N#~Z@%mBj3dCDxYn-NpEu^yQV|eFj1;J*g_6imxwr7sl304a&5Nlg!cd z2sJ6PQ%Td)E{IF)uyINe6otH8P`uTTc|zB}Ct zw-@yKcCv3WSQSC1oFUc8u88176Mg%8%hCE4?aw#+=8+29Cbi-RekyJmzZZ!YgN1Kc zKrkT3H}~fLiD7_8DGIDBc(BEOUZ9H&6XaMyNX~?&rQB`ed4*ShcCs-FQ0Yi{p~)>D zXGZk?RT+&w>A*d-@rj8O|GAEII$N8x)!VEY)v@T%vXQS$vDDPL+Q+xH`N~RFD2fw0 z+2YTW<(Eh$yo=fQbA|H*J`>jq&pKW;Hy4Ujzi3L#z zJ^ct9y2vM2Wu5B>G*LOT421I}eH4{8Tv~b z8p4@`t_Wlrg$k|s!VURk7F-oLs+4In(WH|tEibBZ9~>KlgLM4Mn}UZWWof;4SA~9f zkeLXB0|c+P53{U$ZoXFjs=3+qc7D@~3NXtbPxH)W;e0d`z^7!KEHa;b(0gKVCh{5T zm1?=+97&{i3qCry4<)H)cgvM3N?np4ySyYZgMSy;)wayH{S% z{((bo@k!MKyzSoIN_{XrL97X(c#qxD4ZX=%fZxV!aROgwMQIVP{KWkV=Z+1-Y&GPJ zkWSk`6=s2xmRhH7ra{F*ED7PA?z^Kt_H_O}Kf^W`?tSPUU${|5X=!PvWspd;8#ksp zJ%>sEim7s&;pJ()B{7i&G10YPc9YBZTd#G@Eif7ET685b(Z?2!#|)p^1?l*_XUOf? z4#NHj6T(owohOg-Q+9plOuw;lh=+$uXiU%2QjfCnIUq~u>qF+LsYmrAyompm0$>%Q zE9gASn;h_D#?JL!L$$?+=J^`aOoSHa<0vCaCm2CBnuQt&a}2y+xXfS=L|QNW`&I1> z=itb7$NGl>9Y9#G&K|8i^~_R>>cG_0j)8&Mzkh$#w+@~-UeRo|ONBtR=W>uzXx_S#DH57%N1 zYl@g&iy#uwbrt&F`i8LwP_De?@g}(ILeunzxtPJ=5r(eO8rz=;9iXKYltxBULStN7 zesy>>c84(;6w;FB8e*M|ID8t&xBlHo#S|4r1kLl>cIb5>=fZ3#g(etiCyJW6zQex3 zPKE>A0Hv1h8FM~2zE3d~I%j%T8=Z&%(qPlXZBjyomUT+F&hPb$)B4*aig%flwtHLX zC=rUyQC4?de?YR_$9r`T*!uBs#e!CY|B87$1rv+4PwrNShNY( zd8VtajSfGvnaI>Jd`_*a|63^=$(PYVr*gkU&)jlNvNcb8_O{&f{9ROtHeFX+CD|uQ z2t2%h*;$akh@-=#6x%s}s=AgBd_}UqC?jgkTW-GhoEl5|Lr>$k(fW1 zFh?hkOHFmW$$fS8+vC}W-eBdRl{NdyZ*6~*@Z-N_8Z^q5O&MuW~;9DB(PJEhStFs<8fSCYNfsOZsE$HyjgF}GtJur z1!`n&+S*Tp#+p^i;i28zN4EC6I2V-Z!d6u4#pUarIKs<%(T>ZTpa@1g7u)x;!vm+= zEJ&0cUL=Fcv2-+i+r1bs9b%2I{~86g0Tgp=5mm*St(7x^hAp=@8;q~dy{w6aEr*d) zJ2S5VRJ<;?#~lIna25ky)5?DK6Hs=)$=-B>*YCs&dre+`lMd^sXS4GO%c4pocv|#l zQXZ=cP zc@FUOklmJBAvkr0m`UQGr!{eW#>t7>?tEr}Kl|&^BqOf_p<;<_&)uQ^{{EhOcq7|P zf?gl!wO(HoeK0%WN{khR(L~JM+w{i~`lC$#0hIA3Ta#Naivy%I_BibtG}2fTSLx_l z=a2gr?lGn|W_oxF)Omx_n!n_S&lMn+<+Cv|m@Aj)?7m!&kIwLKqw@U3sG2C+5_6(L zE2$hOc^Jzn28vv8r+vEIx`(THh9%oa#1D1GC51nphsQ0G%mi`T@a_nJpqiDnwL8i` z7IV<7%Z@*`)h%EkF@`!iJBC7qO}p>f<_*G1n<1OutxSdfJCxm&q>C`3mu`Z~EDwBL zSWiQvakt-XMl#`(89MGw5y(zTB);tP8oo?ZTUdSJVx9>_WfpifHUEw8B9q)nI~E|65zLv@}<#*?PpOcgW3? zE2G=XqMRBT^V{dTAIH-EZK9Fw5; zP9RZ%_XwjHhZ4E<=fygdr^Y)%tR!uH%SoH&CB$!GDsp2^elv6GLewY0$;(@1GYXYy zd_yk0>tYv_Ea_1T+|cUHg?RlqzU%gIJ3}zNUg_Pkj;h>ju{&3Np0B!wLuVQ^aR4xr z%EO1p1)*e1V&b#FMSbxB05Ps0d}yL?yGy7N_!IsV;9d4f(I0|URPL{NW!5Nd|B=~z z>+tj!W}qXKF~NNZA0j4V$;PEmpc$Fq=e^q&`i4ZRP(zIFRMt2x967!~pWyjXmlXE* zuk@kpR+rGnYZ(M@C+v9BOzB-XV;wf(1U%6HP_F}H{eSeNfN?QzK#t7YTc z7^n*QUoBMLE;;r!m_M1JX?DLb>+}d>El&QQPydgmt51{_8_z{Zz)-J5oxS56?#keU zr!ojZI>d`W0@RI;6n_f-`%7TQ00_)mxRjvQpU4b?4>SGRV9HOF_k3pP3H#Xy!36L8 zzFH^!_ZaAb9hm&twj_HQ^LbQO~_|m5WpzFnvFSY85155v{1D}!{D2A z5dRBXrDDpIu`8}F&8kYmy$GD4L+Jf$IGy)NL+NTPF$@!8U{XJ01KCgK0?eTL$JY(E zVMOo)qnLk8oJ$%kHZ%|tQ&0OES(o?^=b ze8`k=X+SoABFr$crt)P=WugY(PG0lL$s;5xEe;Ux(#*zYYqr73i;+BCV#+Ja!PeB$ z60BLG!L#B8g;mgDbSrwR!zX?S8T&#$=-bPW%`KulQhd3@SGg7W zcw7Ptf}=M2KYW#l9;JPG5+Z0+JiP6$`Kc+4jF1137OkCM6VpslqmXm>zjQ`J{hO6_S z&eiVn&Ix)9J@Dnzl?N0;{V#o#BH1Oz-<>3^0J@;|0t@{g5b(d;4_f%3_CHhuVC66g zsQp0^_7s@y|8lEH>mv-b7Axn}Rq6%QpHIL-{$DkCiYS8)ww4D`kc;@g@BGBYLNo$X zONLDC|JpeE369zMCtd0_wDEVyp(d;UWh5#os9O5}Y=t-ZTR`|u1xW69`d|b8XE$n& zNg9L?&}sT#Ci-8*&p>Y!>f`C&{*}ko^573W7?8vp0Ia&Y<++ycG6Zy?pNe!^9#FXh z0og7w0!bfB&~b~@L+GS-+(2+OFBtA{_Z5`TkeUVE4E!Q=+_aR?Z~D*UkD^CHvHoEs z9x+qw)odu%Kj4pGZ@u#Hg?5qC5hG9&s2EbrjXFc(c|KBYjES%E>A$jk|76-%D(5%i zaS$$=eP#;!SAdPpv*KF_P}+V4T5FH#@t$*-3f)>;4|CUe%!KEs}$>G#5yc;tM^M}|89-)OIAz1Opf6rb16vg@%??d32SG;+?y!RHu;wcdy(`1M{p|saCa+k@YCM$UY&Sj0Wu(gfurwF#EHC7r zDisTB`gkZ(Qho|3;~3()(GrY+F2j}wpRgcIChVklMW43Ja_>K~-r8ck4kck?P4^;b zQw0;i4${&+l{JM|^`zx1&+qY7(m;R~l3e5i5(DSsBRN*YvHh`a8>F@c*OZ`KG$Iwwvc<;`;i!mooa?`7MdH(QrFaw+}7n6-%U)zc4s=@Ba1` z6&>2WbTUYG0PVlNACHaC3h3{{(A6|utgXWwB1fHu=Rk{il%IhF%-A_1IvP=RUFzU% z^XAPPE2|gT!vv~)nRRyjiE<5PS0Hh`skd-BzJf@$n*nJ=>ibDea`Su%0QQSpeLE9=3D6`Z8cUg340vN5K8vGXHOm27%ec@88TxpDneeb3G-n3-$hyal92Ux8{b6?)kgL)hO&7JhEUBjEkOBvZC^YK1_HWI-cu zIUO4^>}IBHwQxaAhzY%8bF+vEF)Vz0O>E>?kxD6&ACzZ@}3XD4jwBogy)2usKVX} z1d1&N5*xlVa9cgL;>nz!pGogu!Dt-vNWZ}(Lj8ENQGWG~T$)gFyu(#-|4eCLAX@O9 zhlhuq=_nDJ0b)gBge_v>vUK6Hsr8EAl~iQ*UOX`HUJha5+xzhS`_Vl00?!QUth6LdNjg?Pd8b-v>bv z1+iwYJFvJvWDDm#ls6-ki8~&Gxpq6-MD_KfMSNvzlB#AV+m_5WR|6xaanrnTdt=aR zFg5FQ{C2KtyD+rxAfl)rUw)M8G{!jGZl=yIJV(2H;zsKBG4-1V%wcaB5{r`0ES?vb ztGZG+T*)3JXTyDvfyD4zBc#0IsGl&t$TF0Z?ObeFW8tKIU#!i^_((SCk?KI<23>Uh z5eEm8mfZ{ys)>U`w84VoZVj#MnDxnGXK;IuGhe1sf#5I=wWKPlCJq z6Fi!cW3P?+9K+o>lE#_r{h7Cwdyc;Ami9<~;L8Nu0x$Ejrp$pXh)H%UnsO$8$`0}w z(9*?4G{@Ae&K6%smxSfX6siaM>AiSeq{&=>hZk-pY@<`)e3V?$?Qk|+_uUp8gpti; z9odv8luicq=U-|p(^lkM-n^kc-)B>BigmdA)|9P~DJY%x0j$leT1PoStuTQ}Zq#&% z!M#a2tRdq$YDdDfG}FYdfmW;8Z|4 z$$X{`&6FpevN-S2u#3cSW@@U*!n1P_ABzrRz+?-GioSfsI>a#bkiuu^DWgJ<+Vbd_ z$g6_CU;9pox_C4!yrzdE+>f*2B{aONsya85`4~aaQ1-{1J=K|S#|RS_3X*6u%eEdN z@CAR3Zfx$d;HBoL#+l=XpyuR@*sY~nW-X>q8xQBw%jJ%|Bkz5m5oVH+nrii`j0%z2 z`cQ-wQus}ERh1(M{##Dy0d_Qbdg=7v#ISx#{;V<{P<$D72!lDwFS9 z+Pp3vo0~HLIr90kiLFFANy2WB@U~kAW?NdY*6T)R?$btj;dmZfFkZXGXVxHQIq<$c7 z_l})Toc=_$-OPyqM*23)pz|Bp#xyE)KdLyY-`3nsXF23E+KwuI* z_Bu0gy?ohcgoTUu8x)}5PANzYubJyuTri<0^jX}<8t!;}nWV+!*defxH5+h@wFf>CB40V;8; z9VHy6*@}dct}$aZ9?ehD{g@DOv|!`mm?9FuNyfaryQ5ro7$Oj@KOtW*>hsQ#SUSp^ zm@Cs_K~^bG%`q%3e1qU0Pk+Y*R47yPy)2&UM_~djB5RH7u?wF)$)20In{vVzsoeIK zVk-0of6_m&Sh4}2H4zy<5kIPPGTX{AIdWG5x>LF>Gkea*zya#BFouaqj9J&}mR3En zQXEBvWUkd^&K|B#SFTudKU@yYw~AWgd>q*}8XqS&W~-;Ec_=V&>Zgsxlqjj%g?t6W z_50(V)m!!Rh`d*}V6<#&vr$Q+LEqlicA)il-3}PJd;)p_=3)nvF+p*ZmfAI3s*^1w zWaqC=@t9J!(2fyP*x$*?l%-c-+Dy5=Duph-QNiIOPDRsU6x`B}d0_b@B}p#i!QN5B z7S+HokbF_rwQ>@5c4R^Cx36A}S#zCE2IEjx1rVW%@_nMmXt2*yQ`M|hU1+VOnTd;s z=Wv6!jKG^gc%Pm@K=4>EjVUb0ZsKtijeMN&RDa!0WF(f>iBz}pdT%tP(YJT)3la*r zsKZB-!MBn3{%dkXkRMdnvmotnzD0+oy@!WaiBf0-TF~n_I7q4QZtxfOq7#LGS{iIYf>xZe2gWVbQYX?2gIC2 z;QUynjmbUB`|>O7ue)KMu6s=ja=9-a0;gfCB9qYt?TYtP( zqsxP(UeF|zL$9Qukdc;JIA)z_-Thv_*D54uxy$LkEY!hD3#11^9SKalr|<96yT=Oh+G(U|pI9ok2M2^#8;s5(hJ+}zjF zcO>2#H-#88+WN+aGD~3$yBJKoC!csmJyW+;*IeDSr95Eqh+}E(Wv3S1l_lxv(2~(o z{Z$_zDO-(MYrIjQt=*~@b4zB}lOaU`Hj_^j7O=gy!3CN7i+qy?+4V#?-qlA1Vi=JVeC zU8wLpzRxUeO%F{yHLnB?8ovLW=m%6YPMK!Ik?yWkvcJ@o1rvwY=;-KrdqKOIww~TY zr@a@xrcm2aj(b+JQUOP*0!wnWot|n!Utb@{gl@hr+r$zoG8h?H#8@%Ts74XRWlxP* zb5F~!H1AK#myHYfY#!3t)|2>h<{_-(ok6l@PuUjWs#3X&uaqhVPNJJ(xALx{e=b5Y zt=(tw$@1!$Y#bbhY4A}$Hz?c>!LWu+*W$+d`wJV45Ux+!Dz7tdtJvl{rH0wi#3@pQ_ruC4;Z@#d+hx?ZcK zS|!TL%J7>Am>`s&#RnuopT!M%?T#NHO_u&b(!yk;ad&7a%3R0CtV&oX>fXg-KWBkU zMK8;TN;ng`*z@>GyP2c!f4HCHXtn4Y!EJTM8=TgDMq`aY8A_QP2cM(#RE5(rg}eV2E4 zjJB(6u0NUIN48KS)zPv)@~^zYo)L|(d)o1MYCc=hg-gWj9&cHM)gBx51|r5ji)v)g z1d~3RC@xMst!x}EIH~Xde5Zesa9@ACN;Ot*up0ZF39@ED6ViD2zIj_a1%E z%%tF3U$MXe4jFIHfEUImlZR_l{ zaZxojZxDWuO3if&l(XyX((O_=>Oo5(UhJBCl6hf~b(%$~mT9}~OkKWJ2}*|G7&3Yh zuXgA3-a1rM$5%f0z$c;@m}gg1AFW9?^e8keuQpk2L(x^y96J(_RMj9`$E)=p#Xb5w_34T`D&3t@F6R%Wo!(vl*lo0}ACHgw#C)iIK zXiE&_@Iy|kM_3J~Xqi+AZ^QuG-fgqg0nXT#g@oVUJrchCy6?o}W{WXI9{{I>Xw0+k z?qW>)9d~^t2v9|-Sid>lX9TZ*&A#Ql7G%Bou*z36mGb_1d}>ApXtge%e3?!|)TMhZ zU1|08gaZb{mxBH;fafbmaKyI~>7HZg^NY|zT+(^Tln>|>Ew~x1Z_Pc2zG{9I`#LzF z{R#dyi6!M-?5tiIYUbpvQ!C}WP;KFpC&bDlK<9Z&VI-jOu74~)uem461$NUliAvTG zN-mxb&LU6yWNBi5fKKOaEeY#kb+KraX6{0MSXpYW5ew4 z--Mp`WaH9^U%FX9QK+vL$W)5_s_giEB%rA|47x=@ieSdn)HG|2g0R+S)|EJEM?0z8 z(Je>iYZh;(RI6rC)@YLjFh>HN35)%|VI5P#=%~Va z@dXYWpS%+4=;`SJnW#mG=fu%QMn@CG(xZdX?>?r|^;ZCSN4D#0CxZ!>IjLOLa*(2{ zWMa%z;(3gf)IIjNwYGL|k|*&{VE8@x@DX61*hJEmWTyMccO~xp)2r}I_)b+{l%%RN zxpb-4jn3=sq$p;z>{7;(rRm!^il#C$N4~TFq#pS|l1;?O&d%;YQTjkVW~{_V!Ga<2 z!01-@<)HX$X$iq^8)Ev=Qwu<1nkN+ zY8;p+Nbg!*yZM}eE3=n~-5s+M(!j6Ew}g@}U-cJ*;L&QB5aYrNxg|gIRjU{{ z!`*VYL7_&LfO|)_eVkIk6!~5fXs1&JkPS`^<{hH2+RmdKj(pyeKOZX^NNsH`9c;Fy zsx8Iu6;;>s6ws&TTcxe?VEBPJMt-ZUwgO5)|L1hd@CipWkcp+F4xC;b)gBxeoAM_r z^mt!dwbVxTDXb1$^5MOw`B~zOEqtHslit6Og8LvWWO9B2Gd;y%(2P*>0Y}a3BgSMs z&y8n%*A$^4b5&wV<`FF73jYvemdU17iQ)$awkYoD>FLiXdzbidE|^TCM(DiYqQ@l!$dZBNO{T1}tEMg8bPQ7~%N`X1gj z5$+~c(@M{n@-tEjMQ{?A078$?7Mqyi(9Q#MgAKfhn|Mu(`lt`z0^Q7MR8 zK$Jus{lh5v8K7?NFlwGZ&CM`a)bVm30rwUINn6xXMG7WwV@VOiRv`i=i|x@%NLqGc`}vuc6WV1mOE>$uj+e!y=B^} zw&6r?&-r}`@Y=O>^b;c(PTe|YXLXgxh-)hW|>*nm*a?i@Q0^Kbxb2_rqToE1T-}0>hT^n3+r1U<< z^CSJs_6L*_Ua2u8s;OO7mjRL?ILz;|nP8O$a3gqvKw4gN45S(R%q+lL^ffIkeN{Rv z#P#)LQ3M%o_s9{Alg=m#=nwM%BxbD%>gyG2F>oOq4S2EXXsadgGALkuP~D%_z~qe>Za_jDbjyMGiUvK0R`xE~@>*TRI9jB&FTL}DA1JpNKpaK5O$m~i>E0hBVdb_B@e@*#t- zy$kg8`W%EC&VibvF56^+JtOt%tm2v%ord%NYCO49exJ z2)~bVhy>ir=r`~lLi3H69W?u`pf$>=V^LkMRs-)f|0rV|UwP46^Ow*52Xiw?-36c` ze)JfA^yq#f337S*y^&VDL@+TtSez0eDmR*L94%KS5$b0Z`idWzMwb|VR3T!o#SzqA?Or^XS3<2FMg0Uk0=4po#7auzKCj z&d+DT0o?gW#H&DI6sArliZkQ(LqeFb3aTjXGub5Cj{!ABRYcVIGPnW&&WR>70Rt2J z9MflZPEcBrIN0u|O+}60=cWrjcE5!$38qkDryKp9w!Y>6qlj~lXL|qR z_>@j{lH`z;OOgtaOC!0)&pM4(skTGqZnen>O%Yvm5Q>^xb3F|!TTM-EtfYu#%dNSL zg?<%F(@{E;Tm0VZ{PFw#vB$Q@_Wk4Y{eIt{_xt&Jz288=9J<4BTAO+m5_*u4k(;3= zm&vrEsx}mouK=syAE}`>YjL=(x#`9j z5^i8y#EGuGjZwm3&aRw>8k-kNc>p7;EipV`w&}MUa=xiA#BJE)!Q@Y_>xHRwol zZqGKZ>?9s$b?=Q|I9kJQ97)iBy2fzbM`u_MFP2%HhXJYYZeFWUnIz>A`nd4a^;0(U zPRjJ+Xmk@2`s)={x9g#m!36-wq=&bGz2x8I;#nZHXF#e{KxpBFODDTNK-G21Im^N^ z){oaoJbfq4x&7ps$(L4Z&<`LmnQ8Y@?(J^mlI`N_{;)e_>f0-rT8dB(yg^`iCy}Fv znS;ca)X-iz)r6tdgldJZoNf8qXcB52=grK(*vo&&kW-KfN@O?|&NmNO^P_M)l_0+; zXYl(ssu@1%sR}_{{2&<^=>9gnyDSKAmPY{i&3|v7DJG)ArFLo?^F?X&m%H<`qIPUS zdrS89Q;p$NVw7Al)7GZGdgn?36ziRLeDQVuRlV|S@qU_3>7 zir9j^hiAQvF6+G0$2Cu*8ZW1R%ROucwUutPgRqb^DU8FHgKcxX6ZEP6gor z0CavA=Z*<2Joel<$Ly9axgxqYykO+jE9fL56rQu9o6IEkA2X|7Tz{ev*P^7`@;?C9 zB%V3b%Q5HPxWXU_UwH5jw#cG^LM?GK$rp`z5|0;T@Y{B{D_vE4_{GJ=?zavofgOk< zPMfGUXU!6&TQG2$Po4sZ-gNZ|vb2X%=VQ)f9$|Gm zwI9-IvQo1<|0;<_yub?M>L4F)M4RP8^_K)N-PVYC*v#XDe&A4(D+n^Zl-NMuJYx)# z$!8zkme1pILFlCYSiizB?DBrX7^oq?bI6YQqU9x9G=WH9#V~8(HLPK|n6%)PqRt!y zvc|!CYYC#}u+@FG7R!^adsaxS&*hM%xt@gJuurdO+p`Ru9TIjz;adt?r1ErytuFm< zoX*Xlq#9Do80YqoLse zBROc#cxwF{;5CZ)?~cXk^nE*d=B!@%Ff9z(j3x^fHCePce%M-X!W^agcMT7e=fs(9 zZ=NV<1&S$Gps?s$r$%N6BJ%rH*oy}Z7C&2e0Qaab{Jh?^k-h+eUbbx*)w z`&033=ML;42CZzF>2SDRSs-IIdETMGKiG08;j4C&MIz(RwvfVxoBgCnZP^=W8$^|v zFp2N7crc|Z!bLT8KaO=`T`W2)=pKmAym;;L|}Qs#R&A&}S2*=g_Xee*geNDdo9t*JUog{Lvn z(uq}9AE2RADPUcRkIWoxnoch$sgKXVO44&u+)| z&5dw-iKB|_GxY_xO5Cs9yJyjiEEZt$Vnc^*siUe?6xKpSU+L|p(G@wsp@aw-<1=+X zE-&uaQ+(=X&BgwE=_t|h@%+b+9g%ABjWOL@KQTx8GutFY6rB{BSRE7XmQfFq=$ee24&S`wAp6qx zWQ~KKXzzFScpProYC)8q0Kz0&&OKr=^gW>dd>xOMimp5rf;Ye`xZ%^2k4`(xW=~3aoE;e@;t0R$(YP7LjDBG&CZ7 zw71?6`n+3)Siq%x6-EE{OtwKC{6hGjqZR{kKTUf-KSZqa)LNXN9J_&F@2))k #include -#include -#include +#include #include #include @@ -78,7 +77,7 @@ std::optional calculate_pose_ahead_of_collision( const auto interpolated_pose = motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l); const auto interpolated_footprint = project_to_pose(footprint, interpolated_pose); - if (boost::geometry::intersects(interpolated_footprint, point_to_avoid.outside_ring)) { + if (!boost::geometry::disjoint(interpolated_footprint, point_to_avoid.out_overlaps)) { return interpolated_pose; } } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp index 6febd15ef8052..7d845eb788ef8 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp @@ -21,11 +21,12 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include +#include #include #include @@ -36,6 +37,7 @@ #include #include +#include namespace autoware::motion_velocity_planner::out_of_lane::debug { @@ -151,53 +153,59 @@ size_t add_stop_line_markers( } return max_id; } -} // namespace -visualization_msgs::msg::MarkerArray create_debug_marker_array( - const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, - const autoware_perception_msgs::msg::PredictedObjects & objects, DebugData & debug_data) +void add_out_lanelets( + visualization_msgs::msg::MarkerArray & marker_array, visualization_msgs::msg::Marker base_marker, + const lanelet::ConstLanelets & out_lanelets) { - const auto z = ego_data.pose.position.z; - visualization_msgs::msg::MarkerArray debug_marker_array; - - auto base_marker = get_base_marker(); - base_marker.pose.position.z = z + 0.5; - base_marker.ns = "footprints"; - base_marker.color = universe_utils::createMarkerColor(1.0, 1.0, 1.0, 1.0); - // TODO(Maxime): move the debug marker publishing AFTER the trajectory generation - // disabled to prevent performance issues when publishing the debug markers - // add_polygons_markers(debug_marker_array, base_marker, ego_data.trajectory_footprints); - lanelet::BasicPolygons2d drivable_lane_polygons; - for (const auto & poly : ego_data.drivable_lane_polygons) { - drivable_lane_polygons.push_back(poly.outer); + for (const auto & ll : out_lanelets) { + drivable_lane_polygons.push_back(ll.polygon2d().basicPolygon()); } - base_marker.ns = "ego_lane"; + base_marker.ns = "out_lanelets"; base_marker.color = universe_utils::createMarkerColor(0.0, 0.0, 1.0, 1.0); - add_polygons_markers(debug_marker_array, base_marker, drivable_lane_polygons); + add_polygons_markers(marker_array, base_marker, drivable_lane_polygons); +} - lanelet::BasicPolygons2d out_of_lane_areas; - for (const auto & p : out_of_lane_data.outside_points) { - out_of_lane_areas.push_back(p.outside_ring); +void add_out_of_lane_overlaps( + visualization_msgs::msg::MarkerArray & marker_array, visualization_msgs::msg::Marker base_marker, + const std::vector & outside_points, + const std::vector & trajectory) +{ + lanelet::BasicPolygons2d out_of_lane_overlaps; + lanelet::BasicPolygon2d out_of_lane_overlap; + for (const auto & p : outside_points) { + for (const auto & overlap : p.out_overlaps) { + boost::geometry::convert(overlap, out_of_lane_overlap); + out_of_lane_overlaps.push_back(out_of_lane_overlap); + } } base_marker.ns = "out_of_lane_areas"; base_marker.color = universe_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0); - add_polygons_markers(debug_marker_array, base_marker, out_of_lane_areas); - for (const auto & [bbox, i] : out_of_lane_data.outside_areas_rtree) { - const auto & a = out_of_lane_data.outside_points[i]; - debug_marker_array.markers.back().points.push_back( - ego_data.trajectory_points[a.trajectory_index].pose.position); - const auto centroid = boost::geometry::return_centroid(a.outside_ring); - debug_marker_array.markers.back().points.push_back( - geometry_msgs::msg::Point().set__x(centroid.x()).set__y(centroid.y())); + add_polygons_markers(marker_array, base_marker, out_of_lane_overlaps); + for (const auto & p : outside_points) { + for (const auto & a : p.out_overlaps) { + marker_array.markers.back().points.push_back(trajectory[p.trajectory_index].pose.position); + const auto centroid = boost::geometry::return_centroid(a); + marker_array.markers.back().points.push_back( + geometry_msgs::msg::Point().set__x(centroid.x()).set__y(centroid.y())); + } } - +} +void add_predicted_paths( + visualization_msgs::msg::MarkerArray & marker_array, visualization_msgs::msg::Marker base_marker, + const autoware_perception_msgs::msg::PredictedObjects & objects, + const geometry_msgs::msg::Pose & ego_pose) +{ + base_marker.ns = "objects"; + base_marker.color = universe_utils::createMarkerColor(0.0, 1.0, 0.0, 1.0); lanelet::BasicPolygons2d object_polygons; + constexpr double max_draw_distance = 50.0; for (const auto & o : objects.objects) { for (const auto & path : o.kinematics.predicted_paths) { for (const auto & pose : path.path) { // limit the draw distance to improve performance - if (universe_utils::calcDistance2d(pose, ego_data.pose) < 50.0) { + if (universe_utils::calcDistance2d(pose, ego_pose) < max_draw_distance) { const auto poly = universe_utils::toPolygon2d(pose, o.shape).outer(); lanelet::BasicPolygon2d ll_poly(poly.begin(), poly.end()); object_polygons.push_back(ll_poly); @@ -205,9 +213,28 @@ visualization_msgs::msg::MarkerArray create_debug_marker_array( } } } - base_marker.ns = "objects"; - base_marker.color = universe_utils::createMarkerColor(0.0, 1.0, 0.0, 1.0); - add_polygons_markers(debug_marker_array, base_marker, object_polygons); + add_polygons_markers(marker_array, base_marker, object_polygons); +} +} // namespace + +visualization_msgs::msg::MarkerArray create_debug_marker_array( + const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, + const autoware_perception_msgs::msg::PredictedObjects & objects, DebugData & debug_data) +{ + const auto z = ego_data.pose.position.z; + visualization_msgs::msg::MarkerArray debug_marker_array; + + auto base_marker = get_base_marker(); + base_marker.pose.position.z = z + 0.5; + base_marker.ns = "footprints"; + base_marker.color = universe_utils::createMarkerColor(1.0, 1.0, 1.0, 1.0); + // TODO(Maxime): move the debug marker publishing AFTER the trajectory generation + // disabled to prevent performance issues when publishing the debug markers + // add_polygons_markers(debug_marker_array, base_marker, ego_data.trajectory_footprints); + add_out_lanelets(debug_marker_array, base_marker, ego_data.out_lanelets); + add_out_of_lane_overlaps( + debug_marker_array, base_marker, out_of_lane_data.outside_points, ego_data.trajectory_points); + add_predicted_paths(debug_marker_array, base_marker, objects, ego_data.pose); add_current_overlap_marker(debug_marker_array, ego_data.current_footprint, z); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp index 564bf5de7ef2e..6e31f8c8455fd 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp @@ -18,6 +18,8 @@ #include +#include + #include #include @@ -37,6 +39,7 @@ universe_utils::Polygon2d make_base_footprint(const PlannerParam & p, const bool {p.front_offset + front_offset, p.right_offset - right_offset}, {p.rear_offset - rear_offset, p.right_offset - right_offset}, {p.rear_offset - rear_offset, p.left_offset + left_offset}}; + boost::geometry::correct(base_footprint); return base_footprint; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp index f520a564519f0..e4840d724e43d 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -16,16 +16,24 @@ #include "types.hpp" +#include +#include #include +#include #include +#include #include +#include #include +#include #include +#include #include #include +#include namespace autoware::motion_velocity_planner::out_of_lane { @@ -77,15 +85,13 @@ lanelet::ConstLanelets get_missing_lane_change_lanelets( } lanelet::ConstLanelets calculate_trajectory_lanelets( - const EgoData & ego_data, const route_handler::RouteHandler & route_handler) + const universe_utils::LineString2d & trajectory_ls, + const route_handler::RouteHandler & route_handler) { const auto lanelet_map_ptr = route_handler.getLaneletMapPtr(); lanelet::ConstLanelets trajectory_lanelets; - lanelet::BasicLineString2d trajectory_ls; - for (const auto & p : ego_data.trajectory_points) - trajectory_ls.emplace_back(p.pose.position.x, p.pose.position.y); - const auto candidates = - lanelet_map_ptr->laneletLayer.search(lanelet::geometry::boundingBox2d(trajectory_ls)); + const auto candidates = lanelet_map_ptr->laneletLayer.search( + boost::geometry::return_envelope(trajectory_ls)); for (const auto & ll : candidates) { if ( is_road_lanelet(ll) && @@ -115,51 +121,77 @@ lanelet::ConstLanelets calculate_ignored_lanelets( return ignored_lanelets; } -void calculate_drivable_lane_polygons( - EgoData & ego_data, const route_handler::RouteHandler & route_handler) +lanelet::ConstLanelets calculate_out_lanelets( + const lanelet::LaneletLayer & lanelet_layer, + const universe_utils::MultiPolygon2d & trajectory_footprints, + const lanelet::ConstLanelets & trajectory_lanelets, + const lanelet::ConstLanelets & ignored_lanelets) { - const auto route_lanelets = calculate_trajectory_lanelets(ego_data, route_handler); - const auto ignored_lanelets = - out_of_lane::calculate_ignored_lanelets(route_lanelets, route_handler); - for (const auto & ll : route_lanelets) { - out_of_lane::Polygons tmp; - boost::geometry::union_(ego_data.drivable_lane_polygons, ll.polygon2d().basicPolygon(), tmp); - ego_data.drivable_lane_polygons = tmp; - } - for (const auto & ll : ignored_lanelets) { - out_of_lane::Polygons tmp; - boost::geometry::union_(ego_data.drivable_lane_polygons, ll.polygon2d().basicPolygon(), tmp); - ego_data.drivable_lane_polygons = tmp; + lanelet::ConstLanelets out_lanelets; + const auto candidates = lanelet_layer.search( + boost::geometry::return_envelope(trajectory_footprints)); + for (const auto & lanelet : candidates) { + const auto id = lanelet.id(); + if ( + contains_lanelet(trajectory_lanelets, id) || contains_lanelet(ignored_lanelets, id) || + !is_road_lanelet(lanelet)) { + continue; + } + if (!boost::geometry::disjoint(trajectory_footprints, lanelet.polygon2d().basicPolygon())) { + out_lanelets.push_back(lanelet); + } } + return out_lanelets; } -void calculate_overlapped_lanelets( - OutOfLanePoint & out_of_lane_point, const route_handler::RouteHandler & route_handler) +OutLaneletRtree calculate_out_lanelet_rtree(const lanelet::ConstLanelets & lanelets) { - out_of_lane_point.overlapped_lanelets = lanelet::ConstLanelets(); - const auto candidates = route_handler.getLaneletMapPtr()->laneletLayer.search( - lanelet::geometry::boundingBox2d(out_of_lane_point.outside_ring)); - for (const auto & ll : candidates) { - if ( - is_road_lanelet(ll) && !contains_lanelet(out_of_lane_point.overlapped_lanelets, ll.id()) && - boost::geometry::within(out_of_lane_point.outside_ring, ll.polygon2d().basicPolygon())) { - out_of_lane_point.overlapped_lanelets.push_back(ll); - } + std::vector nodes; + nodes.reserve(lanelets.size()); + for (auto i = 0UL; i < lanelets.size(); ++i) { + nodes.emplace_back( + boost::geometry::return_envelope( + lanelets[i].polygon2d().basicPolygon()), + i); } + return {nodes.begin(), nodes.end()}; } -void calculate_overlapped_lanelets( - OutOfLaneData & out_of_lane_data, const route_handler::RouteHandler & route_handler) +void calculate_out_lanelet_rtree( + EgoData & ego_data, const route_handler::RouteHandler & route_handler, + const PlannerParam & params) { - for (auto it = out_of_lane_data.outside_points.begin(); - it != out_of_lane_data.outside_points.end();) { - calculate_overlapped_lanelets(*it, route_handler); - if (it->overlapped_lanelets.empty()) { - // do not keep out of lane points that do not overlap any lanelet - out_of_lane_data.outside_points.erase(it); - } else { - ++it; - } + universe_utils::LineString2d trajectory_ls; + for (const auto & p : ego_data.trajectory_points) { + trajectory_ls.emplace_back(p.pose.position.x, p.pose.position.y); } + // add a point beyond the last trajectory point to account for the ego front offset + const auto pose_beyond = universe_utils::calcOffsetPose( + ego_data.trajectory_points.back().pose, params.front_offset, 0.0, 0.0, 0.0); + trajectory_ls.emplace_back(pose_beyond.position.x, pose_beyond.position.y); + const auto trajectory_lanelets = calculate_trajectory_lanelets(trajectory_ls, route_handler); + const auto ignored_lanelets = calculate_ignored_lanelets(trajectory_lanelets, route_handler); + + const auto max_ego_footprint_offset = std::max({ + params.front_offset + params.extra_front_offset, + params.left_offset + params.extra_left_offset, + params.right_offset + params.extra_right_offset, + params.rear_offset + params.extra_rear_offset, + }); + universe_utils::MultiPolygon2d trajectory_footprints; + const boost::geometry::strategy::buffer::distance_symmetric distance_strategy( + max_ego_footprint_offset); + const boost::geometry::strategy::buffer::join_miter join_strategy; + const boost::geometry::strategy::buffer::end_flat end_strategy; + const boost::geometry::strategy::buffer::point_square circle_strategy; + const boost::geometry::strategy::buffer::side_straight side_strategy; + boost::geometry::buffer( + trajectory_ls, trajectory_footprints, distance_strategy, side_strategy, join_strategy, + end_strategy, circle_strategy); + + ego_data.out_lanelets = calculate_out_lanelets( + route_handler.getLaneletMapPtr()->laneletLayer, trajectory_footprints, trajectory_lanelets, + ignored_lanelets); + ego_data.out_lanelets_rtree = calculate_out_lanelet_rtree(ego_data.out_lanelets); } } // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp index a7729deb539b6..0cb9e223c6456 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp @@ -18,6 +18,7 @@ #include "types.hpp" #include +#include #include @@ -43,7 +44,8 @@ inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lane /// @param [in] route_handler route handler /// @return lanelets crossed by the ego vehicle lanelet::ConstLanelets calculate_trajectory_lanelets( - const EgoData & ego_data, const std::shared_ptr route_handler); + const universe_utils::LineString2d & trajectory_ls, + const std::shared_ptr route_handler); /// @brief calculate lanelets that may not be crossed by the trajectory but may be overlapped during /// a lane change @@ -66,14 +68,18 @@ lanelet::ConstLanelets calculate_ignored_lanelets( /// @brief calculate the polygons representing the ego lane and add it to the ego data /// @param [inout] ego_data ego data /// @param [in] route_handler route handler with map information -void calculate_drivable_lane_polygons( - EgoData & ego_data, const route_handler::RouteHandler & route_handler); +// void calculate_drivable_lane_polygons( +// EgoData & ego_data, const route_handler::RouteHandler & route_handler); /// @brief calculate the lanelets overlapped at each out of lane point /// @param [out] out_of_lane_data data with the out of lane points /// @param [in] route_handler route handler with the lanelet map void calculate_overlapped_lanelets( OutOfLaneData & out_of_lane_data, const route_handler::RouteHandler & route_handler); + +void calculate_out_lanelet_rtree( + EgoData & ego_data, const route_handler::RouteHandler & route_handler, + const PlannerParam & params); } // namespace autoware::motion_velocity_planner::out_of_lane #endif // LANELETS_SELECTION_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp index aef035200b6cc..176b0d2aed2b3 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp @@ -20,15 +20,15 @@ #include #include -#include +#include -#include -#include #include #include +#include #include #include +#include #include #include @@ -47,7 +47,7 @@ void update_collision_times( auto & out_of_lane_point = out_of_lane_data.outside_points[index]; if (out_of_lane_point.collision_times.count(time) == 0UL) { const auto has_collision = - !boost::geometry::disjoint(out_of_lane_point.outside_ring, object_footprint.outer()); + !boost::geometry::disjoint(out_of_lane_point.out_overlaps, object_footprint.outer()); if (has_collision) { out_of_lane_point.collision_times.insert(time); } @@ -129,20 +129,39 @@ void calculate_collisions_to_avoid( } } +OutOfLanePoint calculate_out_of_lane_point( + const lanelet::BasicPolygon2d & footprint, const lanelet::ConstLanelets & out_lanelets, + const OutLaneletRtree & out_lanelets_rtree) +{ + OutOfLanePoint p; + std::vector candidates; + out_lanelets_rtree.query( + boost::geometry::index::intersects(footprint), std::back_inserter(candidates)); + for (const auto & [_, idx] : candidates) { + const auto & lanelet = out_lanelets[idx]; + lanelet::BasicPolygons2d intersections; + boost::geometry::intersection(footprint, lanelet.polygon2d().basicPolygon(), intersections); + for (const auto & intersection : intersections) { + universe_utils::Polygon2d poly; + boost::geometry::convert(intersection, poly); + p.out_overlaps.push_back(poly); + } + if (!intersections.empty()) { + p.overlapped_lanelets.push_back(lanelet); + } + } + return p; +} std::vector calculate_out_of_lane_points(const EgoData & ego_data) { std::vector out_of_lane_points; - OutOfLanePoint p; for (auto i = 0UL; i < ego_data.trajectory_footprints.size(); ++i) { - p.trajectory_index = i + ego_data.first_trajectory_idx; const auto & footprint = ego_data.trajectory_footprints[i]; - Polygons out_of_lane_polygons; - boost::geometry::difference(footprint, ego_data.drivable_lane_polygons, out_of_lane_polygons); - for (const auto & area : out_of_lane_polygons) { - if (!area.outer.empty()) { - p.outside_ring = area.outer; - out_of_lane_points.push_back(p); - } + OutOfLanePoint p = + calculate_out_of_lane_point(footprint, ego_data.out_lanelets, ego_data.out_lanelets_rtree); + p.trajectory_index = ego_data.first_trajectory_idx + i; + if (!p.overlapped_lanelets.empty()) { + out_of_lane_points.push_back(p); } } return out_of_lane_points; @@ -152,11 +171,12 @@ void prepare_out_of_lane_areas_rtree(OutOfLaneData & out_of_lane_data) { std::vector rtree_nodes; for (auto i = 0UL; i < out_of_lane_data.outside_points.size(); ++i) { - OutAreaNode n; - n.first = boost::geometry::return_envelope( - out_of_lane_data.outside_points[i].outside_ring); - n.second = i; - rtree_nodes.push_back(n); + for (const auto & out_overlap : out_of_lane_data.outside_points[i].out_overlaps) { + OutAreaNode n; + n.first = boost::geometry::return_envelope(out_overlap); + n.second = i; + rtree_nodes.push_back(n); + } } out_of_lane_data.outside_areas_rtree = {rtree_nodes.begin(), rtree_nodes.end()}; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp index 33f0842c56d36..f0e0360ef1c15 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index c97e10cc8706e..347a138b651fa 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -221,12 +221,10 @@ void prepare_stop_lines_rtree( ego_data.stop_lines_rtree = {rtree_nodes.begin(), rtree_nodes.end()}; } -out_of_lane::OutOfLaneData prepare_out_of_lane_data( - const out_of_lane::EgoData & ego_data, const route_handler::RouteHandler & route_handler) +out_of_lane::OutOfLaneData prepare_out_of_lane_data(const out_of_lane::EgoData & ego_data) { out_of_lane::OutOfLaneData out_of_lane_data; out_of_lane_data.outside_points = out_of_lane::calculate_out_of_lane_points(ego_data); - out_of_lane::calculate_overlapped_lanelets(out_of_lane_data, route_handler); out_of_lane::prepare_out_of_lane_areas_rtree(out_of_lane_data); return out_of_lane_data; } @@ -255,11 +253,11 @@ VelocityPlanningResult OutOfLaneModule::plan( const auto calculate_trajectory_footprints_us = stopwatch.toc("calculate_trajectory_footprints"); stopwatch.tic("calculate_lanelets"); - out_of_lane::calculate_drivable_lane_polygons(ego_data, *planner_data->route_handler); + out_of_lane::calculate_out_lanelet_rtree(ego_data, *planner_data->route_handler, params_); const auto calculate_lanelets_us = stopwatch.toc("calculate_lanelets"); stopwatch.tic("calculate_out_of_lane_areas"); - auto out_of_lane_data = prepare_out_of_lane_data(ego_data, *planner_data->route_handler); + auto out_of_lane_data = prepare_out_of_lane_data(ego_data); const auto calculate_out_of_lane_areas_us = stopwatch.toc("calculate_out_of_lane_areas"); stopwatch.tic("filter_predicted_objects"); @@ -275,9 +273,12 @@ VelocityPlanningResult OutOfLaneModule::plan( out_of_lane::calculate_collisions_to_avoid(out_of_lane_data, ego_data.trajectory_points, params_); const auto calculate_times_us = stopwatch.toc("calculate_times"); - if ( - params_.skip_if_already_overlapping && !ego_data.drivable_lane_polygons.empty() && - !lanelet::geometry::within(ego_data.current_footprint, ego_data.drivable_lane_polygons)) { + const auto is_already_overlapping = + params_.skip_if_already_overlapping && + std::find_if(ego_data.out_lanelets.begin(), ego_data.out_lanelets.end(), [&](const auto & ll) { + return !boost::geometry::disjoint(ll.polygon2d().basicPolygon(), ego_data.current_footprint); + }) != ego_data.out_lanelets.end(); + if (is_already_overlapping) { RCLCPP_WARN(logger_, "Ego is already out of lane, skipping the module\n"); debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array( ego_data, out_of_lane_data, objects, debug_data_)); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp index c3714c5609135..edf4aff8bc85c 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp @@ -88,6 +88,8 @@ using StopLineNode = std::pair; using StopLinesRtree = bgi::rtree>; using OutAreaNode = std::pair; using OutAreaRtree = bgi::rtree>; +using LaneletNode = std::pair; +using OutLaneletRtree = bgi::rtree>; /// @brief data related to the ego vehicle struct EgoData @@ -100,7 +102,8 @@ struct EgoData double min_slowdown_distance{}; double min_stop_arc_length{}; - Polygons drivable_lane_polygons; + lanelet::ConstLanelets out_lanelets; + OutLaneletRtree out_lanelets_rtree; lanelet::BasicPolygon2d current_footprint; std::vector trajectory_footprints; @@ -108,10 +111,11 @@ struct EgoData StopLinesRtree stop_lines_rtree; }; +/// @brief data related to an out of lane trajectory point struct OutOfLanePoint { size_t trajectory_index; - lanelet::BasicPolygon2d outside_ring; + universe_utils::MultiPolygon2d out_overlaps; std::set collision_times; std::optional min_object_arrival_time; std::optional max_object_arrival_time; @@ -119,6 +123,8 @@ struct OutOfLanePoint lanelet::ConstLanelets overlapped_lanelets; bool to_avoid = false; }; + +/// @brief data related to the out of lane points struct OutOfLaneData { std::vector outside_points; From 1750903107404cdfe70d397bdbe7bd970490592a Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 30 Sep 2024 16:51:40 +0900 Subject: [PATCH 70/95] fix(autoware_universe_utils): fix unmatchedSuppression (#8986) fix:unmatchedSuppression Signed-off-by: kobayu858 --- common/autoware_universe_utils/src/ros/marker_helper.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/common/autoware_universe_utils/src/ros/marker_helper.cpp b/common/autoware_universe_utils/src/ros/marker_helper.cpp index 378dc795a7421..0d360e2335df7 100644 --- a/common/autoware_universe_utils/src/ros/marker_helper.cpp +++ b/common/autoware_universe_utils/src/ros/marker_helper.cpp @@ -16,7 +16,6 @@ namespace autoware::universe_utils { -// cppcheck-suppress unusedFunction visualization_msgs::msg::Marker createDefaultMarker( const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id, const int32_t type, const geometry_msgs::msg::Vector3 & scale, @@ -41,7 +40,6 @@ visualization_msgs::msg::Marker createDefaultMarker( return marker; } -// cppcheck-suppress unusedFunction visualization_msgs::msg::Marker createDeletedDefaultMarker( const rclcpp::Time & now, const std::string & ns, const int32_t id) { @@ -55,7 +53,6 @@ visualization_msgs::msg::Marker createDeletedDefaultMarker( return marker; } -// cppcheck-suppress unusedFunction void appendMarkerArray( const visualization_msgs::msg::MarkerArray & additional_marker_array, visualization_msgs::msg::MarkerArray * marker_array, From 5a0e1003bab6ea617f2754f266fd8af3309a0ff0 Mon Sep 17 00:00:00 2001 From: melike tanrikulu <41450930+meliketanrikulu@users.noreply.github.com> Date: Mon, 30 Sep 2024 17:15:09 +0300 Subject: [PATCH 71/95] docs(autoware_pose_cov_modifier): fix line breaks and dead links (#8991) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * fix(autoware_pose_cov_modifier): fix line breaks Signed-off-by: Melike Tanrıkulu * fix dead links Signed-off-by: Melike Tanrıkulu --------- Signed-off-by: Melike Tanrıkulu --- .../README.md | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/localization/autoware_pose_covariance_modifier/README.md b/localization/autoware_pose_covariance_modifier/README.md index b834bc88116a3..60ecf4bde3508 100644 --- a/localization/autoware_pose_covariance_modifier/README.md +++ b/localization/autoware_pose_covariance_modifier/README.md @@ -73,13 +73,13 @@ Here is a flowchart depicting the process and the predefined thresholds: ```mermaid graph TD - gnss_poser["gnss_poser"] --> |"/sensing/gnss/\npose_with_covariance"| pose_covariance_modifier_node - ndt_scan_matcher["ndt_scan_matcher"] --> |"/localization/pose_estimator/ndt_scan_matcher/\npose_with_covariance"| pose_covariance_modifier_node + gnss_poser["gnss_poser"] --> |"/sensing/gnss/
pose_with_covariance"| pose_covariance_modifier_node + ndt_scan_matcher["ndt_scan_matcher"] --> |"/localization/pose_estimator/ndt_scan_matcher/
pose_with_covariance"| pose_covariance_modifier_node subgraph pose_covariance_modifier_node ["Pose Covariance Modifier Node"] - pc1{{"gnss_pose_yaw\nstddev"}} - pc1 -->|"<= 0.3 rad"| pc2{{"gnss_pose_z\nstddev"}} - pc2 -->|"<= 0.1 m"| pc3{{"gnss_pose_xy\nstddev"}} + pc1{{"gnss_pose_yaw
stddev"}} + pc1 -->|"<= 0.3 rad"| pc2{{"gnss_pose_z
stddev"}} + pc2 -->|"<= 0.1 m"| pc3{{"gnss_pose_xy
stddev"}} pc2 -->|"> 0.1 m"| ndt_pose("NDT Pose") pc3 -->|"<= 0.1 m"| gnss_pose("GNSS Pose") pc3 -->|"0.1 m < x <= 0.2 m"| gnss_ndt_pose("`Both GNSS and NDT Pose @@ -117,8 +117,8 @@ the [pose_twist_estimator.launch.xml](../../launch/tier4_localization_launch/lau ### Without this condition (default) -- The output of the [ndt_scan_matcher](../../localization/ndt_scan_matcher) is directly sent - to [ekf_localizer](../../localization/ekf_localizer). +- The output of the [ndt_scan_matcher](../../localization/autoware_ndt_scan_matcher) is directly sent + to [ekf_localizer](../../localization/autoware_ekf_localizer). - It has a preset covariance value. - **topic name:** `/localization/pose_estimator/pose_with_covariance` - The GNSS pose does not enter the ekf_localizer. @@ -126,11 +126,11 @@ the [pose_twist_estimator.launch.xml](../../launch/tier4_localization_launch/lau ### With this condition -- The output of the [ndt_scan_matcher](../../localization/ndt_scan_matcher) is renamed +- The output of the [ndt_scan_matcher](../../localization/autoware_ndt_scan_matcher) is renamed - **from:** `/localization/pose_estimator/pose_with_covariance`. - **to:** `/localization/pose_estimator/ndt_scan_matcher/pose_with_covariance`. - The `ndt_scan_matcher` output enters the `autoware_pose_covariance_modifier`. -- The output of this package goes to [ekf_localizer](../../localization/ekf_localizer) with: +- The output of this package goes to [ekf_localizer](../../localization/autoware_ekf_localizer) with: - **topic name:** `/localization/pose_estimator/pose_with_covariance`. ## Node From 90ba42b360291e424bcc6e24f613a41b72d03118 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 1 Oct 2024 11:18:27 +0900 Subject: [PATCH 72/95] refactor(goal_planner): refactor PullOverPlannseBase to instantiate only valid path (#8983) Signed-off-by: Mamoru Sobue --- .../CMakeLists.txt | 1 + .../examples/plot_map.cpp | 4 +- .../goal_planner_module.hpp | 13 +- .../pull_over_planner/freespace_pull_over.hpp | 2 +- .../pull_over_planner/geometric_pull_over.hpp | 2 +- .../pull_over_planner_base.hpp | 177 ++++++------------ .../pull_over_planner/shift_pull_over.hpp | 4 +- .../src/goal_planner_module.cpp | 139 +++++++------- .../pull_over_planner/freespace_pull_over.cpp | 13 +- .../pull_over_planner/geometric_pull_over.cpp | 13 +- .../pull_over_planner_base.cpp | 154 +++++++++++++++ .../src/pull_over_planner/shift_pull_over.cpp | 26 +-- .../src/util.cpp | 12 +- 13 files changed, 332 insertions(+), 228 deletions(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt index 626b47b6bdb0d..d3f7f7a4015f0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt @@ -10,6 +10,7 @@ autoware_package() pluginlib_export_plugin_description_file(autoware_behavior_path_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED + src/pull_over_planner/pull_over_planner_base.cpp src/pull_over_planner/freespace_pull_over.cpp src/pull_over_planner/geometric_pull_over.cpp src/pull_over_planner/shift_pull_over.cpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp index a51b2fd337512..7e9ccea9ac0c2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp @@ -265,7 +265,7 @@ int main(int argc, char ** argv) auto shift_pull_over_planner = autoware::behavior_path_planner::ShiftPullOver( *node, goal_planner_parameter, lane_departure_checker); const auto pull_over_path_opt = - shift_pull_over_planner.plan(planner_data, reference_path, route_msg.goal_pose); + shift_pull_over_planner.plan(0, 0, planner_data, reference_path, route_msg.goal_pose); pybind11::scoped_interpreter guard{}; auto plt = matplotlibcpp17::pyplot::import(); @@ -282,7 +282,7 @@ int main(int argc, char ** argv) std::cout << pull_over_path_opt.has_value() << std::endl; if (pull_over_path_opt) { const auto & pull_over_path = pull_over_path_opt.value(); - const auto full_path = pull_over_path.getFullPath(); + const auto & full_path = pull_over_path.full_path; plot_path_with_lane_id(ax, full_path); } ax.set_aspect(Args("equal")); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 6c96ca0eafbb6..4858319a26365 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -160,17 +160,16 @@ class ThreadSafeData return false; } - return pull_over_path_->isValidPath(); + return true; } - PullOverPlannerType getPullOverPlannerType() const + std::optional getPullOverPlannerType() const { const std::lock_guard lock(mutex_); if (!pull_over_path_) { - return PullOverPlannerType::NONE; + return std::nullopt; } - - return pull_over_path_->type; + return pull_over_path_->type(); }; void reset() @@ -206,7 +205,7 @@ class ThreadSafeData void set_pull_over_path_no_lock(const PullOverPath & path) { pull_over_path_ = std::make_shared(path); - if (path.type != PullOverPlannerType::NONE && path.type != PullOverPlannerType::FREESPACE) { + if (path.type() != PullOverPlannerType::FREESPACE) { lane_parking_pull_over_path_ = std::make_shared(path); } @@ -216,7 +215,7 @@ class ThreadSafeData void set_pull_over_path_no_lock(const std::shared_ptr & path) { pull_over_path_ = path; - if (path->type != PullOverPlannerType::NONE && path->type != PullOverPlannerType::FREESPACE) { + if (path->type() != PullOverPlannerType::FREESPACE) { lane_parking_pull_over_path_ = path; } last_path_update_time_ = clock_->now(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp index e3dccb3fc57ec..6ea27a48c0361 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp @@ -42,7 +42,7 @@ class FreespacePullOver : public PullOverPlannerBase PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; } std::optional plan( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override; protected: diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp index 2d7872707dd28..2dfcfb3dc6e9e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp @@ -43,7 +43,7 @@ class GeometricPullOver : public PullOverPlannerBase Pose getCl() const { return planner_.getCl(); } std::optional plan( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override; std::vector generatePullOverPaths( diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp index e9332041a8321..d952f8ddbd22d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp @@ -31,7 +31,6 @@ using tier4_planning_msgs::msg::PathWithLaneId; namespace autoware::behavior_path_planner { enum class PullOverPlannerType { - NONE = 0, SHIFT, ARC_FORWARD, ARC_BACKWARD, @@ -40,137 +39,77 @@ enum class PullOverPlannerType { struct PullOverPath { - PullOverPlannerType type{PullOverPlannerType::NONE}; - std::vector partial_paths{}; - size_t path_idx{0}; - // accelerate with constant acceleration to the target velocity - std::vector> pairs_terminal_velocity_and_accel{}; - Pose start_pose{}; - Pose end_pose{}; - std::vector debug_poses{}; - size_t goal_id{}; - size_t id{}; - bool decided_velocity{false}; - - /** - * @brief Set paths and start/end poses - * By setting partial_paths, full_path, parking_path and curvature are also set at the same time - * @param input_partial_paths partial paths - * @param input_start_pose start pose - * @param input_end_pose end pose - */ - void setPaths( - const std::vector input_partial_paths, const Pose & input_start_pose, - const Pose & input_end_pose) - { - partial_paths = input_partial_paths; - start_pose = input_start_pose; - end_pose = input_end_pose; +public: + static std::optional create( + const PullOverPlannerType & type, const size_t goal_id, const size_t id, + const std::vector & partial_paths, const Pose & start_pose, + const Pose & end_pose, + const std::vector> & pairs_terminal_velocity_and_accel); - updatePathData(); - } + PullOverPath(const PullOverPath & other); - // Note: return copy value (not const&) because the value is used in multi threads - PathWithLaneId getFullPath() const { return full_path; } + PullOverPlannerType type() const { return type_; } + size_t goal_id() const { return goal_id_; } + size_t id() const { return id_; } + Pose start_pose() const { return start_pose_; } + Pose end_pose() const { return end_pose_; } - PathWithLaneId getParkingPath() const { return parking_path; } + std::vector & partial_paths() { return partial_paths_; } + const std::vector & partial_paths() const { return partial_paths_; } - PathWithLaneId getCurrentPath() const - { - if (partial_paths.empty()) { - return PathWithLaneId{}; - } else if (partial_paths.size() <= path_idx) { - return partial_paths.back(); - } - return partial_paths.at(path_idx); - } + // TODO(soblin): use reference to avoid copy once thread-safe design is finished + PathWithLaneId full_path() const { return full_path_; } + PathWithLaneId parking_path() const { return parking_path_; } + std::vector full_path_curvatures() { return full_path_curvatures_; } + std::vector parking_path_curvatures() const { return parking_path_curvatures_; } + double full_path_max_curvature() const { return full_path_max_curvature_; } + double parking_path_max_curvature() const { return parking_path_max_curvature_; } + size_t path_idx() const { return path_idx_; } - bool incrementPathIndex() - { - if (partial_paths.size() - 1 <= path_idx) { - return false; - } - path_idx += 1; - return true; - } + bool incrementPathIndex(); - bool isValidPath() const { return type != PullOverPlannerType::NONE; } + // TODO(soblin): this cannot be const due to decelerateBeforeSearchStart + PathWithLaneId & getCurrentPath(); - std::vector getFullPathCurvatures() const { return full_path_curvatures; } - std::vector getParkingPathCurvatures() const { return parking_path_curvatures; } - double getFullPathMaxCurvature() const { return full_path_max_curvature; } - double getParkingPathMaxCurvature() const { return parking_path_max_curvature; } + const PathWithLaneId & getCurrentPath() const; -private: - void updatePathData() + std::pair getPairsTerminalVelocityAndAccel() const { - updateFullPath(); - updateParkingPath(); - updateCurvatures(); - } - - void updateFullPath() - { - PathWithLaneId path{}; - for (size_t i = 0; i < partial_paths.size(); ++i) { - if (i == 0) { - path.points.insert( - path.points.end(), partial_paths.at(i).points.begin(), partial_paths.at(i).points.end()); - } else { - // skip overlapping point - path.points.insert( - path.points.end(), next(partial_paths.at(i).points.begin()), - partial_paths.at(i).points.end()); - } + if (pairs_terminal_velocity_and_accel_.size() <= path_idx_) { + return std::make_pair(0.0, 0.0); } - full_path.points = autoware::motion_utils::removeOverlapPoints(path.points); + return pairs_terminal_velocity_and_accel_.at(path_idx_); } - void updateParkingPath() - { - if (full_path.points.empty()) { - updateFullPath(); - } - const size_t start_idx = - autoware::motion_utils::findNearestIndex(full_path.points, start_pose.position); - - PathWithLaneId path{}; - std::copy( - full_path.points.begin() + start_idx, full_path.points.end(), - std::back_inserter(path.points)); - parking_path = path; - } + std::vector debug_poses{}; - void updateCurvatures() - { - const auto calculateCurvaturesAndMax = - [](const auto & path) -> std::pair, double> { - std::vector curvatures = autoware::motion_utils::calcCurvature(path.points); - double max_curvature = 0.0; - if (!curvatures.empty()) { - max_curvature = std::abs(*std::max_element( - curvatures.begin(), curvatures.end(), - [](const double & a, const double & b) { return std::abs(a) < std::abs(b); })); - } - return std::make_pair(curvatures, max_curvature); - }; - std::tie(full_path_curvatures, full_path_max_curvature) = - calculateCurvaturesAndMax(getFullPath()); - std::tie(parking_path_curvatures, parking_path_max_curvature) = - calculateCurvaturesAndMax(getParkingPath()); - } +private: + PullOverPath( + const PullOverPlannerType & type, const size_t goal_id, const size_t id, + const Pose & start_pose, const Pose & end_pose, + const std::vector & partial_paths, const PathWithLaneId & full_path, + const PathWithLaneId & parking_path, const std::vector & full_path_curvatures, + const std::vector & parking_path_curvatures, const double full_path_max_curvature, + const double parking_path_max_curvature, + const std::vector> & pairs_terminal_velocity_and_accel); + + PullOverPlannerType type_; + size_t goal_id_; + size_t id_; + Pose start_pose_; + Pose end_pose_; + + std::vector partial_paths_; + PathWithLaneId full_path_; + PathWithLaneId parking_path_; + std::vector full_path_curvatures_; + std::vector parking_path_curvatures_; + double full_path_max_curvature_; + double parking_path_max_curvature_; - // curvatures - std::vector full_path_curvatures{}; - std::vector parking_path_curvatures{}; - std::vector current_path_curvatures{}; - double parking_path_max_curvature{0.0}; - double full_path_max_curvature{0.0}; - double current_path_max_curvature{0.0}; - - // path - PathWithLaneId full_path{}; - PathWithLaneId parking_path{}; + // accelerate with constant acceleration to the target velocity + size_t path_idx_; + std::vector> pairs_terminal_velocity_and_accel_; }; class PullOverPlannerBase @@ -186,7 +125,7 @@ class PullOverPlannerBase virtual PullOverPlannerType getPlannerType() const = 0; virtual std::optional plan( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) = 0; protected: diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp index c5640d9b4949f..9baceb4430dec 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp @@ -36,7 +36,7 @@ class ShiftPullOver : public PullOverPlannerBase const LaneDepartureChecker & lane_departure_checker); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::SHIFT; }; std::optional plan( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override; protected: @@ -46,7 +46,7 @@ class ShiftPullOver : public PullOverPlannerBase std::optional cropPrevModulePath( const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const; std::optional generatePullOverPath( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, const double lateral_jerk) const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 4a59857725a5e..ee68800b81ea1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -20,7 +20,6 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" @@ -298,22 +297,20 @@ void GoalPlannerModule::onTimer() const auto planCandidatePaths = [&]( const std::shared_ptr & planner, const GoalCandidate & goal_candidate) { - auto pull_over_path = - planner->plan(local_planner_data, previous_module_output, goal_candidate.goal_pose); - if (pull_over_path && pull_over_path->getParkingPath().points.size() >= 3) { - pull_over_path->goal_id = goal_candidate.id; - pull_over_path->id = path_candidates.size(); - + const auto pull_over_path = planner->plan( + goal_candidate.id, path_candidates.size(), local_planner_data, previous_module_output, + goal_candidate.goal_pose); + if (pull_over_path) { // calculate absolute maximum curvature of parking path(start pose to end pose) for path // priority path_candidates.push_back(*pull_over_path); // calculate closest pull over start pose for stop path const double start_arc_length = - lanelet::utils::getArcCoordinates(current_lanes, pull_over_path->start_pose).length; + lanelet::utils::getArcCoordinates(current_lanes, pull_over_path->start_pose()).length; if (start_arc_length < min_start_arc_length) { min_start_arc_length = start_arc_length; // closest start pose is stop point when not finding safe path - closest_start_pose = pull_over_path->start_pose; + closest_start_pose = pull_over_path->start_pose(); } } }; @@ -797,10 +794,10 @@ bool GoalPlannerModule::planFreespacePath( if (!goal_candidate.is_safe) { continue; } - auto freespace_path = freespace_planner_->plan( - planner_data, BehaviorModuleOutput{}, // NOTE: not used so passing {} is OK + const auto freespace_path = freespace_planner_->plan( + goal_candidate.id, 0, planner_data, + BehaviorModuleOutput{}, // NOTE: not used so passing {} is OK goal_candidate.goal_pose); - freespace_path->goal_id = goal_candidate.id; if (!freespace_path) { continue; } @@ -832,8 +829,8 @@ bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & conte return false; } - const PathWithLaneId path = lane_parking_path->getFullPath(); - const std::vector curvatures = lane_parking_path->getFullPathCurvatures(); + const PathWithLaneId path = lane_parking_path->full_path(); + const std::vector curvatures = lane_parking_path->full_path_curvatures(); if ( parameters_->use_object_recognition && goal_planner_utils::checkObjectsCollision( @@ -931,7 +928,7 @@ std::optional> GoalPlannerModule::selectP } for (size_t i = 0; i < pull_over_path_candidates.size(); ++i) { const auto & path = pull_over_path_candidates[i]; - const auto goal_candidate_it = goal_candidate_map.find(path.goal_id); + const auto goal_candidate_it = goal_candidate_map.find(path.goal_id()); if (goal_candidate_it != goal_candidate_map.end() && goal_candidate_it->second.is_safe) { sorted_path_indices.push_back(i); } @@ -976,14 +973,14 @@ std::optional> GoalPlannerModule::selectP [&](const size_t a_i, const size_t b_i) { const auto & a = pull_over_path_candidates[a_i]; const auto & b = pull_over_path_candidates[b_i]; - return goal_id_to_index[a.goal_id] < goal_id_to_index[b.goal_id]; + return goal_id_to_index[a.goal_id()] < goal_id_to_index[b.goal_id()]; }); // compare to sort pull_over_path_candidates based on the order in efficient_path_order const auto comparePathTypePriority = [&](const PullOverPath & a, const PullOverPath & b) -> bool { const auto & order = parameters_->efficient_path_order; - const auto a_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(a.type)); - const auto b_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(b.type)); + const auto a_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(a.type())); + const auto b_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(b.type())); return a_pos < b_pos; }; @@ -1005,13 +1002,13 @@ std::optional> GoalPlannerModule::selectP for (const size_t i : sorted_path_indices) { const auto & path = pull_over_path_candidates[i]; const double distance = utils::path_safety_checker::calculateRoughDistanceToObjects( - path.getParkingPath(), target_objects, planner_data_->parameters, false, "max"); + path.parking_path(), target_objects, planner_data_->parameters, false, "max"); auto it = std::lower_bound( margins_with_zero.begin(), margins_with_zero.end(), distance, std::greater()); if (it == margins_with_zero.end()) { - path_id_to_rough_margin_map[path.id] = margins_with_zero.back(); + path_id_to_rough_margin_map[path.id()] = margins_with_zero.back(); } else { - path_id_to_rough_margin_map[path.id] = *it; + path_id_to_rough_margin_map[path.id()] = *it; } } @@ -1022,27 +1019,29 @@ std::optional> GoalPlannerModule::selectP const auto & a = pull_over_path_candidates[a_i]; const auto & b = pull_over_path_candidates[b_i]; if ( - std::abs(path_id_to_rough_margin_map[a.id] - path_id_to_rough_margin_map[b.id]) < 0.01) { + std::abs(path_id_to_rough_margin_map[a.id()] - path_id_to_rough_margin_map[b.id()]) < + 0.01) { return false; } - return path_id_to_rough_margin_map[a.id] > path_id_to_rough_margin_map[b.id]; + return path_id_to_rough_margin_map[a.id()] > path_id_to_rough_margin_map[b.id()]; }); // STEP2-3: Sort by curvature // If the curvature is less than the threshold, prioritize the path. const auto isHighCurvature = [&](const PullOverPath & path) -> bool { - return path.getParkingPathMaxCurvature() >= parameters_->high_curvature_threshold; + return path.parking_path_max_curvature() >= parameters_->high_curvature_threshold; }; const auto isSoftMargin = [&](const PullOverPath & path) -> bool { - const double margin = path_id_to_rough_margin_map[path.id]; + const double margin = path_id_to_rough_margin_map[path.id()]; return std::any_of( soft_margins.begin(), soft_margins.end(), [margin](const double soft_margin) { return std::abs(margin - soft_margin) < 0.01; }); }; const auto isSameHardMargin = [&](const PullOverPath & a, const PullOverPath & b) -> bool { return !isSoftMargin(a) && !isSoftMargin(b) && - std::abs(path_id_to_rough_margin_map[a.id] - path_id_to_rough_margin_map[b.id]) < 0.01; + std::abs(path_id_to_rough_margin_map[a.id()] - path_id_to_rough_margin_map[b.id()]) < + 0.01; }; // NOTE: this is just partition sort based on curvature threshold within each sub partitions @@ -1110,8 +1109,8 @@ std::optional> GoalPlannerModule::selectP parameters_->object_recognition_collision_check_hard_margins.back(); for (const size_t i : sorted_path_indices) { const auto & path = pull_over_path_candidates[i]; - const PathWithLaneId parking_path = path.getParkingPath(); - const auto parking_path_curvatures = path.getParkingPathCurvatures(); + const PathWithLaneId & parking_path = path.parking_path(); + const auto & parking_path_curvatures = path.parking_path_curvatures(); if ( parameters_->use_object_recognition && goal_planner_utils::checkObjectsCollision( @@ -1127,7 +1126,7 @@ std::optional> GoalPlannerModule::selectP checkOccupancyGridCollision(parking_path, occupancy_grid_map_)) { continue; } - return std::make_pair(path, goal_candidates.at(goal_id_to_index.at(path.goal_id))); + return std::make_pair(path, goal_candidates.at(goal_id_to_index.at(path.goal_id()))); } return {}; } @@ -1199,7 +1198,8 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) { + const auto planner_type_opt = thread_safe_data_.getPullOverPlannerType(); + if (planner_type_opt && planner_type_opt.value() == PullOverPlannerType::FREESPACE) { const double drivable_area_margin = planner_data_->parameters.vehicle_width; output.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; @@ -1262,7 +1262,8 @@ void GoalPlannerModule::decideVelocity() const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; - auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths.front(); + // partial_paths + auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths().front(); const auto vel = static_cast(std::max(current_vel, parameters_->pull_over_minimum_velocity)); for (auto & p : first_path.points) { @@ -1329,8 +1330,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( return getPreviousModuleOutput(); } + const auto planner_type_opt = thread_safe_data_.getPullOverPlannerType(); const bool is_freespace = - thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE; + planner_type_opt && planner_type_opt.value() == PullOverPlannerType::FREESPACE; if ( path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::NOT_DECIDED && @@ -1361,7 +1363,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( deceleratePath(pull_over_path); thread_safe_data_.set(goal_candidates, pull_over_path, modified_goal); RCLCPP_DEBUG( - getLogger(), "selected pull over path: path_id: %ld, goal_id: %ld", pull_over_path.id, + getLogger(), "selected pull over path: path_id: %ld, goal_id: %ld", pull_over_path.id(), modified_goal.id); } else { thread_safe_data_.set(goal_candidates); @@ -1390,7 +1392,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( } path_candidate_ = - std::make_shared(thread_safe_data_.get_pull_over_path()->getFullPath()); + std::make_shared(thread_safe_data_.get_pull_over_path()->full_path()); return output; } @@ -1422,12 +1424,12 @@ void GoalPlannerModule::postProcess() } updateSteeringFactor( - {thread_safe_data_.get_pull_over_path()->start_pose, + {thread_safe_data_.get_pull_over_path()->start_pose(), thread_safe_data_.get_modified_goal_pose()->goal_pose}, {distance_to_path_change.first, distance_to_path_change.second}, has_decided_path ? SteeringFactor::TURNING : SteeringFactor::APPROACHING); - setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath()); + setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->full_path()); } BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() @@ -1458,7 +1460,7 @@ std::pair GoalPlannerModule::calcDistanceToPathChange() const return {std::numeric_limits::max(), std::numeric_limits::max()}; } - const auto full_path = thread_safe_data_.get_pull_over_path()->getFullPath(); + const auto full_path = thread_safe_data_.get_pull_over_path()->full_path(); const auto ego_segment_idx = autoware::motion_utils::findNearestSegmentIndex( full_path.points, planner_data_->self_odometry->pose.pose, std::numeric_limits::max(), @@ -1468,10 +1470,10 @@ std::pair GoalPlannerModule::calcDistanceToPathChange() const } const size_t start_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( - full_path.points, thread_safe_data_.get_pull_over_path()->start_pose.position); + full_path.points, thread_safe_data_.get_pull_over_path()->start_pose().position); const double dist_to_parking_start_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, - thread_safe_data_.get_pull_over_path()->start_pose.position, start_pose_segment_idx); + thread_safe_data_.get_pull_over_path()->start_pose().position, start_pose_segment_idx); const size_t goal_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( full_path.points, thread_safe_data_.get_modified_goal_pose()->goal_pose.position); const double dist_to_parking_finish_pose = calcSignedArcLength( @@ -1540,7 +1542,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const std::invoke([&]() -> std::optional> { if (thread_safe_data_.foundPullOverPath()) { return std::make_pair( - thread_safe_data_.get_pull_over_path()->start_pose, "stop at selected start pose"); + thread_safe_data_.get_pull_over_path()->start_pose(), "stop at selected start pose"); } if (thread_safe_data_.get_closest_start_pose()) { return std::make_pair( @@ -1759,12 +1761,12 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto path = thread_safe_data_.get_pull_over_path()->getFullPath(); + const auto path = thread_safe_data_.get_pull_over_path()->full_path(); if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info; const auto & current_pose = planner_data_->self_odometry->pose.pose; - const auto & start_pose = thread_safe_data_.get_pull_over_path()->start_pose; - const auto & end_pose = thread_safe_data_.get_pull_over_path()->end_pose; + const auto & start_pose = thread_safe_data_.get_pull_over_path()->start_pose(); + const auto & end_pose = thread_safe_data_.get_pull_over_path()->end_pose(); const auto shift_start_idx = autoware::motion_utils::findNearestIndex(path.points, start_pose.position); @@ -1808,12 +1810,13 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() constexpr bool is_lane_change = false; constexpr bool is_pull_over = true; const bool override_ego_stopped_check = std::invoke([&]() { - if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::SHIFT) { + const auto planner_type_opt = thread_safe_data_.getPullOverPlannerType(); + if (planner_type_opt && planner_type_opt.value() == PullOverPlannerType::SHIFT) { return false; } constexpr double distance_threshold = 1.0; const auto stop_point = - thread_safe_data_.get_pull_over_path()->partial_paths.front().points.back(); + thread_safe_data_.get_pull_over_path()->partial_paths().front().points.back(); const double distance_from_ego_to_stop_point = std::abs(autoware::motion_utils::calcSignedArcLength( path.points, stop_point.point.pose.position, current_pose.position)); @@ -1850,9 +1853,9 @@ bool GoalPlannerModule::hasEnoughDistance( // so need enough distance to restart. // distance to restart should be less than decide_path_distance. // otherwise, the goal would change immediately after departure. - const bool is_separated_path = pull_over_path.partial_paths.size() > 1; + const bool is_separated_path = pull_over_path.partial_paths().size() > 1; const double distance_to_start = calcSignedArcLength( - long_tail_reference_path.points, current_pose.position, pull_over_path.start_pose.position); + long_tail_reference_path.points, current_pose.position, pull_over_path.start_pose().position); const double distance_to_restart = parameters_->decide_path_distance / 2; const double eps_vel = 0.01; const bool is_stopped = std::abs(current_vel) < eps_vel; @@ -1916,9 +1919,9 @@ void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const const auto closest_goal_candidate = goal_searcher_->getClosetGoalCandidateAlongLanes( thread_safe_data_.get_goal_candidates(), planner_data_); const auto decel_pose = calcLongitudinalOffsetPose( - pull_over_path.getFullPath().points, closest_goal_candidate.goal_pose.position, + pull_over_path.full_path().points, closest_goal_candidate.goal_pose.position, -approximate_pull_over_distance_); - auto & first_path = pull_over_path.partial_paths.front(); + auto & first_path = pull_over_path.partial_paths().front(); if (decel_pose) { decelerateBeforeSearchStart(*decel_pose, first_path); return; @@ -2082,8 +2085,8 @@ bool GoalPlannerModule::isCrossingPossible( bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) const { const lanelet::ConstLanelets lanes = utils::transformToLanelets(generateDrivableLanes()); - const Pose & start_pose = pull_over_path.start_pose; - const Pose & end_pose = pull_over_path.end_pose; + const Pose & start_pose = pull_over_path.start_pose(); + const Pose & end_pose = pull_over_path.end_pose(); return isCrossingPossible(start_pose, end_pose, lanes); } @@ -2154,7 +2157,7 @@ bool GoalPlannerModule::isSafePath( return false; } const auto & pull_over_path = pull_over_path_opt.value(); - const auto current_pull_over_path = pull_over_path.getCurrentPath(); + const auto & current_pull_over_path = pull_over_path.getCurrentPath(); const auto & current_pose = planner_data->self_odometry->pose.pose; const double current_velocity = std::hypot( planner_data->self_odometry->twist.twist.linear.x, @@ -2169,8 +2172,7 @@ bool GoalPlannerModule::isSafePath( parameters.forward_goal_search_length); const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(current_pull_over_path.points); const std::pair terminal_velocity_and_accel = - utils::parking_departure::getPairsTerminalVelocityAndAccel( - pull_over_path.pairs_terminal_velocity_and_accel, pull_over_path.path_idx); + pull_over_path.getPairsTerminalVelocityAndAccel(); RCLCPP_DEBUG( getLogger(), "pairs_terminal_velocity_and_accel for goal_planner: %f, %f", terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); @@ -2347,18 +2349,18 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) // Visualize path and related pose if (thread_safe_data_.foundPullOverPath()) { add(createPoseMarkerArray( - thread_safe_data_.get_pull_over_path()->start_pose, "pull_over_start_pose", 0, 0.3, 0.3, + thread_safe_data_.get_pull_over_path()->start_pose(), "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( - thread_safe_data_.get_pull_over_path()->end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); + thread_safe_data_.get_pull_over_path()->end_pose(), "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); add(createPathMarkerArray( - thread_safe_data_.get_pull_over_path()->getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); + thread_safe_data_.get_pull_over_path()->full_path(), "full_path", 0, 0.0, 0.5, 0.9)); add(createPathMarkerArray( thread_safe_data_.get_pull_over_path()->getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0)); // visualize each partial path - for (size_t i = 0; i < thread_safe_data_.get_pull_over_path()->partial_paths.size(); ++i) { - const auto & partial_path = thread_safe_data_.get_pull_over_path()->partial_paths.at(i); + for (size_t i = 0; i < thread_safe_data_.get_pull_over_path()->partial_paths().size(); ++i) { + const auto & partial_path = thread_safe_data_.get_pull_over_path()->partial_paths().at(i); add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } @@ -2462,11 +2464,12 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) marker.pose = thread_safe_data_.get_modified_goal_pose() ? thread_safe_data_.get_modified_goal_pose()->goal_pose : planner_data_->self_odometry->pose.pose; - marker.text = magic_enum::enum_name(thread_safe_data_.getPullOverPlannerType()); - if (thread_safe_data_.foundPullOverPath()) { + const auto planner_type_opt = thread_safe_data_.getPullOverPlannerType(); + if (planner_type_opt) { + marker.text = magic_enum::enum_name(planner_type_opt.value()); marker.text += - " " + std::to_string(thread_safe_data_.get_pull_over_path()->path_idx) + "/" + - std::to_string(thread_safe_data_.get_pull_over_path()->partial_paths.size() - 1); + " " + std::to_string(thread_safe_data_.get_pull_over_path()->path_idx()) + "/" + + std::to_string(thread_safe_data_.get_pull_over_path()->partial_paths().size() - 1); } if (isStuck( @@ -2673,8 +2676,8 @@ PathDecisionState PathDecisionStateController::get_next_state( } // check current parking path collision - const auto & parking_path = pull_over_path.getParkingPath(); - const std::vector parking_path_curvatures = pull_over_path.getParkingPathCurvatures(); + const auto & parking_path = pull_over_path.parking_path(); + const std::vector parking_path_curvatures = pull_over_path.parking_path_curvatures(); const double margin = parameters.object_recognition_collision_check_hard_margins.back() * hysteresis_factor; if (goal_planner_utils::checkObjectsCollision( @@ -2710,10 +2713,10 @@ PathDecisionState PathDecisionStateController::get_next_state( autoware::motion_utils::findNearestSegmentIndex(current_path.points, current_pose.position); const size_t start_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( - current_path.points, pull_over_path.start_pose.position); + current_path.points, pull_over_path.start_pose().position); const double dist_to_parking_start_pose = calcSignedArcLength( - current_path.points, current_pose.position, ego_segment_idx, pull_over_path.start_pose.position, - start_pose_segment_idx); + current_path.points, current_pose.position, ego_segment_idx, + pull_over_path.start_pose().position, start_pose_segment_idx); if (dist_to_parking_start_pose > parameters.decide_path_distance) { next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; return next_state; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp index 4b377aed46f7c..6127d66802dd8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp @@ -50,7 +50,7 @@ FreespacePullOver::FreespacePullOver( } std::optional FreespacePullOver::plan( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, [[maybe_unused]] const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) { const Pose & current_pose = planner_data->self_odometry->pose.pose; @@ -138,10 +138,13 @@ std::optional FreespacePullOver::plan( } } - PullOverPath pull_over_path{}; - pull_over_path.pairs_terminal_velocity_and_accel = pairs_terminal_velocity_and_accel; - pull_over_path.setPaths(partial_paths, current_pose, goal_pose); - pull_over_path.type = getPlannerType(); + auto pull_over_path_opt = PullOverPath::create( + getPlannerType(), goal_id, id, partial_paths, current_pose, goal_pose, + pairs_terminal_velocity_and_accel); + if (!pull_over_path_opt) { + return {}; + } + auto & pull_over_path = pull_over_path_opt.value(); return pull_over_path; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp index eecdc11937463..438618a2282fe 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp @@ -38,7 +38,7 @@ GeometricPullOver::GeometricPullOver( } std::optional GeometricPullOver::plan( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, [[maybe_unused]] const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) { const auto & route_handler = planner_data->route_handler; @@ -72,10 +72,13 @@ std::optional GeometricPullOver::plan( // check lane departure with road and shoulder lanes if (lane_departure_checker_.checkPathWillLeaveLane(lanes, arc_path)) return {}; - PullOverPath pull_over_path{}; - pull_over_path.type = getPlannerType(); - pull_over_path.pairs_terminal_velocity_and_accel = planner_.getPairsTerminalVelocityAndAccel(); - pull_over_path.setPaths(planner_.getPaths(), planner_.getStartPose(), planner_.getArcEndPose()); + auto pull_over_path_opt = PullOverPath::create( + getPlannerType(), goal_id, id, planner_.getPaths(), planner_.getStartPose(), + planner_.getArcEndPose(), planner_.getPairsTerminalVelocityAndAccel()); + if (!pull_over_path_opt) { + return {}; + } + auto & pull_over_path = pull_over_path_opt.value(); return pull_over_path; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp new file mode 100644 index 0000000000000..f6535e7adb8f8 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp @@ -0,0 +1,154 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +namespace autoware::behavior_path_planner +{ + +std::optional PullOverPath::create( + const PullOverPlannerType & type, const size_t goal_id, const size_t id, + const std::vector & partial_paths, const Pose & start_pose, const Pose & end_pose, + const std::vector> & pairs_terminal_velocity_and_accel) +{ + if (partial_paths.empty()) { + return std::nullopt; + } + PathWithLaneId path{}; + for (size_t i = 0; i < partial_paths.size(); ++i) { + if (i == 0) { + path.points.insert( + path.points.end(), partial_paths.at(i).points.begin(), partial_paths.at(i).points.end()); + } else { + // skip overlapping point + path.points.insert( + path.points.end(), next(partial_paths.at(i).points.begin()), + partial_paths.at(i).points.end()); + } + } + PathWithLaneId full_path{}; + full_path.points = autoware::motion_utils::removeOverlapPoints(path.points); + if (full_path.points.size() < 3) { + return std::nullopt; + } + + const size_t start_idx = + autoware::motion_utils::findNearestIndex(full_path.points, start_pose.position); + + PathWithLaneId parking_path{}; + std::copy( + full_path.points.begin() + start_idx, full_path.points.end(), + std::back_inserter(parking_path.points)); + + if (parking_path.points.size() < 3) { + return std::nullopt; + } + + const auto calculateCurvaturesAndMax = + [](const auto & path) -> std::pair, double> { + std::vector curvatures = autoware::motion_utils::calcCurvature(path.points); + double max_curvature = 0.0; + if (!curvatures.empty()) { + max_curvature = std::abs(*std::max_element( + curvatures.begin(), curvatures.end(), + [](const double & a, const double & b) { return std::abs(a) < std::abs(b); })); + } + return std::make_pair(curvatures, max_curvature); + }; + + std::vector full_path_curvatures{}; + std::vector parking_path_curvatures{}; + double full_path_max_curvature{}; + double parking_path_max_curvature{}; + std::tie(full_path_curvatures, full_path_max_curvature) = calculateCurvaturesAndMax(full_path); + std::tie(parking_path_curvatures, parking_path_max_curvature) = + calculateCurvaturesAndMax(full_path); + + return PullOverPath( + type, goal_id, id, start_pose, end_pose, partial_paths, full_path, parking_path, + full_path_curvatures, parking_path_curvatures, full_path_max_curvature, + parking_path_max_curvature, pairs_terminal_velocity_and_accel); +} + +PullOverPath::PullOverPath(const PullOverPath & other) +: type_(other.type_), + goal_id_(other.goal_id_), + id_(other.id_), + start_pose_(other.start_pose_), + end_pose_(other.end_pose_), + partial_paths_(other.partial_paths_), + full_path_(other.full_path_), + parking_path_(other.parking_path_), + full_path_curvatures_(other.full_path_curvatures_), + parking_path_curvatures_(other.parking_path_curvatures_), + full_path_max_curvature_(other.full_path_max_curvature_), + parking_path_max_curvature_(other.parking_path_max_curvature_), + path_idx_(other.path_idx_), + pairs_terminal_velocity_and_accel_(other.pairs_terminal_velocity_and_accel_) +{ +} + +PullOverPath::PullOverPath( + const PullOverPlannerType & type, const size_t goal_id, const size_t id, const Pose & start_pose, + const Pose & end_pose, const std::vector & partial_paths, + const PathWithLaneId & full_path, const PathWithLaneId & parking_path, + const std::vector & full_path_curvatures, + const std::vector & parking_path_curvatures, const double full_path_max_curvature, + const double parking_path_max_curvature, + const std::vector> & pairs_terminal_velocity_and_accel) +: type_(type), + goal_id_(goal_id), + id_(id), + start_pose_(start_pose), + end_pose_(end_pose), + partial_paths_(partial_paths), + full_path_(full_path), + parking_path_(parking_path), + full_path_curvatures_(full_path_curvatures), + parking_path_curvatures_(parking_path_curvatures), + full_path_max_curvature_(full_path_max_curvature), + parking_path_max_curvature_(parking_path_max_curvature), + path_idx_(0), + pairs_terminal_velocity_and_accel_(pairs_terminal_velocity_and_accel) +{ +} + +bool PullOverPath::incrementPathIndex() +{ + { + if (partial_paths_.size() - 1 <= path_idx_) { + return false; + } + path_idx_ += 1; + return true; + } +} + +PathWithLaneId & PullOverPath::getCurrentPath() +{ + if (partial_paths_.size() <= path_idx_) { + return partial_paths_.back(); + } + return partial_paths_.at(path_idx_); +} + +const PathWithLaneId & PullOverPath::getCurrentPath() const +{ + if (partial_paths_.size() <= path_idx_) { + return partial_paths_.back(); + } + return partial_paths_.at(path_idx_); +} + +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp index 4536fc8873b40..645d74b6385da 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp @@ -35,7 +35,7 @@ ShiftPullOver::ShiftPullOver( { } std::optional ShiftPullOver::plan( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) { const auto & route_handler = planner_data->route_handler; @@ -59,7 +59,8 @@ std::optional ShiftPullOver::plan( // find safe one from paths with different jerk for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { const auto pull_over_path = generatePullOverPath( - planner_data, previous_module_output, road_lanes, pull_over_lanes, goal_pose, lateral_jerk); + goal_id, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, goal_pose, + lateral_jerk); if (!pull_over_path) continue; return *pull_over_path; } @@ -126,7 +127,7 @@ std::optional ShiftPullOver::cropPrevModulePath( } std::optional ShiftPullOver::generatePullOverPath( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, const double lateral_jerk) const @@ -254,13 +255,14 @@ std::optional ShiftPullOver::generatePullOverPath( } // set pull over path - PullOverPath pull_over_path{}; - pull_over_path.type = getPlannerType(); - std::vector partial_paths{shifted_path.path}; - pull_over_path.setPaths( - partial_paths, path_shifter.getShiftLines().front().start, - path_shifter.getShiftLines().front().end); - pull_over_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(pull_over_velocity, 0)); + auto pull_over_path_opt = PullOverPath::create( + getPlannerType(), goal_id, id, {shifted_path.path}, path_shifter.getShiftLines().front().start, + path_shifter.getShiftLines().front().end, {std::make_pair(pull_over_velocity, 0)}); + + if (!pull_over_path_opt) { + return {}; + } + auto & pull_over_path = pull_over_path_opt.value(); pull_over_path.debug_poses.push_back(shift_end_pose_prev_module_path); pull_over_path.debug_poses.push_back(actual_shift_end_pose); pull_over_path.debug_poses.push_back(goal_pose); @@ -275,7 +277,7 @@ std::optional ShiftPullOver::generatePullOverPath( const auto parking_lot_polygons = lanelet::utils::query::getAllParkingLots(planner_data->route_handler->getLaneletMapPtr()); const auto path_footprints = goal_planner_utils::createPathFootPrints( - pull_over_path.getParkingPath(), p.base_link2front, p.base_link2rear, p.vehicle_width); + pull_over_path.parking_path(), p.base_link2front, p.base_link2rear, p.vehicle_width); const auto is_footprint_in_any_polygon = [&parking_lot_polygons](const auto & footprint) { return std::any_of( parking_lot_polygons.begin(), parking_lot_polygons.end(), @@ -299,7 +301,7 @@ std::optional ShiftPullOver::generatePullOverPath( const auto combined_drivable = utils::combineDrivableLanes( expanded_lanes, previous_module_output.drivable_area_info.drivable_lanes); return !lane_departure_checker_.checkPathWillLeaveLane( - utils::transformToLanelets(combined_drivable), pull_over_path.getParkingPath()); + utils::transformToLanelets(combined_drivable), pull_over_path.parking_path()); }); if (!is_in_parking_lots && !is_in_lanes) { return {}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index beed916a31a1b..145c2ecb9c4f7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -671,16 +671,16 @@ std::string makePathPriorityDebugMessage( for (size_t i = 0; i < sorted_path_indices.size(); ++i) { const auto & path = pull_over_path_candidates[sorted_path_indices[i]]; // goal_index is same to goal priority including unsafe goal - const int goal_index = static_cast(goal_id_to_index.at(path.goal_id)); + const int goal_index = static_cast(goal_id_to_index.at(path.goal_id())); const bool is_safe_goal = goal_candidates[goal_index].is_safe; - const int goal_priority = goal_id_and_priority[path.goal_id]; + const int goal_priority = goal_id_and_priority[path.goal_id()]; - ss << "path_priority: " << i << ", path_type: " << magic_enum::enum_name(path.type) - << ", path_id: " << path.id << ", goal_id: " << path.goal_id + ss << "path_priority: " << i << ", path_type: " << magic_enum::enum_name(path.type()) + << ", path_id: " << path.id() << ", goal_id: " << path.goal_id() << ", goal_priority: " << (is_safe_goal ? std::to_string(goal_priority) : "unsafe") - << ", margin: " << path_id_to_rough_margin_map.at(path.id) + << ", margin: " << path_id_to_rough_margin_map.at(path.id()) << (isSoftMargin(path) ? " (soft)" : " (hard)") - << ", curvature: " << path.getParkingPathMaxCurvature() + << ", curvature: " << path.parking_path_max_curvature() << (isHighCurvature(path) ? " (high)" : " (low)") << "\n"; } ss << "-----------------------------------------------------------\n"; From 1d4e9498c69079ef28032df9716001345fc2a768 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Tue, 1 Oct 2024 12:07:02 +0900 Subject: [PATCH 73/95] refactor(autoware_pointcloud_preprocessor): rework ring outlier filter parameters (#8468) * feat: rework ring outlier parameters Signed-off-by: vividf * chore: add explicit cast Signed-off-by: vividf * chore: add boundary Signed-off-by: vividf * chore: remove filter.param Signed-off-by: vividf * chore: set default frame Signed-off-by: vividf * chore: add maximum boundary Signed-off-by: vividf * chore: boundary to float type Signed-off-by: vividf --------- Signed-off-by: vividf --- .../CMakeLists.txt | 2 +- .../ring_outlier_filter_node.param.yaml | 14 +++ .../docs/ring-outlier-filter.md | 15 +-- ...delet.hpp => ring_outlier_filter_node.hpp} | 8 +- .../ring_outlier_filter_node.launch.xml | 16 +++ .../ring_outlier_filter_node.schema.json | 113 ++++++++++++++++++ ...delet.cpp => ring_outlier_filter_node.cpp} | 33 +++-- 7 files changed, 165 insertions(+), 36 deletions(-) create mode 100644 sensing/autoware_pointcloud_preprocessor/config/ring_outlier_filter_node.param.yaml rename sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/{ring_outlier_filter_nodelet.hpp => ring_outlier_filter_node.hpp} (97%) create mode 100644 sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml create mode 100644 sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json rename sensing/autoware_pointcloud_preprocessor/src/outlier_filter/{ring_outlier_filter_nodelet.cpp => ring_outlier_filter_node.cpp} (93%) diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 02ce2a0098220..87f198e3f48ae 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -70,7 +70,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/downsample_filter/random_downsample_filter_node.cpp src/downsample_filter/approximate_downsample_filter_nodelet.cpp src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp - src/outlier_filter/ring_outlier_filter_nodelet.cpp + src/outlier_filter/ring_outlier_filter_node.cpp src/outlier_filter/radius_search_2d_outlier_filter_node.cpp src/outlier_filter/voxel_grid_outlier_filter_node.cpp src/outlier_filter/dual_return_outlier_filter_nodelet.cpp diff --git a/sensing/autoware_pointcloud_preprocessor/config/ring_outlier_filter_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/ring_outlier_filter_node.param.yaml new file mode 100644 index 0000000000000..76bf68958f504 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/ring_outlier_filter_node.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + distance_ratio: 1.03 + object_length_threshold: 0.1 + num_points_threshold: 4 + max_rings_num: 128 + max_points_num_per_ring: 4000 + publish_outlier_pointcloud: false + min_azimuth_deg: 0.0 + max_azimuth_deg: 360.0 + max_distance: 12.0 + vertical_bins: 128 + horizontal_bins: 36 + noise_threshold: 2 diff --git a/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md index 7f2efe90a674f..95ea2ed8ba969 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -56,20 +56,7 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Core Parameters -| Name | Type | Default Value | Description | -| ---------------------------- | ------- | ------------- | ----------------------------------------------------------------------------------------------------------------------------- | -| `distance_ratio` | double | 1.03 | | -| `object_length_threshold` | double | 0.1 | | -| `num_points_threshold` | int | 4 | | -| `max_rings_num` | uint_16 | 128 | | -| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` | -| `publish_outlier_pointcloud` | bool | false | Flag to publish outlier pointcloud and visibility score. Due to performance concerns, please set to false during experiments. | -| `min_azimuth_deg` | float | 0.0 | The left limit of azimuth for visibility score calculation | -| `max_azimuth_deg` | float | 360.0 | The right limit of azimuth for visibility score calculation | -| `max_distance` | float | 12.0 | The limit distance for visibility score calculation | -| `vertical_bins` | int | 128 | The number of vertical bin for visibility histogram | -| `horizontal_bins` | int | 36 | The number of horizontal bin for visibility histogram | -| `noise_threshold` | int | 2 | The threshold value for distinguishing noise from valid points in the frequency image | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json") }} | ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp similarity index 97% rename from sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp index caedeac62b88a..cf396e3e3a4be 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODE_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODE_HPP_ #include "autoware/pointcloud_preprocessor/filter.hpp" #include "autoware/pointcloud_preprocessor/transform_info.hpp" @@ -108,4 +108,4 @@ class RingOutlierFilterComponent : public autoware::pointcloud_preprocessor::Fil }; } // namespace autoware::pointcloud_preprocessor -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODE_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml new file mode 100644 index 0000000000000..68076f5c9c321 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json new file mode 100644 index 0000000000000..1c0cb406584ac --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json @@ -0,0 +1,113 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ring Outlier Filter Node", + "type": "object", + "definitions": { + "ring_outlier_filter": { + "type": "object", + "properties": { + "distance_ratio": { + "type": "number", + "description": "distance_ratio", + "default": "1.03", + "minimum": 0.0 + }, + "object_length_threshold": { + "type": "number", + "description": "object_length_threshold", + "default": "0.1", + "minimum": 0.0 + }, + "num_points_threshold": { + "type": "integer", + "description": "num_points_threshold", + "default": "4", + "minimum": 0 + }, + "max_rings_num": { + "type": "integer", + "description": "max_rings_num", + "default": "128", + "minimum": 1 + }, + "max_points_num_per_ring": { + "type": "integer", + "description": "Set this value large enough such that HFoV / resolution < max_points_num_per_ring", + "default": "4000", + "minimum": 0 + }, + "publish_outlier_pointcloud": { + "type": "boolean", + "description": "Flag to publish outlier pointcloud and visibility score. Due to performance concerns, please set to false during experiments.", + "default": "false" + }, + "min_azimuth_deg": { + "type": "number", + "description": "The left limit of azimuth for visibility score calculation", + "default": "0.0", + "minimum": 0.0 + }, + "max_azimuth_deg": { + "type": "number", + "description": "The right limit of azimuth for visibility score calculation", + "default": "360.0", + "minimum": 0.0, + "maximum": 360.0 + }, + "max_distance": { + "type": "number", + "description": "The limit distance for visibility score calculation", + "default": "12.0", + "minimum": 0.0 + }, + "vertical_bins": { + "type": "integer", + "description": "The number of vertical bin for visibility histogram", + "default": "128", + "minimum": 1 + }, + "horizontal_bins": { + "type": "integer", + "description": "The number of horizontal bin for visibility histogram", + "default": "36", + "minimum": 1 + }, + "noise_threshold": { + "type": "integer", + "description": "The threshold value for distinguishing noise from valid points in the frequency image", + "default": "2", + "minimum": 0 + } + }, + "required": [ + "distance_ratio", + "object_length_threshold", + "num_points_threshold", + "max_rings_num", + "max_points_num_per_ring", + "publish_outlier_pointcloud", + "min_azimuth_deg", + "max_azimuth_deg", + "max_distance", + "vertical_bins", + "horizontal_bins", + "noise_threshold" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/ring_outlier_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp similarity index 93% rename from sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp index e71158ccf32d6..bf140e662440b 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp" #include "autoware_point_types/types.hpp" @@ -47,22 +47,21 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions // set initial parameters { - distance_ratio_ = static_cast(declare_parameter("distance_ratio", 1.03)); - object_length_threshold_ = - static_cast(declare_parameter("object_length_threshold", 0.1)); - num_points_threshold_ = static_cast(declare_parameter("num_points_threshold", 4)); - max_rings_num_ = static_cast(declare_parameter("max_rings_num", 128)); + distance_ratio_ = declare_parameter("distance_ratio"); + object_length_threshold_ = declare_parameter("object_length_threshold"); + num_points_threshold_ = declare_parameter("num_points_threshold"); + max_rings_num_ = static_cast(declare_parameter("max_rings_num")); max_points_num_per_ring_ = - static_cast(declare_parameter("max_points_num_per_ring", 4000)); - publish_outlier_pointcloud_ = - static_cast(declare_parameter("publish_outlier_pointcloud", false)); - - min_azimuth_deg_ = static_cast(declare_parameter("min_azimuth_deg", 0.0)); - max_azimuth_deg_ = static_cast(declare_parameter("max_azimuth_deg", 360.0)); - max_distance_ = static_cast(declare_parameter("max_distance", 12.0)); - vertical_bins_ = static_cast(declare_parameter("vertical_bins", 128)); - horizontal_bins_ = static_cast(declare_parameter("horizontal_bins", 36)); - noise_threshold_ = static_cast(declare_parameter("noise_threshold", 2)); + static_cast(declare_parameter("max_points_num_per_ring")); + + publish_outlier_pointcloud_ = declare_parameter("publish_outlier_pointcloud"); + + min_azimuth_deg_ = declare_parameter("min_azimuth_deg"); + max_azimuth_deg_ = declare_parameter("max_azimuth_deg"); + max_distance_ = declare_parameter("max_distance"); + vertical_bins_ = declare_parameter("vertical_bins"); + horizontal_bins_ = declare_parameter("horizontal_bins"); + noise_threshold_ = declare_parameter("noise_threshold"); } using std::placeholders::_1; From 35c4f594a923dd7ad1b02392f090515182ee126e Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 1 Oct 2024 12:45:50 +0900 Subject: [PATCH 74/95] refactor(goal_planner): remove unused header and divide ThreadSafeData to another file (#8990) Signed-off-by: Mamoru Sobue --- .../decision_state.hpp | 87 +++++++ .../goal_planner_module.hpp | 238 +----------------- .../goal_searcher.hpp | 1 - .../manager.hpp | 9 +- .../pull_over_planner/freespace_pull_over.hpp | 5 - .../thread_data.hpp | 202 +++++++++++++++ .../util.hpp | 2 - .../src/default_fixed_goal_planner.cpp | 2 - .../src/goal_planner_module.cpp | 5 + .../src/goal_searcher.cpp | 3 - .../src/manager.cpp | 10 +- .../pull_over_planner/freespace_pull_over.cpp | 11 +- .../pull_over_planner/geometric_pull_over.cpp | 4 +- .../src/util.cpp | 2 - 14 files changed, 316 insertions(+), 265 deletions(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp new file mode 100644 index 0000000000000..67aa41a5af7e5 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp @@ -0,0 +1,87 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_ + +#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" +#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" + +#include +#include + +namespace autoware::behavior_path_planner +{ + +class PathDecisionState +{ +public: + enum class DecisionKind { + NOT_DECIDED, + DECIDING, + DECIDED, + }; + + DecisionKind state{DecisionKind::NOT_DECIDED}; + rclcpp::Time stamp{}; + bool is_stable_safe{false}; + std::optional safe_start_time{std::nullopt}; +}; + +class PathDecisionStateController +{ +public: + PathDecisionStateController() = default; + + /** + * @brief update current state and save old current state to prev state + */ + void transit_state( + const bool found_pull_over_path, const rclcpp::Time & now, + const autoware_perception_msgs::msg::PredictedObjects & static_target_objects, + const autoware_perception_msgs::msg::PredictedObjects & dynamic_target_objects, + const std::optional modified_goal_opt, + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const bool is_current_safe, const GoalPlannerParameters & parameters, + const std::shared_ptr goal_searcher, const bool is_activated, + const std::optional & pull_over_path, + std::vector & ego_polygons_expanded); + + PathDecisionState get_current_state() const { return current_state_; } + + PathDecisionState get_prev_state() const { return prev_state_; } + +private: + PathDecisionState current_state_{}; + PathDecisionState prev_state_{}; + + /** + * @brief update current state and save old current state to prev state + */ + PathDecisionState get_next_state( + const bool found_pull_over_path, const rclcpp::Time & now, + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const std::optional modified_goal_opt, + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const bool is_current_safe, const GoalPlannerParameters & parameters, + const std::shared_ptr goal_searcher, const bool is_activated, + const std::optional & pull_over_path_opt, + std::vector & ego_polygons_expanded) const; +}; + +} // namespace autoware::behavior_path_planner +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 4858319a26365..d029b2f5953a7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -15,24 +15,15 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ -#include "autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" +#include "autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp" #include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp" -#include "autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp" -#include "autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp" -#include "autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp" +#include "autoware/behavior_path_goal_planner_module/thread_data.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include "autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" -#include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "autoware/behavior_path_planner_common/utils/utils.hpp" -#include -#include #include -#include -#include #include #include @@ -41,7 +32,6 @@ #include #include -#include #include #include #include @@ -61,8 +51,6 @@ using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm; using autoware::freespace_planning_algorithms::AstarParam; using autoware::freespace_planning_algorithms::AstarSearch; using autoware::freespace_planning_algorithms::PlannerCommonParam; -using autoware::freespace_planning_algorithms::RRTStar; -using autoware::freespace_planning_algorithms::RRTStarParam; using autoware::behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; @@ -71,186 +59,6 @@ using autoware::behavior_path_planner::utils::path_safety_checker::SafetyCheckPa using autoware::behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using autoware::universe_utils::Polygon2d; -#define DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ -public: \ - void set_##NAME(const TYPE & value) \ - { \ - const std::lock_guard lock(mutex_); \ - NAME##_ = value; \ - } - -#define DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) \ -public: \ - TYPE get_##NAME() const \ - { \ - const std::lock_guard lock(mutex_); \ - return NAME##_; \ - } - -#define DEFINE_SETTER_GETTER_WITH_MUTEX(TYPE, NAME) \ - DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ - DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) - -class PathDecisionState -{ -public: - enum class DecisionKind { - NOT_DECIDED, - DECIDING, - DECIDED, - }; - - DecisionKind state{DecisionKind::NOT_DECIDED}; - rclcpp::Time stamp{}; - bool is_stable_safe{false}; - std::optional safe_start_time{std::nullopt}; -}; - -class ThreadSafeData -{ -public: - ThreadSafeData(std::recursive_mutex & mutex, rclcpp::Clock::SharedPtr clock) - : mutex_(mutex), clock_(clock) - { - } - - bool incrementPathIndex() - { - const std::lock_guard lock(mutex_); - if (!pull_over_path_) { - return false; - } - - if (pull_over_path_->incrementPathIndex()) { - last_path_idx_increment_time_ = clock_->now(); - return true; - } - return false; - } - - void set_pull_over_path(const PullOverPath & path) - { - const std::lock_guard lock(mutex_); - set_pull_over_path_no_lock(path); - } - - void set_pull_over_path(const std::shared_ptr & path) - { - const std::lock_guard lock(mutex_); - set_pull_over_path_no_lock(path); - } - - template - void set(Args... args) - { - std::lock_guard lock(mutex_); - (..., set_no_lock(args)); - } - - void clearPullOverPath() - { - const std::lock_guard lock(mutex_); - pull_over_path_ = nullptr; - } - - bool foundPullOverPath() const - { - const std::lock_guard lock(mutex_); - if (!pull_over_path_) { - return false; - } - - return true; - } - - std::optional getPullOverPlannerType() const - { - const std::lock_guard lock(mutex_); - if (!pull_over_path_) { - return std::nullopt; - } - return pull_over_path_->type(); - }; - - void reset() - { - const std::lock_guard lock(mutex_); - pull_over_path_ = nullptr; - pull_over_path_candidates_.clear(); - goal_candidates_.clear(); - modified_goal_pose_ = std::nullopt; - last_path_update_time_ = std::nullopt; - last_path_idx_increment_time_ = std::nullopt; - closest_start_pose_ = std::nullopt; - last_previous_module_output_ = std::nullopt; - prev_data_ = PathDecisionState{}; - } - - DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, pull_over_path) - DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, lane_parking_pull_over_path) - DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_update_time) - DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_idx_increment_time) - - DEFINE_SETTER_GETTER_WITH_MUTEX(std::vector, pull_over_path_candidates) - DEFINE_SETTER_GETTER_WITH_MUTEX(GoalCandidates, goal_candidates) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, modified_goal_pose) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, closest_start_pose) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, last_previous_module_output) - DEFINE_SETTER_GETTER_WITH_MUTEX(CollisionCheckDebugMap, collision_check) - DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, static_target_objects) - DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, dynamic_target_objects) - DEFINE_SETTER_GETTER_WITH_MUTEX(PathDecisionState, prev_data) - -private: - void set_pull_over_path_no_lock(const PullOverPath & path) - { - pull_over_path_ = std::make_shared(path); - if (path.type() != PullOverPlannerType::FREESPACE) { - lane_parking_pull_over_path_ = std::make_shared(path); - } - - last_path_update_time_ = clock_->now(); - } - - void set_pull_over_path_no_lock(const std::shared_ptr & path) - { - pull_over_path_ = path; - if (path->type() != PullOverPlannerType::FREESPACE) { - lane_parking_pull_over_path_ = path; - } - last_path_update_time_ = clock_->now(); - } - - void set_no_lock(const GoalCandidates & arg) { goal_candidates_ = arg; } - void set_no_lock(const std::vector & arg) { pull_over_path_candidates_ = arg; } - void set_no_lock(const std::shared_ptr & arg) { set_pull_over_path_no_lock(arg); } - void set_no_lock(const PullOverPath & arg) { set_pull_over_path_no_lock(arg); } - void set_no_lock(const GoalCandidate & arg) { modified_goal_pose_ = arg; } - void set_no_lock(const BehaviorModuleOutput & arg) { last_previous_module_output_ = arg; } - void set_no_lock(const CollisionCheckDebugMap & arg) { collision_check_ = arg; } - - std::shared_ptr pull_over_path_{nullptr}; - std::shared_ptr lane_parking_pull_over_path_{nullptr}; - std::vector pull_over_path_candidates_; - GoalCandidates goal_candidates_{}; - std::optional modified_goal_pose_; - std::optional last_path_update_time_; - std::optional last_path_idx_increment_time_; - std::optional closest_start_pose_{}; - std::optional last_previous_module_output_{}; - CollisionCheckDebugMap collision_check_{}; - PredictedObjects static_target_objects_{}; - PredictedObjects dynamic_target_objects_{}; - PathDecisionState prev_data_{}; - - std::recursive_mutex & mutex_; - rclcpp::Clock::SharedPtr clock_; -}; - -#undef DEFINE_SETTER_WITH_MUTEX -#undef DEFINE_GETTER_WITH_MUTEX -#undef DEFINE_SETTER_GETTER_WITH_MUTEX - struct FreespacePlannerDebugData { bool is_planning{false}; @@ -313,48 +121,6 @@ struct PullOverContextData const PredictedObjects dynamic_target_objects; }; -class PathDecisionStateController -{ -public: - PathDecisionStateController() = default; - - /** - * @brief update current state and save old current state to prev state - */ - void transit_state( - const bool found_pull_over_path, const rclcpp::Time & now, - const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, - const std::optional modified_goal_opt, - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const bool is_current_safe, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher, const bool is_activated, - const std::optional & pull_over_path, - std::vector & ego_polygons_expanded); - - PathDecisionState get_current_state() const { return current_state_; } - - PathDecisionState get_prev_state() const { return prev_state_; } - -private: - PathDecisionState current_state_{}; - PathDecisionState prev_state_{}; - - /** - * @brief update current state and save old current state to prev state - */ - PathDecisionState get_next_state( - const bool found_pull_over_path, const rclcpp::Time & now, - const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, - const std::optional modified_goal_opt, - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const bool is_current_safe, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher, const bool is_activated, - const std::optional & pull_over_path_opt, - std::vector & ego_polygons_expanded) const; -}; - class GoalPlannerModule : public SceneModuleInterface { public: diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp index af2e756e64ca5..923fb6ae5bd44 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp @@ -16,7 +16,6 @@ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ #include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" -#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp index 8ba6239630497..6e2aaedd98b0e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ -#include "autoware/behavior_path_goal_planner_module/goal_planner_module.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -38,12 +38,7 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface void init(rclcpp::Node * node) override; - std::unique_ptr createNewSceneModuleInstance() override - { - return std::make_unique( - name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); - } + std::unique_ptr createNewSceneModuleInstance() override; void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp index 6ea27a48c0361..34743ae5fbf5f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp @@ -18,19 +18,14 @@ #include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" #include -#include -#include #include #include -#include namespace autoware::behavior_path_planner { using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm; -using autoware::freespace_planning_algorithms::AstarSearch; -using autoware::freespace_planning_algorithms::RRTStar; class FreespacePullOver : public PullOverPlannerBase { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp new file mode 100644 index 0000000000000..3bfaa90b845fb --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp @@ -0,0 +1,202 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__THREAD_DATA_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__THREAD_DATA_HPP_ + +#include "autoware/behavior_path_goal_planner_module/decision_state.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_path_planner +{ + +#define DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ +public: \ + void set_##NAME(const TYPE & value) \ + { \ + const std::lock_guard lock(mutex_); \ + NAME##_ = value; \ + } + +#define DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) \ +public: \ + TYPE get_##NAME() const \ + { \ + const std::lock_guard lock(mutex_); \ + return NAME##_; \ + } + +#define DEFINE_SETTER_GETTER_WITH_MUTEX(TYPE, NAME) \ + DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ + DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) + +class ThreadSafeData +{ +public: + ThreadSafeData(std::recursive_mutex & mutex, rclcpp::Clock::SharedPtr clock) + : mutex_(mutex), clock_(clock) + { + } + + bool incrementPathIndex() + { + const std::lock_guard lock(mutex_); + if (!pull_over_path_) { + return false; + } + + if (pull_over_path_->incrementPathIndex()) { + last_path_idx_increment_time_ = clock_->now(); + return true; + } + return false; + } + + void set_pull_over_path(const PullOverPath & path) + { + const std::lock_guard lock(mutex_); + set_pull_over_path_no_lock(path); + } + + void set_pull_over_path(const std::shared_ptr & path) + { + const std::lock_guard lock(mutex_); + set_pull_over_path_no_lock(path); + } + + template + void set(Args... args) + { + std::lock_guard lock(mutex_); + (..., set_no_lock(args)); + } + + void clearPullOverPath() + { + const std::lock_guard lock(mutex_); + pull_over_path_ = nullptr; + } + + bool foundPullOverPath() const + { + const std::lock_guard lock(mutex_); + if (!pull_over_path_) { + return false; + } + + return true; + } + + std::optional getPullOverPlannerType() const + { + const std::lock_guard lock(mutex_); + if (!pull_over_path_) { + return std::nullopt; + } + return pull_over_path_->type(); + }; + + void reset() + { + const std::lock_guard lock(mutex_); + pull_over_path_ = nullptr; + pull_over_path_candidates_.clear(); + goal_candidates_.clear(); + modified_goal_pose_ = std::nullopt; + last_path_update_time_ = std::nullopt; + last_path_idx_increment_time_ = std::nullopt; + closest_start_pose_ = std::nullopt; + last_previous_module_output_ = std::nullopt; + prev_data_ = PathDecisionState{}; + } + + DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, pull_over_path) + DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, lane_parking_pull_over_path) + DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_update_time) + DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_idx_increment_time) + + DEFINE_SETTER_GETTER_WITH_MUTEX(std::vector, pull_over_path_candidates) + DEFINE_SETTER_GETTER_WITH_MUTEX(GoalCandidates, goal_candidates) + DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, modified_goal_pose) + DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, closest_start_pose) + DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, last_previous_module_output) + DEFINE_SETTER_GETTER_WITH_MUTEX( + utils::path_safety_checker::CollisionCheckDebugMap, collision_check) + DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, static_target_objects) + DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, dynamic_target_objects) + DEFINE_SETTER_GETTER_WITH_MUTEX(PathDecisionState, prev_data) + +private: + void set_pull_over_path_no_lock(const PullOverPath & path) + { + pull_over_path_ = std::make_shared(path); + if (path.type() != PullOverPlannerType::FREESPACE) { + lane_parking_pull_over_path_ = std::make_shared(path); + } + + last_path_update_time_ = clock_->now(); + } + + void set_pull_over_path_no_lock(const std::shared_ptr & path) + { + pull_over_path_ = path; + if (path->type() != PullOverPlannerType::FREESPACE) { + lane_parking_pull_over_path_ = path; + } + last_path_update_time_ = clock_->now(); + } + + void set_no_lock(const GoalCandidates & arg) { goal_candidates_ = arg; } + void set_no_lock(const std::vector & arg) { pull_over_path_candidates_ = arg; } + void set_no_lock(const std::shared_ptr & arg) { set_pull_over_path_no_lock(arg); } + void set_no_lock(const PullOverPath & arg) { set_pull_over_path_no_lock(arg); } + void set_no_lock(const GoalCandidate & arg) { modified_goal_pose_ = arg; } + void set_no_lock(const BehaviorModuleOutput & arg) { last_previous_module_output_ = arg; } + void set_no_lock(const utils::path_safety_checker::CollisionCheckDebugMap & arg) + { + collision_check_ = arg; + } + + std::shared_ptr pull_over_path_{nullptr}; + std::shared_ptr lane_parking_pull_over_path_{nullptr}; + std::vector pull_over_path_candidates_; + GoalCandidates goal_candidates_{}; + std::optional modified_goal_pose_; + std::optional last_path_update_time_; + std::optional last_path_idx_increment_time_; + std::optional closest_start_pose_{}; + std::optional last_previous_module_output_{}; + utils::path_safety_checker::CollisionCheckDebugMap collision_check_{}; + PredictedObjects static_target_objects_{}; + PredictedObjects dynamic_target_objects_{}; + PathDecisionState prev_data_{}; + + std::recursive_mutex & mutex_; + rclcpp::Clock::SharedPtr clock_; +}; + +#undef DEFINE_SETTER_WITH_MUTEX +#undef DEFINE_GETTER_WITH_MUTEX +#undef DEFINE_SETTER_GETTER_WITH_MUTEX + +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__THREAD_DATA_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index f68800b47f5a5..52dfbbddc79ff 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -17,7 +17,6 @@ #include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" #include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include @@ -31,7 +30,6 @@ #include #include -#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp index 2709dbe8635d3..160bb33dc07de 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp @@ -14,8 +14,6 @@ #include "autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" -#include "autoware/behavior_path_goal_planner_module/util.hpp" -#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index ee68800b81ea1..147fe9f79dba0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -14,8 +14,13 @@ #include "autoware/behavior_path_goal_planner_module/goal_planner_module.hpp" +#include "autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp" #include "autoware/behavior_path_goal_planner_module/util.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index 8536001c6a08e..ea0816954d0e1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -15,13 +15,11 @@ #include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp" #include "autoware/behavior_path_goal_planner_module/util.hpp" -#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware_lanelet2_extension/regulatory_elements/no_parking_area.hpp" #include "autoware_lanelet2_extension/regulatory_elements/no_stopping_area.hpp" -#include "autoware_lanelet2_extension/utility/query.hpp" #include "autoware_lanelet2_extension/utility/utilities.hpp" #include @@ -33,7 +31,6 @@ namespace autoware::behavior_path_planner { -using autoware::lane_departure_checker::LaneDepartureChecker; using autoware::universe_utils::calcOffsetPose; using lanelet::autoware::NoParkingArea; using lanelet::autoware::NoStoppingArea; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index 32d6c55276876..34248a75eb1f5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -14,7 +14,8 @@ #include "autoware/behavior_path_goal_planner_module/manager.hpp" -#include "autoware/behavior_path_goal_planner_module/util.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_planner_module.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "autoware/universe_utils/ros/update_param.hpp" #include @@ -26,6 +27,13 @@ namespace autoware::behavior_path_planner { +std::unique_ptr GoalPlannerModuleManager::createNewSceneModuleInstance() +{ + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); +} + GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters( rclcpp::Node * node, const std::string & base_ns) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp index 6127d66802dd8..dbdac08c8778c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp @@ -20,11 +20,18 @@ #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include +#include + #include #include namespace autoware::behavior_path_planner { + +using autoware::freespace_planning_algorithms::AstarSearch; +using autoware::freespace_planning_algorithms::RRTStar; + FreespacePullOver::FreespacePullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) @@ -144,8 +151,6 @@ std::optional FreespacePullOver::plan( if (!pull_over_path_opt) { return {}; } - auto & pull_over_path = pull_over_path_opt.value(); - - return pull_over_path; + return pull_over_path_opt.value(); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp index 438618a2282fe..6c4aee5b96abf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp @@ -78,8 +78,6 @@ std::optional GeometricPullOver::plan( if (!pull_over_path_opt) { return {}; } - auto & pull_over_path = pull_over_path_opt.value(); - - return pull_over_path; + return pull_over_path_opt.value(); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index 145c2ecb9c4f7..3815acc4561e7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -14,7 +14,6 @@ #include "autoware/behavior_path_goal_planner_module/util.hpp" -#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware_lanelet2_extension/regulatory_elements/bus_stop_area.hpp" @@ -34,7 +33,6 @@ #include #include -#include #include #include From bcfaed72a8335cc9040dbc7aa1361dbc5f16f81d Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Tue, 1 Oct 2024 12:46:50 +0900 Subject: [PATCH 75/95] feat(codecov): add codecov component for planning and control (#8992) * add planning-tier-iv-maintained-packages Signed-off-by: Y.Hisaki * add control component Signed-off-by: Y.Hisaki --------- Signed-off-by: Y.Hisaki --- codecov.yaml | 82 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 82 insertions(+) diff --git a/codecov.yaml b/codecov.yaml index c907c24818735..75fc6f8bacadd 100644 --- a/codecov.yaml +++ b/codecov.yaml @@ -31,3 +31,85 @@ ignore: - "**/test/*" - "**/test/**/*" - "**/debug.*" + +component_management: + individual_components: + - component_id: planning-tier-iv-maintained-packages + name: Planning TIER IV Maintained Packages + paths: + - planning/autoware_costmap_generator/** + - planning/autoware_external_velocity_limit_selector/** + - planning/autoware_freespace_planner/** + - planning/autoware_freespace_planning_algorithms/** + - planning/autoware_mission_planner/** + # - planning/autoware_objects_of_interest_marker_interface/** + - planning/autoware_obstacle_cruise_planner/** + # - planning/autoware_obstacle_stop_planner/** + - planning/autoware_path_optimizer/** + - planning/autoware_path_smoother/** + - planning/autoware_planning_test_manager/** + - planning/autoware_planning_topic_converter/** + - planning/autoware_planning_validator/** + - planning/autoware_remaining_distance_time_calculator/** + - planning/autoware_route_handler/** + - planning/autoware_rtc_interface/** + - planning/autoware_scenario_selector/** + - planning/autoware_static_centerline_generator/** + - planning/autoware_surround_obstacle_checker/** + - planning/autoware_velocity_smoother/** + ##### behavior_path_planner ##### + # - planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/** + - planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/** + - planning/behavior_path_planner/autoware_behavior_path_planner_common/** + - planning/behavior_path_planner/autoware_behavior_path_start_planner_module/** + - planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/** + - planning/behavior_path_planner/autoware_behavior_path_lane_change_module/** + # - planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/** + - planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/** + # - planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/** + - planning/behavior_path_planner/autoware_behavior_path_planner/** + - planning/behavior_path_planner/autoware_behavior_path_side_shift_module/** + ##### behavior_velocity_planner ##### + - planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/** + # - planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/** + # - planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/** + # - planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_planner/** + # - planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/** + # - planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/** + # - planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/** + ##### motion_velocity_planner ##### + - planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/** + - planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** + - planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** + - planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** + - planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** + + - component_id: control-tier-iv-maintained-packages + name: Control TIER IV Maintained Packages + paths: + - control/autoware_autonomous_emergency_braking/** + - control/autoware_control_validator/** + - control/autoware_external_cmd_selector/** + # - control/autoware_joy_controller/** + - control/autoware_lane_departure_checker/** + - control/autoware_mpc_lateral_controller/** + - control/autoware_operation_mode_transition_manager/** + - control/autoware_pid_longitudinal_controller/** + # - control/autoware_pure_pursuit/** + - control/autoware_shift_decider/** + # - control/autoware_smart_mpc_trajectory_follower/** + - control/autoware_trajectory_follower_base/** + - control/autoware_trajectory_follower_node/** + - control/autoware_vehicle_cmd_gate/** + # - control/control_performance_analysis/** + - control/obstacle_collision_checker/** + # - control/predicted_path_checker/** From 89d0a516502b980368cadd2739260e766dd2e129 Mon Sep 17 00:00:00 2001 From: Mitsuhiro Sakamoto <50359861+mitukou1109@users.noreply.github.com> Date: Tue, 1 Oct 2024 15:53:32 +0900 Subject: [PATCH 76/95] fix(universe_utils): avoid test timeout (#8993) reduce number of polygons to be generated Signed-off-by: mitukou1109 --- .../test/src/geometry/test_alt_geometry.cpp | 14 +++++++------- .../test/src/geometry/test_geometry.cpp | 4 ++-- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp index b74c747fc8054..90dfb1ede4701 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp @@ -723,7 +723,7 @@ TEST(alt_geometry, within) TEST(alt_geometry, areaRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -763,7 +763,7 @@ TEST(alt_geometry, areaRand) TEST(alt_geometry, convexHullRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -811,7 +811,7 @@ TEST(alt_geometry, convexHullRand) TEST(alt_geometry, coveredByRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -878,7 +878,7 @@ TEST(alt_geometry, coveredByRand) TEST(alt_geometry, disjointRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -944,7 +944,7 @@ TEST(alt_geometry, disjointRand) TEST(alt_geometry, intersectsRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -1010,7 +1010,7 @@ TEST(alt_geometry, intersectsRand) TEST(alt_geometry, touchesRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -1077,7 +1077,7 @@ TEST(alt_geometry, touchesRand) TEST(alt_geometry, withinPolygonRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; diff --git a/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp index ab5e9f4236bad..af90ab1f32c57 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp @@ -1937,7 +1937,7 @@ TEST( TEST(geometry, intersectPolygonRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -2227,7 +2227,7 @@ TEST(geometry, intersectConcavePolygonRand) { std::vector polygons; std::vector> triangulations; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; From a3015399022abbad54965578d55c39d3a968aaf0 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Tue, 1 Oct 2024 18:19:49 +0900 Subject: [PATCH 77/95] feat(autonomous_emergency_braking): add sanity chackes (#8998) add sanity chackes Signed-off-by: Daniel Sanchez --- .../src/node.cpp | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index ec53a677cdd81..34a7453b51726 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -461,7 +461,9 @@ bool AEB::checkCollision(MarkerArray & debug_markers) const auto ego_polys = generatePathFootprint(path, expand_width_); std::vector objects; // Crop out Pointcloud using an extra wide ego path - if (use_pointcloud_data_ && !points_belonging_to_cluster_hulls->empty()) { + if ( + use_pointcloud_data_ && points_belonging_to_cluster_hulls && + !points_belonging_to_cluster_hulls->empty()) { const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp; getClosestObjectsOnPath(path, current_time, points_belonging_to_cluster_hulls, objects); } @@ -712,6 +714,9 @@ void AEB::generatePathFootprint( const Path & path, const double extra_width_margin, std::vector & polygons) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (path.empty()) { + return; + } for (size_t i = 0; i < path.size() - 1; ++i) { polygons.push_back( createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin)); @@ -721,8 +726,11 @@ void AEB::generatePathFootprint( std::vector AEB::generatePathFootprint( const Path & path, const double extra_width_margin) { - std::vector polygons; autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (path.empty()) { + return {}; + } + std::vector polygons; for (size_t i = 0; i < path.size() - 1; ++i) { polygons.push_back( createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin)); @@ -735,7 +743,7 @@ void AEB::createObjectDataUsingPredictedObjects( std::vector & object_data_vector) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (predicted_objects_ptr_->objects.empty()) return; + if (predicted_objects_ptr_->objects.empty() || ego_polys.empty()) return; const double current_ego_speed = current_velocity_ptr_->longitudinal_velocity; const auto & objects = predicted_objects_ptr_->objects; @@ -926,6 +934,9 @@ void AEB::cropPointCloudWithEgoFootprintPath( const std::vector & ego_polys, PointCloud::Ptr filtered_objects) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (ego_polys.empty()) { + return; + } PointCloud::Ptr full_points_ptr(new PointCloud); pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *full_points_ptr); // Create a Point cloud with the points of the ego footprint From a2446ca664ae82000db563e0fbf417b8270c7bcb Mon Sep 17 00:00:00 2001 From: Yuxuan Liu <619684051@qq.com> Date: Tue, 1 Oct 2024 18:28:15 +0900 Subject: [PATCH 78/95] fix(docs): fix file name for bluetooth monitor schema (#8308) * fix file name for schema Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * the variable name should be addresses instead Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --------- Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- ...ooth_monitor.shcema.json => bluetooth_monitor.schema.json} | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) rename system/bluetooth_monitor/schema/{bluetooth_monitor.shcema.json => bluetooth_monitor.schema.json} (93%) diff --git a/system/bluetooth_monitor/schema/bluetooth_monitor.shcema.json b/system/bluetooth_monitor/schema/bluetooth_monitor.schema.json similarity index 93% rename from system/bluetooth_monitor/schema/bluetooth_monitor.shcema.json rename to system/bluetooth_monitor/schema/bluetooth_monitor.schema.json index 6951ecd6aed88..0914c7ec9a9b9 100644 --- a/system/bluetooth_monitor/schema/bluetooth_monitor.shcema.json +++ b/system/bluetooth_monitor/schema/bluetooth_monitor.schema.json @@ -6,7 +6,7 @@ "bluetooth_monitor": { "type": "object", "properties": { - "address": { + "addresses": { "type": "array", "description": "Bluetooth addresses of the device to monitor", "items": { @@ -30,7 +30,7 @@ "default": 0.1 } }, - "required": ["address", "port", "timeout", "rtt_warn"], + "required": ["addresses", "port", "timeout", "rtt_warn"], "additionalProperties": false } }, From 6a246835bbe5992d23b49a92e50421168549bb76 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 1 Oct 2024 18:58:12 +0900 Subject: [PATCH 79/95] fix(autoware_lidar_centerpoint): fix twist covariance orientation (#8996) * fix(autoware_lidar_centerpoint): fix covariance converter considering the twist covariance matrix is based on the object coordinate Signed-off-by: Taekjin LEE fix style * fix: update test of box3DToDetectedObject function Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../autoware/lidar_centerpoint/ros_utils.hpp | 2 +- .../lib/ros_utils.cpp | 21 ++++++++++++++----- .../test/test_ros_utils.cpp | 7 ++++--- 3 files changed, 21 insertions(+), 9 deletions(-) diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/ros_utils.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/ros_utils.hpp index de901b06e83c3..9a17cad91efb8 100644 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/ros_utils.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/ros_utils.hpp @@ -35,7 +35,7 @@ uint8_t getSemanticType(const std::string & class_name); std::array convertPoseCovarianceMatrix(const Box3D & box3d); -std::array convertTwistCovarianceMatrix(const Box3D & box3d); +std::array convertTwistCovarianceMatrix(const Box3D & box3d, const float yaw); bool isCarLikeVehicleLabel(const uint8_t label); diff --git a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp index fc383a8d1da00..feeab969e88bd 100644 --- a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp @@ -50,7 +50,7 @@ void box3DToDetectedObject( // pose and shape // mmdet3d yaw format to ros yaw format - float yaw = -box3d.yaw - autoware::universe_utils::pi / 2; + const float yaw = -box3d.yaw - autoware::universe_utils::pi / 2; obj.kinematics.pose_with_covariance.pose.position = autoware::universe_utils::createPoint(box3d.x, box3d.y, box3d.z); obj.kinematics.pose_with_covariance.pose.orientation = @@ -67,6 +67,8 @@ void box3DToDetectedObject( if (has_twist) { float vel_x = box3d.vel_x; float vel_y = box3d.vel_y; + + // twist of the object is based on the object coordinate system geometry_msgs::msg::Twist twist; twist.linear.x = std::cos(yaw) * vel_x + std::sin(yaw) * vel_y; twist.linear.y = -std::sin(yaw) * vel_x + std::cos(yaw) * vel_y; @@ -76,7 +78,7 @@ void box3DToDetectedObject( obj.kinematics.has_twist = has_twist; if (has_variance) { obj.kinematics.has_twist_covariance = has_variance; - obj.kinematics.twist_with_covariance.covariance = convertTwistCovarianceMatrix(box3d); + obj.kinematics.twist_with_covariance.covariance = convertTwistCovarianceMatrix(box3d, yaw); } } } @@ -113,12 +115,21 @@ std::array convertPoseCovarianceMatrix(const Box3D & box3d) return pose_covariance; } -std::array convertTwistCovarianceMatrix(const Box3D & box3d) +std::array convertTwistCovarianceMatrix(const Box3D & box3d, const float yaw) { using POSE_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + + // twist covariance matrix is based on the object coordinate system std::array twist_covariance{}; - twist_covariance[POSE_IDX::X_X] = box3d.vel_x_variance; - twist_covariance[POSE_IDX::Y_Y] = box3d.vel_y_variance; + const float cos_yaw = std::cos(yaw); + const float sin_yaw = std::sin(yaw); + twist_covariance[POSE_IDX::X_X] = + box3d.vel_x_variance * cos_yaw * cos_yaw + box3d.vel_y_variance * sin_yaw * sin_yaw; + twist_covariance[POSE_IDX::X_Y] = + (box3d.vel_y_variance - box3d.vel_x_variance) * sin_yaw * cos_yaw; + twist_covariance[POSE_IDX::Y_X] = twist_covariance[POSE_IDX::X_Y]; + twist_covariance[POSE_IDX::Y_Y] = + box3d.vel_x_variance * sin_yaw * sin_yaw + box3d.vel_y_variance * cos_yaw * cos_yaw; return twist_covariance; } diff --git a/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp b/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp index 555d820ec3644..5605d2df6a9d9 100644 --- a/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp +++ b/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp @@ -125,13 +125,14 @@ TEST(TestSuite, convertPoseCovarianceMatrix) TEST(TestSuite, convertTwistCovarianceMatrix) { autoware::lidar_centerpoint::Box3D box3d; - box3d.vel_x_variance = 0.1; + box3d.vel_x_variance = 0.5; box3d.vel_y_variance = 0.2; + float yaw = 0; std::array twist_covariance = - autoware::lidar_centerpoint::convertTwistCovarianceMatrix(box3d); + autoware::lidar_centerpoint::convertTwistCovarianceMatrix(box3d, yaw); - EXPECT_FLOAT_EQ(twist_covariance[0], 0.1); + EXPECT_FLOAT_EQ(twist_covariance[0], 0.5); EXPECT_FLOAT_EQ(twist_covariance[7], 0.2); } From 6fccdf5da390df6e67ff9fc1117f81e30934f79f Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Tue, 1 Oct 2024 20:25:44 +0900 Subject: [PATCH 80/95] refactor(autoware_pointcloud_preprocessor): rework dual return outlier filter parameters (#8475) * feat: rework dual return outlier filter parameters Signed-off-by: vividf * chore: fix readme Signed-off-by: vividf * chore: change launch file name Signed-off-by: vividf * chore: fix type Signed-off-by: vividf * chore: add boundary Signed-off-by: vividf * chore: change boundary Signed-off-by: vividf * chore: fix boundary Signed-off-by: vividf * chore: fix json schema Signed-off-by: vividf * Update sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter_node.schema.json Co-authored-by: Kenzo Lobos Tsunekawa * chore: fix grammar error Signed-off-by: vividf * chore: fix description for weak_first_local_noise_threshold Signed-off-by: vividf * chore: change minimum and maximum to float Signed-off-by: vividf --------- Signed-off-by: vividf Co-authored-by: Kenzo Lobos Tsunekawa --- .../CMakeLists.txt | 2 +- ...dual_return_outlier_filter_node.param.yaml | 19 +++ .../docs/dual-return-outlier-filter.md | 20 +-- ...pp => dual_return_outlier_filter_node.hpp} | 8 +- .../dual_return_outlier_filter.launch.xml | 21 --- ...dual_return_outlier_filter_node.launch.xml | 17 ++ ...ual_return_outlier_filter_node.schema.json | 146 ++++++++++++++++++ ...pp => dual_return_outlier_filter_node.cpp} | 45 +++--- 8 files changed, 208 insertions(+), 70 deletions(-) create mode 100644 sensing/autoware_pointcloud_preprocessor/config/dual_return_outlier_filter_node.param.yaml rename sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/{dual_return_outlier_filter_nodelet.hpp => dual_return_outlier_filter_node.hpp} (95%) delete mode 100644 sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml create mode 100644 sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml create mode 100644 sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter_node.schema.json rename sensing/autoware_pointcloud_preprocessor/src/outlier_filter/{dual_return_outlier_filter_nodelet.cpp => dual_return_outlier_filter_node.cpp} (90%) diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 87f198e3f48ae..2c93731972dc8 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -73,7 +73,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/outlier_filter/ring_outlier_filter_node.cpp src/outlier_filter/radius_search_2d_outlier_filter_node.cpp src/outlier_filter/voxel_grid_outlier_filter_node.cpp - src/outlier_filter/dual_return_outlier_filter_nodelet.cpp + src/outlier_filter/dual_return_outlier_filter_node.cpp src/passthrough_filter/passthrough_filter_node.cpp src/passthrough_filter/passthrough_filter_uint16_node.cpp src/passthrough_filter/passthrough_uint16.cpp diff --git a/sensing/autoware_pointcloud_preprocessor/config/dual_return_outlier_filter_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/dual_return_outlier_filter_node.param.yaml new file mode 100644 index 0000000000000..5454176d7f319 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/dual_return_outlier_filter_node.param.yaml @@ -0,0 +1,19 @@ +/**: + ros__parameters: + x_max: 18.0 + x_min: -12.0 + y_max: 2.0 + y_min: -2.0 + z_max: 10.0 + z_min: 0.0 + min_azimuth_deg: 135.0 + max_azimuth_deg: 225.0 + max_distance: 12.0 + vertical_bins: 128 + max_azimuth_diff: 50.0 + weak_first_distance_ratio: 1.004 + general_distance_ratio: 1.03 + weak_first_local_noise_threshold: 2 + roi_mode: "Fixed_xyz_ROI" + visibility_error_threshold: 0.5 + visibility_warn_threshold: 0.7 diff --git a/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md index 6c9a7ed14c2eb..8f4da273861a3 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md @@ -50,25 +50,7 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Core Parameters -| Name | Type | Description | -| ---------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------- | -| `vertical_bins` | int | The number of vertical bin for visibility histogram | -| `max_azimuth_diff` | float | Threshold for ring_outlier_filter | -| `weak_first_distance_ratio` | double | Threshold for ring_outlier_filter | -| `general_distance_ratio` | double | Threshold for ring_outlier_filter | -| `weak_first_local_noise_threshold` | int | The parameter for determining whether it is noise | -| `visibility_error_threshold` | float | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes ERR | -| `visibility_warn_threshold` | float | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes WARN | -| `roi_mode` | string | The name of ROI mode for switching | -| `min_azimuth_deg` | float | The left limit of azimuth for `Fixed_azimuth_ROI` mode | -| `max_azimuth_deg` | float | The right limit of azimuth for `Fixed_azimuth_ROI` mode | -| `max_distance` | float | The limit distance for for `Fixed_azimuth_ROI` mode | -| `x_max` | float | Maximum of x for `Fixed_xyz_ROI` mode | -| `x_min` | float | Minimum of x for `Fixed_xyz_ROI` mode | -| `y_max` | float | Maximum of y for `Fixed_xyz_ROI` mode | -| `y_min` | float | Minimum of y for `Fixed_xyz_ROI` mode | -| `z_max` | float | Maximum of z for `Fixed_xyz_ROI` mode | -| `z_min` | float | Minimum of z for `Fixed_xyz_ROI` mode | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter_node.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp similarity index 95% rename from sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp index b8aba769b17a5..ef33e88ef5c98 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODE_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODE_HPP_ #include "autoware/pointcloud_preprocessor/filter.hpp" @@ -94,5 +94,5 @@ class DualReturnOutlierFilterComponent : public autoware::pointcloud_preprocesso } // namespace autoware::pointcloud_preprocessor // clang-format off -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ // NOLINT +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODE_HPP_ // NOLINT // clang-format on diff --git a/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml deleted file mode 100644 index c65996fbcdc65..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml new file mode 100644 index 0000000000000..d0293ca140e4f --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter_node.schema.json new file mode 100644 index 0000000000000..baaa0fa1f4604 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter_node.schema.json @@ -0,0 +1,146 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Dual Return Outlier Filter Node", + "type": "object", + "definitions": { + "dual_return_outlier_filter": { + "type": "object", + "properties": { + "x_max": { + "type": "number", + "description": "Maximum of x in meters for `Fixed_xyz_ROI` mode", + "default": "18.0" + }, + "x_min": { + "type": "number", + "description": "Minimum of x in meters for `Fixed_xyz_ROI` mode", + "default": "-12.0" + }, + "y_max": { + "type": "number", + "description": "Maximum of y in meters for `Fixed_xyz_ROI` mode", + "default": "2.0" + }, + "y_min": { + "type": "number", + "description": "Minimum of y in meters for `Fixed_xyz_ROI` mode", + "default": "-2.0" + }, + "z_max": { + "type": "number", + "description": "Maximum of z in meters for `Fixed_xyz_ROI` mode", + "default": "10.0" + }, + "z_min": { + "type": "number", + "description": "Minimum of z in meters for `Fixed_xyz_ROI` mode", + "default": "0.0" + }, + "min_azimuth_deg": { + "type": "number", + "description": "The left limit of azimuth in degrees for `Fixed_azimuth_ROI` mode", + "default": "135.0", + "minimum": 0, + "maximum": 360 + }, + "max_azimuth_deg": { + "type": "number", + "description": "The right limit of azimuth in degrees for `Fixed_azimuth_ROI` mode", + "default": "225.0", + "minimum": 0, + "maximum": 360 + }, + "max_distance": { + "type": "number", + "description": "The limit distance in meters for `Fixed_azimuth_ROI` mode", + "default": "12.0", + "minimum": 0.0 + }, + "vertical_bins": { + "type": "integer", + "description": "The number of vertical bins for the visibility histogram", + "default": "128", + "minimum": 1 + }, + "max_azimuth_diff": { + "type": "number", + "description": "The azimuth difference threshold in degrees for ring_outlier_filter", + "default": "50.0", + "minimum": 0.0 + }, + "weak_first_distance_ratio": { + "type": "number", + "description": "The maximum allowed ratio between consecutive weak point distances", + "default": "1.004", + "minimum": 0.0 + }, + "general_distance_ratio": { + "type": "number", + "description": "The maximum allowed ratio between consecutive normal point distances", + "default": "1.03", + "minimum": 0.0 + }, + "weak_first_local_noise_threshold": { + "type": "integer", + "description": "If the number of outliers among weak points is less than the weak_first_local_noise_threshold in the (max_azimuth - min_azimuth) / horizontal_bins interval, all points within the interval will not be filtered out.", + "default": "2", + "minimum": 0 + }, + "roi_mode": { + "type": "string", + "description": "roi mode", + "default": "Fixed_xyz_ROI", + "enum": ["Fixed_xyz_ROI", "No_ROI", "Fixed_azimuth_ROI"] + }, + "visibility_error_threshold": { + "type": "number", + "description": "When the proportion of white pixels in the binary histogram falls below this parameter the diagnostic status becomes ERR", + "default": "0.5", + "minimum": 0.0, + "maximum": 1.0 + }, + "visibility_warn_threshold": { + "type": "number", + "description": "When the proportion of white pixels in the binary histogram falls below this parameter the diagnostic status becomes WARN", + "default": "0.7", + "minimum": 0.0, + "maximum": 1.0 + } + }, + "required": [ + "x_max", + "x_min", + "y_max", + "y_min", + "z_max", + "z_min", + "min_azimuth_deg", + "max_azimuth_deg", + "max_distance", + "vertical_bins", + "max_azimuth_diff", + "weak_first_distance_ratio", + "general_distance_ratio", + "weak_first_local_noise_threshold", + "roi_mode", + "visibility_error_threshold", + "visibility_warn_threshold" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/dual_return_outlier_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp similarity index 90% rename from sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp index 5fd3262088d83..d3f81473ed06f 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp" #include "autoware_point_types/types.hpp" @@ -38,29 +38,24 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent( { // set initial parameters { - x_max_ = static_cast(declare_parameter("x_max", 18.0)); - x_min_ = static_cast(declare_parameter("x_min", -12.0)); - y_max_ = static_cast(declare_parameter("y_max", 2.0)); - y_min_ = static_cast(declare_parameter("y_min", -2.0)); - z_max_ = static_cast(declare_parameter("z_max", 10.0)); - z_min_ = static_cast(declare_parameter("z_min", 0.0)); - min_azimuth_deg_ = static_cast(declare_parameter("min_azimuth_deg", 135.0)); - max_azimuth_deg_ = static_cast(declare_parameter("max_azimuth_deg", 225.0)); - max_distance_ = static_cast(declare_parameter("max_distance", 12.0)); - vertical_bins_ = static_cast(declare_parameter("vertical_bins", 128)); - max_azimuth_diff_ = static_cast(declare_parameter("max_azimuth_diff", 50.0)); - weak_first_distance_ratio_ = - static_cast(declare_parameter("weak_first_distance_ratio", 1.004)); - general_distance_ratio_ = - static_cast(declare_parameter("general_distance_ratio", 1.03)); - - weak_first_local_noise_threshold_ = - static_cast(declare_parameter("weak_first_local_noise_threshold", 2)); - roi_mode_ = static_cast(declare_parameter("roi_mode", "Fixed_xyz_ROI")); - visibility_error_threshold_ = - static_cast(declare_parameter("visibility_error_threshold", 0.5)); - visibility_warn_threshold_ = - static_cast(declare_parameter("visibility_warn_threshold", 0.7)); + x_max_ = declare_parameter("x_max"); + x_min_ = declare_parameter("x_min"); + y_max_ = declare_parameter("y_max"); + y_min_ = declare_parameter("y_min"); + z_max_ = declare_parameter("z_max"); + z_min_ = declare_parameter("z_min"); + min_azimuth_deg_ = declare_parameter("min_azimuth_deg"); + max_azimuth_deg_ = declare_parameter("max_azimuth_deg"); + max_distance_ = declare_parameter("max_distance"); + vertical_bins_ = declare_parameter("vertical_bins"); + max_azimuth_diff_ = declare_parameter("max_azimuth_diff"); + weak_first_distance_ratio_ = declare_parameter("weak_first_distance_ratio"); + general_distance_ratio_ = declare_parameter("general_distance_ratio"); + + weak_first_local_noise_threshold_ = declare_parameter("weak_first_local_noise_threshold"); + roi_mode_ = declare_parameter("roi_mode"); + visibility_error_threshold_ = declare_parameter("visibility_error_threshold"); + visibility_warn_threshold_ = declare_parameter("visibility_warn_threshold"); } updater_.setHardwareID("dual_return_outlier_filter"); updater_.add( From 6e2cffb3909fc9854298d40b39f1fe007f383f2d Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Wed, 2 Oct 2024 11:09:53 +0900 Subject: [PATCH 81/95] refactor(autoware_pointcloud_preprocessor): rework approximate downsample filter parameters (#8480) * feat: rework approximate downsample parameters Signed-off-by: vividf * chore: add boundary Signed-off-by: vividf * chore: change double to float Signed-off-by: vividf * feat: rework approximate downsample parameters Signed-off-by: vividf * chore: add boundary Signed-off-by: vividf * chore: change double to float Signed-off-by: vividf * chore: fix grammatical error Signed-off-by: vividf * chore: fix variables from double to float in header Signed-off-by: vividf * chore: change minimum to float Signed-off-by: vividf * chore: fix CMakeLists Signed-off-by: vividf --------- Signed-off-by: vividf --- .../CMakeLists.txt | 2 +- ...roximate_downsample_filter_node.param.yaml | 5 ++ .../docs/downsample-filter.md | 6 +-- ...=> approximate_downsample_filter_node.hpp} | 14 +++--- .../approximate_downsample_filter.launch.xml | 16 +++++++ ...oximate_downsample_filter_node.schema.json | 46 +++++++++++++++++++ ...=> approximate_downsample_filter_node.cpp} | 10 ++-- 7 files changed, 81 insertions(+), 18 deletions(-) create mode 100644 sensing/autoware_pointcloud_preprocessor/config/approximate_downsample_filter_node.param.yaml rename sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/{approximate_downsample_filter_nodelet.hpp => approximate_downsample_filter_node.hpp} (92%) create mode 100644 sensing/autoware_pointcloud_preprocessor/launch/approximate_downsample_filter.launch.xml create mode 100644 sensing/autoware_pointcloud_preprocessor/schema/approximate_downsample_filter_node.schema.json rename sensing/autoware_pointcloud_preprocessor/src/downsample_filter/{approximate_downsample_filter_nodelet.cpp => approximate_downsample_filter_node.cpp} (93%) diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 2c93731972dc8..94deffd5ae9d0 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -68,7 +68,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/crop_box_filter/crop_box_filter_nodelet.cpp src/downsample_filter/voxel_grid_downsample_filter_node.cpp src/downsample_filter/random_downsample_filter_node.cpp - src/downsample_filter/approximate_downsample_filter_nodelet.cpp + src/downsample_filter/approximate_downsample_filter_node.cpp src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp src/outlier_filter/ring_outlier_filter_node.cpp src/outlier_filter/radius_search_2d_outlier_filter_node.cpp diff --git a/sensing/autoware_pointcloud_preprocessor/config/approximate_downsample_filter_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/approximate_downsample_filter_node.param.yaml new file mode 100644 index 0000000000000..239e47f09632b --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/approximate_downsample_filter_node.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + voxel_size_x: 0.3 + voxel_size_y: 0.3 + voxel_size_z: 0.1 diff --git a/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md index c6a45f7212c46..92ca1d3b3081c 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md @@ -36,11 +36,7 @@ These implementations inherit `autoware::pointcloud_preprocessor::Filter` class, #### Approximate Downsample Filter -| Name | Type | Default Value | Description | -| -------------- | ------ | ------------- | ---------------- | -| `voxel_size_x` | double | 0.3 | voxel size x [m] | -| `voxel_size_y` | double | 0.3 | voxel size y [m] | -| `voxel_size_z` | double | 0.1 | voxel size z [m] | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/approximate_downsample_filter_node.schema.json") }} ### Random Downsample Filter diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_node.hpp similarity index 92% rename from sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_node.hpp index 7f5d3f83d3a30..abef5acad59dc 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. All rights reserved. +// Copyright 2024 TIER IV, Inc. 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. @@ -48,8 +48,8 @@ * $Id: voxel_grid.cpp 35876 2011-02-09 01:04:36Z rusu $ * */ -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODE_HPP_ // NOLINT +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODE_HPP_ // NOLINT #include "autoware/pointcloud_preprocessor/filter.hpp" @@ -72,9 +72,9 @@ class ApproximateDownsampleFilterComponent : public autoware::pointcloud_preproc rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); private: - double voxel_size_x_; - double voxel_size_y_; - double voxel_size_z_; + float voxel_size_x_; + float voxel_size_y_; + float voxel_size_z_; public: PCL_MAKE_ALIGNED_OPERATOR_NEW @@ -83,5 +83,5 @@ class ApproximateDownsampleFilterComponent : public autoware::pointcloud_preproc } // namespace autoware::pointcloud_preprocessor // clang-format off -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODE_HPP_ // NOLINT // clang-format on diff --git a/sensing/autoware_pointcloud_preprocessor/launch/approximate_downsample_filter.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/approximate_downsample_filter.launch.xml new file mode 100644 index 0000000000000..f4375e5a5cd2f --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/approximate_downsample_filter.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/schema/approximate_downsample_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/approximate_downsample_filter_node.schema.json new file mode 100644 index 0000000000000..7b582aa5377b4 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/approximate_downsample_filter_node.schema.json @@ -0,0 +1,46 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Approximate Downsample Filter Node", + "type": "object", + "definitions": { + "approximate_downsample_filter": { + "type": "object", + "properties": { + "voxel_size_x": { + "type": "number", + "description": "voxel size along the x-axis [m]", + "default": "0.3", + "minimum": 0.0 + }, + "voxel_size_y": { + "type": "number", + "description": "voxel size along the y-axis [m]", + "default": "0.3", + "minimum": 0.0 + }, + "voxel_size_z": { + "type": "number", + "description": "voxel size along the z-axis [m]", + "default": "0.1", + "minimum": 0.0 + } + }, + "required": ["voxel_size_x", "voxel_size_y", "voxel_size_z"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/approximate_downsample_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_node.cpp similarity index 93% rename from sensing/autoware_pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_node.cpp index a4ccca9beea72..8ae814cf0ef1d 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. All rights reserved. +// Copyright 2024 TIER IV, Inc. 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. @@ -49,7 +49,7 @@ * */ -#include "autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_node.hpp" #include #include @@ -64,9 +64,9 @@ ApproximateDownsampleFilterComponent::ApproximateDownsampleFilterComponent( : Filter("ApproximateDownsampleFilter", options) { { - voxel_size_x_ = static_cast(declare_parameter("voxel_size_x", 0.3)); - voxel_size_y_ = static_cast(declare_parameter("voxel_size_y", 0.3)); - voxel_size_z_ = static_cast(declare_parameter("voxel_size_z", 0.1)); + voxel_size_x_ = declare_parameter("voxel_size_x"); + voxel_size_y_ = declare_parameter("voxel_size_y"); + voxel_size_z_ = declare_parameter("voxel_size_z"); } using std::placeholders::_1; From 2debc785861387723c5fd93a6a984d8c1b9a9ac7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Wed, 2 Oct 2024 06:58:21 +0200 Subject: [PATCH 82/95] fix(autoware_behavior_velocity_planner_common): add node clock, fix use sim time (#8876) Signed-off-by: Dawid Moszynski --- .../behavior_velocity_planner_common/planner_data.hpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp index 6e7f8b679d32a..4075f356c187a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp @@ -51,7 +51,8 @@ class BehaviorVelocityPlannerNode; struct PlannerData { explicit PlannerData(rclcpp::Node & node) - : vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()) + : clock_(node.get_clock()), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()) { max_stop_acceleration_threshold = node.declare_parameter( "max_accel"); // TODO(someone): read min_acc in velocity_controller.param.yaml? @@ -60,6 +61,8 @@ struct PlannerData delay_response_time = node.declare_parameter("delay_response_time"); } + rclcpp::Clock::SharedPtr clock_; + // msgs from callbacks that are used for data-ready geometry_msgs::msg::PoseStamped::ConstSharedPtr current_odometry; geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity; @@ -108,7 +111,7 @@ struct PlannerData } // Get velocities within stop_duration - const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); + const auto now = clock_->now(); std::vector vs; for (const auto & velocity : velocity_buffer) { vs.push_back(velocity.twist.linear.x); From 469f8278603c1202c4500912e8be2a7e66baed94 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Wed, 2 Oct 2024 14:58:14 +0900 Subject: [PATCH 83/95] fix(autoware_behavior_path_avoidance_by_lane_change_module): fix unmatchedSuppression (#8987) fix:unmatchedSuppression Signed-off-by: kobayu858 --- .../src/interface.cpp | 2 -- .../src/manager.cpp | 1 - 2 files changed, 3 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp index d47f76e399b4c..1051a3460be3b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -44,14 +44,12 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( { } -// cppcheck-suppress unusedFunction bool AvoidanceByLaneChangeInterface::isExecutionRequested() const { return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck() && module_type_->isValidPath(); } -// cppcheck-suppress unusedFunction void AvoidanceByLaneChangeInterface::processOnEntry() { waitApproval(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index ea5b853051e9a..28ff6c9e110d6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -186,7 +186,6 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) avoidance_parameters_ = std::make_shared(p); } -// cppcheck-suppress unusedFunction SMIPtr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() { return std::make_unique( From 2b179f44d647a597577667aef43c7df595a6f890 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 2 Oct 2024 15:16:04 +0900 Subject: [PATCH 84/95] feat(autoware_behavior_path_planner_common): disable feature of turning off blinker at low velocity (#9005) * feat(turn_signal_decider): disable feature of turning off blinker at low velocity Signed-off-by: Kyoichi Sugahara --------- Signed-off-by: Kyoichi Sugahara --- .../src/turn_signal_decider.cpp | 24 +++++++++++++++---- 1 file changed, 19 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp index 0e005166a97c0..22d80da2cfa67 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp @@ -138,11 +138,12 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( const size_t current_seg_idx, const RouteHandler & route_handler, const double nearest_dist_threshold, const double nearest_yaw_threshold) { - const auto requires_turn_signal = [&](const auto & lane_attribute) { + const auto requires_turn_signal = [¤t_vel]( + const auto & turn_direction, const bool is_in_turn_lane) { constexpr double stop_velocity_threshold = 0.1; return ( - lane_attribute == "right" || lane_attribute == "left" || - (lane_attribute == "straight" && current_vel < stop_velocity_threshold)); + turn_direction == "right" || turn_direction == "left" || + (turn_direction == "straight" && current_vel < stop_velocity_threshold && !is_in_turn_lane)); }; // base search distance const double base_search_distance = @@ -160,6 +161,19 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( } } + bool is_in_turn_lane = false; + for (const auto & lane_id : unique_lane_ids) { + const auto lanelet = route_handler.getLaneletsFromId(lane_id); + const std::string turn_direction = lanelet.attributeOr("turn_direction", "none"); + if (turn_direction == "left" || turn_direction == "right") { + const auto & position = current_pose.position; + const lanelet::BasicPoint2d point(position.x, position.y); + if (lanelet::geometry::inside(lanelet, point)) { + is_in_turn_lane = true; + break; + } + } + } // combine consecutive lanes of the same turn direction // stores lanes that have already been combine std::set processed_lanes; @@ -175,7 +189,7 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( // Get the lane and its attribute const std::string lane_attribute = current_lane.attributeOr("turn_direction", std::string("none")); - if (!requires_turn_signal(lane_attribute)) continue; + if (!requires_turn_signal(lane_attribute, is_in_turn_lane)) continue; do { processed_lanes.insert(current_lane.id()); @@ -256,7 +270,7 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( } else if (search_distance <= dist_to_front_point) { continue; } - if (requires_turn_signal(lane_attribute)) { + if (requires_turn_signal(lane_attribute, is_in_turn_lane)) { // update map if necessary if (desired_start_point_map_.find(lane_id) == desired_start_point_map_.end()) { desired_start_point_map_.emplace(lane_id, current_pose); From 5b8219eae6e6370af0589a29efcae3bb9da2542e Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Wed, 2 Oct 2024 17:53:50 +0900 Subject: [PATCH 85/95] feat(autonomous_emergency_braking): set max imu path length (#9004) * set a limit to the imu path length Signed-off-by: Daniel Sanchez * fix test and add a new one Signed-off-by: Daniel Sanchez * update readme Signed-off-by: Daniel Sanchez * pre-commit Signed-off-by: Daniel Sanchez * use velocity and time directly to get arc length Signed-off-by: Daniel Sanchez * refactor to reduce repeated code Signed-off-by: Daniel Sanchez * cleaning code Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../README.md | 3 +- .../autonomous_emergency_braking.param.yaml | 3 +- .../autonomous_emergency_braking/node.hpp | 3 +- .../src/node.cpp | 51 ++++++++++--------- .../test/test.cpp | 22 +++++++- 5 files changed, 53 insertions(+), 29 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index c3583982dde39..42b3f4c9f96de 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -213,7 +213,8 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t | cluster_minimum_height | [m] | double | at least one point in a cluster must be higher than this value for the cluster to be included in the set of possible collision targets | 0.1 | | minimum_cluster_size | [-] | int | minimum required amount of points contained by a cluster for it to be considered as a possible target obstacle | 10 | | maximum_cluster_size | [-] | int | maximum amount of points contained by a cluster for it to be considered as a possible target obstacle | 10000 | -| min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | +| min_generated_imu_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | +| max_generated_imu_path_length | [m] | double | maximum distance for a predicted path generated by sensors | 10.0 | | expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 | | longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 | | t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 | diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 75b54fe547e32..f7548536beaef 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -7,7 +7,8 @@ use_predicted_object_data: false use_object_velocity_calculation: true check_autoware_state: true - min_generated_path_length: 0.5 + min_generated_imu_path_length: 0.5 + max_generated_imu_path_length: 10.0 imu_prediction_time_horizon: 1.5 imu_prediction_time_interval: 0.1 mpc_prediction_time_horizon: 4.5 diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index 6daa0a34dbaf4..214a6dc309210 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -562,7 +562,8 @@ class AEB : public rclcpp::Node double voxel_grid_x_; double voxel_grid_y_; double voxel_grid_z_; - double min_generated_path_length_; + double min_generated_imu_path_length_; + double max_generated_imu_path_length_; double expand_width_; double longitudinal_offset_; double t_response_; diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 34a7453b51726..b3988ba238fe7 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -165,7 +165,8 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) voxel_grid_x_ = declare_parameter("voxel_grid_x"); voxel_grid_y_ = declare_parameter("voxel_grid_y"); voxel_grid_z_ = declare_parameter("voxel_grid_z"); - min_generated_path_length_ = declare_parameter("min_generated_path_length"); + min_generated_imu_path_length_ = declare_parameter("min_generated_imu_path_length"); + max_generated_imu_path_length_ = declare_parameter("max_generated_imu_path_length"); expand_width_ = declare_parameter("expand_width"); longitudinal_offset_ = declare_parameter("longitudinal_offset"); t_response_ = declare_parameter("t_response"); @@ -227,7 +228,8 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( updateParam(parameters, "voxel_grid_x", voxel_grid_x_); updateParam(parameters, "voxel_grid_y", voxel_grid_y_); updateParam(parameters, "voxel_grid_z", voxel_grid_z_); - updateParam(parameters, "min_generated_path_length", min_generated_path_length_); + updateParam(parameters, "min_generated_imu_path_length", min_generated_imu_path_length_); + updateParam(parameters, "max_generated_imu_path_length", max_generated_imu_path_length_); updateParam(parameters, "expand_width", expand_width_); updateParam(parameters, "longitudinal_offset", longitudinal_offset_); updateParam(parameters, "t_response", t_response_); @@ -639,39 +641,35 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) ini_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); ini_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); path.push_back(ini_pose); - - if (std::abs(curr_v) < 0.1) { - // if current velocity is too small, assume it stops at the same point + const double & dt = imu_prediction_time_interval_; + const double distance_between_points = curr_v * dt; + constexpr double minimum_distance_between_points{1e-2}; + // if current velocity is too small, assume it stops at the same point + // if distance between points is too small, arc length calculation is unreliable, so we skip + // creating the path + if (std::abs(curr_v) < 0.1 || distance_between_points < minimum_distance_between_points) { return path; } - constexpr double epsilon = std::numeric_limits::epsilon(); - const double & dt = imu_prediction_time_interval_; const double & horizon = imu_prediction_time_horizon_; - for (double t = 0.0; t < horizon + epsilon; t += dt) { - curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; - curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; - curr_yaw = curr_yaw + curr_w * dt; - geometry_msgs::msg::Pose current_pose; - current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); - current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); - if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-2) { - continue; - } - path.push_back(current_pose); - } + double path_arc_length = 0.0; + double t = 0.0; - // If path is shorter than minimum path length - while (autoware::motion_utils::calcArcLength(path) < min_generated_path_length_) { + bool finished_creating_path = false; + while (!finished_creating_path) { curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; curr_yaw = curr_yaw + curr_w * dt; geometry_msgs::msg::Pose current_pose; current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); - if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-2) { - continue; - } + + t += dt; + path_arc_length += distance_between_points; + + finished_creating_path = (t > horizon) && (path_arc_length > min_generated_imu_path_length_); + finished_creating_path = + (finished_creating_path) || (path_arc_length > max_generated_imu_path_length_); path.push_back(current_pose); } return path; @@ -691,12 +689,15 @@ std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) time_keeper_->start_track("createPath"); Path path; path.reserve(predicted_traj.points.size()); + constexpr double minimum_distance_between_points{1e-2}; for (size_t i = 0; i < predicted_traj.points.size(); ++i) { geometry_msgs::msg::Pose map_pose; tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped.value()); // skip points that are too close to the last point in the path - if (autoware::universe_utils::calcDistance2d(path.back(), map_pose) < 1e-2) { + if ( + autoware::universe_utils::calcDistance2d(path.back(), map_pose) < + minimum_distance_between_points) { continue; } diff --git a/control/autoware_autonomous_emergency_braking/test/test.cpp b/control/autoware_autonomous_emergency_braking/test/test.cpp index 609a7a36b7d67..c2a58941cc144 100644 --- a/control/autoware_autonomous_emergency_braking/test/test.cpp +++ b/control/autoware_autonomous_emergency_braking/test/test.cpp @@ -171,7 +171,7 @@ TEST_F(TestAEB, checkIncompleteImuPathGeneration) { const double dt = aeb_node_->imu_prediction_time_interval_; const double horizon = aeb_node_->imu_prediction_time_horizon_; - const double min_generated_path_length = aeb_node_->min_generated_path_length_; + const double min_generated_path_length = aeb_node_->min_generated_imu_path_length_; const double slow_velocity = min_generated_path_length / (2.0 * horizon); constexpr double yaw_rate = 0.05; const auto imu_path = aeb_node_->generateEgoPath(slow_velocity, yaw_rate); @@ -185,6 +185,26 @@ TEST_F(TestAEB, checkIncompleteImuPathGeneration) ASSERT_TRUE(footprint.size() == imu_path.size() - 1); } +TEST_F(TestAEB, checkImuPathGenerationIsCut) +{ + const double dt = aeb_node_->imu_prediction_time_interval_; + const double horizon = aeb_node_->imu_prediction_time_horizon_; + const double max_generated_path_length = aeb_node_->max_generated_imu_path_length_; + const double fast_velocity = 2.0 * max_generated_path_length / (horizon); + constexpr double yaw_rate = 0.05; + const auto imu_path = aeb_node_->generateEgoPath(fast_velocity, yaw_rate); + + ASSERT_FALSE(imu_path.empty()); + constexpr double epsilon{1e-3}; + ASSERT_TRUE( + autoware::motion_utils::calcArcLength(imu_path) <= + max_generated_path_length + dt * fast_velocity + epsilon); + + const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0); + ASSERT_FALSE(footprint.empty()); + ASSERT_TRUE(footprint.size() == imu_path.size() - 1); +} + TEST_F(TestAEB, checkEmptyPathAtZeroSpeed) { const double velocity = 0.0; From 921be33aa319573f48234e185e256d5f6c46b96c Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 3 Oct 2024 10:39:38 +0900 Subject: [PATCH 86/95] refactor(mission_planner): move anonymous functions to utils and add namespace (#9012) feat(mission_planner): move functions to utils and add namespace Signed-off-by: kosuke55 --- .../src/lanelet2_plugins/default_planner.cpp | 93 +----------------- .../lanelet2_plugins/utility_functions.cpp | 94 +++++++++++++++++++ .../lanelet2_plugins/utility_functions.hpp | 17 ++++ 3 files changed, 112 insertions(+), 92 deletions(-) diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp index 3cf6e73bc2bc6..50a6becf2e124 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -41,96 +41,6 @@ #include #include -namespace -{ -using RouteSections = std::vector; - -bool is_in_lane(const lanelet::ConstLanelet & lanelet, const lanelet::ConstPoint3d & point) -{ - // check if goal is on a lane at appropriate angle - const auto distance = boost::geometry::distance( - lanelet.polygon2d().basicPolygon(), lanelet::utils::to2D(point).basicPoint()); - constexpr double th_distance = std::numeric_limits::epsilon(); - return distance < th_distance; -} - -bool is_in_parking_space( - const lanelet::ConstLineStrings3d & parking_spaces, const lanelet::ConstPoint3d & point) -{ - for (const auto & parking_space : parking_spaces) { - lanelet::ConstPolygon3d parking_space_polygon; - if (!lanelet::utils::lineStringWithWidthToPolygon(parking_space, &parking_space_polygon)) { - continue; - } - - const double distance = boost::geometry::distance( - lanelet::utils::to2D(parking_space_polygon).basicPolygon(), - lanelet::utils::to2D(point).basicPoint()); - constexpr double th_distance = std::numeric_limits::epsilon(); - if (distance < th_distance) { - return true; - } - } - return false; -} - -bool is_in_parking_lot( - const lanelet::ConstPolygons3d & parking_lots, const lanelet::ConstPoint3d & point) -{ - for (const auto & parking_lot : parking_lots) { - const double distance = boost::geometry::distance( - lanelet::utils::to2D(parking_lot).basicPolygon(), lanelet::utils::to2D(point).basicPoint()); - constexpr double th_distance = std::numeric_limits::epsilon(); - if (distance < th_distance) { - return true; - } - } - return false; -} - -double project_goal_to_map( - const lanelet::ConstLanelet & lanelet_component, const lanelet::ConstPoint3d & goal_point) -{ - const lanelet::ConstLineString3d center_line = - lanelet::utils::generateFineCenterline(lanelet_component); - lanelet::BasicPoint3d project = lanelet::geometry::project(center_line, goal_point.basicPoint()); - return project.z(); -} - -geometry_msgs::msg::Pose get_closest_centerline_pose( - const lanelet::ConstLanelets & road_lanelets, const geometry_msgs::msg::Pose & point, - autoware::vehicle_info_utils::VehicleInfo vehicle_info) -{ - lanelet::Lanelet closest_lanelet; - if (!lanelet::utils::query::getClosestLaneletWithConstrains( - road_lanelets, point, &closest_lanelet, 0.0)) { - // point is not on any lanelet. - return point; - } - - const auto refined_center_line = lanelet::utils::generateFineCenterline(closest_lanelet, 1.0); - closest_lanelet.setCenterline(refined_center_line); - - const double lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, point.position); - - const auto nearest_idx = autoware::motion_utils::findNearestIndex( - convertCenterlineToPoints(closest_lanelet), point.position); - const auto nearest_point = closest_lanelet.centerline()[nearest_idx]; - - // shift nearest point on its local y axis so that vehicle's right and left edges - // would have approx the same clearance from road border - const auto shift_length = (vehicle_info.right_overhang_m - vehicle_info.left_overhang_m) / 2.0; - const auto delta_x = -shift_length * std::sin(lane_yaw); - const auto delta_y = shift_length * std::cos(lane_yaw); - - lanelet::BasicPoint3d refined_point( - nearest_point.x() + delta_x, nearest_point.y() + delta_y, nearest_point.z()); - - return convertBasicPoint3dToPose(refined_point, lane_yaw); -} - -} // anonymous namespace - namespace autoware::mission_planner::lanelet2 { @@ -395,8 +305,7 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points) std::stringstream log_ss; for (const auto & point : points) { - log_ss << "x: " << point.position.x << " " - << "y: " << point.position.y << std::endl; + log_ss << "x: " << point.position.x << " " << "y: " << point.position.y << std::endl; } RCLCPP_DEBUG_STREAM( logger, "start planning route with check points: " << std::endl diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp index 34b16ef35f603..08345282a9264 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -14,10 +14,18 @@ #include "utility_functions.hpp" +#include +#include +#include + #include #include +#include + +namespace autoware::mission_planner::lanelet2 +{ autoware::universe_utils::Polygon2d convert_linear_ring_to_polygon( autoware::universe_utils::LinearRing2d footprint) { @@ -64,3 +72,89 @@ geometry_msgs::msg::Pose convertBasicPoint3dToPose( return pose; } + +bool is_in_lane(const lanelet::ConstLanelet & lanelet, const lanelet::ConstPoint3d & point) +{ + // check if goal is on a lane at appropriate angle + const auto distance = boost::geometry::distance( + lanelet.polygon2d().basicPolygon(), lanelet::utils::to2D(point).basicPoint()); + constexpr double th_distance = std::numeric_limits::epsilon(); + return distance < th_distance; +} + +bool is_in_parking_space( + const lanelet::ConstLineStrings3d & parking_spaces, const lanelet::ConstPoint3d & point) +{ + for (const auto & parking_space : parking_spaces) { + lanelet::ConstPolygon3d parking_space_polygon; + if (!lanelet::utils::lineStringWithWidthToPolygon(parking_space, &parking_space_polygon)) { + continue; + } + + const double distance = boost::geometry::distance( + lanelet::utils::to2D(parking_space_polygon).basicPolygon(), + lanelet::utils::to2D(point).basicPoint()); + constexpr double th_distance = std::numeric_limits::epsilon(); + if (distance < th_distance) { + return true; + } + } + return false; +} + +bool is_in_parking_lot( + const lanelet::ConstPolygons3d & parking_lots, const lanelet::ConstPoint3d & point) +{ + for (const auto & parking_lot : parking_lots) { + const double distance = boost::geometry::distance( + lanelet::utils::to2D(parking_lot).basicPolygon(), lanelet::utils::to2D(point).basicPoint()); + constexpr double th_distance = std::numeric_limits::epsilon(); + if (distance < th_distance) { + return true; + } + } + return false; +} + +double project_goal_to_map( + const lanelet::ConstLanelet & lanelet_component, const lanelet::ConstPoint3d & goal_point) +{ + const lanelet::ConstLineString3d center_line = + lanelet::utils::generateFineCenterline(lanelet_component); + lanelet::BasicPoint3d project = lanelet::geometry::project(center_line, goal_point.basicPoint()); + return project.z(); +} + +geometry_msgs::msg::Pose get_closest_centerline_pose( + const lanelet::ConstLanelets & road_lanelets, const geometry_msgs::msg::Pose & point, + autoware::vehicle_info_utils::VehicleInfo vehicle_info) +{ + lanelet::Lanelet closest_lanelet; + if (!lanelet::utils::query::getClosestLaneletWithConstrains( + road_lanelets, point, &closest_lanelet, 0.0)) { + // point is not on any lanelet. + return point; + } + + const auto refined_center_line = lanelet::utils::generateFineCenterline(closest_lanelet, 1.0); + closest_lanelet.setCenterline(refined_center_line); + + const double lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, point.position); + + const auto nearest_idx = autoware::motion_utils::findNearestIndex( + convertCenterlineToPoints(closest_lanelet), point.position); + const auto nearest_point = closest_lanelet.centerline()[nearest_idx]; + + // shift nearest point on its local y axis so that vehicle's right and left edges + // would have approx the same clearance from road border + const auto shift_length = (vehicle_info.right_overhang_m - vehicle_info.left_overhang_m) / 2.0; + const auto delta_x = -shift_length * std::sin(lane_yaw); + const auto delta_y = shift_length * std::cos(lane_yaw); + + lanelet::BasicPoint3d refined_point( + nearest_point.x() + delta_x, nearest_point.y() + delta_y, nearest_point.z()); + + return convertBasicPoint3dToPose(refined_point, lane_yaw); +} + +} // namespace autoware::mission_planner::lanelet2 diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp index 6e3d2a0e88941..36a2e17fb5ff0 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -29,6 +29,10 @@ #include +namespace autoware::mission_planner::lanelet2 +{ +using RouteSections = std::vector; + template bool exists(const std::vector & vectors, const T & item) { @@ -48,4 +52,17 @@ void insert_marker_array( std::vector convertCenterlineToPoints(const lanelet::Lanelet & lanelet); geometry_msgs::msg::Pose convertBasicPoint3dToPose( const lanelet::BasicPoint3d & point, const double lane_yaw); + +bool is_in_lane(const lanelet::ConstLanelet & lanelet, const lanelet::ConstPoint3d & point); +bool is_in_parking_space( + const lanelet::ConstLineStrings3d & parking_spaces, const lanelet::ConstPoint3d & point); +bool is_in_parking_lot( + const lanelet::ConstPolygons3d & parking_lots, const lanelet::ConstPoint3d & point); +double project_goal_to_map( + const lanelet::ConstLanelet & lanelet_component, const lanelet::ConstPoint3d & goal_point); +geometry_msgs::msg::Pose get_closest_centerline_pose( + const lanelet::ConstLanelets & road_lanelets, const geometry_msgs::msg::Pose & point, + autoware::vehicle_info_utils::VehicleInfo vehicle_info); + +} // namespace autoware::mission_planner::lanelet2 #endif // LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ From 552786f751933c92f05f18998776b40a102bb1ab Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 3 Oct 2024 10:47:13 +0900 Subject: [PATCH 87/95] refactor(autoware_multi_object_tracker): separate detected object covariance modeling (#9001) * refactor: update object model includes in tracker models Signed-off-by: Taekjin LEE * feat: add uncertainty processor for object tracking feat: refactor uncertainty processing for object tracking feat: impl obj class model feat: Update object model measurement covariances Refactor the object model measurement covariances in the `object_model.hpp` file. Update the velocity long and velocity lat measurement covariances for different object model types. refactor: Model object uncertainty in multi_object_tracker_node.cpp feat: Update object model measurement covariances in object_model.hpp feat: Update uncertainty processing for object tracking fix: remove uncertainty modelling in trackers refactor: Remove unused function isLargeVehicleLabel The function isLargeVehicleLabel in utils.hpp is no longer used and can be safely removed. Revert "refactor: Remove unused function isLargeVehicleLabel" This reverts commit 23e3eff511b21ef8ceeacb7db47c74f747009a32. feat: Normalize uncertainty in object tracking This commit adds a new function `normalizeUncertainty` to the `uncertainty_processor.hpp` and `uncertainty_processor.cpp` files. The function normalizes the position and twist covariance matrices of detected objects to ensure minimum values for distance, radius, and velocity. This helps improve the accuracy and reliability of object tracking. Signed-off-by: Taekjin LEE * refactor: update motion model parameters for object tracking Signed-off-by: Taekjin LEE * refactor: update yaw rate limit in object model Signed-off-by: Taekjin LEE * Revert "refactor: update yaw rate limit in object model" This reverts commit 6e8b201582cb65673678029dc3a781f2b7126f81. Signed-off-by: Taekjin LEE * refactor: update object model measurement covariances Refactor the object model measurement covariances in the `object_model.hpp` file. Update the velocity long and velocity lat measurement covariances for different object model types. Signed-off-by: Taekjin LEE * refactor: update motion model parameters comments Signed-off-by: Taekjin LEE * refactor: remove comment Signed-off-by: Taekjin LEE * style(pre-commit): autofix * feat: Update copyright notice in uncertainty_processor.hpp Update the copyright notice in the uncertainty_processor.hpp file to reflect the correct company name. Signed-off-by: Taekjin LEE * refactor: update runProcess function parameters in multi_object_tracker_node.hpp Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../CMakeLists.txt | 1 + .../object_model/object_model.hpp | 17 ++- .../tracker/model/bicycle_tracker.hpp | 2 +- .../tracker/model/big_vehicle_tracker.hpp | 2 +- .../tracker/model/normal_vehicle_tracker.hpp | 2 +- .../tracker/model/pedestrian_tracker.hpp | 2 +- .../motion_model/bicycle_motion_model.hpp | 35 +++-- .../motion_model/ctrv_motion_model.hpp | 18 +-- .../tracker/motion_model/cv_motion_model.hpp | 16 +- .../uncertainty/uncertainty_processor.hpp | 51 +++++++ .../lib/tracker/model/bicycle_tracker.cpp | 36 ----- .../lib/tracker/model/big_vehicle_tracker.cpp | 44 ------ .../tracker/model/normal_vehicle_tracker.cpp | 44 ------ .../lib/tracker/model/pedestrian_tracker.cpp | 36 ----- .../motion_model/bicycle_motion_model.cpp | 38 +---- .../motion_model/ctrv_motion_model.cpp | 22 +-- .../tracker/motion_model/cv_motion_model.cpp | 18 --- .../lib/uncertainty/uncertainty_processor.cpp | 139 ++++++++++++++++++ .../src/multi_object_tracker_node.cpp | 13 +- .../src/multi_object_tracker_node.hpp | 2 +- 20 files changed, 255 insertions(+), 283 deletions(-) rename perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/{tracker => }/object_model/object_model.hpp (93%) create mode 100644 perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp create mode 100644 perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp diff --git a/perception/autoware_multi_object_tracker/CMakeLists.txt b/perception/autoware_multi_object_tracker/CMakeLists.txt index fe4546cc9bc60..73edabaa09429 100644 --- a/perception/autoware_multi_object_tracker/CMakeLists.txt +++ b/perception/autoware_multi_object_tracker/CMakeLists.txt @@ -44,6 +44,7 @@ set(${PROJECT_NAME}_lib lib/tracker/model/pedestrian_and_bicycle_tracker.cpp lib/tracker/model/unknown_tracker.cpp lib/tracker/model/pass_through_tracker.cpp + lib/uncertainty/uncertainty_processor.cpp ) ament_auto_add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_src} diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/object_model.hpp similarity index 93% rename from perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/object_model.hpp index b49464109eec8..4fea038d00667 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/object_model.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Taekjin Lee // -#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ -#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ #include @@ -53,6 +53,7 @@ namespace object_model { enum class ObjectModelType { NormalVehicle, BigVehicle, Bicycle, Pedestrian, Unknown }; + struct ObjectSize { double length{0.0}; // [m] @@ -287,7 +288,16 @@ class ObjectModel measurement_covariance.pos_x = sq(0.4); measurement_covariance.pos_y = sq(0.4); measurement_covariance.yaw = sq(deg2rad(30.0)); + measurement_covariance.vel_long = sq(kmph2mps(5.0)); + break; + case ObjectModelType::Unknown: + // measurement noise model + measurement_covariance.pos_x = sq(1.0); + measurement_covariance.pos_y = sq(1.0); + measurement_covariance.yaw = sq(deg2rad(360.0)); + measurement_covariance.vel_long = sq(kmph2mps(10.0)); + measurement_covariance.vel_lat = sq(kmph2mps(10.0)); break; default: @@ -302,8 +312,9 @@ static const ObjectModel normal_vehicle(ObjectModelType::NormalVehicle); static const ObjectModel big_vehicle(ObjectModelType::BigVehicle); static const ObjectModel bicycle(ObjectModelType::Bicycle); static const ObjectModel pedestrian(ObjectModelType::Pedestrian); +static const ObjectModel unknown(ObjectModelType::Unknown); } // namespace object_model } // namespace autoware::multi_object_tracker -#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 7cb2963d38ef1..8501c68b0cf23 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -20,9 +20,9 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ #include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/object_model.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 227e6cd01f4dc..489f656f750cb 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -20,9 +20,9 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ #include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/object_model.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 8f5bab65c6aed..0bfc065c7cc68 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -20,9 +20,9 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ #include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/object_model.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index 500148ba41081..f20b38f73e95f 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -20,9 +20,9 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ #include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/object_model.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" -#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp index 123eb30e63d6c..b84f8a4bd3bd7 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp @@ -45,24 +45,25 @@ class BicycleMotionModel : public MotionModel double lf_; double lr_; - // motion parameters + // motion parameters: process noise and motion limits struct MotionParams { - double q_stddev_acc_long; - double q_stddev_acc_lat; - double q_cov_acc_long; - double q_cov_acc_lat; - double q_stddev_yaw_rate_min; - double q_stddev_yaw_rate_max; - double q_cov_slip_rate_min; - double q_cov_slip_rate_max; - double q_max_slip_angle; - double lf_ratio; - double lr_ratio; - double lf_min; - double lr_min; - double max_vel; - double max_slip; + double q_stddev_acc_long = 3.43; // [m/s^2] uncertain longitudinal acceleration, 0.35G + double q_stddev_acc_lat = 1.47; // [m/s^2] uncertain longitudinal acceleration, 0.15G + double q_cov_acc_long = 11.8; // [m/s^2] uncertain longitudinal acceleration, 0.35G + double q_cov_acc_lat = 2.16; // [m/s^2] uncertain lateral acceleration, 0.15G + double q_stddev_yaw_rate_min = 0.02618; // [rad/s] uncertain yaw change rate, 1.5deg/s + double q_stddev_yaw_rate_max = 0.2618; // [rad/s] uncertain yaw change rate, 15deg/s + double q_cov_slip_rate_min = + 2.7416e-5; // [rad^2/s^2] uncertain slip angle change rate, 0.3 deg/s + double q_cov_slip_rate_max = 0.03046; // [rad^2/s^2] uncertain slip angle change rate, 10 deg/s + double q_max_slip_angle = 0.5236; // [rad] max slip angle, 30deg + double lf_ratio = 0.3; // [-] ratio of the distance from the center to the front wheel + double lr_ratio = 0.25; // [-] ratio of the distance from the center to the rear wheel + double lf_min = 1.0; // [m] minimum distance from the center to the front wheel + double lr_min = 1.0; // [m] minimum distance from the center to the rear wheel + double max_vel = 27.8; // [m/s] maximum velocity, 100km/h + double max_slip = 0.5236; // [rad] maximum slip angle, 30deg } motion_params_; public: @@ -76,8 +77,6 @@ class BicycleMotionModel : public MotionModel const std::array & pose_cov, const double & vel, const double & vel_cov, const double & slip, const double & slip_cov, const double & length); - void setDefaultParams(); - void setMotionParams( const double & q_stddev_acc_long, const double & q_stddev_acc_lat, const double & q_stddev_yaw_rate_min, const double & q_stddev_yaw_rate_max, diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp index 2632d99047053..812b91fc8acf0 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp @@ -42,16 +42,16 @@ class CTRVMotionModel : public MotionModel // attributes rclcpp::Logger logger_; - // motion parameters + // motion parameters: process noise and motion limits struct MotionParams { - double q_cov_x; - double q_cov_y; - double q_cov_yaw; - double q_cov_vel; - double q_cov_wz; - double max_vel; - double max_wz; + double q_cov_x = 0.025; // [m^2/s^2] uncertain position in x, 0.5m/s + double q_cov_y = 0.025; // [m^2/s^2] uncertain position in y, 0.5m/s + double q_cov_yaw = 0.1218; // [rad^2/s^2] uncertain yaw angle, 20deg/s + double q_cov_vel = 8.6436; // [m^2/s^4] uncertain velocity, 0.3G m/s^2 + double q_cov_wz = 0.5236; // [rad^2/s^4] uncertain yaw rate, 30deg/s^2 + double max_vel = 2.78; // [m/s] maximum velocity + double max_wz = 0.5236; // [rad/s] maximum yaw rate, 30deg/s } motion_params_; public: @@ -65,8 +65,6 @@ class CTRVMotionModel : public MotionModel const std::array & pose_cov, const double & vel, const double & vel_cov, const double & wz, const double & wz_cov); - void setDefaultParams(); - void setMotionParams( const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_yaw, const double & q_stddev_vx, const double & q_stddev_wz); diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp index 26799f1916741..dad1b1024879a 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp @@ -41,15 +41,15 @@ class CVMotionModel : public MotionModel // attributes rclcpp::Logger logger_; - // motion parameters + // motion parameters: process noise and motion limits struct MotionParams { - double q_cov_x; - double q_cov_y; - double q_cov_vx; - double q_cov_vy; - double max_vx; - double max_vy; + double q_cov_x = 0.025; // [m^2/s^2] uncertain position in x, 0.5m/s + double q_cov_y = 0.025; // [m^2/s^2] uncertain position in y, 0.5m/s + double q_cov_vx = 8.6436; // [m^2/s^4] uncertain velocity, 0.3G m/s^2 + double q_cov_vy = 8.6436; // [m^2/s^4] uncertain velocity, 0.3G m/s^2 + double max_vx = 16.67; // [m/s] maximum velocity, 60km/h + double max_vy = 16.67; // [m/s] maximum velocity, 60km/h } motion_params_; public: @@ -63,8 +63,6 @@ class CVMotionModel : public MotionModel const std::array & pose_cov, const double & vx, const double & vy, const std::array & twist_cov); - void setDefaultParams(); - void setMotionParams( const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_vx, const double & q_stddev_vy); diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp new file mode 100644 index 0000000000000..52781be1b8201 --- /dev/null +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp @@ -0,0 +1,51 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee + +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__UNCERTAINTY__UNCERTAINTY_PROCESSOR_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__UNCERTAINTY__UNCERTAINTY_PROCESSOR_HPP_ + +#include "autoware/multi_object_tracker/object_model/object_model.hpp" + +#include + +#include + +namespace autoware::multi_object_tracker +{ + +namespace uncertainty +{ + +using autoware::multi_object_tracker::object_model::ObjectModel; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::ObjectClassification; + +ObjectModel decodeObjectModel(const ObjectClassification & object_class); + +DetectedObjects modelUncertainty(const DetectedObjects & detected_objects); + +object_model::StateCovariance covarianceFromObjectClass( + const DetectedObject & detected_object, const ObjectClassification & object_class); + +void normalizeUncertainty(DetectedObjects & detected_objects); + +} // namespace uncertainty + +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__UNCERTAINTY__UNCERTAINTY_PROCESSOR_HPP_ diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index ba53ccec1ad43..38a49e48d8d10 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -162,42 +162,6 @@ autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( } } - // UNCERTAINTY MODEL - if (!object.kinematics.has_position_covariance) { - // measurement noise covariance - auto r_cov_x = object_model_.measurement_covariance.pos_x; - auto r_cov_y = object_model_.measurement_covariance.pos_y; - - // yaw angle fix - const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const bool is_yaw_available = - object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; - - // fill covariance matrix - using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; - const double cos_yaw = std::cos(pose_yaw); - const double sin_yaw = std::sin(pose_yaw); - const double sin_2yaw = std::sin(2.0 * pose_yaw); - pose_cov[XYZRPY_COV_IDX::X_X] = - r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - pose_cov[XYZRPY_COV_IDX::Y_Y] = - r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x - pose_cov[XYZRPY_COV_IDX::X_YAW] = 0.0; // x - yaw - pose_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; // y - yaw - pose_cov[XYZRPY_COV_IDX::YAW_X] = 0.0; // yaw - x - pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y - pose_cov[XYZRPY_COV_IDX::YAW_YAW] = object_model_.measurement_covariance.yaw; // yaw - yaw - if (!is_yaw_available) { - pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value - } - auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; - twist_cov[XYZRPY_COV_IDX::X_X] = object_model_.measurement_covariance.vel_long; // vel - vel - } - return updating_object; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp index 0593b7fc9dc12..d5913eccdf0bf 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp @@ -193,50 +193,6 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje bounding_box_.width, bounding_box_.length, nearest_corner_index, bbox_object, tracked_yaw, updating_object, tracking_offset_); - // UNCERTAINTY MODEL - if (!object.kinematics.has_position_covariance) { - // measurement noise covariance - auto r_cov_x = object_model_.measurement_covariance.pos_x; - auto r_cov_y = object_model_.measurement_covariance.pos_y; - const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (label == autoware_perception_msgs::msg::ObjectClassification::CAR) { - // if label is changed, enlarge the measurement noise covariance - constexpr float r_stddev_x = 2.0; // [m] - constexpr float r_stddev_y = 2.0; // [m] - r_cov_x = r_stddev_x * r_stddev_x; - r_cov_y = r_stddev_y * r_stddev_y; - } - - // yaw angle fix - const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const bool is_yaw_available = - object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; - - // fill covariance matrix - using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; - const double cos_yaw = std::cos(pose_yaw); - const double sin_yaw = std::sin(pose_yaw); - const double sin_2yaw = std::sin(2.0 * pose_yaw); - pose_cov[XYZRPY_COV_IDX::X_X] = - r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - pose_cov[XYZRPY_COV_IDX::Y_Y] = - r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x - pose_cov[XYZRPY_COV_IDX::X_YAW] = 0.0; // x - yaw - pose_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; // y - yaw - pose_cov[XYZRPY_COV_IDX::YAW_X] = 0.0; // yaw - x - pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y - pose_cov[XYZRPY_COV_IDX::YAW_YAW] = object_model_.measurement_covariance.yaw; // yaw - yaw - if (!is_yaw_available) { - pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value - } - auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; - twist_cov[XYZRPY_COV_IDX::X_X] = object_model_.measurement_covariance.vel_long; // vel - vel - } - return updating_object; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp index ef345692b32ca..a9260201722b3 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp @@ -195,50 +195,6 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO bounding_box_.width, bounding_box_.length, nearest_corner_index, bbox_object, tracked_yaw, updating_object, tracking_offset_); - // UNCERTAINTY MODEL - if (!object.kinematics.has_position_covariance) { - // measurement noise covariance - auto r_cov_x = object_model_.measurement_covariance.pos_x; - auto r_cov_y = object_model_.measurement_covariance.pos_y; - const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (utils::isLargeVehicleLabel(label)) { - // if label is changed, enlarge the measurement noise covariance - constexpr float r_stddev_x = 2.0; // [m] - constexpr float r_stddev_y = 2.0; // [m] - r_cov_x = r_stddev_x * r_stddev_x; - r_cov_y = r_stddev_y * r_stddev_y; - } - - // yaw angle fix - const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const bool is_yaw_available = - object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; - - // fill covariance matrix - using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; - const double cos_yaw = std::cos(pose_yaw); - const double sin_yaw = std::sin(pose_yaw); - const double sin_2yaw = std::sin(2.0 * pose_yaw); - pose_cov[XYZRPY_COV_IDX::X_X] = - r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - pose_cov[XYZRPY_COV_IDX::Y_Y] = - r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x - pose_cov[XYZRPY_COV_IDX::X_YAW] = 0.0; // x - yaw - pose_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; // y - yaw - pose_cov[XYZRPY_COV_IDX::YAW_X] = 0.0; // yaw - x - pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y - pose_cov[XYZRPY_COV_IDX::YAW_YAW] = object_model_.measurement_covariance.yaw; // yaw - yaw - if (!is_yaw_available) { - pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value - } - auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; - twist_cov[XYZRPY_COV_IDX::X_X] = object_model_.measurement_covariance.vel_long; // vel - vel - } - return updating_object; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index ee50c2e5449ed..591453a14f116 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -154,42 +154,6 @@ autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObje { autoware_perception_msgs::msg::DetectedObject updating_object = object; - // UNCERTAINTY MODEL - if (!object.kinematics.has_position_covariance) { - // measurement noise covariance - auto r_cov_x = object_model_.measurement_covariance.pos_x; - auto r_cov_y = object_model_.measurement_covariance.pos_y; - - // yaw angle fix - const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const bool is_yaw_available = - object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; - - // fill covariance matrix - using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; - const double cos_yaw = std::cos(pose_yaw); - const double sin_yaw = std::sin(pose_yaw); - const double sin_2yaw = std::sin(2.0 * pose_yaw); - pose_cov[XYZRPY_COV_IDX::X_X] = - r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - pose_cov[XYZRPY_COV_IDX::Y_Y] = - r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x - pose_cov[XYZRPY_COV_IDX::X_YAW] = 0.0; // x - yaw - pose_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; // y - yaw - pose_cov[XYZRPY_COV_IDX::YAW_X] = 0.0; // yaw - x - pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y - pose_cov[XYZRPY_COV_IDX::YAW_YAW] = object_model_.measurement_covariance.yaw; // yaw - yaw - if (!is_yaw_available) { - pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value - } - auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; - twist_cov[XYZRPY_COV_IDX::X_X] = object_model_.measurement_covariance.vel_long; // vel - vel - } - return updating_object; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index 215e55cb4ac62..3c088b8f64b39 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -38,40 +38,6 @@ using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; BicycleMotionModel::BicycleMotionModel() : logger_(rclcpp::get_logger("BicycleMotionModel")) { - // Initialize motion parameters - setDefaultParams(); -} - -void BicycleMotionModel::setDefaultParams() -{ - // set default motion parameters - constexpr double q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - constexpr double q_stddev_yaw_rate_min = - autoware::universe_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate - constexpr double q_stddev_yaw_rate_max = - autoware::universe_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate - constexpr double q_stddev_slip_rate_min = - autoware::universe_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate - constexpr double q_stddev_slip_rate_max = - autoware::universe_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate - constexpr double q_max_slip_angle = - autoware::universe_utils::deg2rad(30.0); // [rad] max slip angle - // extended state parameters - constexpr double lf_ratio = 0.3; // 30% front from the center - constexpr double lf_min = 1.0; // minimum of 1.0m - constexpr double lr_ratio = 0.25; // 25% rear from the center - constexpr double lr_min = 1.0; // minimum of 1.0m - setMotionParams( - q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, - q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, - lr_min); - - // set motion limitations - constexpr double max_vel = autoware::universe_utils::kmph2mps(100); // [m/s] maximum velocity - constexpr double max_slip = 30.0; // [deg] maximum slip angle - setMotionLimits(max_vel, max_slip); - // set prediction parameters constexpr double dt_max = 0.11; // [s] maximum time interval for prediction setMaxDeltaTime(dt_max); @@ -101,8 +67,8 @@ void BicycleMotionModel::setMotionParams( logger_, "BicycleMotionModel::setMotionParams: minimum wheel position should be greater than 0.01m."); } - motion_params_.lf_min = (lf_min < minimum_wheel_pos) ? minimum_wheel_pos : lf_min; - motion_params_.lr_min = (lr_min < minimum_wheel_pos) ? minimum_wheel_pos : lr_min; + motion_params_.lf_min = std::max(minimum_wheel_pos, lf_min); + motion_params_.lr_min = std::max(minimum_wheel_pos, lr_min); motion_params_.lf_ratio = lf_ratio; motion_params_.lr_ratio = lr_ratio; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp index bf5950bdd4023..a838bf62e5bcb 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp @@ -36,26 +36,6 @@ using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; CTRVMotionModel::CTRVMotionModel() : logger_(rclcpp::get_logger("CTRVMotionModel")) { - // Initialize motion parameters - setDefaultParams(); -} - -void CTRVMotionModel::setDefaultParams() -{ - // process noise covariance - constexpr double q_stddev_x = 0.5; // [m/s] - constexpr double q_stddev_y = 0.5; // [m/s] - constexpr double q_stddev_yaw = autoware::universe_utils::deg2rad(20); // [rad/s] - constexpr double q_stddev_vel = 9.8 * 0.3; // [m/(s*s)] - constexpr double q_stddev_wz = autoware::universe_utils::deg2rad(30); // [rad/(s*s)] - - setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vel, q_stddev_wz); - - // set motion limitations - constexpr double max_vel = autoware::universe_utils::kmph2mps(10); // [m/s] maximum velocity - constexpr double max_wz = 30.0; // [deg] maximum yaw rate - setMotionLimits(max_vel, max_wz); - // set prediction parameters constexpr double dt_max = 0.11; // [s] maximum time interval for prediction setMaxDeltaTime(dt_max); @@ -77,7 +57,7 @@ void CTRVMotionModel::setMotionLimits(const double & max_vel, const double & max { // set motion limitations motion_params_.max_vel = max_vel; - motion_params_.max_wz = autoware::universe_utils::deg2rad(max_wz); + motion_params_.max_wz = max_wz; } bool CTRVMotionModel::initialize( diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp index 2e2ba660a6e0d..e139419197d79 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp @@ -38,24 +38,6 @@ using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; CVMotionModel::CVMotionModel() : logger_(rclcpp::get_logger("CVMotionModel")) { - // Initialize motion parameters - setDefaultParams(); -} - -void CVMotionModel::setDefaultParams() -{ - // process noise covariance - constexpr double q_stddev_x = 0.5; // [m/s] standard deviation of x - constexpr double q_stddev_y = 0.5; // [m/s] standard deviation of y - constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] standard deviation of vx - constexpr double q_stddev_vy = 9.8 * 0.3; // [m/(s*s)] standard deviation of vy - setMotionParams(q_stddev_x, q_stddev_y, q_stddev_vx, q_stddev_vy); - - // set motion limitations - constexpr double max_vx = autoware::universe_utils::kmph2mps(60); // [m/s] maximum x velocity - constexpr double max_vy = autoware::universe_utils::kmph2mps(60); // [m/s] maximum y velocity - setMotionLimits(max_vx, max_vy); - // set prediction parameters constexpr double dt_max = 0.11; // [s] maximum time interval for prediction setMaxDeltaTime(dt_max); diff --git a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp new file mode 100644 index 0000000000000..6770b298bdbda --- /dev/null +++ b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp @@ -0,0 +1,139 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee + +#include "autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp" + +namespace autoware::multi_object_tracker +{ +namespace uncertainty +{ +using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + +object_model::StateCovariance covarianceFromObjectClass(const ObjectClassification & object_class) +{ + const auto & label = object_class.label; + ObjectModel obj_class_model(object_model::ObjectModelType::Unknown); + switch (label) { + case ObjectClassification::CAR: + obj_class_model = object_model::normal_vehicle; + break; + case ObjectClassification::BUS: + case ObjectClassification::TRUCK: + case ObjectClassification::TRAILER: + obj_class_model = object_model::big_vehicle; + break; + case ObjectClassification::BICYCLE: + case ObjectClassification::MOTORCYCLE: + obj_class_model = object_model::bicycle; + break; + case ObjectClassification::PEDESTRIAN: + obj_class_model = object_model::pedestrian; + break; + default: + obj_class_model = object_model::normal_vehicle; + break; + } + return obj_class_model.measurement_covariance; +} + +DetectedObject modelUncertaintyByClass( + const DetectedObject & object, const ObjectClassification & object_class) +{ + DetectedObject updating_object = object; + + // measurement noise covariance + const object_model::StateCovariance measurement_covariance = + covarianceFromObjectClass(object_class); + + const auto & r_cov_x = measurement_covariance.pos_x; + const auto & r_cov_y = measurement_covariance.pos_y; + + // yaw angle + const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + + // fill position covariance matrix + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + const double cos_yaw = std::cos(pose_yaw); + const double sin_yaw = std::sin(pose_yaw); + const double sin_2yaw = std::sin(2.0 * pose_yaw); + pose_cov[XYZRPY_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[XYZRPY_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x + pose_cov[XYZRPY_COV_IDX::X_YAW] = 0.0; // x - yaw + pose_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; // y - yaw + pose_cov[XYZRPY_COV_IDX::YAW_X] = 0.0; // yaw - x + pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = measurement_covariance.yaw; // yaw - yaw + const bool is_yaw_available = + object.kinematics.orientation_availability != + autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + if (!is_yaw_available) { + pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value + } + + // fill twist covariance matrix + auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; + twist_cov[XYZRPY_COV_IDX::X_X] = measurement_covariance.vel_long; + twist_cov[XYZRPY_COV_IDX::X_Y] = 0.0; + twist_cov[XYZRPY_COV_IDX::Y_X] = 0.0; + twist_cov[XYZRPY_COV_IDX::Y_Y] = measurement_covariance.vel_lat; + + return updating_object; +} + +DetectedObjects modelUncertainty(const DetectedObjects & detected_objects) +{ + DetectedObjects updating_objects; + updating_objects.header = detected_objects.header; + for (const auto & object : detected_objects.objects) { + if (object.kinematics.has_position_covariance) { + updating_objects.objects.push_back(object); + continue; + } + const ObjectClassification & object_class = + object_recognition_utils::getHighestProbClassification(object.classification); + updating_objects.objects.push_back(modelUncertaintyByClass(object, object_class)); + } + return updating_objects; +} + +void normalizeUncertainty(DetectedObjects & detected_objects) +{ + constexpr double min_cov_dist = 1e-4; + constexpr double min_cov_rad = 1e-6; + constexpr double min_cov_vel = 1e-4; + + for (auto & object : detected_objects.objects) { + // normalize position covariance + auto & pose_cov = object.kinematics.pose_with_covariance.covariance; + pose_cov[XYZRPY_COV_IDX::X_X] = std::max(pose_cov[XYZRPY_COV_IDX::X_X], min_cov_dist); + pose_cov[XYZRPY_COV_IDX::Y_Y] = std::max(pose_cov[XYZRPY_COV_IDX::Y_Y], min_cov_dist); + pose_cov[XYZRPY_COV_IDX::Z_Z] = std::max(pose_cov[XYZRPY_COV_IDX::Z_Z], min_cov_dist); + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = std::max(pose_cov[XYZRPY_COV_IDX::YAW_YAW], min_cov_rad); + + // normalize twist covariance + auto & twist_cov = object.kinematics.twist_with_covariance.covariance; + twist_cov[XYZRPY_COV_IDX::X_X] = std::max(twist_cov[XYZRPY_COV_IDX::X_X], min_cov_vel); + twist_cov[XYZRPY_COV_IDX::Y_Y] = std::max(twist_cov[XYZRPY_COV_IDX::Y_Y], min_cov_vel); + } +} + +} // namespace uncertainty +} // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index ce7f8157a31b6..e5ffddb31ccce 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -17,6 +17,7 @@ #include "multi_object_tracker_node.hpp" +#include "autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" #include @@ -260,11 +261,11 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list) } void MultiObjectTracker::runProcess( - const DetectedObjects & input_objects_msg, const uint & channel_index) + const DetectedObjects & input_objects, const uint & channel_index) { // Get the time of the measurement const rclcpp::Time measurement_time = - rclcpp::Time(input_objects_msg.header.stamp, this->now().get_clock_type()); + rclcpp::Time(input_objects.header.stamp, this->now().get_clock_type()); // Get the transform of the self frame const auto self_transform = @@ -273,10 +274,16 @@ void MultiObjectTracker::runProcess( return; } + // Model the object uncertainty if it is empty + DetectedObjects input_objects_with_uncertainty = uncertainty::modelUncertainty(input_objects); + + // Normalize the object uncertainty + uncertainty::normalizeUncertainty(input_objects_with_uncertainty); + // Transform the objects to the world frame DetectedObjects transformed_objects; if (!object_recognition_utils::transformObjects( - input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) { + input_objects_with_uncertainty, world_frame_id_, tf_buffer_, transformed_objects)) { return; } diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp index 3c23e3f066488..04d83ebd47acb 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp @@ -100,7 +100,7 @@ class MultiObjectTracker : public rclcpp::Node void onMessage(const ObjectsList & objects_list); // publish processes - void runProcess(const DetectedObjects & input_objects_msg, const uint & channel_index); + void runProcess(const DetectedObjects & input_objects, const uint & channel_index); void checkAndPublish(const rclcpp::Time & time); void publish(const rclcpp::Time & time) const; inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; From 8f09a112b778b5deadda8c3b2e6b1ca15bdf0f0c Mon Sep 17 00:00:00 2001 From: Ismet Atabay <56237550+ismetatabay@users.noreply.github.com> Date: Thu, 3 Oct 2024 04:58:24 +0300 Subject: [PATCH 88/95] fix(tier4_control_launch): fix aeb input predicted object topic name (#8874) fix aeb input predicted object topic Signed-off-by: ismetatabay --- launch/tier4_control_launch/launch/control.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/tier4_control_launch/launch/control.launch.xml b/launch/tier4_control_launch/launch/control.launch.xml index 0eda6faaf4e05..cc24f339ed404 100644 --- a/launch/tier4_control_launch/launch/control.launch.xml +++ b/launch/tier4_control_launch/launch/control.launch.xml @@ -192,7 +192,7 @@ - + From b401eba7503ea12f319423c85474cbffa8be1baa Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 3 Oct 2024 12:23:23 +0900 Subject: [PATCH 89/95] refactor(goal_planner): use the PullOverPath, PullOverPathCandidates copied from ThreadData to reduce access (#8994) Signed-off-by: Mamoru Sobue --- .../goal_planner_module.hpp | 31 ++- .../pull_over_planner_base.hpp | 3 +- .../src/goal_planner_module.cpp | 254 ++++++++++-------- 3 files changed, 168 insertions(+), 120 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index d029b2f5953a7..12d564237db3c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -110,15 +110,22 @@ struct PullOverContextData PullOverContextData() = delete; explicit PullOverContextData( const bool is_stable_safe_path, const PredictedObjects & static_objects, - const PredictedObjects & dynamic_objects) + const PredictedObjects & dynamic_objects, std::optional && pull_over_path_opt, + const std::vector & pull_over_path_candidates) : is_stable_safe_path(is_stable_safe_path), static_target_objects(static_objects), - dynamic_target_objects(dynamic_objects) + dynamic_target_objects(dynamic_objects), + pull_over_path_opt(pull_over_path_opt), + pull_over_path_candidates(pull_over_path_candidates) { } const bool is_stable_safe_path; const PredictedObjects static_target_objects; const PredictedObjects dynamic_target_objects; + // TODO(soblin): due to deceleratePath(), this cannot be const member(velocity is modified by it) + std::optional pull_over_path_opt; + const std::vector pull_over_path_candidates; + // TODO(soblin): goal_candidate_from_thread, modifed_goal_pose_from_thread, closest_start_pose }; class GoalPlannerModule : public SceneModuleInterface @@ -354,7 +361,7 @@ class GoalPlannerModule : public SceneModuleInterface void decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const; void decelerateBeforeSearchStart( const Pose & search_start_offset_pose, PathWithLaneId & path) const; - PathWithLaneId generateStopPath() const; + PathWithLaneId generateStopPath(const PullOverContextData & context_data) const; PathWithLaneId generateFeasibleStopPath(const PathWithLaneId & path) const; void keepStoppedWithCurrentPath(PathWithLaneId & path) const; @@ -364,7 +371,7 @@ class GoalPlannerModule : public SceneModuleInterface bool isStopped(); bool isStopped( std::deque & odometry_buffer, const double time); - bool hasFinishedCurrentPath(); + bool hasFinishedCurrentPath(const PullOverContextData & ctx_data); bool isOnModifiedGoal(const Pose & current_pose, const GoalPlannerParameters & parameters) const; double calcModuleRequestLength() const; bool needPathUpdate( @@ -406,16 +413,18 @@ class GoalPlannerModule : public SceneModuleInterface // lanes and drivable area std::vector generateDrivableLanes() const; - void setDrivableAreaInfo(BehaviorModuleOutput & output) const; + void setDrivableAreaInfo( + const PullOverContextData & context_data, BehaviorModuleOutput & output) const; // output setter void setOutput(const PullOverContextData & context_data, BehaviorModuleOutput & output); - void setModifiedGoal(BehaviorModuleOutput & output) const; - void setTurnSignalInfo(BehaviorModuleOutput & output); + void setModifiedGoal( + const PullOverContextData & context_data, BehaviorModuleOutput & output) const; + void setTurnSignalInfo(const PullOverContextData & context_data, BehaviorModuleOutput & output); // new turn signal - TurnSignalInfo calcTurnSignalInfo(); + TurnSignalInfo calcTurnSignalInfo(const PullOverContextData & context_data); std::optional ignore_signal_{std::nullopt}; bool hasPreviousModulePathShapeChanged(const BehaviorModuleOutput & previous_module_output) const; @@ -431,10 +440,12 @@ class GoalPlannerModule : public SceneModuleInterface // steering factor void updateSteeringFactor( - const std::array & pose, const std::array distance, const uint16_t type); + const PullOverContextData & context_data, const std::array & pose, + const std::array distance, const uint16_t type); // rtc - std::pair calcDistanceToPathChange() const; + std::pair calcDistanceToPathChange( + const PullOverContextData & context_data) const; // safety check void initializeSafetyCheckParameters(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp index d952f8ddbd22d..47367164b2517 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp @@ -47,6 +47,7 @@ struct PullOverPath const std::vector> & pairs_terminal_velocity_and_accel); PullOverPath(const PullOverPath & other); + PullOverPath & operator=(const PullOverPath & other) = default; PullOverPlannerType type() const { return type_; } size_t goal_id() const { return goal_id_; } @@ -58,7 +59,7 @@ struct PullOverPath const std::vector & partial_paths() const { return partial_paths_; } // TODO(soblin): use reference to avoid copy once thread-safe design is finished - PathWithLaneId full_path() const { return full_path_; } + const PathWithLaneId & full_path() const { return full_path_; } PathWithLaneId parking_path() const { return parking_path_; } std::vector full_path_curvatures() { return full_path_curvatures_; } std::vector parking_path_curvatures() const { return parking_path_curvatures_; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 147fe9f79dba0..55cbd4c0e29d5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -540,21 +540,24 @@ void GoalPlannerModule::updateData() path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); const bool found_pull_over_path = thread_safe_data_.foundPullOverPath(); - const std::optional pull_over_path = + std::optional pull_over_path_recv = found_pull_over_path ? std::make_optional(*thread_safe_data_.get_pull_over_path()) : std::nullopt; const bool is_current_safe = isSafePath( - planner_data_, found_pull_over_path, pull_over_path, *parameters_, ego_predicted_path_params_, - objects_filtering_params_, safety_check_params_); + planner_data_, found_pull_over_path, pull_over_path_recv, *parameters_, + ego_predicted_path_params_, objects_filtering_params_, safety_check_params_); path_decision_controller_.transit_state( found_pull_over_path, clock_->now(), static_target_objects, dynamic_target_objects, thread_safe_data_.get_modified_goal_pose(), planner_data_, occupancy_grid_map_, is_current_safe, - *parameters_, goal_searcher_, isActivated(), pull_over_path, debug_data_.ego_polygons_expanded); + *parameters_, goal_searcher_, isActivated(), pull_over_path_recv, + debug_data_.ego_polygons_expanded); context_data_.emplace( path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, - dynamic_target_objects); + dynamic_target_objects, std::move(pull_over_path_recv), + thread_safe_data_.get_pull_over_path_candidates()); + const auto & ctx_data = context_data_.value(); // update goal searcher and generate goal candidates if (thread_safe_data_.get_goal_candidates().empty()) { @@ -569,7 +572,7 @@ void GoalPlannerModule::updateData() return; } - if (hasFinishedCurrentPath()) { + if (hasFinishedCurrentPath(ctx_data)) { thread_safe_data_.incrementPathIndex(); } @@ -891,15 +894,13 @@ BehaviorModuleOutput GoalPlannerModule::plan() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (!context_data_) { - RCLCPP_ERROR(getLogger(), " [pull_over] plan() is called without valid context_data"); - } - const auto context_data = context_data_.has_value() - ? context_data_.value() - : PullOverContextData(true, PredictedObjects{}, PredictedObjects{}); - if (utils::isAllowedGoalModification(planner_data_->route_handler)) { - return planPullOver(context_data); + if (!context_data_) { + RCLCPP_ERROR(getLogger(), " [pull_over] plan() is called without valid context_data"); + } else { + const auto & context_data = context_data_.value(); + return planPullOver(context_data); + } } fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); @@ -1157,23 +1158,23 @@ void GoalPlannerModule::setOutput( output.reference_path = getPreviousModuleOutput().reference_path; - if (!thread_safe_data_.foundPullOverPath()) { + if (!context_data.pull_over_path_opt) { // situation : not safe against static objects use stop_path - output.path = generateStopPath(); + output.path = generateStopPath(context_data); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); - setDrivableAreaInfo(output); + setDrivableAreaInfo(context_data, output); return; } + const auto & pull_over_path = context_data.pull_over_path_opt.value(); if ( parameters_->safety_check_params.enable_safety_check && !context_data.is_stable_safe_path && isActivated()) { // situation : not safe against dynamic objects after approval // insert stop point in current path if ego is able to stop with acceleration and jerk // constraints - output.path = - generateFeasibleStopPath(thread_safe_data_.get_pull_over_path()->getCurrentPath()); + output.path = generateFeasibleStopPath(pull_over_path.getCurrentPath()); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not safe against dynamic objects, generate stop path"); debug_stop_pose_with_info_.set(std::string("feasible stop after approval")); @@ -1182,29 +1183,31 @@ void GoalPlannerModule::setOutput( // before approval) don't stop // keep stop if not enough time passed, // because it takes time for the trajectory to be reflected - auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); + auto current_path = pull_over_path.getCurrentPath(); keepStoppedWithCurrentPath(current_path); output.path = current_path; } - setModifiedGoal(output); - setDrivableAreaInfo(output); + setModifiedGoal(context_data, output); + setDrivableAreaInfo(context_data, output); // set hazard and turn signal if ( path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::DECIDED && isActivated()) { - setTurnSignalInfo(output); + setTurnSignalInfo(context_data, output); } } -void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const +void GoalPlannerModule::setDrivableAreaInfo( + const PullOverContextData & context_data, BehaviorModuleOutput & output) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto planner_type_opt = thread_safe_data_.getPullOverPlannerType(); - if (planner_type_opt && planner_type_opt.value() == PullOverPlannerType::FREESPACE) { + if ( + context_data.pull_over_path_opt && + context_data.pull_over_path_opt.value().type() == PullOverPlannerType::FREESPACE) { const double drivable_area_margin = planner_data_->parameters.vehicle_width; output.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; @@ -1219,10 +1222,11 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const } } -void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const +void GoalPlannerModule::setModifiedGoal( + const PullOverContextData & context_data, BehaviorModuleOutput & output) const { const auto & route_handler = planner_data_->route_handler; - if (thread_safe_data_.foundPullOverPath()) { + if (context_data.pull_over_path_opt) { PoseWithUuidStamped modified_goal{}; modified_goal.uuid = route_handler->getRouteUuid(); modified_goal.pose = thread_safe_data_.get_modified_goal_pose()->goal_pose; @@ -1233,10 +1237,11 @@ void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const } } -void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) +void GoalPlannerModule::setTurnSignalInfo( + const PullOverContextData & context_data, BehaviorModuleOutput & output) { const auto original_signal = getPreviousModuleOutput().turn_signal_info; - const auto new_signal = calcTurnSignalInfo(); + const auto new_signal = calcTurnSignalInfo(context_data); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, original_signal, new_signal, @@ -1245,10 +1250,11 @@ void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) } void GoalPlannerModule::updateSteeringFactor( - const std::array & pose, const std::array distance, const uint16_t type) + const PullOverContextData & context_data, const std::array & pose, + const std::array distance, const uint16_t type) { - const uint16_t steering_factor_direction = std::invoke([this]() { - const auto turn_signal = calcTurnSignalInfo(); + const uint16_t steering_factor_direction = std::invoke([&]() { + const auto turn_signal = calcTurnSignalInfo(context_data); if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { return SteeringFactor::LEFT; } else if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { @@ -1268,6 +1274,7 @@ void GoalPlannerModule::decideVelocity() const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; // partial_paths + // TODO(soblin): only update velocity on main thread side, use that on main thread side auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths().front(); const auto vel = static_cast(std::max(current_vel, parameters_->pull_over_minimum_velocity)); @@ -1295,14 +1302,14 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate( universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // if pull over path candidates generation is not finished, use previous module output - if (thread_safe_data_.get_pull_over_path_candidates().empty()) { + if (context_data.pull_over_path_candidates.empty()) { return getPreviousModuleOutput(); } BehaviorModuleOutput output{}; const BehaviorModuleOutput pull_over_output = planPullOverAsOutput(context_data); output.modified_goal = pull_over_output.modified_goal; - output.path = generateStopPath(); + output.path = generateStopPath(context_data); output.reference_path = getPreviousModuleOutput().reference_path; const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( @@ -1313,7 +1320,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate( output.drivable_area_info = utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - if (!thread_safe_data_.foundPullOverPath()) { + if (!context_data.pull_over_path_opt) { return output; } @@ -1331,13 +1338,19 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( start = std::chrono::system_clock::now(); // if pull over path candidates generation is not finished, use previous module output - if (thread_safe_data_.get_pull_over_path_candidates().empty()) { + if (context_data.pull_over_path_candidates.empty()) { return getPreviousModuleOutput(); } - const auto planner_type_opt = thread_safe_data_.getPullOverPlannerType(); + auto context_data_with_velocity = context_data; + /** + NOTE(soblin): this path originates from the previously selected(by main thread) pull_over_path + which was originally generated by either road_parking or freespace thread + */ + auto & pull_over_path_with_velocity_opt = context_data_with_velocity.pull_over_path_opt; const bool is_freespace = - planner_type_opt && planner_type_opt.value() == PullOverPlannerType::FREESPACE; + pull_over_path_with_velocity_opt && + pull_over_path_with_velocity_opt.value().type() == PullOverPlannerType::FREESPACE; if ( path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::NOT_DECIDED && @@ -1354,19 +1367,34 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( // update goal candidates auto goal_candidates = thread_safe_data_.get_goal_candidates(); goal_searcher_->update( - goal_candidates, occupancy_grid_map_, planner_data_, - thread_safe_data_.get_static_target_objects()); + goal_candidates, occupancy_grid_map_, planner_data_, context_data.static_target_objects); // Select a path that is as safe as possible and has a high priority. - const auto pull_over_path_candidates = thread_safe_data_.get_pull_over_path_candidates(); + const auto & pull_over_path_candidates = context_data.pull_over_path_candidates; const auto path_and_goal_opt = selectPullOverPath(context_data, pull_over_path_candidates, goal_candidates); // update thread_safe_data_ if (path_and_goal_opt) { auto [pull_over_path, modified_goal] = *path_and_goal_opt; - deceleratePath(pull_over_path); + /** TODO(soblin): since thread_safe_data::pull_over_path was used as a global variable, old + * code was setting deceleration to thread_safe_data::pull_over_path and setOutput() accessed + * to the velocity profile in thread_safe_data::pull_over_path, which is a very bad usage of + * member variable + * + * set this selected pull_over_path to ThreadSafeData, but actually RoadParking thread does + * not use pull_over_path, but only FreespaceParking thread use this selected pull_over_path. + * As the next action item, only set this selected pull_over_path to only + * FreespaceThreadSafeData. + */ thread_safe_data_.set(goal_candidates, pull_over_path, modified_goal); + if (pull_over_path_with_velocity_opt) { + auto & pull_over_path_with_velocity = pull_over_path_with_velocity_opt.value(); + // copy the path for later setOutput() + pull_over_path_with_velocity = pull_over_path; + // modify the velocity for latest setOutput() + deceleratePath(pull_over_path_with_velocity); + } RCLCPP_DEBUG( getLogger(), "selected pull over path: path_id: %ld, goal_id: %ld", pull_over_path.id(), modified_goal.id); @@ -1377,27 +1405,27 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( // set output and status BehaviorModuleOutput output{}; - setOutput(context_data, output); + setOutput(context_data_with_velocity, output); // return to lane parking if it is possible - if (is_freespace && canReturnToLaneParking(context_data)) { + if (is_freespace && canReturnToLaneParking(context_data_with_velocity)) { thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path()); } // For debug - setDebugData(context_data); + setDebugData(context_data_with_velocity); if (parameters_->print_debug_info) { // For evaluations printParkingPositionError(); } - if (!thread_safe_data_.foundPullOverPath()) { + if (!pull_over_path_with_velocity_opt) { return output; } path_candidate_ = - std::make_shared(thread_safe_data_.get_pull_over_path()->full_path()); + std::make_shared(pull_over_path_with_velocity_opt.value().full_path()); return output; } @@ -1409,63 +1437,66 @@ void GoalPlannerModule::postProcess() if (!context_data_) { RCLCPP_ERROR(getLogger(), " [pull_over] postProcess() is called without valid context_data"); } - const auto context_data = context_data_.has_value() - ? context_data_.value() - : PullOverContextData(true, PredictedObjects{}, PredictedObjects{}); + const auto context_data_dummy = + PullOverContextData(true, PredictedObjects{}, PredictedObjects{}, std::nullopt, {}); + const auto & context_data = + context_data_.has_value() ? context_data_.value() : context_data_dummy; const bool has_decided_path = path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::DECIDED; - context_data_ = std::nullopt; - - if (!thread_safe_data_.foundPullOverPath()) { + if (!context_data.pull_over_path_opt) { + context_data_ = std::nullopt; return; } + const auto & pull_over_path = context_data.pull_over_path_opt.value(); - const auto distance_to_path_change = calcDistanceToPathChange(); + const auto distance_to_path_change = calcDistanceToPathChange(context_data); if (has_decided_path) { updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); } updateSteeringFactor( - {thread_safe_data_.get_pull_over_path()->start_pose(), - thread_safe_data_.get_modified_goal_pose()->goal_pose}, + context_data, + {pull_over_path.start_pose(), thread_safe_data_.get_modified_goal_pose()->goal_pose}, {distance_to_path_change.first, distance_to_path_change.second}, has_decided_path ? SteeringFactor::TURNING : SteeringFactor::APPROACHING); - setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->full_path()); + setStopReason(StopReason::GOAL_PLANNER, pull_over_path.full_path()); + + context_data_ = std::nullopt; } BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (!context_data_) { - RCLCPP_ERROR( - getLogger(), " [pull_over] planWaitingApproval() is called without valid context_data"); - } - const auto context_data = context_data_.has_value() - ? context_data_.value() - : PullOverContextData(true, PredictedObjects{}, PredictedObjects{}); - if (utils::isAllowedGoalModification(planner_data_->route_handler)) { - return planPullOverAsCandidate(context_data); + if (!context_data_) { + RCLCPP_ERROR( + getLogger(), " [pull_over] planWaitingApproval() is called without valid context_data"); + } else { + const auto & context_data = context_data_.value(); + return planPullOverAsCandidate(context_data); + } } fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); return fixed_goal_planner_->plan(planner_data_); } -std::pair GoalPlannerModule::calcDistanceToPathChange() const +std::pair GoalPlannerModule::calcDistanceToPathChange( + const PullOverContextData & context_data) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (!thread_safe_data_.foundPullOverPath()) { + if (!context_data.pull_over_path_opt) { return {std::numeric_limits::max(), std::numeric_limits::max()}; } + const auto & pull_over_path = context_data.pull_over_path_opt.value(); - const auto full_path = thread_safe_data_.get_pull_over_path()->full_path(); + const auto & full_path = context_data.pull_over_path_opt.value().full_path(); const auto ego_segment_idx = autoware::motion_utils::findNearestSegmentIndex( full_path.points, planner_data_->self_odometry->pose.pose, std::numeric_limits::max(), @@ -1475,10 +1506,10 @@ std::pair GoalPlannerModule::calcDistanceToPathChange() const } const size_t start_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( - full_path.points, thread_safe_data_.get_pull_over_path()->start_pose().position); + full_path.points, pull_over_path.start_pose().position); const double dist_to_parking_start_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, - thread_safe_data_.get_pull_over_path()->start_pose().position, start_pose_segment_idx); + pull_over_path.start_pose().position, start_pose_segment_idx); const size_t goal_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( full_path.points, thread_safe_data_.get_modified_goal_pose()->goal_pose.position); const double dist_to_parking_finish_pose = calcSignedArcLength( @@ -1493,7 +1524,7 @@ void GoalPlannerModule::setParameters(const std::shared_ptr std::optional> { - if (thread_safe_data_.foundPullOverPath()) { + if (context_data.pull_over_path_opt) { return std::make_pair( - thread_safe_data_.get_pull_over_path()->start_pose(), "stop at selected start pose"); + context_data.pull_over_path_opt.value().start_pose(), "stop at selected start pose"); } if (thread_safe_data_.get_closest_start_pose()) { return std::make_pair( @@ -1710,7 +1741,7 @@ bool GoalPlannerModule::isStuck( return false; } -bool GoalPlannerModule::hasFinishedCurrentPath() +bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1744,8 +1775,11 @@ bool GoalPlannerModule::hasFinishedCurrentPath() } // check if self pose is near the end of current path - const auto current_path_end = - thread_safe_data_.get_pull_over_path()->getCurrentPath().points.back(); + if (!ctx_data.pull_over_path_opt) { + return false; + } + const auto & current_path_end = + ctx_data.pull_over_path_opt.value().getCurrentPath().points.back(); const auto & self_pose = planner_data_->self_odometry->pose.pose; return autoware::universe_utils::calcDistance2d(current_path_end, self_pose) < parameters_->th_arrived_distance; @@ -1762,16 +1796,21 @@ bool GoalPlannerModule::isOnModifiedGoal( parameters.th_arrived_distance; } -TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() +TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo(const PullOverContextData & context_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto path = thread_safe_data_.get_pull_over_path()->full_path(); + if (!context_data.pull_over_path_opt) { + return {}; + } + const auto & pull_over_path = context_data.pull_over_path_opt.value(); + + const auto & path = pull_over_path.full_path(); if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info; const auto & current_pose = planner_data_->self_odometry->pose.pose; - const auto & start_pose = thread_safe_data_.get_pull_over_path()->start_pose(); - const auto & end_pose = thread_safe_data_.get_pull_over_path()->end_pose(); + const auto & start_pose = pull_over_path.start_pose(); + const auto & end_pose = pull_over_path.end_pose(); const auto shift_start_idx = autoware::motion_utils::findNearestIndex(path.points, start_pose.position); @@ -1815,13 +1854,11 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() constexpr bool is_lane_change = false; constexpr bool is_pull_over = true; const bool override_ego_stopped_check = std::invoke([&]() { - const auto planner_type_opt = thread_safe_data_.getPullOverPlannerType(); - if (planner_type_opt && planner_type_opt.value() == PullOverPlannerType::SHIFT) { + if (pull_over_path.type() == PullOverPlannerType::SHIFT) { return false; } constexpr double distance_threshold = 1.0; - const auto stop_point = - thread_safe_data_.get_pull_over_path()->partial_paths().front().points.back(); + const auto stop_point = pull_over_path.partial_paths().front().points.back(); const double distance_from_ego_to_stop_point = std::abs(autoware::motion_utils::calcSignedArcLength( path.points, stop_point.point.pose.position, current_pose.position)); @@ -2352,20 +2389,17 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) } // Visualize path and related pose - if (thread_safe_data_.foundPullOverPath()) { - add(createPoseMarkerArray( - thread_safe_data_.get_pull_over_path()->start_pose(), "pull_over_start_pose", 0, 0.3, 0.3, - 0.9)); - add(createPoseMarkerArray( - thread_safe_data_.get_pull_over_path()->end_pose(), "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); - add(createPathMarkerArray( - thread_safe_data_.get_pull_over_path()->full_path(), "full_path", 0, 0.0, 0.5, 0.9)); - add(createPathMarkerArray( - thread_safe_data_.get_pull_over_path()->getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0)); + if (context_data.pull_over_path_opt) { + const auto & pull_over_path = context_data.pull_over_path_opt.value(); + add( + createPoseMarkerArray(pull_over_path.start_pose(), "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); + add(createPoseMarkerArray(pull_over_path.end_pose(), "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); + add(createPathMarkerArray(pull_over_path.full_path(), "full_path", 0, 0.0, 0.5, 0.9)); + add(createPathMarkerArray(pull_over_path.getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0)); // visualize each partial path - for (size_t i = 0; i < thread_safe_data_.get_pull_over_path()->partial_paths().size(); ++i) { - const auto & partial_path = thread_safe_data_.get_pull_over_path()->partial_paths().at(i); + for (size_t i = 0; i < context_data.pull_over_path_opt.value().partial_paths().size(); ++i) { + const auto & partial_path = context_data.pull_over_path_opt.value().partial_paths().at(i); add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } @@ -2398,7 +2432,7 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) } // Visualize debug poses - const auto & debug_poses = thread_safe_data_.get_pull_over_path()->debug_poses; + const auto & debug_poses = pull_over_path.debug_poses; for (size_t i = 0; i < debug_poses.size(); ++i) { add(createPoseMarkerArray( debug_poses.at(i), "debug_pose_" + std::to_string(i), 0, 0.3, 0.3, 0.3)); @@ -2460,28 +2494,30 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) // Visualize planner type text { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = thread_safe_data_.foundPullOverPath() - ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = context_data.pull_over_path_opt ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); marker.pose = thread_safe_data_.get_modified_goal_pose() ? thread_safe_data_.get_modified_goal_pose()->goal_pose : planner_data_->self_odometry->pose.pose; - const auto planner_type_opt = thread_safe_data_.getPullOverPlannerType(); - if (planner_type_opt) { - marker.text = magic_enum::enum_name(planner_type_opt.value()); - marker.text += - " " + std::to_string(thread_safe_data_.get_pull_over_path()->path_idx()) + "/" + - std::to_string(thread_safe_data_.get_pull_over_path()->partial_paths().size() - 1); + if (context_data.pull_over_path_opt) { + const auto & pull_over_path = context_data.pull_over_path_opt.value(); + marker.text = magic_enum::enum_name(pull_over_path.type()); + marker.text += " " + std::to_string(pull_over_path.path_idx()) + "/" + + std::to_string(pull_over_path.partial_paths().size() - 1); } + /* + TODO(soblin): disable until thread safe design is done if (isStuck( context_data.static_target_objects, context_data.dynamic_target_objects, planner_data_, occupancy_grid_map_, *parameters_)) { marker.text += " stuck"; - } else if (isStopped()) { + } + */ + if (isStopped()) { marker.text += " stopped"; } From 01d9d6735964a7428593b7a4e9285e30e79d8164 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 3 Oct 2024 19:23:24 +0900 Subject: [PATCH 90/95] ci(codecov): update planning target component (#9021) Signed-off-by: Mamoru Sobue --- codecov.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/codecov.yaml b/codecov.yaml index 75fc6f8bacadd..0341bdfda9c56 100644 --- a/codecov.yaml +++ b/codecov.yaml @@ -72,13 +72,13 @@ component_management: ##### behavior_velocity_planner ##### - planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/** - planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/** - # - planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/** - planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/** # - planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/** - planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/** # - planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/** - planning/behavior_velocity_planner/autoware_behavior_velocity_planner/** - # - planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/** - planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/** # - planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/** - planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/** From 6a379e95b9aaff04e5803eb40f53887d8f666a97 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Fri, 4 Oct 2024 08:46:48 +0900 Subject: [PATCH 91/95] refactor(lane_change): refactor code using transient data (#8997) * add target lane length and ego arc length along current and target lanes to transient data Signed-off-by: mohammad alqudah * refactor code using transient data Signed-off-by: mohammad alqudah * refactor get_lane_change_paths function Signed-off-by: mohammad alqudah * minor refactoring Signed-off-by: mohammad alqudah * refactor util functions Signed-off-by: mohammad alqudah * refactor getPrepareSegment function Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah --- .../scene.hpp | 15 +- .../utils/base_class.hpp | 5 +- .../utils/data_structs.hpp | 5 + .../utils/utils.hpp | 16 +- .../src/scene.cpp | 291 +++++++++--------- .../src/utils/utils.cpp | 33 +- 6 files changed, 184 insertions(+), 181 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index f76d776150875..68ff887e65522 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -142,9 +142,8 @@ class NormalLaneChange : public LaneChangeBase FilteredByLanesObjects filterObjectsByLanelets( const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const; - PathWithLaneId getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double backward_path_length, - const double prepare_length) const override; + bool get_prepare_segment( + PathWithLaneId & prepare_segment, const double prepare_length) const override; PathWithLaneId getTargetSegment( const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, @@ -155,17 +154,21 @@ class NormalLaneChange : public LaneChangeBase const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const; + std::vector get_prepare_metrics() const; + std::vector get_lane_changing_metrics( + const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metrics, + const double shift_length, const double dist_to_reg_element) const; + bool get_lane_change_paths(LaneChangePaths & candidate_paths) const; LaneChangePath get_candidate_path( const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, - const Pose & lc_start_pose, const double target_lane_length, const double shift_length, - const double next_lc_buffer, const bool is_goal_in_route) const; + const Pose & lc_start_pose, const double shift_length) const; bool check_candidate_path_safety( const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, - const double lane_change_buffer, const bool is_stuck) const; + const bool is_stuck) const; std::optional calcTerminalLaneChangePath( const lanelet::ConstLanelets & current_lanes, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index 72a40caca1d6a..580c5709cb5c1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -238,9 +238,8 @@ class LaneChangeBase protected: virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0; - virtual PathWithLaneId getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double backward_path_length, - const double prepare_length) const = 0; + virtual bool get_prepare_segment( + PathWithLaneId & prepare_segment, const double prepare_length) const = 0; virtual bool isValidPath(const PathWithLaneId & path) const = 0; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index fc5e78e44b77f..4cdd3d1cbad3e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -338,6 +338,11 @@ struct TransientData double max_prepare_length{ std::numeric_limits::max()}; // maximum prepare length, starting from ego's base link + double target_lane_length{std::numeric_limits::min()}; + + lanelet::ArcCoordinates current_lanes_ego_arc; // arc coordinates of ego pose along current lanes + lanelet::ArcCoordinates target_lanes_ego_arc; // arc coordinates of ego pose along target lanes + bool is_ego_near_current_terminal_start{false}; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 363fa970f54c4..8103434e34a61 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -79,9 +79,7 @@ std::vector replaceWithSortedIds( const std::vector & original_lane_ids, const std::vector> & sorted_lane_ids); -std::vector> getSortedLaneIds( - const RouteHandler & route_handler, const Pose & current_pose, - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); +std::vector> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr); lanelet::ConstLanelets getTargetNeighborLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, @@ -104,12 +102,9 @@ ShiftLine get_lane_changing_shift_line( const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, const PathWithLaneId & reference_path, const double shift_length); -PathWithLaneId getReferencePathFromTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & lane_changing_start_pose, const double target_lane_length, - const double lane_changing_length, const double forward_path_length, - const double resample_interval, const bool is_goal_in_route, - const double next_lane_change_buffer); +PathWithLaneId get_reference_path_from_target_Lane( + const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, + const double lane_changing_length, const double resample_interval); std::vector generateDrivableLanes( const std::vector & original_drivable_lanes, const RouteHandler & route_handler, @@ -145,8 +140,7 @@ bool isParkedObject( bool passed_parked_objects( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, const double minimum_lane_change_length, - CollisionCheckDebugMap & object_debug); + const std::vector & objects, CollisionCheckDebugMap & object_debug); std::optional getLeadingStaticObjectIdx( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index a91aaba86bb1a..2bfde9d400aeb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -129,6 +129,15 @@ void NormalLaneChange::update_transient_data() transient_data.max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); + transient_data.target_lane_length = + lanelet::utils::getLaneletLength2d(common_data_ptr_->lanes_ptr->target); + + transient_data.current_lanes_ego_arc = lanelet::utils::getArcCoordinates( + common_data_ptr_->lanes_ptr->current, common_data_ptr_->get_ego_pose()); + + transient_data.target_lanes_ego_arc = lanelet::utils::getArcCoordinates( + common_data_ptr_->lanes_ptr->target, common_data_ptr_->get_ego_pose()); + transient_data.is_ego_near_current_terminal_start = transient_data.dist_to_terminal_start < transient_data.max_prepare_length; @@ -166,8 +175,7 @@ void NormalLaneChange::updateLaneChangeStatus() status_.is_valid_path = found_valid_path; status_.is_safe = found_safe_path; - const auto arclength_start = lanelet::utils::getArcCoordinates(get_target_lanes(), getEgoPose()); - status_.start_distance = arclength_start.length; + status_.start_distance = common_data_ptr_->transient_data.target_lanes_ego_arc.length; status_.lane_change_path.path.header = getRouteHeader(); } @@ -233,10 +241,7 @@ bool NormalLaneChange::is_near_regulatory_element() const { if (!common_data_ptr_ || !common_data_ptr_->is_data_available()) return false; - const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - const auto dist_to_terminal_start = common_data_ptr_->transient_data.dist_to_terminal_start; - - if (dist_to_terminal_start <= max_prepare_length) return false; + if (common_data_ptr_->transient_data.is_ego_near_current_terminal_start) return false; const bool only_tl = getStopTime() >= lane_change_parameters_->stop_time_threshold; @@ -244,8 +249,9 @@ bool NormalLaneChange::is_near_regulatory_element() const RCLCPP_DEBUG(logger_, "Stop time is over threshold. Ignore crosswalk and intersection checks."); } - return max_prepare_length > utils::lane_change::get_distance_to_next_regulatory_element( - common_data_ptr_, only_tl, only_tl); + return common_data_ptr_->transient_data.max_prepare_length > + utils::lane_change::get_distance_to_next_regulatory_element( + common_data_ptr_, only_tl, only_tl); } bool NormalLaneChange::isStoppedAtRedTrafficLight() const @@ -593,12 +599,13 @@ std::optional NormalLaneChange::extendPath() } auto & target_lanes = common_data_ptr_->lanes_ptr->target; - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); - const auto dist_in_target = lanelet::utils::getArcCoordinates(target_lanes, getEgoPose()); + const auto & transient_data = common_data_ptr_->transient_data; const auto forward_path_length = getCommonParam().forward_path_length; - if ((target_lane_length - dist_in_target.length) > forward_path_length) { + if ( + (transient_data.target_lane_length - transient_data.target_lanes_ego_arc.length) > + forward_path_length) { return std::nullopt; } @@ -646,6 +653,7 @@ std::optional NormalLaneChange::extendPath() return getRouteHandler()->getCenterLinePath( target_lanes, dist_to_end_of_path, dist_to_target_pose); } + void NormalLaneChange::resetParameters() { is_abort_path_approved_ = false; @@ -662,11 +670,10 @@ void NormalLaneChange::resetParameters() TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto & pose = getEgoPose(); const auto & current_lanes = get_current_lanes(); const auto & shift_line = status_.lane_change_path.info.shift_line; const auto & shift_path = status_.lane_change_path.shifted_path; - const auto current_shift_length = lanelet::utils::getArcCoordinates(current_lanes, pose).distance; + const auto current_shift_length = common_data_ptr_->transient_data.current_lanes_ego_arc.distance; constexpr bool is_driving_forward = true; // The getBehaviorTurnSignalInfo method expects the shifted line to be generated off of the ego's // current lane, lane change is different, so we set this flag to false. @@ -789,7 +796,7 @@ bool NormalLaneChange::hasFinishedLaneChange() const return false; } - const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); + const auto & arc_length = common_data_ptr_->transient_data.target_lanes_ego_arc; const auto reach_target_lane = std::abs(arc_length.distance) < lane_change_parameters_->finish_judge_lateral_threshold; @@ -1037,21 +1044,41 @@ std::vector NormalLaneChange::calcPrepareDuration( return prepare_durations; } -PathWithLaneId NormalLaneChange::getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double backward_path_length, - const double prepare_length) const +bool NormalLaneChange::get_prepare_segment( + PathWithLaneId & prepare_segment, const double prepare_length) const { - if (current_lanes.empty()) { - return PathWithLaneId(); + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; + const auto backward_path_length = common_data_ptr_->bpp_param_ptr->backward_path_length; + + if (current_lanes.empty() || target_lanes.empty()) { + return false; } - auto prepare_segment = prev_module_output_.path; + prepare_segment = prev_module_output_.path; const size_t current_seg_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( prepare_segment.points, getEgoPose(), 3.0, 1.0); utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length); - return prepare_segment; + if (prepare_segment.points.empty()) return false; + + const auto & lc_start_pose = prepare_segment.points.back().point.pose; + + // TODO(Quda, Azu): Is it possible to remove these checks if we ensure prepare segment length is + // larger than distance to target lane start + if (!is_valid_start_point(common_data_ptr_, lc_start_pose)) return false; + + // lane changing start is at the end of prepare segment + const auto target_length_from_lane_change_start_pose = + utils::getArcLengthToTargetLanelet(current_lanes, target_lanes.front(), lc_start_pose); + + // Check if the lane changing start point is not on the lanes next to target lanes, + if (target_length_from_lane_change_start_pose > std::numeric_limits::epsilon()) { + throw std::logic_error("lane change start is behind target lanelet!"); + } + + return true; } lane_change::TargetObjects NormalLaneChange::getTargetObjects( @@ -1370,73 +1397,90 @@ bool NormalLaneChange::hasEnoughLength( return true; } -bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const +std::vector NormalLaneChange::get_prepare_metrics() const { - lane_change_debug_.collision_check_objects.clear(); - - const auto & current_lanes = get_current_lanes(); - const auto & target_lanes = get_target_lanes(); - const auto is_stuck = isVehicleStuck(current_lanes); - - const auto is_empty = [&](const auto & data, const auto & s) { - if (!data.empty()) return false; - RCLCPP_WARN(logger_, "%s is empty. Not expected.", s); - return true; - }; - - const auto target_lane_neighbors_polygon_2d = - common_data_ptr_->lanes_polygon_ptr->target_neighbor; - if ( - is_empty(current_lanes, "current_lanes") || is_empty(target_lanes, "target_lanes") || - is_empty(target_lane_neighbors_polygon_2d, "target_lane_neighbors_polygon_2d")) { - return false; - } - - const auto & route_handler = *getRouteHandler(); - const auto & common_parameters = planner_data_->parameters; - - // get velocity + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto current_velocity = getEgoVelocity(); // get sampling acceleration values const auto longitudinal_acc_sampling_values = sampleLongitudinalAccValues(current_lanes, target_lanes); - const auto is_goal_in_route = common_data_ptr_->lanes_ptr->target_lane_in_goal_section; + const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); - const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; - const auto next_min_dist_buffer = common_data_ptr_->transient_data.next_dist_buffer.min; + RCLCPP_DEBUG( + logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", + prepare_durations.size(), longitudinal_acc_sampling_values.size()); const auto dist_to_target_start = calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); - const auto dist_to_terminal_end = common_data_ptr_->transient_data.dist_to_terminal_end; - const auto dist_to_terminal_start = common_data_ptr_->transient_data.dist_to_terminal_start; + return calculation::calc_prepare_phase_metrics( + common_data_ptr_, prepare_durations, longitudinal_acc_sampling_values, current_velocity, + dist_to_target_start, common_data_ptr_->transient_data.dist_to_terminal_start); +} + +std::vector NormalLaneChange::get_lane_changing_metrics( + const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metric, + const double shift_length, const double dist_to_reg_element) const +{ + const auto & route_handler = getRouteHandler(); + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; + const auto & transient_data = common_data_ptr_->transient_data; + const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, + prep_segment.points.back().point.pose); + + const auto max_lane_changing_length = std::invoke([&]() { + double max_length = + transient_data.is_ego_near_current_terminal_start + ? transient_data.dist_to_terminal_end - prep_metric.length + : std::min(transient_data.dist_to_terminal_end, dist_to_reg_element) - prep_metric.length; + auto target_lane_buffer = lane_change_parameters_->lane_change_finish_judge_buffer + + transient_data.next_dist_buffer.min; + if (std::abs(route_handler->getNumLaneToPreferredLane(target_lanes.back(), direction_)) > 0) { + target_lane_buffer += lane_change_parameters_->backward_length_buffer_for_end_of_lane; + } + max_length = std::min(max_length, dist_lc_start_to_end_of_lanes - target_lane_buffer); + return max_length; + }); - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); + const auto max_path_velocity = prep_segment.points.back().point.longitudinal_velocity_mps; + return calculation::calc_shift_phase_metrics( + common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, + prep_metric.sampled_lon_accel, max_lane_changing_length); +} - const auto sorted_lane_ids = - utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); +bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const +{ + lane_change_debug_.collision_check_objects.clear(); + + if (!common_data_ptr_->is_lanes_available()) { + RCLCPP_WARN(logger_, "lanes are not available. Not expected."); + return false; + } + if (common_data_ptr_->lanes_polygon_ptr->target_neighbor.empty()) { + RCLCPP_WARN(logger_, "target_lane_neighbors_polygon_2d is empty. Not expected."); + return false; + } + + const auto & current_lanes = get_current_lanes(); + const auto & target_lanes = get_target_lanes(); + + const auto is_stuck = isVehicleStuck(current_lanes); + const auto current_velocity = getEgoVelocity(); + const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); - const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); + const auto prepare_phase_metrics = get_prepare_metrics(); candidate_paths.reserve( - longitudinal_acc_sampling_values.size() * lane_change_parameters_->lateral_acc_sampling_num * - prepare_durations.size()); - - RCLCPP_DEBUG( - logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", - prepare_durations.size(), longitudinal_acc_sampling_values.size()); + prepare_phase_metrics.size() * lane_change_parameters_->lateral_acc_sampling_num); const bool only_tl = getStopTime() >= lane_change_parameters_->stop_time_threshold; const auto dist_to_next_regulatory_element = utils::lane_change::get_distance_to_next_regulatory_element(common_data_ptr_, only_tl, only_tl); - const auto max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); - - const auto prepare_phase_metrics = calculation::calc_prepare_phase_metrics( - common_data_ptr_, prepare_durations, longitudinal_acc_sampling_values, current_velocity, - dist_to_target_start, dist_to_terminal_start); auto check_length_diff = [&](const double prep_length, const double lc_length, const bool check_lc) { @@ -1452,10 +1496,10 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) }; for (const auto & prep_metric : prepare_phase_metrics) { - const auto debug_print = [&](const auto & s) { + const auto debug_print = [&](const std::string & s) { RCLCPP_DEBUG( - logger_, "%s | prep_time: %.5f | lon_acc: %.5f | prep_len: %.5f", s, prep_metric.duration, - prep_metric.actual_lon_accel, prep_metric.length); + logger_, "%s | prep_time: %.5f | lon_acc: %.5f | prep_len: %.5f", s.c_str(), + prep_metric.duration, prep_metric.actual_lon_accel, prep_metric.length); }; if (!check_length_diff(prep_metric.length, 0.0, false)) { @@ -1463,59 +1507,26 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) continue; } - auto prepare_segment = - getPrepareSegment(current_lanes, common_parameters.backward_path_length, prep_metric.length); - - if (prepare_segment.points.empty()) { - debug_print("prepare segment is empty...? Unexpected."); - continue; - } - - // TODO(Quda, Azu): Is it possible to remove these checks if we ensure prepare segment length is - // larger than distance to target lane start - if (!is_valid_start_point(common_data_ptr_, prepare_segment.points.back().point.pose)) { - debug_print( - "Reject: lane changing start point is not within the preferred lanes or its neighbors"); - continue; - } - - // lane changing start is at the end of prepare segment - const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; - const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( - current_lanes, target_lanes.front(), lane_changing_start_pose); - - // Check if the lane changing start point is not on the lanes next to target lanes, - if (target_length_from_lane_change_start_pose > 0.0) { - debug_print("lane change start is behind target lanelet!"); + PathWithLaneId prepare_segment; + try { + if (!get_prepare_segment(prepare_segment, prep_metric.length)) { + debug_print("Reject: failed to get valid prepare segment!"); + continue; + } + } catch (const std::exception & e) { + debug_print(e.what()); break; } debug_print("Prepare path satisfy constraints"); + const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; + const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); - const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( - common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, lane_changing_start_pose); - - const auto max_lane_changing_length = std::invoke([&]() { - double max_length = - dist_to_terminal_start > max_prepare_length - ? std::min(dist_to_terminal_end, dist_to_next_regulatory_element) - prep_metric.length - : dist_to_terminal_end - prep_metric.length; - auto target_lane_buffer = - lane_change_parameters_->lane_change_finish_judge_buffer + next_min_dist_buffer; - if (std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction_)) > 0) { - target_lane_buffer += lane_change_parameters_->backward_length_buffer_for_end_of_lane; - } - max_length = std::min(max_length, dist_lc_start_to_end_of_lanes - target_lane_buffer); - return max_length; - }); - - const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; - const auto lane_changing_metrics = calculation::calc_shift_phase_metrics( - common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, - prep_metric.sampled_lon_accel, max_lane_changing_length); + const auto lane_changing_metrics = get_lane_changing_metrics( + prepare_segment, prep_metric, shift_length, dist_to_next_regulatory_element); utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prep_metric.velocity); @@ -1535,7 +1546,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) try { candidate_path = get_candidate_path( prep_metric, lc_metric, prepare_segment, sorted_lane_ids, lane_changing_start_pose, - target_lane_length, shift_length, next_min_dist_buffer, is_goal_in_route); + shift_length); } catch (const std::exception & e) { debug_print_lat(std::string("Reject: ") + e.what()); continue; @@ -1543,20 +1554,16 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) candidate_paths.push_back(candidate_path); - bool is_safe = false; try { - is_safe = check_candidate_path_safety( - candidate_path, target_objects, current_min_dist_buffer, is_stuck); + if (check_candidate_path_safety(candidate_path, target_objects, is_stuck)) { + debug_print_lat("ACCEPT!!!: it is valid and safe!"); + return true; + } } catch (const std::exception & e) { debug_print_lat(std::string("Reject: ") + e.what()); return false; } - if (is_safe) { - debug_print_lat("ACCEPT!!!: it is valid and safe!"); - return true; - } - debug_print_lat("Reject: sampled path is not safe."); } } @@ -1568,19 +1575,16 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) LaneChangePath NormalLaneChange::get_candidate_path( const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, - const Pose & lc_start_pose, const double target_lane_length, const double shift_length, - const double next_lc_buffer, const bool is_goal_in_route) const + const Pose & lc_start_pose, const double shift_length) const { const auto & route_handler = *getRouteHandler(); const auto & current_lanes = common_data_ptr_->lanes_ptr->current; const auto & target_lanes = common_data_ptr_->lanes_ptr->target; - const auto & forward_path_length = planner_data_->parameters.forward_path_length; const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval(lc_metrics.length, prep_metrics.velocity); - const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( - route_handler, target_lanes, lc_start_pose, target_lane_length, lc_metrics.length, - forward_path_length, resample_interval, is_goal_in_route, next_lc_buffer); + const auto target_lane_reference_path = utils::lane_change::get_reference_path_from_target_Lane( + common_data_ptr_, lc_start_pose, lc_metrics.length, resample_interval); if (target_lane_reference_path.points.empty()) { throw std::logic_error("target_lane_reference_path is empty!"); @@ -1614,12 +1618,12 @@ LaneChangePath NormalLaneChange::get_candidate_path( bool NormalLaneChange::check_candidate_path_safety( const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, - const double lane_change_buffer, const bool is_stuck) const + const bool is_stuck) const { if ( !is_stuck && !utils::lane_change::passed_parked_objects( common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading, - lane_change_buffer, lane_change_debug_.collision_check_objects)) { + lane_change_debug_.collision_check_objects)) { throw std::logic_error( "Ego is not stuck and parked vehicle exists in the target lane. Skip lane change."); } @@ -1669,9 +1673,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( } const auto & route_handler = *getRouteHandler(); - const auto & common_parameters = planner_data_->parameters; - const auto forward_path_length = common_parameters.forward_path_length; const auto minimum_lane_changing_velocity = lane_change_parameters_->minimum_lane_changing_velocity; @@ -1680,10 +1682,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; const auto next_min_dist_buffer = common_data_ptr_->transient_data.next_dist_buffer.min; - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); - - const auto sorted_lane_ids = - utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); + const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); // lane changing start getEgoPose() is at the end of prepare segment const auto current_lane_terminal_point = @@ -1722,6 +1721,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( shift_length, lane_change_parameters_->lane_changing_lateral_jerk, max_lateral_acc); + const auto target_lane_length = common_data_ptr_->transient_data.target_lane_length; const auto target_segment = getTargetSegment( target_lanes, lane_changing_start_pose.value(), target_lane_length, current_min_dist_buffer, minimum_lane_changing_velocity, next_min_dist_buffer); @@ -1752,10 +1752,8 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( current_min_dist_buffer, minimum_lane_changing_velocity); - const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( - route_handler, target_lanes, lane_changing_start_pose.value(), target_lane_length, - current_min_dist_buffer, forward_path_length, resample_interval, is_goal_in_route, - next_min_dist_buffer); + const auto target_lane_reference_path = utils::lane_change::get_reference_path_from_target_Lane( + common_data_ptr_, lane_changing_start_pose.value(), current_min_dist_buffer, resample_interval); if (target_lane_reference_path.points.empty()) { RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!"); @@ -1796,11 +1794,8 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const CollisionCheckDebugMap debug_data; - const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; - const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( - common_data_ptr_, path, filtered_objects_.target_lane_leading, current_min_dist_buffer, - debug_data); + common_data_ptr_, path, filtered_objects_.target_lane_leading, debug_data); if (!has_passed_parked_objects) { RCLCPP_DEBUG(logger_, "Lane change has been delayed."); @@ -2214,8 +2209,7 @@ bool NormalLaneChange::isVehicleStuck( } // Check if any stationary object exist in obstacle_check_distance - using lanelet::utils::getArcCoordinates; - const auto base_distance = getArcCoordinates(current_lanes, getEgoPose()).length; + const auto base_distance = common_data_ptr_->transient_data.current_lanes_ego_arc.length; for (const auto & object : lane_change_debug_.filtered_objects.current_lane) { const auto & p = object.initial_pose; // TODO(Horibe): consider footprint point @@ -2225,7 +2219,8 @@ bool NormalLaneChange::isVehicleStuck( continue; } - const auto ego_to_obj_dist = getArcCoordinates(current_lanes, p).length - base_distance; + const auto ego_to_obj_dist = + lanelet::utils::getArcCoordinates(current_lanes, p).length - base_distance; if (0 < ego_to_obj_dist && ego_to_obj_dist < obstacle_check_distance) { RCLCPP_DEBUG(logger_, "Stationary object is in front of ego."); return true; // Stationary object is in front of ego. diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 8794d79a4d10f..98da9112bbc09 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -322,12 +322,17 @@ std::optional construct_candidate_path( return std::optional{candidate_path}; } -PathWithLaneId getReferencePathFromTargetLane( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & lane_changing_start_pose, const double target_lane_length, - const double lane_changing_length, const double forward_path_length, - const double resample_interval, const bool is_goal_in_route, const double next_lane_change_buffer) +PathWithLaneId get_reference_path_from_target_Lane( + const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, + const double lane_changing_length, const double resample_interval) { + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto target_lane_length = common_data_ptr->transient_data.target_lane_length; + const auto is_goal_in_route = common_data_ptr->lanes_ptr->target_lane_in_goal_section; + const auto next_lc_buffer = common_data_ptr->transient_data.next_dist_buffer.min; + const auto forward_path_length = common_data_ptr->bpp_param_ptr->forward_path_length; + const ArcCoordinates lane_change_start_arc_position = lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); @@ -337,10 +342,10 @@ PathWithLaneId getReferencePathFromTargetLane( if (is_goal_in_route) { const double s_goal = lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length - - next_lane_change_buffer; + next_lc_buffer; return std::min(dist_from_lc_start, s_goal); } - return std::min(dist_from_lc_start, target_lane_length - next_lane_change_buffer); + return std::min(dist_from_lc_start, target_lane_length - next_lc_buffer); }); constexpr double epsilon = 1e-4; @@ -548,10 +553,13 @@ double getLateralShift(const LaneChangePath & path) return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx); } -std::vector> getSortedLaneIds( - const RouteHandler & route_handler, const Pose & current_pose, - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) +std::vector> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr) { + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto & current_pose = common_data_ptr->get_ego_pose(); + const auto rough_shift_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose).distance; @@ -817,8 +825,7 @@ bool isParkedObject( bool passed_parked_objects( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, const double minimum_lane_change_length, - CollisionCheckDebugMap & object_debug) + const std::vector & objects, CollisionCheckDebugMap & object_debug) { const auto route_handler = *common_data_ptr->route_handler_ptr; const auto lane_change_parameters = *common_data_ptr->lc_param_ptr; @@ -868,7 +875,7 @@ bool passed_parked_objects( }); // If there are still enough length after the target object, we delay the lane change - if (min_dist_to_end_of_current_lane <= minimum_lane_change_length) { + if (min_dist_to_end_of_current_lane <= common_data_ptr->transient_data.current_dist_buffer.min) { return true; } From 3288946439ab173266be3e629ab9620a160481c6 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 4 Oct 2024 10:42:26 +0900 Subject: [PATCH 92/95] test(mission_planner): add unit tests of utility functions (#9011) Signed-off-by: kosuke55 --- .../autoware_mission_planner/CMakeLists.txt | 3 +- .../test/test_utility_functions.cpp | 423 ++++++++++++++++++ 2 files changed, 425 insertions(+), 1 deletion(-) create mode 100644 planning/autoware_mission_planner/test/test_utility_functions.cpp diff --git a/planning/autoware_mission_planner/CMakeLists.txt b/planning/autoware_mission_planner/CMakeLists.txt index 74bc8ddbc0a32..939903c2999ff 100644 --- a/planning/autoware_mission_planner/CMakeLists.txt +++ b/planning/autoware_mission_planner/CMakeLists.txt @@ -38,7 +38,8 @@ pluginlib_export_plugin_description_file(autoware_mission_planner plugins/plugin if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - test/test_lanelet2_plugins_default_planner.cpp + test/test_lanelet2_plugins_default_planner.cpp + test/test_utility_functions.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}_lanelet2_plugins diff --git a/planning/autoware_mission_planner/test/test_utility_functions.cpp b/planning/autoware_mission_planner/test/test_utility_functions.cpp new file mode 100644 index 0000000000000..6cde09b7664a2 --- /dev/null +++ b/planning/autoware_mission_planner/test/test_utility_functions.cpp @@ -0,0 +1,423 @@ +// Copyright 2024 TIER IV +// +// 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 <../src/lanelet2_plugins/utility_functions.hpp> +#include +#include +#include + +#include +#include +#include +#include +#include + +using autoware::mission_planner::lanelet2::convert_linear_ring_to_polygon; +using autoware::mission_planner::lanelet2::convertBasicPoint3dToPose; +using autoware::mission_planner::lanelet2::convertCenterlineToPoints; +using autoware::mission_planner::lanelet2::exists; +using autoware::mission_planner::lanelet2::get_closest_centerline_pose; +using autoware::mission_planner::lanelet2::insert_marker_array; +using autoware::mission_planner::lanelet2::is_in_lane; +using autoware::mission_planner::lanelet2::is_in_parking_lot; +using autoware::mission_planner::lanelet2::is_in_parking_space; +using autoware::mission_planner::lanelet2::project_goal_to_map; + +using autoware::vehicle_info_utils::VehicleInfo; +using geometry_msgs::msg::Pose; + +TEST(TestUtilityFunctions, convertLinearRingToPolygon) +{ + // clockwise + { + autoware::universe_utils::LinearRing2d footprint; + footprint.push_back({1.0, 1.0}); + footprint.push_back({1.0, -1.0}); + footprint.push_back({0.0, -1.0}); + footprint.push_back({-1.0, -1.0}); + footprint.push_back({-1.0, 1.0}); + footprint.push_back({0.0, 1.0}); + footprint.push_back({1.0, 1.0}); + autoware::universe_utils::Polygon2d polygon = convert_linear_ring_to_polygon(footprint); + + ASSERT_EQ(polygon.outer().size(), footprint.size()); + for (std::size_t i = 0; i < footprint.size(); ++i) { + EXPECT_DOUBLE_EQ( + boost::geometry::get<0>(polygon.outer()[i]), boost::geometry::get<0>(footprint[i])); + EXPECT_DOUBLE_EQ( + boost::geometry::get<1>(polygon.outer()[i]), boost::geometry::get<1>(footprint[i])); + } + + EXPECT_EQ(polygon.outer().front(), polygon.outer().back()); + + const double area = boost::geometry::area(polygon); + EXPECT_GT(area, 0.0); + } + + // counterclockwise + { + autoware::universe_utils::LinearRing2d footprint; + footprint.push_back({1.0, 1.0}); + footprint.push_back({0.0, 1.0}); + footprint.push_back({-1.0, 1.0}); + footprint.push_back({-1.0, -1.0}); + footprint.push_back({0.0, -1.0}); + footprint.push_back({1.0, -1.0}); + footprint.push_back({1.0, 1.0}); + autoware::universe_utils::Polygon2d polygon = convert_linear_ring_to_polygon(footprint); + + ASSERT_EQ(polygon.outer().size(), footprint.size()); + + // polygon is always clockwise + for (std::size_t i = 0; i < footprint.size(); ++i) { + const std::size_t j = footprint.size() - i - 1; + EXPECT_DOUBLE_EQ( + boost::geometry::get<0>(polygon.outer()[i]), boost::geometry::get<0>(footprint[j])); + EXPECT_DOUBLE_EQ( + boost::geometry::get<1>(polygon.outer()[i]), boost::geometry::get<1>(footprint[j])); + } + + const double area = boost::geometry::area(polygon); + EXPECT_GT(area, 0.0); + } +} + +TEST(TestUtilityFunctions, convertCenterlineToPoints) +{ + lanelet::LineString3d left_bound; + lanelet::LineString3d right_bound; + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1}); + lanelet::Lanelet lanelet{lanelet::InvalId, left_bound, right_bound}; + + lanelet::LineString3d centerline; + centerline.push_back(lanelet::Point3d{lanelet::InvalId, -1, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 0, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 1, 0}); + lanelet.setCenterline(centerline); + + const std::vector points = convertCenterlineToPoints(lanelet); + + ASSERT_EQ(points.size(), centerline.size()); + for (std::size_t i = 0; i < centerline.size(); ++i) { + EXPECT_DOUBLE_EQ(points[i].x, centerline[i].x()); + EXPECT_DOUBLE_EQ(points[i].y, centerline[i].y()); + EXPECT_DOUBLE_EQ(points[i].z, centerline[i].z()); + } +} + +TEST(TestUtilityFunctions, insertMarkerArray) +{ + visualization_msgs::msg::MarkerArray a1; + visualization_msgs::msg::MarkerArray a2; + a1.markers.resize(1); + a2.markers.resize(2); + a1.markers[0].id = 0; + a2.markers[0].id = 1; + a2.markers[1].id = 2; + + insert_marker_array(&a1, a2); + + ASSERT_EQ(a1.markers.size(), 3); + EXPECT_EQ(a1.markers[0].id, 0); + EXPECT_EQ(a1.markers[1].id, 1); + EXPECT_EQ(a1.markers[2].id, 2); +} + +TEST(TestUtilityFunctions, convertBasicPoint3dToPose) +{ + { + const lanelet::BasicPoint3d point(1.0, 2.0, 3.0); + const double lane_yaw = 0.0; + const Pose pose = convertBasicPoint3dToPose(point, lane_yaw); + EXPECT_DOUBLE_EQ(pose.position.x, point.x()); + EXPECT_DOUBLE_EQ(pose.position.y, point.y()); + EXPECT_DOUBLE_EQ(pose.position.z, point.z()); + EXPECT_DOUBLE_EQ(pose.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.z, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.w, 1.0); + } + + { + const lanelet::BasicPoint3d point(1.0, 2.0, 3.0); + const double lane_yaw = M_PI_2; + const Pose pose = convertBasicPoint3dToPose(point, lane_yaw); + EXPECT_DOUBLE_EQ(pose.position.x, point.x()); + EXPECT_DOUBLE_EQ(pose.position.y, point.y()); + EXPECT_DOUBLE_EQ(pose.position.z, point.z()); + EXPECT_DOUBLE_EQ(pose.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.z, 0.7071067811865476); + EXPECT_DOUBLE_EQ(pose.orientation.w, 0.7071067811865476); + } +} + +TEST(TestUtilityFunctions, is_in_lane) +{ + lanelet::LineString3d left_bound; + lanelet::LineString3d right_bound; + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1}); + lanelet::Lanelet lanelet{lanelet::InvalId, left_bound, right_bound}; + + lanelet::LineString3d centerline; + centerline.push_back(lanelet::Point3d{lanelet::InvalId, -1, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 0, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 1, 0}); + lanelet.setCenterline(centerline); + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 0}; + EXPECT_TRUE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 1}; + EXPECT_TRUE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, -1}; + EXPECT_TRUE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 2}; + EXPECT_FALSE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, -2}; + EXPECT_FALSE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 2, 0}; + EXPECT_FALSE(is_in_lane(lanelet, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -2, 0}; + EXPECT_FALSE(is_in_lane(lanelet, point)); + } +} + +TEST(TestUtilityFunctions, is_in_parking_lot) +{ + lanelet::Polygon3d parking_lot; + parking_lot.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + parking_lot.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1}); + parking_lot.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1}); + parking_lot.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1}); + parking_lot.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 0}; + EXPECT_TRUE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 1, 0}; + EXPECT_TRUE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 1}; + EXPECT_TRUE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 2, 0}; + EXPECT_FALSE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 2}; + EXPECT_FALSE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -2, 0}; + EXPECT_FALSE(is_in_parking_lot({parking_lot}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, -2}; + EXPECT_FALSE(is_in_parking_lot({parking_lot}, point)); + } +} + +TEST(TestUtilityFunctions, is_in_parking_space) +{ + lanelet::LineString3d parking_space; + parking_space.push_back(lanelet::Point3d{lanelet::InvalId, -1, 0}); + parking_space.push_back(lanelet::Point3d{lanelet::InvalId, 1, 0}); + parking_space.setAttribute("width", 2.0); + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 0}; + EXPECT_TRUE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 1, 0}; + EXPECT_TRUE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -1, 0}; + EXPECT_TRUE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 2, 0}; + EXPECT_FALSE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 2}; + EXPECT_FALSE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -2, 0}; + EXPECT_FALSE(is_in_parking_space({parking_space}, point)); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, -2}; + EXPECT_FALSE(is_in_parking_space({parking_space}, point)); + } +} + +TEST(TestUtilityFunctions, project_goal_to_map) +{ + const auto create_lane = [&](const double height) -> lanelet::Lanelet { + lanelet::LineString3d left_bound; + lanelet::LineString3d right_bound; + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1, height}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, -1, height}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1, height}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1, height}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, 1, height}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1, height}); + const lanelet::Lanelet lanelet{lanelet::InvalId, left_bound, right_bound}; + return lanelet; + }; + + const auto check_height = [&](const double height) -> void { + const auto lanelet = create_lane(height); + lanelet::Point3d goal_point{lanelet::InvalId, 0, 0, height}; + EXPECT_DOUBLE_EQ(project_goal_to_map(lanelet, goal_point), height); + }; + + check_height(0.0); + check_height(1.0); + check_height(-1.0); +} + +TEST(TestUtilityFunctions, TestUtilityFunctions) +{ + lanelet::LineString3d left_bound; + lanelet::LineString3d right_bound; + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1}); + lanelet::Lanelet lanelet{lanelet::InvalId, left_bound, right_bound}; + + lanelet::LineString3d centerline; + centerline.push_back(lanelet::Point3d{lanelet::InvalId, -1, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 0, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 1, 0}); + lanelet.setCenterline(centerline); + + VehicleInfo vehicle_info; + vehicle_info.left_overhang_m = 0.5; + vehicle_info.right_overhang_m = 0.5; + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 0}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 1, 0}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -1, 0}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, -1.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 2, 0}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, 2.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, -2, 0}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, -2.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, 1}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } + + { + const lanelet::Point3d point{lanelet::InvalId, 0, -1}; + const Pose pose = + get_closest_centerline_pose({lanelet}, convertBasicPoint3dToPose(point, 0.0), vehicle_info); + EXPECT_DOUBLE_EQ(pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(pose.position.z, 0.0); + } +} From 6c825251a42c5bb9c9603497324c5cbaa431969d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 4 Oct 2024 11:37:20 +0900 Subject: [PATCH 93/95] feat(autoware_multi_object_tracker): Set maximum reverse velocity to bicycle and crtv motion models (#9019) * feat: Add maximum reverse velocity to bicycle and CTRV motion models revert the tracker orientation when the velocity exceed the maximum reverse velocity Signed-off-by: Taekjin LEE refactor: Update motion model parameters for bicycle and CTRV motion models * refactor: check the max_reverse_vel configuration is correct max_reverse_vel is expected to be negative Signed-off-by: Taekjin LEE * refactor: remove config checker in the initializer Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../tracker/motion_model/bicycle_motion_model.hpp | 2 ++ .../tracker/motion_model/ctrv_motion_model.hpp | 15 ++++++++------- .../tracker/motion_model/bicycle_motion_model.cpp | 15 +++++++++++++-- .../tracker/motion_model/ctrv_motion_model.cpp | 15 +++++++++++++-- 4 files changed, 36 insertions(+), 11 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp index b84f8a4bd3bd7..821e470054f04 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp @@ -64,6 +64,8 @@ class BicycleMotionModel : public MotionModel double lr_min = 1.0; // [m] minimum distance from the center to the rear wheel double max_vel = 27.8; // [m/s] maximum velocity, 100km/h double max_slip = 0.5236; // [rad] maximum slip angle, 30deg + double max_reverse_vel = + -1.389; // [m/s] maximum reverse velocity, -5km/h. The value is expected to be negative } motion_params_; public: diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp index 812b91fc8acf0..8165b126eda8e 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp @@ -45,13 +45,14 @@ class CTRVMotionModel : public MotionModel // motion parameters: process noise and motion limits struct MotionParams { - double q_cov_x = 0.025; // [m^2/s^2] uncertain position in x, 0.5m/s - double q_cov_y = 0.025; // [m^2/s^2] uncertain position in y, 0.5m/s - double q_cov_yaw = 0.1218; // [rad^2/s^2] uncertain yaw angle, 20deg/s - double q_cov_vel = 8.6436; // [m^2/s^4] uncertain velocity, 0.3G m/s^2 - double q_cov_wz = 0.5236; // [rad^2/s^4] uncertain yaw rate, 30deg/s^2 - double max_vel = 2.78; // [m/s] maximum velocity - double max_wz = 0.5236; // [rad/s] maximum yaw rate, 30deg/s + double q_cov_x = 0.025; // [m^2/s^2] uncertain position in x, 0.5m/s + double q_cov_y = 0.025; // [m^2/s^2] uncertain position in y, 0.5m/s + double q_cov_yaw = 0.1218; // [rad^2/s^2] uncertain yaw angle, 20deg/s + double q_cov_vel = 8.6436; // [m^2/s^4] uncertain velocity, 0.3G m/s^2 + double q_cov_wz = 0.5236; // [rad^2/s^4] uncertain yaw rate, 30deg/s^2 + double max_vel = 2.78; // [m/s] maximum velocity + double max_wz = 0.5236; // [rad/s] maximum yaw rate, 30deg/s + double max_reverse_vel = -1.38; // [m/s] maximum reverse velocity, -5km/h } motion_params_; public: diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index 3c088b8f64b39..399634b63bffe 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -226,15 +226,26 @@ bool BicycleMotionModel::limitStates() Eigen::MatrixXd P_t(DIM, DIM); ekf_.getX(X_t); ekf_.getP(P_t); - X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); + + // maximum reverse velocity + if (motion_params_.max_reverse_vel < 0 && X_t(IDX::VEL) < motion_params_.max_reverse_vel) { + // rotate the object orientation by 180 degrees + X_t(IDX::VEL) = -X_t(IDX::VEL); + X_t(IDX::YAW) = X_t(IDX::YAW) + M_PI; + } + // maximum velocity if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) { X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel; } + // maximum slip angle if (!(-motion_params_.max_slip <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= motion_params_.max_slip)) { X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -motion_params_.max_slip : motion_params_.max_slip; } - ekf_.init(X_t, P_t); + // normalize yaw + X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); + // overwrite state + ekf_.init(X_t, P_t); return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp index a838bf62e5bcb..6f63ecbdce06d 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp @@ -203,15 +203,26 @@ bool CTRVMotionModel::limitStates() Eigen::MatrixXd P_t(DIM, DIM); ekf_.getX(X_t); ekf_.getP(P_t); - X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); + + // maximum reverse velocity + if (X_t(IDX::VEL) < 0 && X_t(IDX::VEL) < motion_params_.max_reverse_vel) { + // rotate the object orientation by 180 degrees + X_t(IDX::VEL) = -X_t(IDX::VEL); + X_t(IDX::YAW) = X_t(IDX::YAW) + M_PI; + } + // maximum velocity if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) { X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel; } + // maximum yaw rate if (!(-motion_params_.max_wz <= X_t(IDX::WZ) && X_t(IDX::WZ) <= motion_params_.max_wz)) { X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -motion_params_.max_wz : motion_params_.max_wz; } - ekf_.init(X_t, P_t); + // normalize yaw + X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW)); + // overwrite state + ekf_.init(X_t, P_t); return true; } From 3a75e98220733b31211104ef7435c0e2ff56a4e6 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 4 Oct 2024 13:25:20 +0900 Subject: [PATCH 94/95] fix(goal_planner): fix parking_path curvature and DecidingState transition (#9022) Signed-off-by: Mamoru Sobue --- .../decision_state.hpp | 9 ++--- .../goal_planner_module.hpp | 2 +- .../src/goal_planner_module.cpp | 38 ++++++++++++++----- .../pull_over_planner_base.cpp | 2 +- 4 files changed, 34 insertions(+), 17 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp index 67aa41a5af7e5..1092047e65030 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp @@ -35,7 +35,7 @@ class PathDecisionState }; DecisionKind state{DecisionKind::NOT_DECIDED}; - rclcpp::Time stamp{}; + std::optional deciding_start_time{std::nullopt}; bool is_stable_safe{false}; std::optional safe_start_time{std::nullopt}; }; @@ -43,7 +43,7 @@ class PathDecisionState class PathDecisionStateController { public: - PathDecisionStateController() = default; + explicit PathDecisionStateController(rclcpp::Logger logger) : logger_(logger) {} /** * @brief update current state and save old current state to prev state @@ -62,11 +62,10 @@ class PathDecisionStateController PathDecisionState get_current_state() const { return current_state_; } - PathDecisionState get_prev_state() const { return prev_state_; } - private: + rclcpp::Logger logger_; + PathDecisionState current_state_{}; - PathDecisionState prev_state_{}; /** * @brief update current state and save old current state to prev state diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 12d564237db3c..70c9fd528b6a5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -321,7 +321,7 @@ class GoalPlannerModule : public SceneModuleInterface // context_data_ is initialized in updateData(), used in plan() and refreshed in postProcess() std::optional context_data_{std::nullopt}; // path_decision_controller is updated in updateData(), and used in plan() - PathDecisionStateController path_decision_controller_{}; + PathDecisionStateController path_decision_controller_{getLogger()}; std::unique_ptr last_approval_data_{nullptr}; // approximate distance from the start point to the end point of pull_over. diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 55cbd4c0e29d5..710700c1f6dc1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -2643,7 +2643,6 @@ void PathDecisionStateController::transit_state( found_pull_over_path, now, static_target_objects, dynamic_target_objects, modified_goal_opt, planner_data, occupancy_grid_map, is_current_safe, parameters, goal_searcher, is_activated, pull_over_path, ego_polygons_expanded); - prev_state_ = current_state_; current_state_ = next_state; } @@ -2658,10 +2657,7 @@ PathDecisionState PathDecisionStateController::get_next_state( const std::optional & pull_over_path_opt, std::vector & ego_polygons_expanded) const { - auto next_state = prev_state_; - - // update timestamp - next_state.stamp = now; + auto next_state = current_state_; // update safety if (!parameters.safety_check_params.enable_safety_check) { @@ -2684,7 +2680,6 @@ PathDecisionState PathDecisionStateController::get_next_state( // Once this function returns true, it will continue to return true thereafter if (next_state.state == PathDecisionState::DecisionKind::DECIDED) { - next_state.state = PathDecisionState::DecisionKind::DECIDED; return next_state; } @@ -2699,12 +2694,16 @@ PathDecisionState PathDecisionStateController::get_next_state( // If it is dangerous against dynamic objects before approval, do not determine the path. // This eliminates a unsafe path to be approved if (enable_safety_check && !next_state.is_stable_safe && !is_activated) { + RCLCPP_DEBUG( + logger_, + "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " + "approval"); next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; return next_state; } const auto & current_path = pull_over_path.getCurrentPath(); - if (prev_state_.state == PathDecisionState::DecisionKind::DECIDING) { + if (current_state_.state == PathDecisionState::DecisionKind::DECIDING) { const double hysteresis_factor = 0.9; // check goal pose collision @@ -2712,7 +2711,9 @@ PathDecisionState PathDecisionStateController::get_next_state( modified_goal_opt && !goal_searcher->isSafeGoalWithMarginScaleFactor( modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map, planner_data, static_target_objects)) { + RCLCPP_DEBUG(logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe"); next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + next_state.deciding_start_time = std::nullopt; return next_state; } @@ -2727,24 +2728,36 @@ PathDecisionState PathDecisionStateController::get_next_state( /*extract_static_objects=*/false, parameters.maximum_deceleration, parameters.object_recognition_collision_check_max_extra_stopping_margin, ego_polygons_expanded, true)) { + RCLCPP_DEBUG( + logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects"); next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; + next_state.deciding_start_time = std::nullopt; return next_state; } if (enable_safety_check && !next_state.is_stable_safe) { + RCLCPP_DEBUG( + logger_, + "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path is not safe against dynamic objects"); next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; return next_state; } // if enough time has passed since deciding status starts, transition to DECIDED constexpr double check_collision_duration = 1.0; - const double elapsed_time_from_deciding = (now - prev_state_.stamp).seconds(); + const double elapsed_time_from_deciding = + (now - current_state_.deciding_start_time.value()).seconds(); if (elapsed_time_from_deciding > check_collision_duration) { + RCLCPP_DEBUG(logger_, "[DecidingPathStatus]: DECIDING->DECIDED. has enough safe time passed"); next_state.state = PathDecisionState::DecisionKind::DECIDED; + next_state.deciding_start_time = std::nullopt; return next_state; } // if enough time has NOT passed since deciding status starts, keep DECIDING + RCLCPP_DEBUG( + logger_, "[DecidingPathStatus]: keep DECIDING. elapsed_time_from_deciding: %f", + elapsed_time_from_deciding); return next_state; } @@ -2766,10 +2779,15 @@ PathDecisionState PathDecisionStateController::get_next_state( // if object recognition for path collision check is enabled, transition to DECIDING to check // collision for a certain period of time. Otherwise, transition to DECIDED directly. if (parameters.use_object_recognition) { - next_state.state = PathDecisionState::DecisionKind::DECIDED; + RCLCPP_DEBUG( + logger_, + "[DecidingPathStatus]: NOT_DECIDED->DECIDING. start checking collision for certain " + "period of time"); + next_state.state = PathDecisionState::DecisionKind::DECIDING; + next_state.deciding_start_time = now; return next_state; } - return {PathDecisionState::DecisionKind::DECIDED, now}; + return {PathDecisionState::DecisionKind::DECIDED, std::nullopt}; } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp index f6535e7adb8f8..c2b05929d597d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp @@ -73,7 +73,7 @@ std::optional PullOverPath::create( double parking_path_max_curvature{}; std::tie(full_path_curvatures, full_path_max_curvature) = calculateCurvaturesAndMax(full_path); std::tie(parking_path_curvatures, parking_path_max_curvature) = - calculateCurvaturesAndMax(full_path); + calculateCurvaturesAndMax(parking_path); return PullOverPath( type, goal_id, id, start_pose, end_pose, partial_paths, full_path, parking_path, From 9e32e926e0ddf1b49a3d6fe03efba352f7efed77 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 4 Oct 2024 13:47:09 +0900 Subject: [PATCH 95/95] ci(codecov): manage perception component (#9025) * list all Signed-off-by: Mamoru Sobue * regex Signed-off-by: Mamoru Sobue --------- Signed-off-by: Mamoru Sobue --- codecov.yaml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/codecov.yaml b/codecov.yaml index 0341bdfda9c56..a2b67da30c3bf 100644 --- a/codecov.yaml +++ b/codecov.yaml @@ -113,3 +113,8 @@ component_management: # - control/control_performance_analysis/** - control/obstacle_collision_checker/** # - control/predicted_path_checker/** + + - component_id: perception-tier-iv-maintained-packages + name: Perception TIER IV Maintained Packages + paths: + - perception/[^lidar_apollo_instance_segmentation]/**