From 35615358eb936aab3520bdb8a233b5b52c8529b8 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Thu, 22 Aug 2024 16:13:54 +0900 Subject: [PATCH 01/46] refactor(map_tf_generator)!: prefix package and namespace with autoware (#8422) * add autoware_ prefix Signed-off-by: a-maumau * add autoware_ prefix Signed-off-by: a-maumau --------- Signed-off-by: a-maumau Co-authored-by: SakodaShintaro --- .github/CODEOWNERS | 2 +- launch/tier4_map_launch/package.xml | 2 +- .../CMakeLists.txt | 12 ++++++------ .../README.md | 8 ++++---- .../config/map_tf_generator.param.yaml | 0 .../launch/map_tf_generator.launch.xml | 11 +++++++++++ .../package.xml | 2 +- .../schema/map_tf_generator.schema.json | 0 .../src/pcd_map_tf_generator_node.cpp | 7 +++++-- .../src}/uniform_random.hpp | 9 ++++++--- .../src/vector_map_tf_generator_node.cpp | 5 ++++- .../test/test_uniform_random.cpp | 6 +++--- .../launch/map_tf_generator.launch.xml | 11 ----------- 13 files changed, 42 insertions(+), 33 deletions(-) rename map/{map_tf_generator => autoware_map_tf_generator}/CMakeLists.txt (80%) rename map/{map_tf_generator => autoware_map_tf_generator}/README.md (89%) rename map/{map_tf_generator => autoware_map_tf_generator}/config/map_tf_generator.param.yaml (100%) create mode 100644 map/autoware_map_tf_generator/launch/map_tf_generator.launch.xml rename map/{map_tf_generator => autoware_map_tf_generator}/package.xml (97%) rename map/{map_tf_generator => autoware_map_tf_generator}/schema/map_tf_generator.schema.json (100%) rename map/{map_tf_generator => autoware_map_tf_generator}/src/pcd_map_tf_generator_node.cpp (95%) rename map/{map_tf_generator/include/map_tf_generator => autoware_map_tf_generator/src}/uniform_random.hpp (84%) rename map/{map_tf_generator => autoware_map_tf_generator}/src/vector_map_tf_generator_node.cpp (95%) rename map/{map_tf_generator => autoware_map_tf_generator}/test/test_uniform_random.cpp (85%) delete mode 100644 map/map_tf_generator/launch/map_tf_generator.launch.xml diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 08fa500a8d546..d97b3fd904d7c 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -102,7 +102,7 @@ localization/yabloc/yabloc_pose_initializer/** anh.nguyen.2@tier4.jp kento.yabuu map/map_height_fitter/** anh.nguyen.2@tier4.jp isamu.takagi@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 map/map_loader/** 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 map/map_projection_loader/** 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 -map/map_tf_generator/** 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 +map/autoware_map_tf_generator/** 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 map/util/lanelet2_map_preprocessor/** 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 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 diff --git a/launch/tier4_map_launch/package.xml b/launch/tier4_map_launch/package.xml index bca5a853d69f7..b364d8eaffebc 100644 --- a/launch/tier4_map_launch/package.xml +++ b/launch/tier4_map_launch/package.xml @@ -18,9 +18,9 @@ ament_cmake_auto autoware_cmake + autoware_map_tf_generator map_loader map_projection_loader - map_tf_generator ament_lint_auto autoware_lint_common diff --git a/map/map_tf_generator/CMakeLists.txt b/map/autoware_map_tf_generator/CMakeLists.txt similarity index 80% rename from map/map_tf_generator/CMakeLists.txt rename to map/autoware_map_tf_generator/CMakeLists.txt index e20289766cdda..eb67ea7330312 100644 --- a/map/map_tf_generator/CMakeLists.txt +++ b/map/autoware_map_tf_generator/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(map_tf_generator) +project(autoware_map_tf_generator) find_package(autoware_cmake REQUIRED) autoware_package() @@ -18,8 +18,8 @@ ament_auto_add_library(pcd_map_tf_generator_node SHARED target_link_libraries(pcd_map_tf_generator_node ${PCL_LIBRARIES}) rclcpp_components_register_node(pcd_map_tf_generator_node - PLUGIN "PcdMapTFGeneratorNode" - EXECUTABLE pcd_map_tf_generator + PLUGIN "autoware::map_tf_generator::PcdMapTFGeneratorNode" + EXECUTABLE autoware_pcd_map_tf_generator ) ament_auto_add_library(vector_map_tf_generator_node SHARED @@ -27,8 +27,8 @@ ament_auto_add_library(vector_map_tf_generator_node SHARED ) rclcpp_components_register_node(vector_map_tf_generator_node - PLUGIN "VectorMapTFGeneratorNode" - EXECUTABLE vector_map_tf_generator + PLUGIN "autoware::map_tf_generator::VectorMapTFGeneratorNode" + EXECUTABLE autoware_vector_map_tf_generator ) if(BUILD_TESTING) @@ -37,7 +37,7 @@ if(BUILD_TESTING) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gmock(${test_name} ${filepath}) - target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) + target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() diff --git a/map/map_tf_generator/README.md b/map/autoware_map_tf_generator/README.md similarity index 89% rename from map/map_tf_generator/README.md rename to map/autoware_map_tf_generator/README.md index eef36c34750ca..ed63e35e37b85 100644 --- a/map/map_tf_generator/README.md +++ b/map/autoware_map_tf_generator/README.md @@ -1,4 +1,4 @@ -# map_tf_generator +# autoware_map_tf_generator ## Purpose @@ -17,13 +17,13 @@ The following are the supported methods to calculate the position of the `viewer ### Input -#### pcd_map_tf_generator +#### autoware_pcd_map_tf_generator | Name | Type | Description | | --------------------- | ------------------------------- | ----------------------------------------------------------------- | | `/map/pointcloud_map` | `sensor_msgs::msg::PointCloud2` | Subscribe pointcloud map to calculate position of `viewer` frames | -#### vector_map_tf_generator +#### autoware_vector_map_tf_generator | Name | Type | Description | | ----------------- | --------------------------------------- | ------------------------------------------------------------- | @@ -43,7 +43,7 @@ None ### Core Parameters -{{ json_to_markdown("map/map_tf_generator/schema/map_tf_generator.schema.json") }} +{{ json_to_markdown("map/autoware_map_tf_generator/schema/map_tf_generator.schema.json") }} ## Assumptions / Known limits diff --git a/map/map_tf_generator/config/map_tf_generator.param.yaml b/map/autoware_map_tf_generator/config/map_tf_generator.param.yaml similarity index 100% rename from map/map_tf_generator/config/map_tf_generator.param.yaml rename to map/autoware_map_tf_generator/config/map_tf_generator.param.yaml diff --git a/map/autoware_map_tf_generator/launch/map_tf_generator.launch.xml b/map/autoware_map_tf_generator/launch/map_tf_generator.launch.xml new file mode 100644 index 0000000000000..00a90f10a17b0 --- /dev/null +++ b/map/autoware_map_tf_generator/launch/map_tf_generator.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/map/map_tf_generator/package.xml b/map/autoware_map_tf_generator/package.xml similarity index 97% rename from map/map_tf_generator/package.xml rename to map/autoware_map_tf_generator/package.xml index 41c3364358f74..eff7ec88adfa0 100644 --- a/map/map_tf_generator/package.xml +++ b/map/autoware_map_tf_generator/package.xml @@ -1,7 +1,7 @@ - map_tf_generator + autoware_map_tf_generator 0.1.0 map_tf_generator package as a ROS 2 node Yamato Ando diff --git a/map/map_tf_generator/schema/map_tf_generator.schema.json b/map/autoware_map_tf_generator/schema/map_tf_generator.schema.json similarity index 100% rename from map/map_tf_generator/schema/map_tf_generator.schema.json rename to map/autoware_map_tf_generator/schema/map_tf_generator.schema.json diff --git a/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp b/map/autoware_map_tf_generator/src/pcd_map_tf_generator_node.cpp similarity index 95% rename from map/map_tf_generator/src/pcd_map_tf_generator_node.cpp rename to map/autoware_map_tf_generator/src/pcd_map_tf_generator_node.cpp index d4b8ccb797224..e6edbe99dd210 100644 --- a/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp +++ b/map/autoware_map_tf_generator/src/pcd_map_tf_generator_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "map_tf_generator/uniform_random.hpp" +#include "uniform_random.hpp" #include @@ -28,6 +28,8 @@ #include #include +namespace autoware::map_tf_generator +{ constexpr size_t n_samples = 20; class PcdMapTFGeneratorNode : public rclcpp::Node @@ -95,6 +97,7 @@ class PcdMapTFGeneratorNode : public rclcpp::Node << ", y:" << coordinate[1] << ", z:" << coordinate[2]); } }; +} // namespace autoware::map_tf_generator #include -RCLCPP_COMPONENTS_REGISTER_NODE(PcdMapTFGeneratorNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::map_tf_generator::PcdMapTFGeneratorNode) diff --git a/map/map_tf_generator/include/map_tf_generator/uniform_random.hpp b/map/autoware_map_tf_generator/src/uniform_random.hpp similarity index 84% rename from map/map_tf_generator/include/map_tf_generator/uniform_random.hpp rename to map/autoware_map_tf_generator/src/uniform_random.hpp index cda3bc5c49a0c..73de77458d09d 100644 --- a/map/map_tf_generator/include/map_tf_generator/uniform_random.hpp +++ b/map/autoware_map_tf_generator/src/uniform_random.hpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MAP_TF_GENERATOR__UNIFORM_RANDOM_HPP_ -#define MAP_TF_GENERATOR__UNIFORM_RANDOM_HPP_ +#ifndef UNIFORM_RANDOM_HPP_ +#define UNIFORM_RANDOM_HPP_ #include #include +namespace autoware::map_tf_generator +{ std::vector inline uniform_random(const size_t max_exclusive, const size_t n) { std::default_random_engine generator; @@ -29,5 +31,6 @@ std::vector inline uniform_random(const size_t max_exclusive, const size } return v; } +} // namespace autoware::map_tf_generator -#endif // MAP_TF_GENERATOR__UNIFORM_RANDOM_HPP_ +#endif // UNIFORM_RANDOM_HPP_ diff --git a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp b/map/autoware_map_tf_generator/src/vector_map_tf_generator_node.cpp similarity index 95% rename from map/map_tf_generator/src/vector_map_tf_generator_node.cpp rename to map/autoware_map_tf_generator/src/vector_map_tf_generator_node.cpp index 093b42037ed72..e4c397bd04cf1 100644 --- a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp +++ b/map/autoware_map_tf_generator/src/vector_map_tf_generator_node.cpp @@ -25,6 +25,8 @@ #include #include +namespace autoware::map_tf_generator +{ class VectorMapTFGeneratorNode : public rclcpp::Node { public: @@ -93,6 +95,7 @@ class VectorMapTFGeneratorNode : public rclcpp::Node << ", y:" << coordinate_y << ", z:" << coordinate_z); } }; +} // namespace autoware::map_tf_generator #include -RCLCPP_COMPONENTS_REGISTER_NODE(VectorMapTFGeneratorNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::map_tf_generator::VectorMapTFGeneratorNode) diff --git a/map/map_tf_generator/test/test_uniform_random.cpp b/map/autoware_map_tf_generator/test/test_uniform_random.cpp similarity index 85% rename from map/map_tf_generator/test/test_uniform_random.cpp rename to map/autoware_map_tf_generator/test/test_uniform_random.cpp index 19e558ebb2add..481f5c6d86859 100644 --- a/map/map_tf_generator/test/test_uniform_random.cpp +++ b/map/autoware_map_tf_generator/test/test_uniform_random.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "map_tf_generator/uniform_random.hpp" +#include "uniform_random.hpp" #include @@ -24,7 +24,7 @@ using testing::Lt; TEST(uniform_random, uniform_random) { { - const std::vector random = uniform_random(4, 0); + const std::vector random = autoware::map_tf_generator::uniform_random(4, 0); ASSERT_EQ(random.size(), static_cast(0)); } @@ -35,7 +35,7 @@ TEST(uniform_random, uniform_random) const size_t max_exclusive = 4; for (int i = 0; i < 50; i++) { - const std::vector random = uniform_random(4, 10); + const std::vector random = autoware::map_tf_generator::uniform_random(4, 10); ASSERT_EQ(random.size(), 10U); ASSERT_THAT(random, Each(AllOf(Ge(min_inclusive), Lt(max_exclusive)))); // in range [0, 4) } diff --git a/map/map_tf_generator/launch/map_tf_generator.launch.xml b/map/map_tf_generator/launch/map_tf_generator.launch.xml deleted file mode 100644 index 197085f31d9c4..0000000000000 --- a/map/map_tf_generator/launch/map_tf_generator.launch.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - From f787f552f0ff0bca477b37ed3e00894119ba23c5 Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Thu, 22 Aug 2024 16:17:23 +0900 Subject: [PATCH 02/46] chore(vehicle_cmd_gate): delete deprecated parameters (#8537) delete deprecated params in vehicle_cmd_gate.param.yaml Signed-off-by: Autumn60 --- .../config/vehicle_cmd_gate.param.yaml | 2 -- 1 file changed, 2 deletions(-) diff --git a/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index 74affea696893..526c4bb4b3d52 100644 --- a/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -4,7 +4,6 @@ system_emergency_heartbeat_timeout: 0.5 use_emergency_handling: false check_external_emergency_heartbeat: $(var check_external_emergency_heartbeat) - use_start_request: false enable_cmd_limit_filter: true filter_activated_count_threshold: 5 filter_activated_velocity_threshold: 1.0 @@ -12,7 +11,6 @@ stop_hold_acceleration: -1.5 emergency_acceleration: -2.4 moderate_stop_service_acceleration: -1.5 - stopped_state_entry_duration_time: 0.1 stop_check_duration: 1.0 nominal: vel_lim: 25.0 From b247c43626871f44b21a77a7b6ef53259a5a9794 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Thu, 22 Aug 2024 16:21:17 +0900 Subject: [PATCH 03/46] docs: fix repository readme (#8570) Signed-off-by: Takagi, Isamu --- .github/{README.md => overall-ci-infrastructure.md} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename .github/{README.md => overall-ci-infrastructure.md} (100%) diff --git a/.github/README.md b/.github/overall-ci-infrastructure.md similarity index 100% rename from .github/README.md rename to .github/overall-ci-infrastructure.md From 257b73c7d0822118e9516fdd923aa059ccde2ddd Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Thu, 22 Aug 2024 16:26:31 +0900 Subject: [PATCH 04/46] feat(ekf_lolcalizer): output variance for z, roll and pitch (#8554) publish covariance of z,roll.pitch Signed-off-by: Yamato Ando --- .../ekf_localizer/include/ekf_localizer/ekf_localizer.hpp | 1 + localization/ekf_localizer/src/ekf_localizer.cpp | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 0b3f64caabe41..7e091a0e88a19 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -87,6 +87,7 @@ class Simple1DFilter }; void set_proc_dev(const double proc_dev) { proc_dev_x_c_ = proc_dev; } [[nodiscard]] double get_x() const { return x_; } + [[nodiscard]] double get_dev() const { return dev_; } private: bool initialized_; diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 37880f4e4139a..090395398026e 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -365,6 +365,12 @@ void EKFLocalizer::publish_estimate_result( pose_cov.header.frame_id = current_ekf_pose.header.frame_id; pose_cov.pose.pose = current_ekf_pose.pose; pose_cov.pose.covariance = ekf_module_->get_current_pose_covariance(); + + using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + pose_cov.pose.covariance[COV_IDX::Z_Z] = z_filter_.get_dev(); + pose_cov.pose.covariance[COV_IDX::ROLL_ROLL] = roll_filter_.get_dev(); + pose_cov.pose.covariance[COV_IDX::PITCH_PITCH] = pitch_filter_.get_dev(); + pub_pose_cov_->publish(pose_cov); geometry_msgs::msg::PoseWithCovarianceStamped biased_pose_cov = pose_cov; From c5e8062b7a4ddb71ed86a1432e88833ec0ee6ec7 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 22 Aug 2024 16:58:57 +0900 Subject: [PATCH 05/46] fix(autoware_lidar_centerpoint): fix unusedFunction (#8572) fix:unusedFunction Signed-off-by: kobayu858 --- perception/autoware_lidar_centerpoint/lib/utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/autoware_lidar_centerpoint/lib/utils.cpp b/perception/autoware_lidar_centerpoint/lib/utils.cpp index a9e7a68f6adaa..0abdf0f452fe4 100644 --- a/perception/autoware_lidar_centerpoint/lib/utils.cpp +++ b/perception/autoware_lidar_centerpoint/lib/utils.cpp @@ -19,7 +19,7 @@ namespace autoware::lidar_centerpoint { // cspell: ignore divup -std::size_t divup(const std::size_t a, const std::size_t b) +std::size_t divup(const std::size_t a, const std::size_t b) // cppcheck-suppress unusedFunction { if (a == 0) { throw std::runtime_error("A dividend of divup isn't positive."); From cf6859e5afb432a50fcd7ce84708980dd76d1e6c Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Thu, 22 Aug 2024 17:26:59 +0900 Subject: [PATCH 06/46] refactor(gyro_odometer)!: prefix package and namespace with autoware (#8340) * add autoware_ prefix Signed-off-by: a-maumau * add missing header Signed-off-by: a-maumau * use target_include_directories instead Signed-off-by: a-maumau * add autoware_ prefix Signed-off-by: a-maumau --------- Signed-off-by: a-maumau --- .github/CODEOWNERS | 2 +- .../pose_twist_estimator/gyro_odometer.launch.xml | 2 +- launch/tier4_localization_launch/package.xml | 2 +- .../CMakeLists.txt | 5 ++++- .../README.md | 6 +++--- .../config/gyro_odometer.param.yaml | 0 .../launch/gyro_odometer.launch.xml | 4 ++-- .../media/diagnostic.png | Bin .../package.xml | 4 ++-- .../schema/gyro_odometer.schema.json | 0 .../src/gyro_odometer_core.cpp | 3 ++- .../src}/gyro_odometer_core.hpp | 6 +++--- .../test/test_gyro_odometer_helper.cpp | 0 .../test/test_gyro_odometer_helper.hpp | 0 .../test/test_gyro_odometer_pubsub.cpp | 2 +- .../test/test_main.cpp | 0 16 files changed, 20 insertions(+), 16 deletions(-) rename localization/{gyro_odometer => autoware_gyro_odometer}/CMakeLists.txt (88%) rename localization/{gyro_odometer => autoware_gyro_odometer}/README.md (94%) rename localization/{gyro_odometer => autoware_gyro_odometer}/config/gyro_odometer.param.yaml (100%) rename localization/{gyro_odometer => autoware_gyro_odometer}/launch/gyro_odometer.launch.xml (87%) rename localization/{gyro_odometer => autoware_gyro_odometer}/media/diagnostic.png (100%) rename localization/{gyro_odometer => autoware_gyro_odometer}/package.xml (92%) rename localization/{gyro_odometer => autoware_gyro_odometer}/schema/gyro_odometer.schema.json (100%) rename localization/{gyro_odometer => autoware_gyro_odometer}/src/gyro_odometer_core.cpp (99%) rename localization/{gyro_odometer/include/gyro_odometer => autoware_gyro_odometer/src}/gyro_odometer_core.hpp (95%) rename localization/{gyro_odometer => autoware_gyro_odometer}/test/test_gyro_odometer_helper.cpp (100%) rename localization/{gyro_odometer => autoware_gyro_odometer}/test/test_gyro_odometer_helper.hpp (100%) rename localization/{gyro_odometer => autoware_gyro_odometer}/test/test_gyro_odometer_pubsub.cpp (99%) rename localization/{gyro_odometer => autoware_gyro_odometer}/test/test_main.cpp (100%) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index d97b3fd904d7c..6333b479b5696 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -87,7 +87,7 @@ localization/autoware_stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier 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/ekf_localizer/** 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 takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp localization/autoware_geo_pose_projector/** 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/gyro_odometer/** 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_gyro_odometer/** 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_error_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 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 diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/gyro_odometer.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/gyro_odometer.launch.xml index 3627c6698f4bd..e78101b0a1809 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/gyro_odometer.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/gyro_odometer.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 2f02be72ec5ad..19d15b43e5d14 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -20,12 +20,12 @@ automatic_pose_initializer autoware_ar_tag_based_localizer autoware_geo_pose_projector + autoware_gyro_odometer autoware_pointcloud_preprocessor eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_rt ekf_localizer - gyro_odometer ndt_scan_matcher pose_estimator_arbiter pose_initializer diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/autoware_gyro_odometer/CMakeLists.txt similarity index 88% rename from localization/gyro_odometer/CMakeLists.txt rename to localization/autoware_gyro_odometer/CMakeLists.txt index a832383ff4761..b4d092531e6af 100644 --- a/localization/gyro_odometer/CMakeLists.txt +++ b/localization/autoware_gyro_odometer/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(gyro_odometer) +project(autoware_gyro_odometer) find_package(autoware_cmake REQUIRED) autoware_package() @@ -22,6 +22,9 @@ if(BUILD_TESTING) target_link_libraries(test_gyro_odometer ${PROJECT_NAME} ) + target_include_directories(test_gyro_odometer PRIVATE + src + ) endif() rclcpp_components_register_node(${PROJECT_NAME} diff --git a/localization/gyro_odometer/README.md b/localization/autoware_gyro_odometer/README.md similarity index 94% rename from localization/gyro_odometer/README.md rename to localization/autoware_gyro_odometer/README.md index aa6f2a96f4838..d2ff600ae4bf0 100644 --- a/localization/gyro_odometer/README.md +++ b/localization/autoware_gyro_odometer/README.md @@ -1,8 +1,8 @@ -# gyro_odometer +# autoware_gyro_odometer ## Purpose -`gyro_odometer` is the package to estimate twist by combining imu and vehicle speed. +`autoware_gyro_odometer` is the package to estimate twist by combining imu and vehicle speed. ## Inputs / Outputs @@ -21,7 +21,7 @@ ## Parameters -{{ json_to_markdown("localization/gyro_odometer/schema/gyro_odometer.schema.json") }} +{{ json_to_markdown("localization/autoware_gyro_odometer/schema/gyro_odometer.schema.json") }} ## Assumptions / Known limits diff --git a/localization/gyro_odometer/config/gyro_odometer.param.yaml b/localization/autoware_gyro_odometer/config/gyro_odometer.param.yaml similarity index 100% rename from localization/gyro_odometer/config/gyro_odometer.param.yaml rename to localization/autoware_gyro_odometer/config/gyro_odometer.param.yaml diff --git a/localization/gyro_odometer/launch/gyro_odometer.launch.xml b/localization/autoware_gyro_odometer/launch/gyro_odometer.launch.xml similarity index 87% rename from localization/gyro_odometer/launch/gyro_odometer.launch.xml rename to localization/autoware_gyro_odometer/launch/gyro_odometer.launch.xml index aed6050858fe1..b9f30a4389df7 100644 --- a/localization/gyro_odometer/launch/gyro_odometer.launch.xml +++ b/localization/autoware_gyro_odometer/launch/gyro_odometer.launch.xml @@ -9,9 +9,9 @@ - + - + diff --git a/localization/gyro_odometer/media/diagnostic.png b/localization/autoware_gyro_odometer/media/diagnostic.png similarity index 100% rename from localization/gyro_odometer/media/diagnostic.png rename to localization/autoware_gyro_odometer/media/diagnostic.png diff --git a/localization/gyro_odometer/package.xml b/localization/autoware_gyro_odometer/package.xml similarity index 92% rename from localization/gyro_odometer/package.xml rename to localization/autoware_gyro_odometer/package.xml index ba08a8852eca5..0e78d0dea51b6 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/autoware_gyro_odometer/package.xml @@ -1,9 +1,9 @@ - gyro_odometer + autoware_gyro_odometer 0.1.0 - The gyro_odometer package as a ROS 2 node + The autoware_gyro_odometer package as a ROS 2 node Yamato Ando Masahiro Sakamoto Kento Yabuuchi diff --git a/localization/gyro_odometer/schema/gyro_odometer.schema.json b/localization/autoware_gyro_odometer/schema/gyro_odometer.schema.json similarity index 100% rename from localization/gyro_odometer/schema/gyro_odometer.schema.json rename to localization/autoware_gyro_odometer/schema/gyro_odometer.schema.json diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp similarity index 99% rename from localization/gyro_odometer/src/gyro_odometer_core.cpp rename to localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp index c661f63d91535..bc2abb4a8a321 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "gyro_odometer/gyro_odometer_core.hpp" +#include "gyro_odometer_core.hpp" #include @@ -23,6 +23,7 @@ #endif #include +#include #include #include #include diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp similarity index 95% rename from localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp rename to localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp index f569ed25a0c7f..1b2c4246a037a 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ -#define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ +#ifndef GYRO_ODOMETER_CORE_HPP_ +#define GYRO_ODOMETER_CORE_HPP_ #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" @@ -85,4 +85,4 @@ class GyroOdometerNode : public rclcpp::Node } // namespace autoware::gyro_odometer -#endif // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ +#endif // GYRO_ODOMETER_CORE_HPP_ diff --git a/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp b/localization/autoware_gyro_odometer/test/test_gyro_odometer_helper.cpp similarity index 100% rename from localization/gyro_odometer/test/test_gyro_odometer_helper.cpp rename to localization/autoware_gyro_odometer/test/test_gyro_odometer_helper.cpp diff --git a/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp b/localization/autoware_gyro_odometer/test/test_gyro_odometer_helper.hpp similarity index 100% rename from localization/gyro_odometer/test/test_gyro_odometer_helper.hpp rename to localization/autoware_gyro_odometer/test/test_gyro_odometer_helper.hpp diff --git a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp b/localization/autoware_gyro_odometer/test/test_gyro_odometer_pubsub.cpp similarity index 99% rename from localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp rename to localization/autoware_gyro_odometer/test/test_gyro_odometer_pubsub.cpp index fc331a638a1dd..21211072054d4 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp +++ b/localization/autoware_gyro_odometer/test/test_gyro_odometer_pubsub.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "gyro_odometer/gyro_odometer_core.hpp" +#include "gyro_odometer_core.hpp" #include "test_gyro_odometer_helper.hpp" #include diff --git a/localization/gyro_odometer/test/test_main.cpp b/localization/autoware_gyro_odometer/test/test_main.cpp similarity index 100% rename from localization/gyro_odometer/test/test_main.cpp rename to localization/autoware_gyro_odometer/test/test_main.cpp From 44e0535d82cd333134c7404e9ddf9811adde74e7 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Thu, 22 Aug 2024 17:50:05 +0900 Subject: [PATCH 07/46] refactor(autoware_pointcloud_preprocessor): rework lanelet2 map filter parameters (#8491) * feat: rework lanelet2 map filter parameters Signed-off-by: vividf * chore: remove unrelated files Signed-off-by: vividf * fix: fix node name in launch Signed-off-by: vividf * chore: fix launcher Signed-off-by: vividf * chore: fix spell error Signed-off-by: vividf * chore: add boundary Signed-off-by: vividf --------- Signed-off-by: vividf --- .../CMakeLists.txt | 2 +- .../lanelet2_map_filter_node.param.yaml | 4 ++ .../docs/vector-map-filter.md | 5 +-- ...delet.hpp => lanelet2_map_filter_node.hpp} | 8 ++-- .../lanelet2_map_filter_node.launch.xml | 12 ++++++ .../lanelet2_map_filter_node.schema.json | 40 +++++++++++++++++++ ...delet.cpp => lanelet2_map_filter_node.cpp} | 8 ++-- 7 files changed, 66 insertions(+), 13 deletions(-) create mode 100644 sensing/autoware_pointcloud_preprocessor/config/lanelet2_map_filter_node.param.yaml rename sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/{lanelet2_map_filter_nodelet.hpp => lanelet2_map_filter_node.hpp} (96%) create mode 100644 sensing/autoware_pointcloud_preprocessor/launch/lanelet2_map_filter_node.launch.xml create mode 100644 sensing/autoware_pointcloud_preprocessor/schema/lanelet2_map_filter_node.schema.json rename sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/{lanelet2_map_filter_nodelet.cpp => lanelet2_map_filter_node.cpp} (98%) diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 60c082cba53da..56c0238f104ad 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -78,7 +78,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/passthrough_filter/passthrough_filter_uint16_nodelet.cpp src/passthrough_filter/passthrough_uint16.cpp src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp - src/vector_map_filter/lanelet2_map_filter_nodelet.cpp + src/vector_map_filter/lanelet2_map_filter_node.cpp src/distortion_corrector/distortion_corrector.cpp src/distortion_corrector/distortion_corrector_node.cpp src/blockage_diag/blockage_diag_node.cpp diff --git a/sensing/autoware_pointcloud_preprocessor/config/lanelet2_map_filter_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/lanelet2_map_filter_node.param.yaml new file mode 100644 index 0000000000000..2aae4c5e886ea --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/lanelet2_map_filter_node.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + voxel_size_x: 0.04 + voxel_size_y: 0.04 diff --git a/sensing/autoware_pointcloud_preprocessor/docs/vector-map-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/vector-map-filter.md index c38a4c719fcf3..8e978adb81657 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/vector-map-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/vector-map-filter.md @@ -25,10 +25,7 @@ The `vector_map_filter` is a node that removes points on the outside of lane by ### Core Parameters -| Name | Type | Default Value | Description | -| -------------- | ------ | ------------- | ----------- | -| `voxel_size_x` | double | 0.04 | voxel size | -| `voxel_size_y` | double | 0.04 | voxel size | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/lanelet2_map_filter_node.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_node.hpp similarity index 96% rename from sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_node.hpp index 4ba773ed618ac..5572a9a6f4588 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_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__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODE_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODE_HPP_ #include #include @@ -97,4 +97,4 @@ class Lanelet2MapFilterComponent : public rclcpp::Node } // namespace autoware::pointcloud_preprocessor -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODE_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/launch/lanelet2_map_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/lanelet2_map_filter_node.launch.xml new file mode 100644 index 0000000000000..745bda3119404 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/lanelet2_map_filter_node.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/schema/lanelet2_map_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/lanelet2_map_filter_node.schema.json new file mode 100644 index 0000000000000..be7c90b5fd4f7 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/lanelet2_map_filter_node.schema.json @@ -0,0 +1,40 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Lanelet2 Map Filter Node", + "type": "object", + "definitions": { + "lanelet2_map_filter": { + "type": "object", + "properties": { + "voxel_size_x": { + "type": "number", + "description": "voxel size along x-axis [m]", + "default": "0.04", + "minimum": 0 + }, + "voxel_size_y": { + "type": "number", + "description": "voxel size along y-axis [m]", + "default": "0.04", + "minimum": 0 + } + }, + "required": ["voxel_size_x", "voxel_size_y"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/lanelet2_map_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_node.cpp similarity index 98% rename from sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_node.cpp index b6de8bb87d5b1..ee788a2fc807a 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_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/vector_map_filter/lanelet2_map_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_node.hpp" #include "autoware/pointcloud_preprocessor/filter.hpp" @@ -39,8 +39,8 @@ Lanelet2MapFilterComponent::Lanelet2MapFilterComponent(const rclcpp::NodeOptions // Set parameters { - voxel_size_x_ = declare_parameter("voxel_size_x", 0.04); - voxel_size_y_ = declare_parameter("voxel_size_y", 0.04); + voxel_size_x_ = declare_parameter("voxel_size_x"); + voxel_size_y_ = declare_parameter("voxel_size_y"); } // Set publisher From 68f2b9545c7148182a8506852f33372eacaf4b0c Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Thu, 22 Aug 2024 18:43:19 +0900 Subject: [PATCH 08/46] refactor(pose_estimator_arbiter)!: prefix package and namespace with autoware (#8386) * add autoware_ prefix Signed-off-by: a-maumau * add autoware_ prefix Signed-off-by: a-maumau * fix link for landmark_based_localizer Signed-off-by: a-maumau * remove Shadowing Signed-off-by: a-maumau --------- Signed-off-by: a-maumau Co-authored-by: SakodaShintaro --- .../pose_twist_estimator.launch.xml | 2 +- launch/tier4_localization_launch/package.xml | 2 +- .../CMakeLists.txt | 13 ++++--- .../README.md | 26 ++++++------- .../example_rule/CMakeLists.txt | 12 +++--- .../example_rule/README.md | 0 .../src}/rule_helper/grid_key.hpp | 14 +++---- .../src}/rule_helper/pcd_occupancy.cpp | 8 ++-- .../src}/rule_helper/pcd_occupancy.hpp | 10 ++--- .../src}/rule_helper/pose_estimator_area.cpp | 6 +-- .../src}/rule_helper/pose_estimator_area.hpp | 10 ++--- .../src}/switch_rule/pcd_map_based_rule.cpp | 6 +-- .../src}/switch_rule/pcd_map_based_rule.hpp | 18 ++++----- .../switch_rule/vector_map_based_rule.cpp | 6 +-- .../switch_rule/vector_map_based_rule.hpp | 18 ++++----- .../test/test_pcd_map_based_rule.cpp | 36 +++++++++--------- .../example_rule/test/test_rule_helper.cpp | 14 +++---- .../test/test_vector_map_based_rule.cpp | 36 +++++++++--------- .../launch/pose_estimator_arbiter.launch.xml | 2 +- .../media/architecture.drawio.svg | 0 .../media/pcd_occupancy.drawio.svg | 0 .../pose_estimator_area_in_vector_map.png | Bin .../media/single_pose_estimator.drawio.svg | 0 .../package.xml | 2 +- .../src}/pose_estimator_arbiter.hpp | 16 ++++---- .../src}/pose_estimator_arbiter_core.cpp | 26 ++++++------- .../src}/pose_estimator_type.hpp | 10 ++--- .../src}/shared_data.hpp | 10 ++--- .../src}/stopper/base_stopper.hpp | 12 +++--- .../src}/stopper/stopper_artag.hpp | 12 +++--- .../src}/stopper/stopper_eagleye.hpp | 12 +++--- .../src}/stopper/stopper_ndt.hpp | 12 +++--- .../src}/stopper/stopper_yabloc.hpp | 12 +++--- .../src}/switch_rule/base_switch_rule.hpp | 15 ++++---- .../src}/switch_rule/enable_all_rule.cpp | 6 +-- .../src}/switch_rule/enable_all_rule.hpp | 14 +++---- .../test/test_pose_estimator_arbiter.py | 2 +- .../test/test_shared_data.cpp | 6 +-- 38 files changed, 205 insertions(+), 201 deletions(-) rename localization/{pose_estimator_arbiter => autoware_pose_estimator_arbiter}/CMakeLists.txt (83%) rename localization/{pose_estimator_arbiter => autoware_pose_estimator_arbiter}/README.md (94%) rename localization/{pose_estimator_arbiter => autoware_pose_estimator_arbiter}/example_rule/CMakeLists.txt (67%) rename localization/{pose_estimator_arbiter => autoware_pose_estimator_arbiter}/example_rule/README.md (100%) rename localization/{pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter => autoware_pose_estimator_arbiter/example_rule/src}/rule_helper/grid_key.hpp (78%) rename localization/{pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter => autoware_pose_estimator_arbiter/example_rule/src}/rule_helper/pcd_occupancy.cpp (93%) rename localization/{pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter => autoware_pose_estimator_arbiter/example_rule/src}/rule_helper/pcd_occupancy.hpp (83%) rename localization/{pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter => autoware_pose_estimator_arbiter/example_rule/src}/rule_helper/pose_estimator_area.cpp (96%) rename localization/{pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter => autoware_pose_estimator_arbiter/example_rule/src}/rule_helper/pose_estimator_area.hpp (83%) rename localization/{pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter => autoware_pose_estimator_arbiter/example_rule/src}/switch_rule/pcd_map_based_rule.cpp (93%) rename localization/{pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter => autoware_pose_estimator_arbiter/example_rule/src}/switch_rule/pcd_map_based_rule.hpp (71%) rename localization/{pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter => autoware_pose_estimator_arbiter/example_rule/src}/switch_rule/vector_map_based_rule.cpp (93%) rename localization/{pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter => autoware_pose_estimator_arbiter/example_rule/src}/switch_rule/vector_map_based_rule.hpp (71%) rename localization/{pose_estimator_arbiter => autoware_pose_estimator_arbiter}/example_rule/test/test_pcd_map_based_rule.cpp (62%) rename localization/{pose_estimator_arbiter => autoware_pose_estimator_arbiter}/example_rule/test/test_rule_helper.cpp (87%) rename localization/{pose_estimator_arbiter => autoware_pose_estimator_arbiter}/example_rule/test/test_vector_map_based_rule.cpp (65%) rename localization/{pose_estimator_arbiter => autoware_pose_estimator_arbiter}/launch/pose_estimator_arbiter.launch.xml (93%) rename localization/{pose_estimator_arbiter => autoware_pose_estimator_arbiter}/media/architecture.drawio.svg (100%) rename localization/{pose_estimator_arbiter => autoware_pose_estimator_arbiter}/media/pcd_occupancy.drawio.svg (100%) rename localization/{pose_estimator_arbiter => autoware_pose_estimator_arbiter}/media/pose_estimator_area_in_vector_map.png (100%) rename localization/{pose_estimator_arbiter => autoware_pose_estimator_arbiter}/media/single_pose_estimator.drawio.svg (100%) rename localization/{pose_estimator_arbiter => autoware_pose_estimator_arbiter}/package.xml (97%) rename localization/{pose_estimator_arbiter/src/pose_estimator_arbiter => autoware_pose_estimator_arbiter/src}/pose_estimator_arbiter.hpp (89%) rename localization/{pose_estimator_arbiter/src/pose_estimator_arbiter => autoware_pose_estimator_arbiter/src}/pose_estimator_arbiter_core.cpp (90%) rename localization/{pose_estimator_arbiter/src/pose_estimator_arbiter => autoware_pose_estimator_arbiter/src}/pose_estimator_type.hpp (73%) rename localization/{pose_estimator_arbiter/src/pose_estimator_arbiter => autoware_pose_estimator_arbiter/src}/shared_data.hpp (93%) rename localization/{pose_estimator_arbiter/src/pose_estimator_arbiter => autoware_pose_estimator_arbiter/src}/stopper/base_stopper.hpp (81%) rename localization/{pose_estimator_arbiter/src/pose_estimator_arbiter => autoware_pose_estimator_arbiter/src}/stopper/stopper_artag.hpp (82%) rename localization/{pose_estimator_arbiter/src/pose_estimator_arbiter => autoware_pose_estimator_arbiter/src}/stopper/stopper_eagleye.hpp (81%) rename localization/{pose_estimator_arbiter/src/pose_estimator_arbiter => autoware_pose_estimator_arbiter/src}/stopper/stopper_ndt.hpp (81%) rename localization/{pose_estimator_arbiter/src/pose_estimator_arbiter => autoware_pose_estimator_arbiter/src}/stopper/stopper_yabloc.hpp (89%) rename localization/{pose_estimator_arbiter/src/pose_estimator_arbiter => autoware_pose_estimator_arbiter/src}/switch_rule/base_switch_rule.hpp (78%) rename localization/{pose_estimator_arbiter/src/pose_estimator_arbiter => autoware_pose_estimator_arbiter/src}/switch_rule/enable_all_rule.cpp (88%) rename localization/{pose_estimator_arbiter/src/pose_estimator_arbiter => autoware_pose_estimator_arbiter/src}/switch_rule/enable_all_rule.hpp (71%) rename localization/{pose_estimator_arbiter => autoware_pose_estimator_arbiter}/test/test_pose_estimator_arbiter.py (98%) rename localization/{pose_estimator_arbiter => autoware_pose_estimator_arbiter}/test/test_shared_data.cpp (90%) diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml index fa6bce0e38e55..f06c9f83efb35 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml @@ -93,7 +93,7 @@ - + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 19d15b43e5d14..efeecd3e37f61 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -22,12 +22,12 @@ autoware_geo_pose_projector autoware_gyro_odometer autoware_pointcloud_preprocessor + autoware_pose_estimator_arbiter eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_rt ekf_localizer ndt_scan_matcher - pose_estimator_arbiter pose_initializer pose_instability_detector topic_tools diff --git a/localization/pose_estimator_arbiter/CMakeLists.txt b/localization/autoware_pose_estimator_arbiter/CMakeLists.txt similarity index 83% rename from localization/pose_estimator_arbiter/CMakeLists.txt rename to localization/autoware_pose_estimator_arbiter/CMakeLists.txt index eefb7fc9a6879..a45da8767b72b 100644 --- a/localization/pose_estimator_arbiter/CMakeLists.txt +++ b/localization/autoware_pose_estimator_arbiter/CMakeLists.txt @@ -1,22 +1,25 @@ cmake_minimum_required(VERSION 3.14) -project(pose_estimator_arbiter) +project(autoware_pose_estimator_arbiter) find_package(autoware_cmake REQUIRED) autoware_package() find_package(PCL REQUIRED COMPONENTS common) -include_directories(SYSTEM ${PCL_INCLUDE_DIRS}) +include_directories( + SYSTEM ${PCL_INCLUDE_DIRS} + src +) # ============================== # pose estimator arbiter node ament_auto_add_library(${PROJECT_NAME} SHARED - src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp - src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp + src/pose_estimator_arbiter_core.cpp + src/switch_rule/enable_all_rule.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC src) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "pose_estimator_arbiter::PoseEstimatorArbiter" + PLUGIN "autoware::pose_estimator_arbiter::PoseEstimatorArbiter" EXECUTABLE ${PROJECT_NAME}_node EXECUTOR MultiThreadedExecutor ) diff --git a/localization/pose_estimator_arbiter/README.md b/localization/autoware_pose_estimator_arbiter/README.md similarity index 94% rename from localization/pose_estimator_arbiter/README.md rename to localization/autoware_pose_estimator_arbiter/README.md index 7b9397dfdba47..938bb20e252ac 100644 --- a/localization/pose_estimator_arbiter/README.md +++ b/localization/autoware_pose_estimator_arbiter/README.md @@ -1,4 +1,4 @@ -# pose_estimator_arbiter +# autoware_pose_estimator_arbiter Table of contents: @@ -35,7 +35,7 @@ Also, even if both can be activated at the same time, the Kalman Filter may be a - [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/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/landmark_based_localizer) +- [landmark_based_localizer](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/autoware_landmark_based_localizer) ### Demonstration @@ -117,8 +117,8 @@ If it does not seems to work, users can get more information in the following wa > [!TIP] > > ```bash -> ros2 service call /localization/pose_estimator_arbiter/config_logger logging_demo/srv/ConfigLogger \ -> '{logger_name: localization.pose_estimator_arbiter, level: debug}' +> ros2 service call /localization/autoware_pose_estimator_arbiter/config_logger logging_demo/srv/ConfigLogger \ +> '{logger_name: localization.autoware_pose_estimator_arbiter, level: debug}' > ``` ## Architecture @@ -135,7 +135,7 @@ Following figure shows the node configuration when NDT, YabLoc Eagleye and AR-Ta ### Case of running multiple pose estimators -When running multiple pose_estimators, pose_estimator_arbiter is executed. +When running multiple pose_estimators, autoware_pose_estimator_arbiter is executed. It comprises a **switching rule** and **stoppers** corresponding to each pose_estimator. - Stoppers control the pose_estimator activity by relaying inputs or outputs, or by requesting a suspend service. @@ -187,14 +187,14 @@ ros2 launch autoware_launch logging_simulator.launch.xml \ Even if `pose_source` includes an unexpected string, it will be filtered appropriately. Please see the table below for details. -| given runtime argument | parsed pose_estimator_arbiter's param (pose_sources) | -| ------------------------------------------- | ---------------------------------------------------- | -| `pose_source:=ndt` | `["ndt"]` | -| `pose_source:=nan` | `[]` | -| `pose_source:=yabloc_ndt` | `["ndt","yabloc"]` | -| `pose_source:=yabloc_ndt_ndt_ndt` | `["ndt","yabloc"]` | -| `pose_source:=ndt_yabloc_eagleye` | `["ndt","yabloc","eagleye"]` | -| `pose_source:=ndt_yabloc_nan_eagleye_artag` | `["ndt","yabloc","eagleye","artag"]` | +| given runtime argument | parsed autoware_pose_estimator_arbiter's param (pose_sources) | +| ------------------------------------------- | ------------------------------------------------------------- | +| `pose_source:=ndt` | `["ndt"]` | +| `pose_source:=nan` | `[]` | +| `pose_source:=yabloc_ndt` | `["ndt","yabloc"]` | +| `pose_source:=yabloc_ndt_ndt_ndt` | `["ndt","yabloc"]` | +| `pose_source:=ndt_yabloc_eagleye` | `["ndt","yabloc","eagleye"]` | +| `pose_source:=ndt_yabloc_nan_eagleye_artag` | `["ndt","yabloc","eagleye","artag"]` | diff --git a/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt b/localization/autoware_pose_estimator_arbiter/example_rule/CMakeLists.txt similarity index 67% rename from localization/pose_estimator_arbiter/example_rule/CMakeLists.txt rename to localization/autoware_pose_estimator_arbiter/example_rule/CMakeLists.txt index b2b5a828e42e7..5125829ffd69c 100644 --- a/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt +++ b/localization/autoware_pose_estimator_arbiter/example_rule/CMakeLists.txt @@ -1,13 +1,13 @@ # example switch rule library ament_auto_add_library(example_rule SHARED - src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp - src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp - src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp - src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp + src/switch_rule/pcd_map_based_rule.cpp + src/switch_rule/vector_map_based_rule.cpp + src/rule_helper/pcd_occupancy.cpp + src/rule_helper/pose_estimator_area.cpp ) target_include_directories(example_rule PUBLIC src example_rule/src) -target_link_libraries(example_rule pose_estimator_arbiter) +target_link_libraries(example_rule autoware_pose_estimator_arbiter) # ============================== # define test definition macro @@ -16,7 +16,7 @@ function(add_testcase filepath) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gtest(${test_name} ${filepath}) - target_link_libraries("${test_name}" pose_estimator_arbiter example_rule) + target_link_libraries("${test_name}" autoware_pose_estimator_arbiter example_rule) target_include_directories(${test_name} PUBLIC src) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() diff --git a/localization/pose_estimator_arbiter/example_rule/README.md b/localization/autoware_pose_estimator_arbiter/example_rule/README.md similarity index 100% rename from localization/pose_estimator_arbiter/example_rule/README.md rename to localization/autoware_pose_estimator_arbiter/example_rule/README.md diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/grid_key.hpp similarity index 78% rename from localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp rename to localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/grid_key.hpp index b4ad7111e4ba0..d4d4409ade979 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/grid_key.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_ -#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_ +#ifndef RULE_HELPER__GRID_KEY_HPP_ +#define RULE_HELPER__GRID_KEY_HPP_ #include #include #include -namespace pose_estimator_arbiter::rule_helper +namespace autoware::pose_estimator_arbiter::rule_helper { struct GridKey { @@ -46,16 +46,16 @@ struct GridKey friend bool operator!=(const GridKey & one, const GridKey & other) { return !(one == other); } }; -} // namespace pose_estimator_arbiter::rule_helper +} // namespace autoware::pose_estimator_arbiter::rule_helper // This is for unordered_map and unordered_set namespace std { template <> -struct hash +struct hash { public: - size_t operator()(const pose_estimator_arbiter::rule_helper::GridKey & grid) const + size_t operator()(const autoware::pose_estimator_arbiter::rule_helper::GridKey & grid) const { std::size_t seed = 0; boost::hash_combine(seed, grid.x); @@ -65,4 +65,4 @@ struct hash }; } // namespace std -#endif // POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_ +#endif // RULE_HELPER__GRID_KEY_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pcd_occupancy.cpp similarity index 93% rename from localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp rename to localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pcd_occupancy.cpp index 4b0188a1638f6..197bff459530d 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pcd_occupancy.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp" +#include "rule_helper/pcd_occupancy.hpp" -#include "pose_estimator_arbiter/rule_helper/grid_key.hpp" +#include "rule_helper/grid_key.hpp" #include @@ -25,7 +25,7 @@ #include #include -namespace pose_estimator_arbiter::rule_helper +namespace autoware::pose_estimator_arbiter::rule_helper { PcdOccupancy::PcdOccupancy(int pcd_density_upper_threshold, int pcd_density_lower_threshold) : pcd_density_upper_threshold_(pcd_density_upper_threshold), @@ -114,4 +114,4 @@ void PcdOccupancy::init(PointCloud2::ConstSharedPtr msg) kdtree_->setInputCloud(occupied_areas_); } -} // namespace pose_estimator_arbiter::rule_helper +} // namespace autoware::pose_estimator_arbiter::rule_helper diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pcd_occupancy.hpp similarity index 83% rename from localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp rename to localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pcd_occupancy.hpp index 098704e11aba9..e38cb1bd1ee38 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pcd_occupancy.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_ -#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_ +#ifndef RULE_HELPER__PCD_OCCUPANCY_HPP_ +#define RULE_HELPER__PCD_OCCUPANCY_HPP_ #include @@ -26,7 +26,7 @@ #include -namespace pose_estimator_arbiter::rule_helper +namespace autoware::pose_estimator_arbiter::rule_helper { class PcdOccupancy { @@ -49,6 +49,6 @@ class PcdOccupancy pcl::KdTreeFLANN::Ptr kdtree_{nullptr}; }; -} // namespace pose_estimator_arbiter::rule_helper +} // namespace autoware::pose_estimator_arbiter::rule_helper -#endif // POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_ +#endif // RULE_HELPER__PCD_OCCUPANCY_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pose_estimator_area.cpp similarity index 96% rename from localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp rename to localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pose_estimator_area.cpp index 1bf359b6ab54d..41ad45430d238 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pose_estimator_area.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp" +#include "rule_helper/pose_estimator_area.hpp" #include #include @@ -26,7 +26,7 @@ #include #include -namespace pose_estimator_arbiter::rule_helper +namespace autoware::pose_estimator_arbiter::rule_helper { using BoostPoint = boost::geometry::model::d2::point_xy; using BoostPolygon = boost::geometry::model::polygon; @@ -141,4 +141,4 @@ bool PoseEstimatorArea::Impl::within( } return false; } -} // namespace pose_estimator_arbiter::rule_helper +} // namespace autoware::pose_estimator_arbiter::rule_helper diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pose_estimator_area.hpp similarity index 83% rename from localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp rename to localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pose_estimator_area.hpp index 74843c66a4eba..262b064615bc1 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/pose_estimator_area.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_ -#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_ +#ifndef RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_ +#define RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_ #include #include @@ -25,7 +25,7 @@ #include #include -namespace pose_estimator_arbiter::rule_helper +namespace autoware::pose_estimator_arbiter::rule_helper { class PoseEstimatorArea { @@ -51,6 +51,6 @@ class PoseEstimatorArea rclcpp::Logger logger_; }; -} // namespace pose_estimator_arbiter::rule_helper +} // namespace autoware::pose_estimator_arbiter::rule_helper -#endif // POSE_ESTIMATOR_ARBITER__RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_ +#endif // RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/pcd_map_based_rule.cpp similarity index 93% rename from localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp rename to localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/pcd_map_based_rule.cpp index 3a565e7f2e4df..cc5414ebd0a68 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/pcd_map_based_rule.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp" +#include "switch_rule/pcd_map_based_rule.hpp" #include #include -namespace pose_estimator_arbiter::switch_rule +namespace autoware::pose_estimator_arbiter::switch_rule { PcdMapBasedRule::PcdMapBasedRule( rclcpp::Node & node, std::unordered_set running_estimator_list, @@ -77,4 +77,4 @@ std::unordered_map PcdMapBasedRule::update() {PoseEstimatorType::artag, false}}; } -} // namespace pose_estimator_arbiter::switch_rule +} // namespace autoware::pose_estimator_arbiter::switch_rule diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/pcd_map_based_rule.hpp similarity index 71% rename from localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp rename to localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/pcd_map_based_rule.hpp index ab4d18dcad66a..45559fe37e606 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/pcd_map_based_rule.hpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_ -#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_ +#ifndef SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_ +#define SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_ -#include "pose_estimator_arbiter/pose_estimator_type.hpp" -#include "pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp" -#include "pose_estimator_arbiter/shared_data.hpp" -#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" +#include "pose_estimator_type.hpp" +#include "rule_helper/pcd_occupancy.hpp" +#include "shared_data.hpp" +#include "switch_rule/base_switch_rule.hpp" #include #include #include #include -namespace pose_estimator_arbiter::switch_rule +namespace autoware::pose_estimator_arbiter::switch_rule { class PcdMapBasedRule : public BaseSwitchRule { @@ -49,6 +49,6 @@ class PcdMapBasedRule : public BaseSwitchRule // Store the reason why which pose estimator is enabled mutable std::string debug_string_; }; -} // namespace pose_estimator_arbiter::switch_rule +} // namespace autoware::pose_estimator_arbiter::switch_rule -#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_ +#endif // SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/vector_map_based_rule.cpp similarity index 93% rename from localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp rename to localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/vector_map_based_rule.cpp index dffb738e87685..58cf9ab09e9a7 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/vector_map_based_rule.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp" +#include "switch_rule/vector_map_based_rule.hpp" #include #include -namespace pose_estimator_arbiter::switch_rule +namespace autoware::pose_estimator_arbiter::switch_rule { VectorMapBasedRule::VectorMapBasedRule( rclcpp::Node & node, std::unordered_set running_estimator_list, @@ -76,4 +76,4 @@ std::unordered_map VectorMapBasedRule::update() return enable_list; } -} // namespace pose_estimator_arbiter::switch_rule +} // namespace autoware::pose_estimator_arbiter::switch_rule diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/vector_map_based_rule.hpp similarity index 71% rename from localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp rename to localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/vector_map_based_rule.hpp index 83723a346054b..356651af223f3 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/src/switch_rule/vector_map_based_rule.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__SWITCH_RULE__VECTOR_MAP_BASED_RULE_HPP_ -#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__VECTOR_MAP_BASED_RULE_HPP_ +#ifndef SWITCH_RULE__VECTOR_MAP_BASED_RULE_HPP_ +#define SWITCH_RULE__VECTOR_MAP_BASED_RULE_HPP_ -#include "pose_estimator_arbiter/pose_estimator_type.hpp" -#include "pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp" -#include "pose_estimator_arbiter/shared_data.hpp" -#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" +#include "pose_estimator_type.hpp" +#include "rule_helper/pose_estimator_area.hpp" +#include "shared_data.hpp" +#include "switch_rule/base_switch_rule.hpp" #include #include @@ -26,7 +26,7 @@ #include #include -namespace pose_estimator_arbiter::switch_rule +namespace autoware::pose_estimator_arbiter::switch_rule { class VectorMapBasedRule : public BaseSwitchRule { @@ -50,6 +50,6 @@ class VectorMapBasedRule : public BaseSwitchRule std::unique_ptr pose_estimator_area_{nullptr}; }; -} // namespace pose_estimator_arbiter::switch_rule +} // namespace autoware::pose_estimator_arbiter::switch_rule -#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__VECTOR_MAP_BASED_RULE_HPP_ +#endif // SWITCH_RULE__VECTOR_MAP_BASED_RULE_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp b/localization/autoware_pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp similarity index 62% rename from localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp rename to localization/autoware_pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp index f53e01dc9c548..0d1f245123806 100644 --- a/localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp" +#include "switch_rule/pcd_map_based_rule.hpp" #include #include @@ -30,21 +30,21 @@ class PcdMapBasedRuleMockNode : public ::testing::Test node->declare_parameter("pcd_density_upper_threshold", 5); const auto running_estimator_list = - std::unordered_set{ - pose_estimator_arbiter::PoseEstimatorType::ndt, - pose_estimator_arbiter::PoseEstimatorType::yabloc, - pose_estimator_arbiter::PoseEstimatorType::eagleye, - pose_estimator_arbiter::PoseEstimatorType::artag}; + std::unordered_set{ + autoware::pose_estimator_arbiter::PoseEstimatorType::ndt, + autoware::pose_estimator_arbiter::PoseEstimatorType::yabloc, + autoware::pose_estimator_arbiter::PoseEstimatorType::eagleye, + autoware::pose_estimator_arbiter::PoseEstimatorType::artag}; - shared_data_ = std::make_shared(); + shared_data_ = std::make_shared(); - rule_ = std::make_shared( + rule_ = std::make_shared( *node, running_estimator_list, shared_data_); } std::shared_ptr node; - std::shared_ptr shared_data_; - std::shared_ptr rule_; + std::shared_ptr shared_data_; + std::shared_ptr rule_; void TearDown() override { rclcpp::shutdown(); } }; @@ -81,10 +81,10 @@ TEST_F(PcdMapBasedRuleMockNode, pcdMapBasedRule) shared_data_->localization_pose_cov.set_and_invoke( std::make_shared(position)); auto ret = rule_->update(); - EXPECT_TRUE(ret.at(pose_estimator_arbiter::PoseEstimatorType::ndt)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::yabloc)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::eagleye)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::artag)); + EXPECT_TRUE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::ndt)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::yabloc)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::eagleye)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::artag)); } { @@ -92,9 +92,9 @@ TEST_F(PcdMapBasedRuleMockNode, pcdMapBasedRule) shared_data_->localization_pose_cov.set_and_invoke( std::make_shared(position)); auto ret = rule_->update(); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::ndt)); - EXPECT_TRUE(ret.at(pose_estimator_arbiter::PoseEstimatorType::yabloc)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::eagleye)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::artag)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::ndt)); + EXPECT_TRUE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::yabloc)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::eagleye)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::artag)); } } diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp b/localization/autoware_pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp similarity index 87% rename from localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp rename to localization/autoware_pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp index 3258a1be34fda..10833b5498b89 100644 --- a/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_estimator_arbiter/rule_helper/grid_key.hpp" -#include "pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp" -#include "pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp" +#include "rule_helper/grid_key.hpp" +#include "rule_helper/pcd_occupancy.hpp" +#include "rule_helper/pose_estimator_area.hpp" #include @@ -64,7 +64,7 @@ TEST_F(RuleHelperMockNode, poseEstimatorArea) HADMapBin msg; lanelet::utils::conversion::toBinMsg(lanelet_map, &msg); - pose_estimator_arbiter::rule_helper::PoseEstimatorArea pose_estimator_area(&(*node)); + autoware::pose_estimator_arbiter::rule_helper::PoseEstimatorArea pose_estimator_area(&(*node)); pose_estimator_area.init(std::make_shared(msg)); EXPECT_TRUE(pose_estimator_area.within(Point().set__x(5).set__y(5).set__z(0), "ndt")); @@ -75,11 +75,11 @@ TEST_F(RuleHelperMockNode, poseEstimatorArea) TEST_F(RuleHelperMockNode, pcdOccupancy) { - using pose_estimator_arbiter::rule_helper::PcdOccupancy; + using autoware::pose_estimator_arbiter::rule_helper::PcdOccupancy; const int pcd_density_upper_threshold = 20; const int pcd_density_lower_threshold = 10; - pose_estimator_arbiter::rule_helper::PcdOccupancy pcd_occupancy( + autoware::pose_estimator_arbiter::rule_helper::PcdOccupancy pcd_occupancy( pcd_density_upper_threshold, pcd_density_lower_threshold); geometry_msgs::msg::Point point; @@ -91,7 +91,7 @@ TEST_F(RuleHelperMockNode, pcdOccupancy) TEST_F(RuleHelperMockNode, gridKey) { - using pose_estimator_arbiter::rule_helper::GridKey; + using autoware::pose_estimator_arbiter::rule_helper::GridKey; EXPECT_TRUE(GridKey(10., -5.) == GridKey(10., -10.)); EXPECT_TRUE(GridKey(10., -5.) != GridKey(10., -15.)); diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp b/localization/autoware_pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp similarity index 65% rename from localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp rename to localization/autoware_pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp index ec8c905bf8311..9d537f2048176 100644 --- a/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp +++ b/localization/autoware_pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp" +#include "switch_rule/vector_map_based_rule.hpp" #include @@ -33,21 +33,21 @@ class VectorMapBasedRuleMockNode : public ::testing::Test node = std::make_shared("test_node"); const auto running_estimator_list = - std::unordered_set{ - pose_estimator_arbiter::PoseEstimatorType::ndt, - pose_estimator_arbiter::PoseEstimatorType::yabloc, - pose_estimator_arbiter::PoseEstimatorType::eagleye, - pose_estimator_arbiter::PoseEstimatorType::artag}; + std::unordered_set{ + autoware::pose_estimator_arbiter::PoseEstimatorType::ndt, + autoware::pose_estimator_arbiter::PoseEstimatorType::yabloc, + autoware::pose_estimator_arbiter::PoseEstimatorType::eagleye, + autoware::pose_estimator_arbiter::PoseEstimatorType::artag}; - shared_data_ = std::make_shared(); + shared_data_ = std::make_shared(); - rule_ = std::make_shared( + rule_ = std::make_shared( *node, running_estimator_list, shared_data_); } std::shared_ptr node; - std::shared_ptr shared_data_; - std::shared_ptr rule_; + std::shared_ptr shared_data_; + std::shared_ptr rule_; void TearDown() override { rclcpp::shutdown(); } }; @@ -90,17 +90,17 @@ TEST_F(VectorMapBasedRuleMockNode, vectorMapBasedRule) { shared_data_->localization_pose_cov.set_and_invoke(create_pose(5, 5)); auto ret = rule_->update(); - EXPECT_TRUE(ret.at(pose_estimator_arbiter::PoseEstimatorType::ndt)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::yabloc)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::eagleye)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::artag)); + EXPECT_TRUE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::ndt)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::yabloc)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::eagleye)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::artag)); } { shared_data_->localization_pose_cov.set_and_invoke(create_pose(15, 15)); auto ret = rule_->update(); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::ndt)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::yabloc)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::eagleye)); - EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::artag)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::ndt)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::yabloc)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::eagleye)); + EXPECT_FALSE(ret.at(autoware::pose_estimator_arbiter::PoseEstimatorType::artag)); } } diff --git a/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml b/localization/autoware_pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml similarity index 93% rename from localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml rename to localization/autoware_pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml index b5be96fc3ce44..d38f0e2a104a8 100644 --- a/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml +++ b/localization/autoware_pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/localization/pose_estimator_arbiter/media/architecture.drawio.svg b/localization/autoware_pose_estimator_arbiter/media/architecture.drawio.svg similarity index 100% rename from localization/pose_estimator_arbiter/media/architecture.drawio.svg rename to localization/autoware_pose_estimator_arbiter/media/architecture.drawio.svg diff --git a/localization/pose_estimator_arbiter/media/pcd_occupancy.drawio.svg b/localization/autoware_pose_estimator_arbiter/media/pcd_occupancy.drawio.svg similarity index 100% rename from localization/pose_estimator_arbiter/media/pcd_occupancy.drawio.svg rename to localization/autoware_pose_estimator_arbiter/media/pcd_occupancy.drawio.svg diff --git a/localization/pose_estimator_arbiter/media/pose_estimator_area_in_vector_map.png b/localization/autoware_pose_estimator_arbiter/media/pose_estimator_area_in_vector_map.png similarity index 100% rename from localization/pose_estimator_arbiter/media/pose_estimator_area_in_vector_map.png rename to localization/autoware_pose_estimator_arbiter/media/pose_estimator_area_in_vector_map.png diff --git a/localization/pose_estimator_arbiter/media/single_pose_estimator.drawio.svg b/localization/autoware_pose_estimator_arbiter/media/single_pose_estimator.drawio.svg similarity index 100% rename from localization/pose_estimator_arbiter/media/single_pose_estimator.drawio.svg rename to localization/autoware_pose_estimator_arbiter/media/single_pose_estimator.drawio.svg diff --git a/localization/pose_estimator_arbiter/package.xml b/localization/autoware_pose_estimator_arbiter/package.xml similarity index 97% rename from localization/pose_estimator_arbiter/package.xml rename to localization/autoware_pose_estimator_arbiter/package.xml index d97c9f15fc8ae..f3aa30a2c6425 100644 --- a/localization/pose_estimator_arbiter/package.xml +++ b/localization/autoware_pose_estimator_arbiter/package.xml @@ -1,7 +1,7 @@ - pose_estimator_arbiter + autoware_pose_estimator_arbiter 0.1.0 The arbiter of multiple pose estimators Yamato Ando diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp b/localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter.hpp similarity index 89% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp rename to localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter.hpp index 013b4b38f9ef6..b70ab378eac67 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp +++ b/localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_ARBITER_HPP_ -#define POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_ARBITER_HPP_ +#ifndef POSE_ESTIMATOR_ARBITER_HPP_ +#define POSE_ESTIMATOR_ARBITER_HPP_ -#include "pose_estimator_arbiter/shared_data.hpp" -#include "pose_estimator_arbiter/stopper/base_stopper.hpp" -#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" +#include "shared_data.hpp" +#include "stopper/base_stopper.hpp" +#include "switch_rule/base_switch_rule.hpp" #include #include @@ -32,7 +32,7 @@ #include #include -namespace pose_estimator_arbiter +namespace autoware::pose_estimator_arbiter { class PoseEstimatorArbiter : public rclcpp::Node { @@ -95,6 +95,6 @@ class PoseEstimatorArbiter : public rclcpp::Node // Timer callback void on_timer(); }; -} // namespace pose_estimator_arbiter +} // namespace autoware::pose_estimator_arbiter -#endif // POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_ARBITER_HPP_ +#endif // POSE_ESTIMATOR_ARBITER_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp b/localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter_core.cpp similarity index 90% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp rename to localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter_core.cpp index 8e25628d6e0fc..9108d44e82fb5 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp +++ b/localization/autoware_pose_estimator_arbiter/src/pose_estimator_arbiter_core.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_estimator_arbiter/pose_estimator_arbiter.hpp" -#include "pose_estimator_arbiter/pose_estimator_type.hpp" -#include "pose_estimator_arbiter/stopper/stopper_artag.hpp" -#include "pose_estimator_arbiter/stopper/stopper_eagleye.hpp" -#include "pose_estimator_arbiter/stopper/stopper_ndt.hpp" -#include "pose_estimator_arbiter/stopper/stopper_yabloc.hpp" -#include "pose_estimator_arbiter/switch_rule/enable_all_rule.hpp" +#include "pose_estimator_arbiter.hpp" +#include "pose_estimator_type.hpp" +#include "stopper/stopper_artag.hpp" +#include "stopper/stopper_eagleye.hpp" +#include "stopper/stopper_ndt.hpp" +#include "stopper/stopper_yabloc.hpp" +#include "switch_rule/enable_all_rule.hpp" #include -namespace pose_estimator_arbiter +namespace autoware::pose_estimator_arbiter { // Parses ros param to get the estimator set that is running static std::unordered_set parse_estimator_name_args( @@ -111,9 +111,9 @@ PoseEstimatorArbiter::PoseEstimatorArbiter(const rclcpp::NodeOptions & options) load_switch_rule(); // Timer callback - auto on_timer = std::bind(&PoseEstimatorArbiter::on_timer, this); - timer_ = - rclcpp::create_timer(this, this->get_clock(), rclcpp::Rate(1).period(), std::move(on_timer)); + auto on_timer_callback = std::bind(&PoseEstimatorArbiter::on_timer, this); + timer_ = rclcpp::create_timer( + this, this->get_clock(), rclcpp::Rate(1).period(), std::move(on_timer_callback)); // Enable all pose estimators at the first toggle_all(true); @@ -210,7 +210,7 @@ void PoseEstimatorArbiter::on_timer() publish_diagnostics(); } -} // namespace pose_estimator_arbiter +} // namespace autoware::pose_estimator_arbiter #include -RCLCPP_COMPONENTS_REGISTER_NODE(pose_estimator_arbiter::PoseEstimatorArbiter) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pose_estimator_arbiter::PoseEstimatorArbiter) diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp b/localization/autoware_pose_estimator_arbiter/src/pose_estimator_type.hpp similarity index 73% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp rename to localization/autoware_pose_estimator_arbiter/src/pose_estimator_type.hpp index d78bfeb05b4f0..98f92612f8bc7 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp +++ b/localization/autoware_pose_estimator_arbiter/src/pose_estimator_type.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_TYPE_HPP_ -#define POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_TYPE_HPP_ +#ifndef POSE_ESTIMATOR_TYPE_HPP_ +#define POSE_ESTIMATOR_TYPE_HPP_ -namespace pose_estimator_arbiter +namespace autoware::pose_estimator_arbiter { enum class PoseEstimatorType : int { ndt = 1, yabloc = 2, eagleye = 4, artag = 8 }; -} // namespace pose_estimator_arbiter +} // namespace autoware::pose_estimator_arbiter -#endif // POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_TYPE_HPP_ +#endif // POSE_ESTIMATOR_TYPE_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp b/localization/autoware_pose_estimator_arbiter/src/shared_data.hpp similarity index 93% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp rename to localization/autoware_pose_estimator_arbiter/src/shared_data.hpp index 0d3dbfab11cbe..6332b77a9ed31 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp +++ b/localization/autoware_pose_estimator_arbiter/src/shared_data.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__SHARED_DATA_HPP_ -#define POSE_ESTIMATOR_ARBITER__SHARED_DATA_HPP_ +#ifndef SHARED_DATA_HPP_ +#define SHARED_DATA_HPP_ #include #include @@ -25,7 +25,7 @@ #include #include -namespace pose_estimator_arbiter +namespace autoware::pose_estimator_arbiter { template struct CallbackInvokingVariable @@ -97,5 +97,5 @@ struct SharedData InitializationState{}.set__state(InitializationState::UNINITIALIZED))}; }; -} // namespace pose_estimator_arbiter -#endif // POSE_ESTIMATOR_ARBITER__SHARED_DATA_HPP_ +} // namespace autoware::pose_estimator_arbiter +#endif // SHARED_DATA_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp b/localization/autoware_pose_estimator_arbiter/src/stopper/base_stopper.hpp similarity index 81% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp rename to localization/autoware_pose_estimator_arbiter/src/stopper/base_stopper.hpp index d64592a12308e..8b47b0e3b3a9a 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp +++ b/localization/autoware_pose_estimator_arbiter/src/stopper/base_stopper.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__STOPPER__BASE_STOPPER_HPP_ -#define POSE_ESTIMATOR_ARBITER__STOPPER__BASE_STOPPER_HPP_ +#ifndef STOPPER__BASE_STOPPER_HPP_ +#define STOPPER__BASE_STOPPER_HPP_ -#include "pose_estimator_arbiter/shared_data.hpp" +#include "shared_data.hpp" #include #include #include -namespace pose_estimator_arbiter::stopper +namespace autoware::pose_estimator_arbiter::stopper { class BaseStopper { @@ -48,6 +48,6 @@ class BaseStopper rclcpp::Logger logger_; std::shared_ptr shared_data_{nullptr}; }; -} // namespace pose_estimator_arbiter::stopper +} // namespace autoware::pose_estimator_arbiter::stopper -#endif // POSE_ESTIMATOR_ARBITER__STOPPER__BASE_STOPPER_HPP_ +#endif // STOPPER__BASE_STOPPER_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp b/localization/autoware_pose_estimator_arbiter/src/stopper/stopper_artag.hpp similarity index 82% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp rename to localization/autoware_pose_estimator_arbiter/src/stopper/stopper_artag.hpp index f334983f33aad..3635ebd39c545 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp +++ b/localization/autoware_pose_estimator_arbiter/src/stopper/stopper_artag.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_ARTAG_HPP_ -#define POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_ARTAG_HPP_ +#ifndef STOPPER__STOPPER_ARTAG_HPP_ +#define STOPPER__STOPPER_ARTAG_HPP_ -#include "pose_estimator_arbiter/stopper/base_stopper.hpp" +#include "stopper/base_stopper.hpp" #include #include #include -namespace pose_estimator_arbiter::stopper +namespace autoware::pose_estimator_arbiter::stopper { class StopperArTag : public BaseStopper { @@ -52,6 +52,6 @@ class StopperArTag : public BaseStopper bool ar_tag_is_enabled_; rclcpp::Publisher::SharedPtr pub_image_; }; -} // namespace pose_estimator_arbiter::stopper +} // namespace autoware::pose_estimator_arbiter::stopper -#endif // POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_ARTAG_HPP_ +#endif // STOPPER__STOPPER_ARTAG_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp b/localization/autoware_pose_estimator_arbiter/src/stopper/stopper_eagleye.hpp similarity index 81% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp rename to localization/autoware_pose_estimator_arbiter/src/stopper/stopper_eagleye.hpp index cc800ad6bdee4..cfddf3e13014f 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp +++ b/localization/autoware_pose_estimator_arbiter/src/stopper/stopper_eagleye.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_EAGLEYE_HPP_ -#define POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_EAGLEYE_HPP_ -#include "pose_estimator_arbiter/stopper/base_stopper.hpp" +#ifndef STOPPER__STOPPER_EAGLEYE_HPP_ +#define STOPPER__STOPPER_EAGLEYE_HPP_ +#include "stopper/base_stopper.hpp" #include #include -namespace pose_estimator_arbiter::stopper +namespace autoware::pose_estimator_arbiter::stopper { class StopperEagleye : public BaseStopper { @@ -49,6 +49,6 @@ class StopperEagleye : public BaseStopper bool eagleye_is_enabled_; rclcpp::Publisher::SharedPtr pub_pose_; }; -} // namespace pose_estimator_arbiter::stopper +} // namespace autoware::pose_estimator_arbiter::stopper -#endif // POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_EAGLEYE_HPP_ +#endif // STOPPER__STOPPER_EAGLEYE_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp b/localization/autoware_pose_estimator_arbiter/src/stopper/stopper_ndt.hpp similarity index 81% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp rename to localization/autoware_pose_estimator_arbiter/src/stopper/stopper_ndt.hpp index 3b7c083ea31e3..eba43118c7860 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp +++ b/localization/autoware_pose_estimator_arbiter/src/stopper/stopper_ndt.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_NDT_HPP_ -#define POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_NDT_HPP_ -#include "pose_estimator_arbiter/stopper/base_stopper.hpp" +#ifndef STOPPER__STOPPER_NDT_HPP_ +#define STOPPER__STOPPER_NDT_HPP_ +#include "stopper/base_stopper.hpp" #include #include -namespace pose_estimator_arbiter::stopper +namespace autoware::pose_estimator_arbiter::stopper { class StopperNdt : public BaseStopper { @@ -49,6 +49,6 @@ class StopperNdt : public BaseStopper rclcpp::Publisher::SharedPtr pub_pointcloud_; bool ndt_is_enabled_; }; -} // namespace pose_estimator_arbiter::stopper +} // namespace autoware::pose_estimator_arbiter::stopper -#endif // POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_NDT_HPP_ +#endif // STOPPER__STOPPER_NDT_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp b/localization/autoware_pose_estimator_arbiter/src/stopper/stopper_yabloc.hpp similarity index 89% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp rename to localization/autoware_pose_estimator_arbiter/src/stopper/stopper_yabloc.hpp index 808a5bf9407ca..77aa9603ac06a 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp +++ b/localization/autoware_pose_estimator_arbiter/src/stopper/stopper_yabloc.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_YABLOC_HPP_ -#define POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_YABLOC_HPP_ -#include "pose_estimator_arbiter/stopper/base_stopper.hpp" +#ifndef STOPPER__STOPPER_YABLOC_HPP_ +#define STOPPER__STOPPER_YABLOC_HPP_ +#include "stopper/base_stopper.hpp" #include #include #include -namespace pose_estimator_arbiter::stopper +namespace autoware::pose_estimator_arbiter::stopper { class StopperYabLoc : public BaseStopper { @@ -86,5 +86,5 @@ class StopperYabLoc : public BaseStopper rclcpp::Client::SharedPtr enable_service_client_; rclcpp::Publisher::SharedPtr pub_image_; }; -} // namespace pose_estimator_arbiter::stopper -#endif // POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_YABLOC_HPP_ +} // namespace autoware::pose_estimator_arbiter::stopper +#endif // STOPPER__STOPPER_YABLOC_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp b/localization/autoware_pose_estimator_arbiter/src/switch_rule/base_switch_rule.hpp similarity index 78% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp rename to localization/autoware_pose_estimator_arbiter/src/switch_rule/base_switch_rule.hpp index 95d10c2b741a8..0462ce6e7d340 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp +++ b/localization/autoware_pose_estimator_arbiter/src/switch_rule/base_switch_rule.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__SWITCH_RULE__BASE_SWITCH_RULE_HPP_ -#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__BASE_SWITCH_RULE_HPP_ +#ifndef SWITCH_RULE__BASE_SWITCH_RULE_HPP_ +#define SWITCH_RULE__BASE_SWITCH_RULE_HPP_ -#include "pose_estimator_arbiter/pose_estimator_type.hpp" +#include "pose_estimator_type.hpp" #include @@ -26,7 +26,7 @@ #include #include -namespace pose_estimator_arbiter::switch_rule +namespace autoware::pose_estimator_arbiter::switch_rule { class BaseSwitchRule { @@ -44,7 +44,8 @@ class BaseSwitchRule BaseSwitchRule(BaseSwitchRule && other) noexcept = default; BaseSwitchRule & operator=(const BaseSwitchRule & other) = default; BaseSwitchRule & operator=(BaseSwitchRule && other) noexcept = default; - virtual std::unordered_map update() = 0; + virtual std::unordered_map + update() = 0; virtual std::string debug_string() { return std::string{}; } virtual MarkerArray debug_marker_array() { return MarkerArray{}; } @@ -53,6 +54,6 @@ class BaseSwitchRule std::shared_ptr logger_ptr_{nullptr}; }; -} // namespace pose_estimator_arbiter::switch_rule +} // namespace autoware::pose_estimator_arbiter::switch_rule -#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__BASE_SWITCH_RULE_HPP_ +#endif // SWITCH_RULE__BASE_SWITCH_RULE_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp b/localization/autoware_pose_estimator_arbiter/src/switch_rule/enable_all_rule.cpp similarity index 88% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp rename to localization/autoware_pose_estimator_arbiter/src/switch_rule/enable_all_rule.cpp index aebad094a0eca..9767e9ef4fba4 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp +++ b/localization/autoware_pose_estimator_arbiter/src/switch_rule/enable_all_rule.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_estimator_arbiter/switch_rule/enable_all_rule.hpp" +#include "switch_rule/enable_all_rule.hpp" #include @@ -22,7 +22,7 @@ #include #include -namespace pose_estimator_arbiter::switch_rule +namespace autoware::pose_estimator_arbiter::switch_rule { EnableAllRule::EnableAllRule( rclcpp::Node & node, std::unordered_set running_estimator_list, @@ -41,4 +41,4 @@ std::unordered_map EnableAllRule::update() }; } -} // namespace pose_estimator_arbiter::switch_rule +} // namespace autoware::pose_estimator_arbiter::switch_rule diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp b/localization/autoware_pose_estimator_arbiter/src/switch_rule/enable_all_rule.hpp similarity index 71% rename from localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp rename to localization/autoware_pose_estimator_arbiter/src/switch_rule/enable_all_rule.hpp index 63142b0e662e1..bf2aa68df35d5 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp +++ b/localization/autoware_pose_estimator_arbiter/src/switch_rule/enable_all_rule.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_ESTIMATOR_ARBITER__SWITCH_RULE__ENABLE_ALL_RULE_HPP_ -#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__ENABLE_ALL_RULE_HPP_ +#ifndef SWITCH_RULE__ENABLE_ALL_RULE_HPP_ +#define SWITCH_RULE__ENABLE_ALL_RULE_HPP_ -#include "pose_estimator_arbiter/shared_data.hpp" -#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" +#include "shared_data.hpp" +#include "switch_rule/base_switch_rule.hpp" #include #include #include #include -namespace pose_estimator_arbiter::switch_rule +namespace autoware::pose_estimator_arbiter::switch_rule { class EnableAllRule : public BaseSwitchRule { @@ -38,6 +38,6 @@ class EnableAllRule : public BaseSwitchRule const std::unordered_set running_estimator_list_; }; -} // namespace pose_estimator_arbiter::switch_rule +} // namespace autoware::pose_estimator_arbiter::switch_rule -#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__ENABLE_ALL_RULE_HPP_ +#endif // SWITCH_RULE__ENABLE_ALL_RULE_HPP_ diff --git a/localization/pose_estimator_arbiter/test/test_pose_estimator_arbiter.py b/localization/autoware_pose_estimator_arbiter/test/test_pose_estimator_arbiter.py similarity index 98% rename from localization/pose_estimator_arbiter/test/test_pose_estimator_arbiter.py rename to localization/autoware_pose_estimator_arbiter/test/test_pose_estimator_arbiter.py index c419fb68e0571..a5ff04c002004 100644 --- a/localization/pose_estimator_arbiter/test/test_pose_estimator_arbiter.py +++ b/localization/autoware_pose_estimator_arbiter/test/test_pose_estimator_arbiter.py @@ -38,7 +38,7 @@ @pytest.mark.launch_test def generate_test_description(): test_pose_estimator_arbiter_launch_file = os.path.join( - get_package_share_directory("pose_estimator_arbiter"), + get_package_share_directory("autoware_pose_estimator_arbiter"), "launch", "pose_estimator_arbiter.launch.xml", ) diff --git a/localization/pose_estimator_arbiter/test/test_shared_data.cpp b/localization/autoware_pose_estimator_arbiter/test/test_shared_data.cpp similarity index 90% rename from localization/pose_estimator_arbiter/test/test_shared_data.cpp rename to localization/autoware_pose_estimator_arbiter/test/test_shared_data.cpp index 3c1fa50b502a1..c0f7044442d71 100644 --- a/localization/pose_estimator_arbiter/test/test_shared_data.cpp +++ b/localization/autoware_pose_estimator_arbiter/test/test_shared_data.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_estimator_arbiter/shared_data.hpp" +#include "shared_data.hpp" #include TEST(SharedData, callback_invoked_correctly) { - pose_estimator_arbiter::CallbackInvokingVariable variable; + autoware::pose_estimator_arbiter::CallbackInvokingVariable variable; // register callback bool callback_invoked = false; @@ -44,7 +44,7 @@ TEST(SharedData, callback_invoked_correctly) TEST(SharedData, multiple_callback_invoked_correctly) { - pose_estimator_arbiter::CallbackInvokingVariable variable; + autoware::pose_estimator_arbiter::CallbackInvokingVariable variable; // register callback int callback_invoked_num = 0; From 201f1647a97c9a16ed4d16c69d17bdc4faabf8ea Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 22 Aug 2024 23:27:21 +0900 Subject: [PATCH 09/46] feat(lane_change): fix delay logic that caused timing to be late (#8549) * RT1-5067 fix delay logic that caused timing to be late Signed-off-by: Muhammad Zulfaqar Azmi * remove autoware namespace Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> --------- Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> --- .../src/utils/utils.cpp | 48 ++++++++++++------- 1 file changed, 30 insertions(+), 18 deletions(-) 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 79c422ac283d1..e4a482def7b3d 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 @@ -14,6 +14,7 @@ #include "autoware/behavior_path_lane_change_module/utils/utils.hpp" +#include "autoware/behavior_path_lane_change_module/utils/calculation.hpp" #include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" #include "autoware/behavior_path_lane_change_module/utils/path.hpp" #include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" @@ -988,30 +989,41 @@ bool passed_parked_objects( } const auto & current_path_end = current_lane_path.points.back().point.pose.position; - double min_dist_to_end_of_current_lane = std::numeric_limits::max(); - const auto & target_lanes = common_data_ptr->lanes_ptr->target; - const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); - for (const auto & point : leading_obj_poly.outer()) { - const auto obj_p = autoware::universe_utils::createPoint(point.x(), point.y(), 0.0); - const double dist = autoware::motion_utils::calcSignedArcLength( - current_lane_path.points, obj_p, current_path_end); - min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane); - if (is_goal_in_route) { + const auto dist_to_path_end = [&](const auto & src_point) { + if (common_data_ptr->lanes_ptr->current_lane_in_goal_section) { const auto goal_pose = route_handler.getGoalPose(); - const double dist_to_goal = autoware::motion_utils::calcSignedArcLength( - current_lane_path.points, obj_p, goal_pose.position); - min_dist_to_end_of_current_lane = std::min(min_dist_to_end_of_current_lane, dist_to_goal); + return motion_utils::calcSignedArcLength( + current_lane_path.points, src_point, goal_pose.position); } - } + return motion_utils::calcSignedArcLength(current_lane_path.points, src_point, current_path_end); + }; + + const auto min_dist_to_end_of_current_lane = std::invoke([&]() { + auto min_dist_to_end_of_current_lane = std::numeric_limits::max(); + for (const auto & point : leading_obj_poly.outer()) { + const auto obj_p = universe_utils::createPoint(point.x(), point.y(), 0.0); + const auto dist = dist_to_path_end(obj_p); + min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane); + } + return min_dist_to_end_of_current_lane; + }); // 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) { - debug.second.unsafe_reason = "delay lane change"; - utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); - return false; + if (min_dist_to_end_of_current_lane <= minimum_lane_change_length) { + return true; } - return true; + 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); + + if (dist_ego_to_obj < lane_change_path.info.length.lane_changing) { + return true; + } + + debug.second.unsafe_reason = "delay lane change"; + utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); + return false; } std::optional getLeadingStaticObjectIdx( From 53f7d82f8c20ec2376fcb0e6bc3763407fb271f4 Mon Sep 17 00:00:00 2001 From: Yuxin Wang <87698138+Ariiees@users.noreply.github.com> Date: Thu, 22 Aug 2024 10:29:33 -0400 Subject: [PATCH 10/46] refactor(autoware_obstacle_stop_planner): rework parameters (#7795) Signed-off-by: Ariiees --- .../autoware_obstacle_stop_planner/README.md | 6 + .../adaptive_cruise_control.schema.json | 216 ++++++++++++ .../schema/common.schema.json | 85 +++++ .../schema/obstacle_stop_planner.schema.json | 322 ++++++++++++++++++ 4 files changed, 629 insertions(+) create mode 100644 planning/autoware_obstacle_stop_planner/schema/adaptive_cruise_control.schema.json create mode 100644 planning/autoware_obstacle_stop_planner/schema/common.schema.json create mode 100644 planning/autoware_obstacle_stop_planner/schema/obstacle_stop_planner.schema.json diff --git a/planning/autoware_obstacle_stop_planner/README.md b/planning/autoware_obstacle_stop_planner/README.md index 10870222aee09..d3965192cd4c3 100644 --- a/planning/autoware_obstacle_stop_planner/README.md +++ b/planning/autoware_obstacle_stop_planner/README.md @@ -31,6 +31,8 @@ ### Common Parameter +{{ json_to_markdown("planning/autoware_obstacle_stop_planner/schema/common.schema.json") }} + | Parameter | Type | Description | | -------------------------------------- | ------ | ----------------------------------------------------------------------------------------- | | `enable_slow_down` | bool | enable slow down planner [-] | @@ -103,6 +105,8 @@ stopped due to other factors. ### Parameters +{{ json_to_markdown("planning/autoware_obstacle_stop_planner/schema/obstacle_stop_planner.schema.json") }} + #### Stop position | Parameter | Type | Description | @@ -186,6 +190,8 @@ down section. ### Parameters +{{ json_to_markdown("planning/autoware_obstacle_stop_planner/schema/adaptive_cruise_control.schema.json") }} + #### Slow down section | Parameter | Type | Description | diff --git a/planning/autoware_obstacle_stop_planner/schema/adaptive_cruise_control.schema.json b/planning/autoware_obstacle_stop_planner/schema/adaptive_cruise_control.schema.json new file mode 100644 index 0000000000000..25a6a9338987b --- /dev/null +++ b/planning/autoware_obstacle_stop_planner/schema/adaptive_cruise_control.schema.json @@ -0,0 +1,216 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameter for adaptive cruise control", + "type": "object", + "definitions": { + "adaptive_cruise_control": { + "type": "object", + "properties": { + "adaptive_cruise_control": { + "type": "object", + "properties": { + "use_object_to_estimate_vel": { + "type": "boolean", + "description": "use tracking objects for estimating object velocity or not", + "default": "true" + }, + "use_pcl_to_estimate_vel": { + "type": "boolean", + "description": "use pcl for estimating object velocity or not", + "default": "true" + }, + "consider_obj_velocity": { + "type": "boolean", + "description": "consider forward vehicle velocity to ACC or not", + "default": "true" + }, + "obstacle_velocity_thresh_to_start_acc": { + "type": "number", + "description": "start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s]", + "default": "1.5" + }, + "obstacle_velocity_thresh_to_stop_acc": { + "type": "number", + "description": "stop adaptive cruise control when the velocity of the forward obstacle falls below this value [m/s]", + "default": "1.0" + }, + "emergency_stop_acceleration": { + "type": "number", + "description": "supposed minimum acceleration (deceleration) in emergency stop [m/ss]", + "default": "-5.0" + }, + "emergency_stop_idling_time": { + "type": "number", + "description": "supposed idling time to start emergency stop [s]", + "default": "0.5" + }, + "min_dist_stop": { + "type": "number", + "description": "minimum distance of emergency stop [m]", + "default": "4.0" + }, + "obstacle_emergency_stop_acceleration": { + "type": "number", + "description": "supposed minimum acceleration (deceleration) in emergency stop [m/ss]", + "default": "-5.0" + }, + "max_standard_acceleration": { + "type": "number", + "description": "supposed maximum acceleration in active cruise control [m/ss]", + "default": "0.5" + }, + "min_standard_acceleration": { + "type": "number", + "description": "supposed minimum acceleration (deceleration) in active cruise control", + "default": "-1.0" + }, + "standard_idling_time": { + "type": "number", + "description": "supposed idling time to react object in active cruise control [s]", + "default": "0.5" + }, + "min_dist_standard": { + "type": "number", + "description": "minimum distance in active cruise control [m]", + "default": "4.0" + }, + "obstacle_min_standard_acceleration": { + "type": "number", + "description": "supposed minimum acceleration of forward obstacle [m/ss]", + "default": "-1.5" + }, + "margin_rate_to_change_vel": { + "type": "number", + "description": "margin to insert upper velocity [-]", + "default": "0.3" + }, + "use_time_compensation_to_calc_distance": { + "type": "boolean", + "description": "use time-compensation to calculate distance to forward vehicle", + "default": "true" + }, + "p_coefficient_positive": { + "type": "number", + "description": "coefficient P in PID control (used when target dist -current_dist >=0) [-]", + "default": "0.1" + }, + "p_coefficient_negative": { + "type": "number", + "description": "coefficient P in PID control (used when target dist -current_dist <0) [-]", + "default": "0.3" + }, + "d_coefficient_positive": { + "type": "number", + "description": "coefficient D in PID control (used when delta_dist >=0) [-]", + "default": "0.0" + }, + "d_coefficient_negative": { + "type": "number", + "description": "coefficient D in PID control (used when delta_dist <0) [-]", + "default": "0.2" + }, + "object_polygon_length_margin": { + "type": "number", + "description": "The distance to extend the polygon length the object in pointcloud-object matching [m]", + "default": "2.0" + }, + "object_polygon_width_margin": { + "type": "number", + "description": "The distance to extend the polygon width the object in pointcloud-object matching [m]", + "default": "0.5" + }, + "valid_estimated_vel_diff_time": { + "type": "number", + "description": "Maximum time difference treated as continuous points in speed estimation using a point cloud [s]", + "default": "1.0" + }, + "valid_vel_que_time": { + "type": "number", + "description": "Time width of information used for speed estimation in speed estimation using a point cloud [s]", + "default": "0.5" + }, + "valid_estimated_vel_max": { + "type": "number", + "description": "Maximum value of valid speed estimation results in speed estimation using a point cloud [m/s]", + "default": "20.0" + }, + "valid_estimated_vel_min": { + "type": "number", + "description": "Minimum value of valid speed estimation results in speed estimation using a point cloud [m/s]", + "default": "-20.0" + }, + "thresh_vel_to_stop": { + "type": "number", + "description": "Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s]", + "default": "1.5" + }, + "lowpass_gain_of_upper_velocity": { + "type": "number", + "description": "Lowpass-gain of upper velocity", + "default": "0.75" + }, + "use_rough_velocity_estimation": { + "type": "boolean", + "description": "Use rough estimated velocity if the velocity estimation is failed (#### If this parameter is true, the vehicle may collide with the front car. Be careful. ####)", + "default": "false" + }, + "rough_velocity_rate": { + "type": "number", + "description": "In the rough velocity estimation, the velocity of front car is estimated as self current velocity * this value", + "default": "0.9" + } + }, + "required": [ + "use_object_to_estimate_vel", + "use_pcl_to_estimate_vel", + "consider_obj_velocity", + "obstacle_velocity_thresh_to_start_acc", + "obstacle_velocity_thresh_to_stop_acc", + "emergency_stop_acceleration", + "emergency_stop_idling_time", + "min_dist_stop", + "obstacle_emergency_stop_acceleration", + "max_standard_acceleration", + "min_standard_acceleration", + "standard_idling_time", + "min_dist_standard", + "obstacle_min_standard_acceleration", + "margin_rate_to_change_vel", + "use_time_compensation_to_calc_distance", + "p_coefficient_positive", + "p_coefficient_negative", + "d_coefficient_positive", + "d_coefficient_negative", + "object_polygon_length_margin", + "object_polygon_width_margin", + "valid_estimated_vel_diff_time", + "valid_vel_que_time", + "valid_estimated_vel_max", + "valid_estimated_vel_min", + "thresh_vel_to_stop", + "lowpass_gain_of_upper_velocity", + "use_rough_velocity_estimation", + "rough_velocity_rate" + ], + "additionalProperties": false + } + }, + "required": ["adaptive_cruise_control"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/adaptive_cruise_control" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/planning/autoware_obstacle_stop_planner/schema/common.schema.json b/planning/autoware_obstacle_stop_planner/schema/common.schema.json new file mode 100644 index 0000000000000..d8360f48c1617 --- /dev/null +++ b/planning/autoware_obstacle_stop_planner/schema/common.schema.json @@ -0,0 +1,85 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameter for common in autoware_obstacle_stop_planner", + "type": "object", + "definitions": { + "common": { + "type": "object", + "properties": { + "max_vel": { + "type": "number", + "description": "max velocity limit [m/s]", + "default": "11.1" + }, + "normal": { + "type": "object", + "properties": { + "min_acc": { + "type": "number", + "description": "min deceleration [m/ss]", + "default": "-0.5" + }, + "max_acc": { + "type": "number", + "description": "max acceleration [m/ss]", + "default": "1.0" + }, + "min_jerk": { + "type": "number", + "description": "min jerk [m/sss]", + "default": "-0.5" + }, + "max_jerk": { + "type": "number", + "description": "max jerk [m/sss]", + "default": "1.0" + } + }, + "required": ["min_acc", "max_acc", "min_jerk", "max_jerk"] + }, + "limit": { + "type": "object", + "properties": { + "min_acc": { + "type": "number", + "description": "min deceleration limit [m/ss]", + "default": "-2.5" + }, + "max_acc": { + "type": "number", + "description": "max acceleration [m/ss]", + "default": "1.0" + }, + "min_jerk": { + "type": "number", + "description": "min jerk [m/sss]", + "default": "-1.5" + }, + "max_jerk": { + "type": "number", + "description": "max jerk [m/sss]", + "default": "1.5" + } + }, + "required": ["min_acc", "max_acc", "min_jerk", "max_jerk"] + } + }, + "required": ["max_vel", "normal", "limit"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/common" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} 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 new file mode 100644 index 0000000000000..868f669ad2004 --- /dev/null +++ b/planning/autoware_obstacle_stop_planner/schema/obstacle_stop_planner.schema.json @@ -0,0 +1,322 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameter for obstacle stop planner", + "type": "object", + "definitions": { + "obstacle_stop_planner": { + "type": "object", + "properties": { + "chattering_threshold": { + "type": "number", + "description": "even if the obstacle disappears, the stop judgment continues for chattering_threshold [s]", + "default": "0.5" + }, + "max_velocity": { + "type": "number", + "description": "max velocity [m/s]", + "default": "20.0" + }, + "ego_nearest_dist_threshold": { + "type": "number", + "description": "[m]", + "default": "3.0" + }, + "ego_nearest_yaw_threshold": { + "type": "number", + "description": "[rad] = 60 [deg]", + "default": "1.046" + }, + "enable_slow_down": { + "type": "boolean", + "description": "whether to use slow down planner [-]", + "default": "false" + }, + "enable_z_axis_obstacle_filtering": { + "type": "boolean", + "description": "filter obstacles in z axis (height) [-]", + "default": "true" + }, + "z_axis_filtering_buffer": { + "type": "number", + "description": "additional buffer for z axis filtering [m]", + "default": "0.0" + }, + "voxel_grid_x": { + "type": "number", + "description": "voxel grid x parameter for filtering pointcloud [m]", + "default": "0.05" + }, + "voxel_grid_y": { + "type": "number", + "description": "voxel grid y parameter for filtering pointcloud [m]", + "default": "0.05" + }, + "voxel_grid_z": { + "type": "number", + "description": "voxel grid z parameter for filtering pointcloud [m]", + "default": "100000.0" + }, + "use_predicted_objects": { + "type": "boolean", + "description": "whether to use predicted objects [-]", + "default": "false" + }, + "publish_obstacle_polygon": { + "type": "boolean", + "description": "whether to publish obstacle polygon [-]", + "default": "false" + }, + "predicted_object_filtering_threshold": { + "type": "number", + "description": "threshold for filtering predicted objects (valid only publish_obstacle_polygon true) [m]", + "default": "1.5" + }, + "stop_planner": { + "type": "object", + "properties": { + "stop_position": { + "type": "object", + "properties": { + "max_longitudinal_margin": { + "type": "number", + "description": "stop margin distance from obstacle on the path [m]", + "default": "5.0" + }, + "max_longitudinal_margin_behind_goal": { + "type": "number", + "description": "stop margin distance from obstacle behind the goal on the path [m]", + "default": "3.0" + }, + "min_longitudinal_margin": { + "type": "number", + "description": "stop margin distance when any other stop point is inserted in stop margin [m]", + "default": "2.0" + }, + "hold_stop_margin_distance": { + "type": "number", + "description": "the ego keeps stopping if the ego is in this margin [m]", + "default": "0.0" + } + }, + "required": [ + "max_longitudinal_margin", + "max_longitudinal_margin_behind_goal", + "min_longitudinal_margin", + "hold_stop_margin_distance" + ] + }, + "detection_area": { + "type": "object", + "properties": { + "lateral_margin": { + "type": "number", + "description": "margin [m]", + "default": "0.0" + }, + "vehicle_lateral_margin": { + "type": "number", + "description": "margin of vehicle footprint [m]", + "default": "0.0" + }, + "pedestrian_lateral_margin": { + "type": "number", + "description": "margin of pedestrian footprint [m]", + "default": "0.0" + }, + "unknown_lateral_margin": { + "type": "number", + "description": "margin of unknown footprint [m]", + "default": "0.0" + }, + "step_length": { + "type": "number", + "description": "step length for pointcloud search range [m]", + "default": "1.0" + }, + "enable_stop_behind_goal_for_obstacle": { + "type": "boolean", + "description": "enable extend trajectory after goal lane for obstacle detection", + "default": "true" + } + }, + "required": [ + "lateral_margin", + "vehicle_lateral_margin", + "pedestrian_lateral_margin", + "unknown_lateral_margin", + "step_length", + "enable_stop_behind_goal_for_obstacle" + ] + } + }, + "required": ["stop_position", "detection_area"] + }, + "slow_down_planner": { + "type": "object", + "properties": { + "slow_down_section": { + "type": "object", + "properties": { + "longitudinal_forward_margin": { + "type": "number", + "description": "margin distance from slow down point to vehicle front [m]", + "default": "5.0" + }, + "longitudinal_backward_margin": { + "type": "number", + "description": "margin distance from slow down point to vehicle rear [m]", + "default": "5.0" + }, + "longitudinal_margin_span": { + "type": "number", + "description": "fineness param for relaxing slow down margin (use this param if consider_constraints is True) [m/s]", + "default": "-0.1" + }, + "min_longitudinal_forward_margin": { + "type": "number", + "description": "min margin for relaxing slow down margin (use this param if consider_constraints is True) [m/s]", + "default": "1.0" + } + }, + "required": [ + "longitudinal_forward_margin", + "longitudinal_backward_margin", + "longitudinal_margin_span", + "min_longitudinal_forward_margin" + ] + }, + "detection_area": { + "type": "object", + "properties": { + "lateral_margin": { + "type": "number", + "description": "offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]", + "default": "1.0" + }, + "vehicle_lateral_margin": { + "type": "number", + "description": "offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]", + "default": "1.0" + }, + "pedestrian_lateral_margin": { + "type": "number", + "description": "offset from pedestrian side edge for expanding the search area of the surrounding point cloud [m]", + "default": "1.0" + }, + "unknown_lateral_margin": { + "type": "number", + "description": "offset from unknown side edge for expanding the search area of the surrounding point cloud [m]", + "default": "1.0" + } + }, + "required": [ + "lateral_margin", + "vehicle_lateral_margin", + "pedestrian_lateral_margin", + "unknown_lateral_margin" + ] + }, + "target_velocity": { + "type": "object", + "properties": { + "max_slow_down_velocity": { + "type": "number", + "description": "max slow down velocity (use this param if consider_constraints is False)[m/s]", + "default": "1.38" + }, + "min_slow_down_velocity": { + "type": "number", + "description": "offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]", + "default": "0.28" + }, + "slow_down_velocity": { + "type": "number", + "description": "target slow down velocity (use this param if consider_constraints is True)[m/s]", + "default": "1.38" + } + }, + "required": ["max_slow_down_velocity", "min_slow_down_velocity", "slow_down_velocity"] + }, + "constraints": { + "type": "object", + "properties": { + "jerk_min_slow_down": { + "type": "number", + "description": "min slow down jerk constraint [m/sss]", + "default": "-0.3" + }, + "jerk_span": { + "type": "number", + "description": "fineness param for planning deceleration jerk [m/sss]", + "default": "-0.01" + }, + "jerk_start": { + "type": "number", + "description": "init jerk used for deceleration planning [m/sss]", + "default": "-0.1" + } + }, + "required": ["jerk_min_slow_down", "jerk_span", "jerk_start"] + }, + "consider_constraints": { + "type": "boolean", + "description": "set 'True', if no decel plan found under jerk/dec constrains, relax target slow down vel [-]", + "default": "false" + }, + "velocity_threshold_decel_complete": { + "type": "number", + "description": "use for judge whether the ego velocity converges the target slow down velocity [m/s]", + "default": "0.2" + }, + "acceleration_threshold_decel_complete": { + "type": "number", + "description": "use for judge whether the ego velocity converges the target slow down velocity [m/ss]", + "default": "0.1" + } + }, + "required": [ + "slow_down_section", + "detection_area", + "target_velocity", + "constraints", + "consider_constraints", + "velocity_threshold_decel_complete", + "acceleration_threshold_decel_complete" + ] + } + }, + "required": [ + "chattering_threshold", + "max_velocity", + "ego_nearest_dist_threshold", + "ego_nearest_yaw_threshold", + "enable_slow_down", + "enable_z_axis_obstacle_filtering", + "z_axis_filtering_buffer", + "voxel_grid_x", + "voxel_grid_y", + "voxel_grid_z", + "use_predicted_objects", + "publish_obstacle_polygon", + "predicted_object_filtering_threshold", + "stop_planner", + "slow_down_planner" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/obstacle_stop_planner" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} From d75f87520e6af854ee3cf2a05738afe88254c2f3 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 23 Aug 2024 09:38:44 +0900 Subject: [PATCH 11/46] feat(autonomous_emergency_braking): enable aeb with only one req path (#8569) * make it so AEB works with only one req path type (imu or MPC) Signed-off-by: Daniel Sanchez * fix missing mpc path return Signed-off-by: Daniel Sanchez * add check Signed-off-by: Daniel Sanchez * modify no path msg Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../src/node.cpp | 29 ++++++++++++------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index cd97a8301ad9d..dc8c876bd56f6 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -348,21 +348,30 @@ bool AEB::fetchLatestData() return missing("object detection method (pointcloud or predicted objects)"); } - const auto imu_ptr = sub_imu_.takeData(); - if (use_imu_path_) { + const bool has_imu_path = std::invoke([&]() { + if (!use_imu_path_) return false; + const auto imu_ptr = sub_imu_.takeData(); if (!imu_ptr) { return missing("imu message"); } // imu_ptr is valid onImu(imu_ptr); - } - if (use_imu_path_ && !angular_velocity_ptr_) { - return missing("imu"); - } + return (!angular_velocity_ptr_) ? missing("imu") : true; + }); - predicted_traj_ptr_ = sub_predicted_traj_.takeData(); - if (use_predicted_trajectory_ && !predicted_traj_ptr_) { - return missing("control predicted trajectory"); + const bool has_predicted_path = std::invoke([&]() { + if (!use_predicted_trajectory_) { + return false; + } + predicted_traj_ptr_ = sub_predicted_traj_.takeData(); + return (!predicted_traj_ptr_) ? missing("control predicted trajectory") : true; + }); + + if (!has_imu_path && !has_predicted_path) { + RCLCPP_INFO_SKIPFIRST_THROTTLE( + get_logger(), *get_clock(), 5000, + "[AEB] At least one path (IMU or predicted trajectory) is required for operation"); + return false; } autoware_state_ = sub_autoware_state_.takeData(); @@ -482,7 +491,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) // step3. make function to check collision with ego path created with sensor data const auto has_collision_ego = [&](pcl::PointCloud::Ptr filtered_objects) -> bool { - if (!use_imu_path_) return false; + if (!use_imu_path_ || !angular_velocity_ptr_) return false; const double current_w = angular_velocity_ptr_->z; constexpr colorTuple debug_color = {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999}; const std::string ns = "ego"; From 8abbc2d546e0bc2220bccbb096552c800880e88f Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 23 Aug 2024 11:16:29 +0900 Subject: [PATCH 12/46] feat(raw_vehicle_cmd_converter): disable actuation to steering (#8588) Signed-off-by: kosuke55 --- .../config/raw_vehicle_cmd_converter.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 e7e503d6d7eee..4eb9f0ab6d442 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 @@ -30,4 +30,4 @@ vgr_coef_a: 15.713 vgr_coef_b: 0.053 vgr_coef_c: 0.042 - convert_actuation_to_steering_status: true + convert_actuation_to_steering_status: false From be5e16b1dd12e41da989b08232b7bfea58df93fa Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Fri, 23 Aug 2024 11:25:52 +0900 Subject: [PATCH 13/46] feat(freespace_planning_algorithms): implement support for multiple goal candidates in A star planner (#8092) * refactor freespace planning algorithms Signed-off-by: mohammad alqudah * fix error Signed-off-by: mohammad alqudah * use vector instead of map for a-star node graph Signed-off-by: mohammad alqudah * remove unnecessary parameters Signed-off-by: mohammad alqudah * precompute average turning radius Signed-off-by: mohammad alqudah * add threshold for minimum distance between direction changes Signed-off-by: mohammad alqudah * apply curvature weight and change in curvature weight Signed-off-by: mohammad alqudah * store total cost instead of heuristic cost Signed-off-by: mohammad alqudah * fix reverse weight application Signed-off-by: mohammad alqudah * fix parameter description in README Signed-off-by: mohammad alqudah * implement edt map to store distance to nearest obstacle for each grid cell Signed-off-by: mohammad alqudah * use obstacle edt in collision check Signed-off-by: mohammad alqudah * add cost for distance to obstacle Signed-off-by: mohammad alqudah * fix formats Signed-off-by: mohammad alqudah * add missing include Signed-off-by: mohammad alqudah * refactor functions Signed-off-by: mohammad alqudah * add missing include Signed-off-by: mohammad alqudah * implement backward search option Signed-off-by: mohammad alqudah * precompute number of margin cells to reduce out of range vertices check necessity Signed-off-by: mohammad alqudah * add reset data function Signed-off-by: mohammad alqudah * remove unnecessary code Signed-off-by: mohammad alqudah * make A-star search work with multiple goal candidates as input Signed-off-by: mohammad alqudah * fix is_back flag logic Signed-off-by: mohammad alqudah * add member function set() to AstarNode struct Signed-off-by: mohammad alqudah * implement adaptive expansion distance Signed-off-by: mohammad alqudah * remove unnecessary code Signed-off-by: mohammad alqudah * interpolate nodes with large expansion distance Signed-off-by: mohammad alqudah * minor refactor Signed-off-by: mohammad alqudah * fix interpolation for backward search Signed-off-by: mohammad alqudah * ensure expansion distance is larger than grid cell diagonal Signed-off-by: mohammad alqudah * compute collision free distance to goal map Signed-off-by: mohammad alqudah * use obstacle edt when computing collision free distance map Signed-off-by: mohammad alqudah * minor refactor Signed-off-by: mohammad alqudah * fix expansion cost function Signed-off-by: mohammad alqudah * set distance map before setting start node Signed-off-by: mohammad alqudah * refactor detect collision function Signed-off-by: mohammad alqudah * use flag instead of enum Signed-off-by: mohammad alqudah * add missing variable initialization Signed-off-by: mohammad alqudah * remove declared but undefined function Signed-off-by: mohammad alqudah * refactor makePlan() function Signed-off-by: mohammad alqudah * remove bool return statement for void function Signed-off-by: mohammad alqudah * remove unnecessary checks Signed-off-by: mohammad alqudah * minor fix Signed-off-by: mohammad alqudah * refactor computeEDTMap function Signed-off-by: mohammad alqudah * enable both forward and backward search options for multiple goal candidates Signed-off-by: mohammad alqudah * remove unnecessary code Signed-off-by: mohammad alqudah * set min and max expansion distance after setting costmap Signed-off-by: mohammad alqudah * refactor detectCollision function Signed-off-by: mohammad alqudah * remove unused function Signed-off-by: mohammad alqudah * change default parameter values Signed-off-by: mohammad alqudah * add missing last waypoint Signed-off-by: mohammad alqudah * fix computeEDTMap function Signed-off-by: mohammad alqudah * rename parameter Signed-off-by: mohammad alqudah * use linear function for obstacle distance cost Signed-off-by: mohammad alqudah * fix rrtstar obstacle check Signed-off-by: mohammad alqudah * add public access function to get distance to nearest obstacle Signed-off-by: mohammad alqudah * compare node index with goal index in isGoal check Signed-off-by: mohammad alqudah * append shifted goal pose to waypoints for more accurate arrival Signed-off-by: mohammad alqudah * remove redundant return statements Signed-off-by: mohammad alqudah * check goal pose validity before setting collision free distance map Signed-off-by: mohammad alqudah * declare variables as const where necessary Signed-off-by: mohammad alqudah * initialize vectors using assign function Signed-off-by: mohammad alqudah * compare front and back lengths when setting min and max dimension Signed-off-by: mohammad alqudah * add docstring and citation for computeEDTMap function Signed-off-by: mohammad alqudah * fix shifted goal pose for backward search Signed-off-by: mohammad alqudah * transform pose to local frame in getDistanceToObstacle funcion Signed-off-by: mohammad alqudah * add cost for lateral distance near goal Signed-off-by: mohammad alqudah * compute distance to obstacle from ego frame instead of base Signed-off-by: mohammad alqudah * update freespace planner parameter schema Signed-off-by: mohammad alqudah * update freespace planner parameter schema Signed-off-by: mohammad alqudah * refactor setPath function Signed-off-by: mohammad alqudah * fix function setPath Signed-off-by: mohammad alqudah * declare bool var as constant Signed-off-by: mohammad alqudah * remove unnecessary includes Signed-off-by: mohammad alqudah * minor refactor Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> --- planning/autoware_freespace_planner/README.md | 2 + .../config/freespace_planner.param.yaml | 4 +- .../schema/freespace_planner.schema.json | 10 + .../freespace_planner_node.cpp | 3 +- .../abstract_algorithm.hpp | 38 +++- .../astar_search.hpp | 28 ++- .../freespace_planning_algorithms/rrtstar.hpp | 3 + .../scripts/bind/astar_search_pybind.cpp | 7 +- .../scripts/example/example.py | 2 + .../src/abstract_algorithm.cpp | 40 ++-- .../src/astar_search.cpp | 204 ++++++++++++++---- .../src/rrtstar.cpp | 7 + .../test_freespace_planning_algorithms.cpp | 14 +- 13 files changed, 290 insertions(+), 72 deletions(-) diff --git a/planning/autoware_freespace_planner/README.md b/planning/autoware_freespace_planner/README.md index db48154ffa214..56e096944e739 100644 --- a/planning/autoware_freespace_planner/README.md +++ b/planning/autoware_freespace_planner/README.md @@ -82,9 +82,11 @@ None | `use_back` | bool | whether using backward trajectory | | `adapt_expansion_distance` | bool | if true, adapt expansion distance based on environment | | `expansion_distance` | double | length of expansion for node transitions | +| `near_goal_distance` | double | near goal distance threshold | | `distance_heuristic_weight` | double | heuristic weight for estimating node's cost | | `smoothness_weight` | double | cost factor for change in curvature | | `obstacle_distance_weight` | double | cost factor for distance to obstacle | +| `goal_lat_distance_weight` | double | cost factor for lateral distance from goal | #### RRT\* search parameters diff --git a/planning/autoware_freespace_planner/config/freespace_planner.param.yaml b/planning/autoware_freespace_planner/config/freespace_planner.param.yaml index 6ea99b2c0c061..b34ffe698aa36 100644 --- a/planning/autoware_freespace_planner/config/freespace_planner.param.yaml +++ b/planning/autoware_freespace_planner/config/freespace_planner.param.yaml @@ -22,7 +22,7 @@ theta_size: 144 angle_goal_range: 6.0 lateral_goal_range: 0.5 - longitudinal_goal_range: 2.0 + longitudinal_goal_range: 1.0 curve_weight: 0.5 reverse_weight: 1.0 direction_change_weight: 1.5 @@ -36,9 +36,11 @@ use_back: true adapt_expansion_distance: true expansion_distance: 0.5 + near_goal_distance: 4.0 distance_heuristic_weight: 1.5 smoothness_weight: 0.5 obstacle_distance_weight: 1.5 + goal_lat_distance_weight: 5.0 # -- RRT* search Configurations -- rrtstar: diff --git a/planning/autoware_freespace_planner/schema/freespace_planner.schema.json b/planning/autoware_freespace_planner/schema/freespace_planner.schema.json index 4494878849897..e42995718974a 100644 --- a/planning/autoware_freespace_planner/schema/freespace_planner.schema.json +++ b/planning/autoware_freespace_planner/schema/freespace_planner.schema.json @@ -146,6 +146,11 @@ "default": 0.5, "description": "Distance for expanding nodes in A* search." }, + "near_goal_distance": { + "type": "number", + "default": 4.0, + "description": "Distance threshold to consider near goal." + }, "distance_heuristic_weight": { "type": "number", "default": 1.0, @@ -160,6 +165,11 @@ "type": "number", "default": 0.5, "description": "Weight for distance to obstacle in A* search." + }, + "goal_lat_distance_weight": { + "type": "number", + "default": 0.5, + "description": "Weight for lateral distance from original goal." } }, "required": [ diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp index 9bff2cfddd2c6..8e8a962ed1071 100644 --- a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp @@ -377,8 +377,7 @@ void FreespacePlannerNode::updateTargetIndex() } else { // Switch to next partial trajectory prev_target_index_ = target_index_; - target_index_ = - getNextTargetIndex(trajectory_.points.size(), reversing_indices_, target_index_); + target_index_ = new_target_index; } } } diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp index 911144f3796e5..8a4d6bf2e42b5 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_ #include +#include #include #include @@ -29,6 +30,10 @@ namespace autoware::freespace_planning_algorithms { +using autoware::universe_utils::normalizeRadian; + +geometry_msgs::msg::Pose transformPose( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform); int discretizeAngle(const double theta, const int theta_size); struct IndexXYT @@ -36,6 +41,11 @@ struct IndexXYT int x; int y; int theta; + + bool operator==(const IndexXYT & index) const + { + return (x == index.x && y == index.y && theta == index.theta); + } }; struct IndexXY @@ -138,6 +148,12 @@ struct PlannerWaypoints double compute_length() const; }; +struct EDTData +{ + double distance; + double angle; +}; + class AbstractPlanningAlgorithm { public: @@ -162,6 +178,9 @@ class AbstractPlanningAlgorithm virtual void setMap(const nav_msgs::msg::OccupancyGrid & costmap); virtual bool makePlan( const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose) = 0; + virtual bool makePlan( + const geometry_msgs::msg::Pose & start_pose, + const std::vector & goal_candidates) = 0; virtual bool hasObstacleOnTrajectory(const geometry_msgs::msg::PoseArray & trajectory) const; const PlannerWaypoints & getWaypoints() const { return waypoints_; } double getDistanceToObstacle(const geometry_msgs::msg::Pose & pose) const; @@ -223,7 +242,7 @@ class AbstractPlanningAlgorithm } template - inline double getObstacleEDT(const IndexType & index) const + inline EDTData getObstacleEDT(const IndexType & index) const { return edt_map_[indexToId(index)]; } @@ -235,6 +254,19 @@ class AbstractPlanningAlgorithm return index.y * costmap_.info.width + index.x; } + inline double getVehicleBaseToFrameDistance(const double angle) const + { + const double normalized_angle = std::abs(normalizeRadian(angle)); + const double w = 0.5 * collision_vehicle_shape_.width; + const double l_b = collision_vehicle_shape_.base2back; + const double l_f = collision_vehicle_shape_.length - l_b; + + if (normalized_angle < atan(w / l_f)) return l_f / cos(normalized_angle); + if (normalized_angle < M_PI_2) return w / sin(normalized_angle); + if (normalized_angle < M_PI_2 + atan(l_b / w)) return w / cos(normalized_angle - M_PI_2); + return l_b / cos(M_PI - normalized_angle); + } + PlannerCommonParam planner_common_param_; VehicleShape collision_vehicle_shape_; @@ -250,8 +282,8 @@ class AbstractPlanningAlgorithm // is_obstacle's table std::vector is_obstacle_table_; - // Euclidean distance transform map (distance to nearest obstacle cell) - std::vector edt_map_; + // Euclidean distance transform map (distance & angle info to nearest obstacle cell) + std::vector edt_map_; // pose in costmap frame geometry_msgs::msg::Pose start_pose_; diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp index 8ac7c60e7ca4f..65ef53d820cba 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp @@ -23,6 +23,8 @@ #include #include +#include + #include #include #include @@ -46,11 +48,13 @@ struct AstarParam bool use_back; // backward search bool adapt_expansion_distance; double expansion_distance; + double near_goal_distance; // search configs double distance_heuristic_weight; // obstacle threshold on grid [0,255] double smoothness_weight; double obstacle_distance_weight; + double goal_lat_distance_weight; }; struct AstarNode @@ -105,15 +109,19 @@ class AstarSearch : public AbstractPlanningAlgorithm node.declare_parameter("astar.use_back"), node.declare_parameter("astar.adapt_expansion_distance"), node.declare_parameter("astar.expansion_distance"), + node.declare_parameter("astar.near_goal_distance"), node.declare_parameter("astar.distance_heuristic_weight"), node.declare_parameter("astar.smoothness_weight"), - node.declare_parameter("astar.obstacle_distance_weight")}) + node.declare_parameter("astar.obstacle_distance_weight"), + node.declare_parameter("astar.goal_lat_distance_weight")}) { } void setMap(const nav_msgs::msg::OccupancyGrid & costmap) override; bool makePlan(const Pose & start_pose, const Pose & goal_pose) override; + bool makePlan(const Pose & start_pose, const std::vector & goal_candidates) override; + const PlannerWaypoints & getWaypoints() const { return waypoints_; } inline int getKey(const IndexXYT & index) @@ -127,16 +135,18 @@ class AstarSearch : public AbstractPlanningAlgorithm void expandNodes(AstarNode & current_node, const bool is_back = false); void resetData(); void setPath(const AstarNode & goal); - void setStartNode(); + void setStartNode(const double cost_offset = 0.0); double estimateCost(const Pose & pose, const IndexXYT & index) const; bool isGoal(const AstarNode & node) const; + void setShiftedGoalPose(const Pose & goal_pose, const double lat_offset) const; Pose node2pose(const AstarNode & node) const; double getExpansionDistance(const AstarNode & current_node) const; double getSteeringCost(const int steering_index) const; double getSteeringChangeCost(const int steering_index, const int prev_steering_index) const; double getDirectionChangeCost(const double dir_distance) const; - double getObsDistanceCost(const double obs_distance) const; + double getObsDistanceCost(const IndexXYT & index, const EDTData & obs_edt) const; + double getLatDistanceCost(const Pose & pose) const; // Algorithm specific param AstarParam astar_param_; @@ -150,6 +160,11 @@ class AstarSearch : public AbstractPlanningAlgorithm // goal node, which may helpful in testing and debugging AstarNode * goal_node_; + mutable boost::optional shifted_goal_pose_; + + // alternate goals for when multiple goal candidates are given + std::vector alternate_goals_; + // distance metric option (removed when the reeds_shepp gets stable) bool use_reeds_shepp_; @@ -158,7 +173,9 @@ class AstarSearch : public AbstractPlanningAlgorithm double avg_turning_radius_; double min_expansion_dist_; double max_expansion_dist_; + double near_goal_dist_; bool is_backward_search_; + bool is_multiple_goals_; // the following constexpr values were found to be best by trial and error, through multiple // tests, and are not expected to be changed regularly, therefore they were not made into ros @@ -169,8 +186,11 @@ class AstarSearch : public AbstractPlanningAlgorithm static constexpr double dist_to_goal_expansion_factor_ = 0.15; static constexpr double dist_to_obs_expansion_factor_ = 0.3; + // initial cost offset for multi goal backward search + static constexpr double multi_goal_backward_cost_offset = 5.0; + // cost free obstacle distance - static constexpr double cost_free_obs_dist = 5.0; + static constexpr double cost_free_obs_dist = 1.0; }; } // namespace autoware::freespace_planning_algorithms diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp index 17e7f1f1ebf56..b30692ce8da2d 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp @@ -58,6 +58,9 @@ class RRTStar : public AbstractPlanningAlgorithm bool makePlan( const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose) override; + bool makePlan( + const geometry_msgs::msg::Pose & start_pose, + const std::vector & goal_candidates) override; bool hasObstacleOnTrajectory(const geometry_msgs::msg::PoseArray & trajectory) const override; private: diff --git a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp index 1c04fd65447b2..c35081bd97f84 100644 --- a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp +++ b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp @@ -120,6 +120,8 @@ PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p) &freespace_planning_algorithms::AstarParam::adapt_expansion_distance) .def_readwrite( "expansion_distance", &freespace_planning_algorithms::AstarParam::expansion_distance) + .def_readwrite( + "near_goal_distance", &freespace_planning_algorithms::AstarParam::near_goal_distance) .def_readwrite( "distance_heuristic_weight", &freespace_planning_algorithms::AstarParam::distance_heuristic_weight) @@ -127,7 +129,10 @@ PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p) "smoothness_weight", &freespace_planning_algorithms::AstarParam::smoothness_weight) .def_readwrite( "obstacle_distance_weight", - &freespace_planning_algorithms::AstarParam::obstacle_distance_weight); + &freespace_planning_algorithms::AstarParam::obstacle_distance_weight) + .def_readwrite( + "goal_lat_distance_weight", + &freespace_planning_algorithms::AstarParam::goal_lat_distance_weight); auto pyPlannerCommonParam = py::class_( p, "PlannerCommonParam", py::dynamic_attr()) diff --git a/planning/autoware_freespace_planning_algorithms/scripts/example/example.py b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py index b4af9ed87a3b9..9f18dc799c9d0 100644 --- a/planning/autoware_freespace_planning_algorithms/scripts/example/example.py +++ b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py @@ -49,9 +49,11 @@ astar_param.use_back = True astar_param.adapt_expansion_distance = True astar_param.expansion_distance = 0.4 +astar_param.near_goal_distance = 3.0 astar_param.distance_heuristic_weight = 1.0 astar_param.smoothness_weight = 1.0 astar_param.obstacle_distance_weight = 1.0 +astar_param.goal_lat_distance_weight = 1.0 astar = fp.AstarSearch(planner_param, vehicle_shape, astar_param) diff --git a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp index 97fd93e31d5cd..b968e70bd2c01 100644 --- a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -19,12 +19,12 @@ #include #include +#include #include namespace autoware::freespace_planning_algorithms { using autoware::universe_utils::createQuaternionFromYaw; -using autoware::universe_utils::normalizeRadian; geometry_msgs::msg::Pose transformPose( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform) @@ -145,19 +145,19 @@ double AbstractPlanningAlgorithm::getDistanceToObstacle(const geometry_msgs::msg if (indexToId(index) >= static_cast(edt_map_.size())) { return std::numeric_limits::max(); } - return getObstacleEDT(index); + return getObstacleEDT(index).distance; } void AbstractPlanningAlgorithm::computeEDTMap() { + edt_map_.clear(); const int height = costmap_.info.height; const int width = costmap_.info.width; const double resolution_m = costmap_.info.resolution; - const double diagonal_m = std::hypot(resolution_m * height, resolution_m * width); - std::vector edt_map; + std::vector> edt_map; edt_map.reserve(costmap_.data.size()); - std::vector temporary_storage(width); + std::vector> temporary_storage(width); // scan rows for (int i = 0; i < height; ++i) { double distance = resolution_m; @@ -165,14 +165,16 @@ void AbstractPlanningAlgorithm::computeEDTMap() // forward scan for (int j = 0; j < width; ++j) { if (isObs(IndexXY{j, i})) { - temporary_storage[j] = 0.0; + temporary_storage[j].first = 0.0; + temporary_storage[j].second.x = 0.0; distance = resolution_m; found_obstacle = true; } else if (found_obstacle) { - temporary_storage[j] = distance; + temporary_storage[j].first = distance; + temporary_storage[j].second.x = -distance; distance += resolution_m; } else { - temporary_storage[j] = diagonal_m; + temporary_storage[j].first = std::numeric_limits::infinity(); } } @@ -183,8 +185,9 @@ void AbstractPlanningAlgorithm::computeEDTMap() if (isObs(IndexXY{j, i})) { distance = resolution_m; found_obstacle = true; - } else if (found_obstacle && temporary_storage[j] > distance) { - temporary_storage[j] = distance; + } else if (found_obstacle && temporary_storage[j].first > distance) { + temporary_storage[j].first = distance; + temporary_storage[j].second.x = distance; distance += resolution_m; } } @@ -197,22 +200,29 @@ void AbstractPlanningAlgorithm::computeEDTMap() for (int j = 0; j < width; ++j) { for (int i = 0; i < height; ++i) { int id = indexToId(IndexXY{j, i}); - double min_value = edt_map[id] * edt_map[id]; + double min_value = edt_map[id].first * edt_map[id].first; + geometry_msgs::msg::Point rel_pos = edt_map[id].second; for (int k = 0; k < height; ++k) { id = indexToId(IndexXY{j, k}); double dist = resolution_m * std::abs(static_cast(i - k)); - double value = edt_map[id] * edt_map[id] + dist * dist; + double value = edt_map[id].first * edt_map[id].first + dist * dist; if (value < min_value) { min_value = value; + rel_pos.x = edt_map[id].second.x; + rel_pos.y = dist; } } - temporary_storage[i] = sqrt(min_value); + temporary_storage[i].first = sqrt(min_value); + temporary_storage[i].second = rel_pos; } for (int i = 0; i < height; ++i) { edt_map[indexToId(IndexXY{j, i})] = temporary_storage[i]; } } - edt_map_ = edt_map; + for (const auto & edt : edt_map) { + const double angle = std::atan2(edt.second.y, edt.second.x); + edt_map_.push_back({edt.first, angle}); + } } void AbstractPlanningAlgorithm::computeCollisionIndexes( @@ -312,7 +322,7 @@ bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) con if (detectBoundaryExit(base_index)) return true; - double obstacle_edt = getObstacleEDT(base_index); + double obstacle_edt = getObstacleEDT(base_index).distance; // if nearest obstacle is further than largest dimension, no collision is guaranteed // if nearest obstacle is closer than smallest dimension, collision is guaranteed diff --git a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp index b0be1da75caec..805f375af4276 100644 --- a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp @@ -14,11 +14,13 @@ #include "autoware/freespace_planning_algorithms/astar_search.hpp" +#include "autoware/freespace_planning_algorithms/abstract_algorithm.hpp" #include "autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp" #include #include +#include #include #include @@ -88,6 +90,9 @@ AstarSearch::AstarSearch( min_expansion_dist_ = astar_param_.expansion_distance; max_expansion_dist_ = collision_vehicle_shape_.base_length * base_length_max_expansion_factor_; + + near_goal_dist_ = + std::max(astar_param.near_goal_distance, planner_common_param.longitudinal_goal_range); } void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & costmap) @@ -100,6 +105,18 @@ void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & costmap) collision_vehicle_shape_.base_length * base_length_max_expansion_factor_, min_expansion_dist_); } +void AstarSearch::resetData() +{ + // clearing openlist is necessary because otherwise remaining elements of openlist + // point to deleted node. + openlist_ = std::priority_queue, NodeComparison>(); + const int nb_of_grid_nodes = costmap_.info.width * costmap_.info.height; + const int total_astar_node_count = nb_of_grid_nodes * planner_common_param_.theta_size; + graph_.assign(total_astar_node_count, AstarNode{}); + col_free_distance_map_.assign(nb_of_grid_nodes, std::numeric_limits::max()); + shifted_goal_pose_ = {}; +} + bool AstarSearch::makePlan(const Pose & start_pose, const Pose & goal_pose) { resetData(); @@ -115,6 +132,8 @@ bool AstarSearch::makePlan(const Pose & start_pose, const Pose & goal_pose) setCollisionFreeDistanceMap(); + is_multiple_goals_ = false; + setStartNode(); if (!search()) { @@ -124,16 +143,54 @@ bool AstarSearch::makePlan(const Pose & start_pose, const Pose & goal_pose) return true; } -void AstarSearch::resetData() +bool AstarSearch::makePlan( + const geometry_msgs::msg::Pose & start_pose, + const std::vector & goal_candidates) { - // clearing openlist is necessary because otherwise remaining elements of openlist - // point to deleted node. - openlist_ = std::priority_queue, NodeComparison>(); - const int nb_of_grid_nodes = costmap_.info.width * costmap_.info.height; - const int total_astar_node_count = nb_of_grid_nodes * planner_common_param_.theta_size; - graph_ = std::vector(total_astar_node_count); - col_free_distance_map_ = - std::vector(nb_of_grid_nodes, std::numeric_limits::max()); + if (goal_candidates.empty()) return false; + + if (goal_candidates.size() == 1) { + return makePlan(start_pose, goal_candidates.front()); + } + + resetData(); + + start_pose_ = global2local(costmap_, start_pose); + + std::vector goals_local; + for (const auto & goal : goal_candidates) { + const auto goal_local = global2local(costmap_, goal); + if (detectCollision(goal_local)) continue; + goals_local.push_back(goal_local); + } + + if (detectCollision(start_pose_) || goals_local.empty()) { + throw std::logic_error("Invalid start or goal pose"); + } + + goal_pose_ = is_backward_search_ ? start_pose_ : goals_local.front(); + + setCollisionFreeDistanceMap(); + + is_multiple_goals_ = true; + + if (is_backward_search_) { + double cost_offset = 0.0; + for (const auto & pose : goals_local) { + start_pose_ = pose; + setStartNode(cost_offset); + cost_offset += multi_goal_backward_cost_offset; + } + } else { + setStartNode(); + alternate_goals_ = goals_local; + } + + if (!search()) { + throw std::logic_error("HA* failed to find path to goal"); + } + + return true; } void AstarSearch::setCollisionFreeDistanceMap() @@ -166,7 +223,7 @@ void AstarSearch::setCollisionFreeDistanceMap() const IndexXY n_index{x, y}; const double offset = std::abs(offset_x) + std::abs(offset_y); if (isOutOfRange(n_index) || isObs(n_index) || offset < 1) continue; - if (getObstacleEDT(n_index) < 0.5 * collision_vehicle_shape_.width) continue; + if (getObstacleEDT(n_index).distance < 0.5 * collision_vehicle_shape_.width) continue; const int n_id = indexToId(n_index); const double dist = current.second + (sqrt(offset) * costmap_.info.resolution); if (closed[n_id] || col_free_distance_map_[n_id] < dist) continue; @@ -177,15 +234,16 @@ void AstarSearch::setCollisionFreeDistanceMap() } } -void AstarSearch::setStartNode() +void AstarSearch::setStartNode(const double cost_offset) { const auto index = pose2index(costmap_, start_pose_, planner_common_param_.theta_size); // Set start node AstarNode * start_node = &graph_[getKey(index)]; - start_node->set(start_pose_, 0.0, estimateCost(start_pose_, index), 0, false); + const double initial_cost = estimateCost(start_pose_, index) + cost_offset; + start_node->set(start_pose_, 0.0, initial_cost, 0, false); start_node->dir_distance = 0.0; start_node->dist_to_goal = calcDistance2d(start_pose_, goal_pose_); - start_node->dist_to_obs = getObstacleEDT(index); + start_node->dist_to_obs = getObstacleEDT(index).distance; start_node->status = NodeStatus::Open; start_node->parent = nullptr; @@ -261,7 +319,7 @@ void AstarSearch::expandNodes(AstarNode & current_node, const bool is_back) AstarNode * next_node = &graph_[getKey(next_index)]; if (next_node->status == NodeStatus::Closed || detectCollision(next_index)) continue; - const double distance_to_obs = getObstacleEDT(next_index); + const auto obs_edt = getObstacleEDT(next_index); const bool is_direction_switch = (current_node.parent != nullptr) && (is_back != current_node.is_back); @@ -271,7 +329,8 @@ void AstarSearch::expandNodes(AstarNode & current_node, const bool is_back) double move_cost = current_node.gc + (total_weight * std::abs(distance)); move_cost += getSteeringChangeCost(steering_index, current_node.steering_index); - move_cost += getObsDistanceCost(distance_to_obs); + move_cost += getObsDistanceCost(next_index, obs_edt); + move_cost += getLatDistanceCost(next_pose); if (is_direction_switch) move_cost += getDirectionChangeCost(current_node.dir_distance); double total_cost = move_cost + estimateCost(next_pose, next_index); @@ -282,7 +341,7 @@ void AstarSearch::expandNodes(AstarNode & current_node, const bool is_back) next_node->dir_distance = std::abs(distance) + (is_direction_switch ? 0.0 : current_node.dir_distance); next_node->dist_to_goal = calcDistance2d(next_pose, goal_pose_); - next_node->dist_to_obs = distance_to_obs; + next_node->dist_to_obs = obs_edt.distance; next_node->parent = ¤t_node; openlist_.push(next_node); continue; @@ -320,10 +379,26 @@ double AstarSearch::getDirectionChangeCost(const double dir_distance) const return planner_common_param_.direction_change_weight * (1.0 + (1.0 / (1.0 + dir_distance))); } -double AstarSearch::getObsDistanceCost(const double obs_distance) const +double AstarSearch::getObsDistanceCost(const IndexXYT & index, const EDTData & obs_edt) const { + if (obs_edt.distance > collision_vehicle_shape_.max_dimension + cost_free_obs_dist) { + return 0.0; + } + const double yaw = index.theta * (2.0 * M_PI / planner_common_param_.theta_size); + const double base_to_frame_dist = getVehicleBaseToFrameDistance(yaw - obs_edt.angle); + const double vehicle_to_obs_dist = std::max(obs_edt.distance - base_to_frame_dist, 0.0); return astar_param_.obstacle_distance_weight * - std::max(1.0 - (obs_distance / cost_free_obs_dist), 0.0); + std::max(1.0 - (vehicle_to_obs_dist / cost_free_obs_dist), 0.0); +} + +double AstarSearch::getLatDistanceCost(const Pose & pose) const +{ + if (is_multiple_goals_) return 0.0; + const auto ref_pose = is_backward_search_ ? start_pose_ : goal_pose_; + const double distance_to_goal = calcDistance2d(pose, ref_pose); + if (distance_to_goal > near_goal_dist_) return 0.0; + const double lat_distance = std::abs(calcRelativePose(ref_pose, pose).position.y); + return astar_param_.goal_lat_distance_weight * lat_distance; } void AstarSearch::setPath(const AstarNode & goal_node) @@ -340,6 +415,11 @@ void AstarSearch::setPath(const AstarNode & goal_node) geometry_msgs::msg::PoseStamped pose; pose.header = header; + if (shifted_goal_pose_) { + pose.pose = local2global(costmap_, shifted_goal_pose_.get()); + waypoints.push_back({pose, goal_node.is_back}); + } + const auto interpolate = [this, &waypoints, &pose](const AstarNode & node) { if (node.parent == nullptr || !astar_param_.adapt_expansion_distance) return; const auto parent_pose = node2pose(*node.parent); @@ -375,22 +455,19 @@ void AstarSearch::setPath(const AstarNode & goal_node) } waypoints_.header = header; - waypoints_.waypoints.clear(); + waypoints_.waypoints = waypoints; - for (size_t i = 0; i < waypoints.size() - 1; ++i) { - const auto & current = waypoints[i]; - const auto & next = waypoints[i + 1]; + if (!is_backward_search_) return; - waypoints_.waypoints.push_back(current); + for (size_t i = 0; i < waypoints_.waypoints.size() - 1; ++i) { + const auto & current = waypoints_.waypoints[i]; + auto & next = waypoints_.waypoints[i + 1]; if (current.is_back != next.is_back) { - waypoints_.waypoints.push_back( - {is_backward_search_ ? next.pose : current.pose, - is_backward_search_ ? current.is_back : next.is_back}); + next.is_back = current.is_back; + ++i; // skip next waypoint } } - - waypoints_.waypoints.push_back(waypoints.back()); } bool AstarSearch::isGoal(const AstarNode & node) const @@ -400,26 +477,65 @@ bool AstarSearch::isGoal(const AstarNode & node) const const double goal_angle = autoware::universe_utils::deg2rad(planner_common_param_.angle_goal_range / 2.0); - const auto relative_pose = calcRelativePose(goal_pose_, node2pose(node)); + const auto node_pose = node2pose(node); - // Check conditions - if (astar_param_.only_behind_solutions && relative_pose.position.x > 0) { - return false; - } + auto checkGoal = [this, &node_pose, &lateral_goal_range, &longitudinal_goal_range, &goal_angle, + &is_back = node.is_back](const Pose & pose) { + const auto node_index = pose2index(costmap_, node_pose, planner_common_param_.theta_size); + const auto goal_index = pose2index(costmap_, pose, planner_common_param_.theta_size); - if ( - std::fabs(relative_pose.position.x) > longitudinal_goal_range || - std::fabs(relative_pose.position.y) > lateral_goal_range) { - return false; - } + if (node_index == goal_index) return true; - const auto angle_diff = - autoware::universe_utils::normalizeRadian(tf2::getYaw(relative_pose.orientation)); - if (std::abs(angle_diff) > goal_angle) { - return false; - } + const auto relative_pose = calcRelativePose(pose, node_pose); - return true; + bool is_behind_goal = relative_pose.position.x <= 0.0; + + if (astar_param_.only_behind_solutions && !is_behind_goal) { + return false; + } + + if ( + std::fabs(relative_pose.position.x) > longitudinal_goal_range || + std::fabs(relative_pose.position.y) > lateral_goal_range) { + return false; + } + + const auto angle_diff = + autoware::universe_utils::normalizeRadian(tf2::getYaw(relative_pose.orientation)); + if (std::abs(angle_diff) > goal_angle) { + return false; + } + + const bool is_set_shifted_goal_pose = + is_backward_search_ ? is_behind_goal == is_back : is_behind_goal != is_back; + if (is_set_shifted_goal_pose) { + setShiftedGoalPose(pose, relative_pose.position.y); + } + + return true; + }; + + if (checkGoal(goal_pose_)) return true; + + return std::any_of( + alternate_goals_.begin(), alternate_goals_.end(), + [&checkGoal](const Pose & pose) { return checkGoal(pose); }); +} + +void AstarSearch::setShiftedGoalPose(const Pose & goal_pose, const double lat_offset) const +{ + tf2::Transform tf; + tf2::convert(goal_pose, tf); + + geometry_msgs::msg::TransformStamped transform; + transform.transform = tf2::toMsg(tf); + + Pose lat_pose; + lat_pose.position = geometry_msgs::build().x(0.0).y(lat_offset).z(0.0); + lat_pose.orientation = + geometry_msgs::build().x(0.0).y(0.0).z(0.0).w(1.0); + + shifted_goal_pose_ = transformPose(lat_pose, transform); } Pose AstarSearch::node2pose(const AstarNode & node) const diff --git a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp index 20a59048e59d1..b9343f680fa1a 100644 --- a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp @@ -41,6 +41,13 @@ RRTStar::RRTStar( } } +bool RRTStar::makePlan( + const geometry_msgs::msg::Pose & start_pose, + const std::vector & goal_candidates) +{ + return makePlan(start_pose, goal_candidates.front()); +} + bool RRTStar::makePlan( const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose) { diff --git a/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp b/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp index 82806bb500b0e..2b2254d66c844 100644 --- a/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp +++ b/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp @@ -214,12 +214,22 @@ std::unique_ptr configure_astar(bool use_multi) const bool use_back = true; const bool adapt_expansion_distance = true; const double expansion_distance = 0.4; + const double near_goal_distance = 4.0; const double distance_heuristic_weight = 2.0; const double smoothness_weight = 0.5; const double obstacle_distance_weight = 1.7; + const double goal_lat_distance_weight = 1.0; const auto astar_param = fpa::AstarParam{ - search_method, only_behind_solutions, use_back, adapt_expansion_distance, - expansion_distance, distance_heuristic_weight, smoothness_weight, obstacle_distance_weight}; + search_method, + only_behind_solutions, + use_back, + adapt_expansion_distance, + expansion_distance, + near_goal_distance, + distance_heuristic_weight, + smoothness_weight, + obstacle_distance_weight, + goal_lat_distance_weight}; auto algo = std::make_unique(planner_common_param, vehicle_shape, astar_param); return algo; From babadcf889a8112fc2debe17dd1c38de07cf89e5 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 23 Aug 2024 11:41:14 +0900 Subject: [PATCH 14/46] fix(autoware_image_projection_based_fusion): fix unusedFunction (#8567) fix:unusedFunction Signed-off-by: kobayu858 --- .../utils/utils.hpp | 8 -- .../src/utils/utils.cpp | 95 ------------------- 2 files changed, 103 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp index 9a0f4838d1fac..058a5994ab8ad 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp @@ -74,9 +74,6 @@ std::optional getTransformStamped( Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t); -void convertCluster2FeatureObject( - const std_msgs::msg::Header & header, const PointCloud & cluster, - DetectedObjectWithFeature & feature_obj); void closest_cluster( const PointCloud2 & cluster, const double cluster_2d_tolerance, const int min_cluster_size, const pcl::PointXYZ & center, PointCloud2 & out_cluster); @@ -90,11 +87,6 @@ void updateOutputFusedObjects( geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); -pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster); -void addShapeAndKinematic( - const pcl::PointCloud & cluster, - tier4_perception_msgs::msg::DetectedObjectWithFeature & feature_obj); - } // namespace autoware::image_projection_based_fusion #endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__UTILS__UTILS_HPP_ diff --git a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp index 1b303e3f01ff5..1f67d2e1cc86f 100644 --- a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp @@ -72,18 +72,6 @@ Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t) return a; } -void convertCluster2FeatureObject( - const std_msgs::msg::Header & header, const PointCloud & cluster, - DetectedObjectWithFeature & feature_obj) -{ - PointCloud2 ros_cluster; - pcl::toROSMsg(cluster, ros_cluster); - ros_cluster.header = header; - feature_obj.feature.cluster = ros_cluster; - feature_obj.object.kinematics.pose_with_covariance.pose.position = getCentroid(ros_cluster); - feature_obj.object.existence_probability = 1.0f; -} - void closest_cluster( const PointCloud2 & cluster, const double cluster_2d_tolerance, const int min_cluster_size, const pcl::PointXYZ & center, PointCloud2 & out_cluster) @@ -198,73 +186,6 @@ void updateOutputFusedObjects( } } -void addShapeAndKinematic( - const pcl::PointCloud & cluster, - tier4_perception_msgs::msg::DetectedObjectWithFeature & feature_obj) -{ - if (cluster.empty()) { - return; - } - pcl::PointXYZ centroid = pcl::PointXYZ(0.0, 0.0, 0.0); - float max_z = -1e6; - float min_z = 1e6; - for (const auto & point : cluster) { - centroid.x += point.x; - centroid.y += point.y; - centroid.z += point.z; - max_z = max_z < point.z ? point.z : max_z; - min_z = min_z > point.z ? point.z : min_z; - } - centroid.x = centroid.x / static_cast(cluster.size()); - centroid.y = centroid.y / static_cast(cluster.size()); - centroid.z = centroid.z / static_cast(cluster.size()); - - std::vector cluster2d; - std::vector cluster2d_convex; - - for (size_t i = 0; i < cluster.size(); ++i) { - cluster2d.push_back( - cv::Point((cluster.at(i).x - centroid.x) * 1000.0, (cluster.at(i).y - centroid.y) * 1000.)); - } - cv::convexHull(cluster2d, cluster2d_convex); - if (cluster2d_convex.empty()) { - return; - } - pcl::PointXYZ polygon_centroid = pcl::PointXYZ(0.0, 0.0, 0.0); - for (size_t i = 0; i < cluster2d_convex.size(); ++i) { - polygon_centroid.x += static_cast(cluster2d_convex.at(i).x) / 1000.0; - polygon_centroid.y += static_cast(cluster2d_convex.at(i).y) / 1000.0; - } - polygon_centroid.x = polygon_centroid.x / static_cast(cluster2d_convex.size()); - polygon_centroid.y = polygon_centroid.y / static_cast(cluster2d_convex.size()); - - autoware_perception_msgs::msg::Shape shape; - for (size_t i = 0; i < cluster2d_convex.size(); ++i) { - geometry_msgs::msg::Point32 point; - point.x = cluster2d_convex.at(i).x / 1000.0; - point.y = cluster2d_convex.at(i).y / 1000.0; - point.z = 0.0; - shape.footprint.points.push_back(point); - } - shape.type = autoware_perception_msgs::msg::Shape::POLYGON; - constexpr float eps = 0.01; - shape.dimensions.x = 0; - shape.dimensions.y = 0; - shape.dimensions.z = std::max((max_z - min_z), eps); - feature_obj.object.shape = shape; - feature_obj.object.kinematics.pose_with_covariance.pose.position.x = - centroid.x + polygon_centroid.x; - feature_obj.object.kinematics.pose_with_covariance.pose.position.y = - centroid.y + polygon_centroid.y; - feature_obj.object.kinematics.pose_with_covariance.pose.position.z = - min_z + shape.dimensions.z * 0.5; - feature_obj.object.existence_probability = 1.0; - feature_obj.object.kinematics.pose_with_covariance.pose.orientation.x = 0; - feature_obj.object.kinematics.pose_with_covariance.pose.orientation.y = 0; - feature_obj.object.kinematics.pose_with_covariance.pose.orientation.z = 0; - feature_obj.object.kinematics.pose_with_covariance.pose.orientation.w = 1; -} - geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud) { geometry_msgs::msg::Point centroid; @@ -285,20 +206,4 @@ geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & poin return centroid; } -pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster) -{ - pcl::PointXYZ closest_point; - double min_dist = 1e6; - pcl::PointXYZ orig_point = pcl::PointXYZ(0.0, 0.0, 0.0); - for (std::size_t i = 0; i < cluster.points.size(); ++i) { - pcl::PointXYZ point = cluster.points.at(i); - double dist_closest_point = autoware::universe_utils::calcDistance2d(point, orig_point); - if (min_dist > dist_closest_point) { - min_dist = dist_closest_point; - closest_point = pcl::PointXYZ(point.x, point.y, point.z); - } - } - return closest_point; -} - } // namespace autoware::image_projection_based_fusion From 103e086573c6a12ae352d19146bd9e561d40fc6f Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 23 Aug 2024 13:01:56 +0900 Subject: [PATCH 15/46] refactor(map_height_fitter)!: prefix package and namespace with autoware (#8421) * add autoware_ prefix Signed-off-by: a-maumau * style(pre-commit): autofix * remove duplicated dependency Signed-off-by: a-maumau --------- Signed-off-by: a-maumau Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: SakodaShintaro --- .github/CODEOWNERS | 2 +- localization/pose_initializer/README.md | 2 +- localization/pose_initializer/package.xml | 2 +- .../src/pose_initializer/gnss_module.hpp | 4 ++-- .../CMakeLists.txt | 8 ++++---- .../README.md | 4 ++-- .../config/map_height_fitter.param.yaml | 0 .../autoware}/map_height_fitter/map_height_fitter.hpp | 10 +++++----- .../launch/map_height_fitter.launch.xml | 4 ++-- .../package.xml | 4 ++-- .../schema/map_height_fitter.schema.json | 0 .../src/map_height_fitter.cpp | 6 +++--- .../src/map_height_fitter_node.cpp | 7 +++++-- .../default_ad_api_helpers/ad_api_adaptors/README.md | 2 +- .../default_ad_api_helpers/ad_api_adaptors/package.xml | 2 +- .../ad_api_adaptors/src/initial_pose_adaptor.hpp | 4 ++-- 16 files changed, 32 insertions(+), 29 deletions(-) rename map/{map_height_fitter => autoware_map_height_fitter}/CMakeLists.txt (74%) rename map/{map_height_fitter => autoware_map_height_fitter}/README.md (87%) rename map/{map_height_fitter => autoware_map_height_fitter}/config/map_height_fitter.param.yaml (100%) rename map/{map_height_fitter/include => autoware_map_height_fitter/include/autoware}/map_height_fitter/map_height_fitter.hpp (82%) rename map/{map_height_fitter => autoware_map_height_fitter}/launch/map_height_fitter.launch.xml (60%) rename map/{map_height_fitter => autoware_map_height_fitter}/package.xml (93%) rename map/{map_height_fitter => autoware_map_height_fitter}/schema/map_height_fitter.schema.json (100%) rename map/{map_height_fitter => autoware_map_height_fitter}/src/map_height_fitter.cpp (98%) rename map/{map_height_fitter => autoware_map_height_fitter}/src/map_height_fitter_node.cpp (88%) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 6333b479b5696..277255d529fe7 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -99,7 +99,7 @@ localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuu 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 localization/yabloc/yabloc_particle_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/yabloc/yabloc_pose_initializer/** 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 -map/map_height_fitter/** anh.nguyen.2@tier4.jp isamu.takagi@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 +map/autoware_map_height_fitter/** anh.nguyen.2@tier4.jp isamu.takagi@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 map/map_loader/** 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 map/map_projection_loader/** 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 map/autoware_map_tf_generator/** 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/pose_initializer/README.md b/localization/pose_initializer/README.md index 9083a6570d33b..94bf98f272487 100644 --- a/localization/pose_initializer/README.md +++ b/localization/pose_initializer/README.md @@ -7,7 +7,7 @@ It receives roughly estimated initial pose from GNSS/user. Passing the pose to `ndt_scan_matcher`, and it gets a calculated ego pose from `ndt_scan_matcher` via service. Finally, it publishes the initial pose to `ekf_localizer`. This node depends on the map height fitter library. -[See here for more details.](../../map/map_height_fitter/README.md) +[See here for more details.](../../map/autoware_map_height_fitter/README.md) ## Interfaces diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml index ca4aa032fc7f9..e1f1233b8b4f5 100644 --- a/localization/pose_initializer/package.xml +++ b/localization/pose_initializer/package.xml @@ -19,13 +19,13 @@ ament_cmake autoware_cmake + autoware_map_height_fitter autoware_motion_utils autoware_universe_utils component_interface_specs component_interface_utils geometry_msgs localization_util - map_height_fitter rclcpp rclcpp_components std_srvs diff --git a/localization/pose_initializer/src/pose_initializer/gnss_module.hpp b/localization/pose_initializer/src/pose_initializer/gnss_module.hpp index fd490b00d0f70..bb3f6933c2aaa 100644 --- a/localization/pose_initializer/src/pose_initializer/gnss_module.hpp +++ b/localization/pose_initializer/src/pose_initializer/gnss_module.hpp @@ -15,7 +15,7 @@ #ifndef POSE_INITIALIZER__GNSS_MODULE_HPP_ #define POSE_INITIALIZER__GNSS_MODULE_HPP_ -#include +#include #include #include @@ -32,7 +32,7 @@ class GnssModule private: void on_pose(PoseWithCovarianceStamped::ConstSharedPtr msg); - map_height_fitter::MapHeightFitter fitter_; + autoware::map_height_fitter::MapHeightFitter fitter_; rclcpp::Clock::SharedPtr clock_; rclcpp::Subscription::SharedPtr sub_gnss_pose_; PoseWithCovarianceStamped::ConstSharedPtr pose_; diff --git a/map/map_height_fitter/CMakeLists.txt b/map/autoware_map_height_fitter/CMakeLists.txt similarity index 74% rename from map/map_height_fitter/CMakeLists.txt rename to map/autoware_map_height_fitter/CMakeLists.txt index 9485ed3fa54cd..5e26a25af3a1d 100644 --- a/map/map_height_fitter/CMakeLists.txt +++ b/map/autoware_map_height_fitter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(map_height_fitter) +project(autoware_map_height_fitter) find_package(autoware_cmake REQUIRED) find_package(PCL REQUIRED COMPONENTS common) @@ -9,14 +9,14 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/map_height_fitter.cpp src/map_height_fitter_node.cpp ) -target_link_libraries(map_height_fitter ${PCL_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES}) # When adding `autoware_lanelet2_extension` to package.xml, many warnings are generated. # These are treated as errors in compile, so pedantic warnings are disabled for this package. -target_compile_options(map_height_fitter PRIVATE -Wno-pedantic) +target_compile_options(${PROJECT_NAME} PRIVATE -Wno-pedantic) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "MapHeightFitterNode" + PLUGIN "autoware::map_height_fitter::MapHeightFitterNode" EXECUTABLE ${PROJECT_NAME}_node EXECUTOR MultiThreadedExecutor ) diff --git a/map/map_height_fitter/README.md b/map/autoware_map_height_fitter/README.md similarity index 87% rename from map/map_height_fitter/README.md rename to map/autoware_map_height_fitter/README.md index 7da70a0ff7766..a61760b142615 100644 --- a/map/map_height_fitter/README.md +++ b/map/autoware_map_height_fitter/README.md @@ -1,4 +1,4 @@ -# map_height_fitter +# autoware_map_height_fitter This library fits the given point with the ground of the point cloud map. The map loading operation is switched by the parameter `enable_partial_load` of the node specified by `map_loader_name`. @@ -6,7 +6,7 @@ The node using this library must use multi thread executor. ## Parameters -{{ json_to_markdown("map/map_height_fitter/schema/map_height_fitter.schema.json") }} +{{ json_to_markdown("map/autoware_map_height_fitter/schema/map_height_fitter.schema.json") }} ## Topic subscription diff --git a/map/map_height_fitter/config/map_height_fitter.param.yaml b/map/autoware_map_height_fitter/config/map_height_fitter.param.yaml similarity index 100% rename from map/map_height_fitter/config/map_height_fitter.param.yaml rename to map/autoware_map_height_fitter/config/map_height_fitter.param.yaml diff --git a/map/map_height_fitter/include/map_height_fitter/map_height_fitter.hpp b/map/autoware_map_height_fitter/include/autoware/map_height_fitter/map_height_fitter.hpp similarity index 82% rename from map/map_height_fitter/include/map_height_fitter/map_height_fitter.hpp rename to map/autoware_map_height_fitter/include/autoware/map_height_fitter/map_height_fitter.hpp index 46baffa270108..e8ab0eef931a3 100644 --- a/map/map_height_fitter/include/map_height_fitter/map_height_fitter.hpp +++ b/map/autoware_map_height_fitter/include/autoware/map_height_fitter/map_height_fitter.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MAP_HEIGHT_FITTER__MAP_HEIGHT_FITTER_HPP_ -#define MAP_HEIGHT_FITTER__MAP_HEIGHT_FITTER_HPP_ +#ifndef AUTOWARE__MAP_HEIGHT_FITTER__MAP_HEIGHT_FITTER_HPP_ +#define AUTOWARE__MAP_HEIGHT_FITTER__MAP_HEIGHT_FITTER_HPP_ #include @@ -23,7 +23,7 @@ #include #include -namespace map_height_fitter +namespace autoware::map_height_fitter { using geometry_msgs::msg::Point; @@ -44,6 +44,6 @@ class MapHeightFitter final std::unique_ptr impl_; }; -} // namespace map_height_fitter +} // namespace autoware::map_height_fitter -#endif // MAP_HEIGHT_FITTER__MAP_HEIGHT_FITTER_HPP_ +#endif // AUTOWARE__MAP_HEIGHT_FITTER__MAP_HEIGHT_FITTER_HPP_ diff --git a/map/map_height_fitter/launch/map_height_fitter.launch.xml b/map/autoware_map_height_fitter/launch/map_height_fitter.launch.xml similarity index 60% rename from map/map_height_fitter/launch/map_height_fitter.launch.xml rename to map/autoware_map_height_fitter/launch/map_height_fitter.launch.xml index 3e01a35a8e519..fa613d102b496 100644 --- a/map/map_height_fitter/launch/map_height_fitter.launch.xml +++ b/map/autoware_map_height_fitter/launch/map_height_fitter.launch.xml @@ -1,9 +1,9 @@ - + - + diff --git a/map/map_height_fitter/package.xml b/map/autoware_map_height_fitter/package.xml similarity index 93% rename from map/map_height_fitter/package.xml rename to map/autoware_map_height_fitter/package.xml index 36e5814365c3f..17f2e95edb159 100644 --- a/map/map_height_fitter/package.xml +++ b/map/autoware_map_height_fitter/package.xml @@ -1,9 +1,9 @@ - map_height_fitter + autoware_map_height_fitter 0.1.0 - The map_height_fitter package + The autoware_map_height_fitter package Takagi, Isamu Yamato Ando Masahiro Sakamoto diff --git a/map/map_height_fitter/schema/map_height_fitter.schema.json b/map/autoware_map_height_fitter/schema/map_height_fitter.schema.json similarity index 100% rename from map/map_height_fitter/schema/map_height_fitter.schema.json rename to map/autoware_map_height_fitter/schema/map_height_fitter.schema.json diff --git a/map/map_height_fitter/src/map_height_fitter.cpp b/map/autoware_map_height_fitter/src/map_height_fitter.cpp similarity index 98% rename from map/map_height_fitter/src/map_height_fitter.cpp rename to map/autoware_map_height_fitter/src/map_height_fitter.cpp index c9e199ad88422..732f13e375f05 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/map/autoware_map_height_fitter/src/map_height_fitter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "map_height_fitter/map_height_fitter.hpp" +#include "autoware/map_height_fitter/map_height_fitter.hpp" #include #include @@ -31,7 +31,7 @@ #include -namespace map_height_fitter +namespace autoware::map_height_fitter { struct MapHeightFitter::Impl @@ -285,4 +285,4 @@ std::optional MapHeightFitter::fit(const Point & position, const std::str return impl_->fit(position, frame); } -} // namespace map_height_fitter +} // namespace autoware::map_height_fitter diff --git a/map/map_height_fitter/src/map_height_fitter_node.cpp b/map/autoware_map_height_fitter/src/map_height_fitter_node.cpp similarity index 88% rename from map/map_height_fitter/src/map_height_fitter_node.cpp rename to map/autoware_map_height_fitter/src/map_height_fitter_node.cpp index fdc7604b68855..99e55c26094b7 100644 --- a/map/map_height_fitter/src/map_height_fitter_node.cpp +++ b/map/autoware_map_height_fitter/src/map_height_fitter_node.cpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "map_height_fitter/map_height_fitter.hpp" +#include "autoware/map_height_fitter/map_height_fitter.hpp" #include #include +namespace autoware::map_height_fitter +{ using tier4_localization_msgs::srv::PoseWithCovarianceStamped; class MapHeightFitterNode : public rclcpp::Node @@ -46,6 +48,7 @@ class MapHeightFitterNode : public rclcpp::Node map_height_fitter::MapHeightFitter fitter_; rclcpp::Service::SharedPtr srv_; }; +} // namespace autoware::map_height_fitter #include -RCLCPP_COMPONENTS_REGISTER_NODE(MapHeightFitterNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::map_height_fitter::MapHeightFitterNode) 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 2e299fd7a2e51..c04503bee70fd 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/README.md +++ b/system/default_ad_api_helpers/ad_api_adaptors/README.md @@ -5,7 +5,7 @@ This node makes it easy to use the localization AD API from RViz. When a initial pose topic is received, call the localization initialize API. This node depends on the map height fitter library. -[See here for more details.](../../../map/map_height_fitter/README.md) +[See here for more details.](../../../map/autoware_map_height_fitter/README.md) | Interface | Local Name | Global Name | Description | | ------------ | ----------- | ---------------------------- | ----------------------------------------- | diff --git a/system/default_ad_api_helpers/ad_api_adaptors/package.xml b/system/default_ad_api_helpers/ad_api_adaptors/package.xml index b070131f1d567..a395eadbe3d2d 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/package.xml +++ b/system/default_ad_api_helpers/ad_api_adaptors/package.xml @@ -14,8 +14,8 @@ autoware_ad_api_specs autoware_adapi_v1_msgs + autoware_map_height_fitter component_interface_utils - map_height_fitter rclcpp rclcpp_components diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp b/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp index 340bc3b0a3058..e5b8d0e33dd3b 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp +++ b/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp @@ -15,9 +15,9 @@ #ifndef INITIAL_POSE_ADAPTOR_HPP_ #define INITIAL_POSE_ADAPTOR_HPP_ +#include #include #include -#include #include #include @@ -36,7 +36,7 @@ class InitialPoseAdaptor : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_initial_pose_; component_interface_utils::Client::SharedPtr cli_initialize_; std::array rviz_particle_covariance_; - map_height_fitter::MapHeightFitter fitter_; + autoware::map_height_fitter::MapHeightFitter fitter_; void on_initial_pose(const PoseWithCovarianceStamped::ConstSharedPtr msg); }; From f58bac6cc1d432d55d4328fb46a1a5e788766243 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 23 Aug 2024 13:44:45 +0900 Subject: [PATCH 16/46] refactor(pose_instability_detector)!: prefix package and namespace with autoware (#8568) * add autoware_ prefix Signed-off-by: a-maumau * add autoware_ prefix Signed-off-by: a-maumau --------- Signed-off-by: a-maumau Co-authored-by: SakodaShintaro --- .github/CODEOWNERS | 2 +- .../pose_twist_fusion_filter.launch.xml | 4 ++-- launch/tier4_localization_launch/package.xml | 2 +- .../CMakeLists.txt | 4 ++-- .../README.md | 4 ++-- .../config/pose_instability_detector.param.yaml | 0 .../pose_instability_detector.hpp | 3 +++ .../launch/pose_instability_detector.launch.xml | 4 ++-- .../media/how_to_snip_twist.png | Bin .../media/lateral_threshold_calculation.png | Bin .../media/pose_instability_detector_overview.png | Bin .../media/pose_instabilty_detector_procedure.svg | 0 .../media/rqt_runtime_monitor.png | Bin .../media/timeline.drawio.svg | 0 .../package.xml | 4 ++-- .../schema/pose_instability_detector.schema.json | 0 .../src/pose_instability_detector.cpp | 5 ++++- .../test/test.cpp | 7 ++++--- .../test/test_message_helper_node.hpp | 0 19 files changed, 23 insertions(+), 16 deletions(-) rename localization/{pose_instability_detector => autoware_pose_instability_detector}/CMakeLists.txt (82%) rename localization/{pose_instability_detector => autoware_pose_instability_detector}/README.md (98%) rename localization/{pose_instability_detector => autoware_pose_instability_detector}/config/pose_instability_detector.param.yaml (100%) rename localization/{pose_instability_detector => autoware_pose_instability_detector}/include/autoware/pose_instability_detector/pose_instability_detector.hpp (97%) rename localization/{pose_instability_detector => autoware_pose_instability_detector}/launch/pose_instability_detector.launch.xml (58%) rename localization/{pose_instability_detector => autoware_pose_instability_detector}/media/how_to_snip_twist.png (100%) rename localization/{pose_instability_detector => autoware_pose_instability_detector}/media/lateral_threshold_calculation.png (100%) rename localization/{pose_instability_detector => autoware_pose_instability_detector}/media/pose_instability_detector_overview.png (100%) rename localization/{pose_instability_detector => autoware_pose_instability_detector}/media/pose_instabilty_detector_procedure.svg (100%) rename localization/{pose_instability_detector => autoware_pose_instability_detector}/media/rqt_runtime_monitor.png (100%) rename localization/{pose_instability_detector => autoware_pose_instability_detector}/media/timeline.drawio.svg (100%) rename localization/{pose_instability_detector => autoware_pose_instability_detector}/package.xml (91%) rename localization/{pose_instability_detector => autoware_pose_instability_detector}/schema/pose_instability_detector.schema.json (100%) rename localization/{pose_instability_detector => autoware_pose_instability_detector}/src/pose_instability_detector.cpp (98%) rename localization/{pose_instability_detector => autoware_pose_instability_detector}/test/test.cpp (96%) rename localization/{pose_instability_detector => autoware_pose_instability_detector}/test/test_message_helper_node.hpp (100%) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 277255d529fe7..403fe7e0f728f 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -93,7 +93,7 @@ localization/localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.j 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/pose_estimator_arbiter/** 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/pose_initializer/** anh.nguyen.2@tier4.jp isamu.takagi@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/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_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/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_fusion_filter/pose_twist_fusion_filter.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml index fa479c71dcbac..dda9c1f2c97ce 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml @@ -35,10 +35,10 @@ - + - + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index efeecd3e37f61..870fc66c1e2e4 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -23,13 +23,13 @@ autoware_gyro_odometer autoware_pointcloud_preprocessor autoware_pose_estimator_arbiter + autoware_pose_instability_detector eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_rt ekf_localizer ndt_scan_matcher pose_initializer - pose_instability_detector topic_tools yabloc_common yabloc_image_processing diff --git a/localization/pose_instability_detector/CMakeLists.txt b/localization/autoware_pose_instability_detector/CMakeLists.txt similarity index 82% rename from localization/pose_instability_detector/CMakeLists.txt rename to localization/autoware_pose_instability_detector/CMakeLists.txt index c6f94ab7df16e..9e7a1d15ee0f2 100644 --- a/localization/pose_instability_detector/CMakeLists.txt +++ b/localization/autoware_pose_instability_detector/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(pose_instability_detector) +project(autoware_pose_instability_detector) find_package(autoware_cmake REQUIRED) autoware_package() @@ -9,7 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "PoseInstabilityDetector" + PLUGIN "autoware::pose_instability_detector::PoseInstabilityDetector" EXECUTABLE ${PROJECT_NAME}_node EXECUTOR SingleThreadedExecutor ) diff --git a/localization/pose_instability_detector/README.md b/localization/autoware_pose_instability_detector/README.md similarity index 98% rename from localization/pose_instability_detector/README.md rename to localization/autoware_pose_instability_detector/README.md index 4ced0fa8eb97b..5beb796f46e12 100644 --- a/localization/pose_instability_detector/README.md +++ b/localization/autoware_pose_instability_detector/README.md @@ -1,4 +1,4 @@ -# pose_instability_detector +# autoware_pose_instability_detector The `pose_instability_detector` is a node designed to monitor the stability of `/localization/kinematic_state`, which is an output topic of the Extended Kalman Filter (EKF). @@ -109,7 +109,7 @@ $$ ## Parameters -{{ json_to_markdown("localization/pose_instability_detector/schema/pose_instability_detector.schema.json") }} +{{ json_to_markdown("localization/autoware_pose_instability_detector/schema/pose_instability_detector.schema.json") }} ## Input diff --git a/localization/pose_instability_detector/config/pose_instability_detector.param.yaml b/localization/autoware_pose_instability_detector/config/pose_instability_detector.param.yaml similarity index 100% rename from localization/pose_instability_detector/config/pose_instability_detector.param.yaml rename to localization/autoware_pose_instability_detector/config/pose_instability_detector.param.yaml diff --git a/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp b/localization/autoware_pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp similarity index 97% rename from localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp rename to localization/autoware_pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp index febff8c2ee244..615b818a2bde0 100644 --- a/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp +++ b/localization/autoware_pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp @@ -26,6 +26,8 @@ #include #include +namespace autoware::pose_instability_detector +{ class PoseInstabilityDetector : public rclcpp::Node { using Quaternion = geometry_msgs::msg::Quaternion; @@ -93,5 +95,6 @@ class PoseInstabilityDetector : public rclcpp::Node std::optional prev_odometry_ = std::nullopt; std::deque twist_buffer_; }; +} // namespace autoware::pose_instability_detector #endif // AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_ diff --git a/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml b/localization/autoware_pose_instability_detector/launch/pose_instability_detector.launch.xml similarity index 58% rename from localization/pose_instability_detector/launch/pose_instability_detector.launch.xml rename to localization/autoware_pose_instability_detector/launch/pose_instability_detector.launch.xml index 5ebe7d7e429e0..716f2b165bb9b 100644 --- a/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml +++ b/localization/autoware_pose_instability_detector/launch/pose_instability_detector.launch.xml @@ -1,12 +1,12 @@ - + - + diff --git a/localization/pose_instability_detector/media/how_to_snip_twist.png b/localization/autoware_pose_instability_detector/media/how_to_snip_twist.png similarity index 100% rename from localization/pose_instability_detector/media/how_to_snip_twist.png rename to localization/autoware_pose_instability_detector/media/how_to_snip_twist.png diff --git a/localization/pose_instability_detector/media/lateral_threshold_calculation.png b/localization/autoware_pose_instability_detector/media/lateral_threshold_calculation.png similarity index 100% rename from localization/pose_instability_detector/media/lateral_threshold_calculation.png rename to localization/autoware_pose_instability_detector/media/lateral_threshold_calculation.png diff --git a/localization/pose_instability_detector/media/pose_instability_detector_overview.png b/localization/autoware_pose_instability_detector/media/pose_instability_detector_overview.png similarity index 100% rename from localization/pose_instability_detector/media/pose_instability_detector_overview.png rename to localization/autoware_pose_instability_detector/media/pose_instability_detector_overview.png diff --git a/localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg b/localization/autoware_pose_instability_detector/media/pose_instabilty_detector_procedure.svg similarity index 100% rename from localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg rename to localization/autoware_pose_instability_detector/media/pose_instabilty_detector_procedure.svg diff --git a/localization/pose_instability_detector/media/rqt_runtime_monitor.png b/localization/autoware_pose_instability_detector/media/rqt_runtime_monitor.png similarity index 100% rename from localization/pose_instability_detector/media/rqt_runtime_monitor.png rename to localization/autoware_pose_instability_detector/media/rqt_runtime_monitor.png diff --git a/localization/pose_instability_detector/media/timeline.drawio.svg b/localization/autoware_pose_instability_detector/media/timeline.drawio.svg similarity index 100% rename from localization/pose_instability_detector/media/timeline.drawio.svg rename to localization/autoware_pose_instability_detector/media/timeline.drawio.svg diff --git a/localization/pose_instability_detector/package.xml b/localization/autoware_pose_instability_detector/package.xml similarity index 91% rename from localization/pose_instability_detector/package.xml rename to localization/autoware_pose_instability_detector/package.xml index 7e0237cbaf780..d59baf5817233 100644 --- a/localization/pose_instability_detector/package.xml +++ b/localization/autoware_pose_instability_detector/package.xml @@ -1,9 +1,9 @@ - pose_instability_detector + autoware_pose_instability_detector 0.1.0 - The pose_instability_detector package + The autoware_pose_instability_detector package Yamato Ando Kento Yabuuchi Masahiro Sakamoto diff --git a/localization/pose_instability_detector/schema/pose_instability_detector.schema.json b/localization/autoware_pose_instability_detector/schema/pose_instability_detector.schema.json similarity index 100% rename from localization/pose_instability_detector/schema/pose_instability_detector.schema.json rename to localization/autoware_pose_instability_detector/schema/pose_instability_detector.schema.json diff --git a/localization/pose_instability_detector/src/pose_instability_detector.cpp b/localization/autoware_pose_instability_detector/src/pose_instability_detector.cpp similarity index 98% rename from localization/pose_instability_detector/src/pose_instability_detector.cpp rename to localization/autoware_pose_instability_detector/src/pose_instability_detector.cpp index 23362dd13c6bc..36aa4f6d7d73e 100644 --- a/localization/pose_instability_detector/src/pose_instability_detector.cpp +++ b/localization/autoware_pose_instability_detector/src/pose_instability_detector.cpp @@ -25,6 +25,8 @@ #include #include +namespace autoware::pose_instability_detector +{ PoseInstabilityDetector::PoseInstabilityDetector(const rclcpp::NodeOptions & options) : rclcpp::Node("pose_instability_detector", options), timer_period_(this->declare_parameter("timer_period")), @@ -409,6 +411,7 @@ PoseInstabilityDetector::clip_out_necessary_twist( } return result_deque; } +} // namespace autoware::pose_instability_detector #include -RCLCPP_COMPONENTS_REGISTER_NODE(PoseInstabilityDetector) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pose_instability_detector::PoseInstabilityDetector) diff --git a/localization/pose_instability_detector/test/test.cpp b/localization/autoware_pose_instability_detector/test/test.cpp similarity index 96% rename from localization/pose_instability_detector/test/test.cpp rename to localization/autoware_pose_instability_detector/test/test.cpp index 482e659e7a13c..bd663a2406903 100644 --- a/localization/pose_instability_detector/test/test.cpp +++ b/localization/autoware_pose_instability_detector/test/test.cpp @@ -36,7 +36,7 @@ class TestPoseInstabilityDetector : public ::testing::Test void SetUp() override { const std::string yaml_path = - ament_index_cpp::get_package_share_directory("pose_instability_detector") + + ament_index_cpp::get_package_share_directory("autoware_pose_instability_detector") + "/config/pose_instability_detector.param.yaml"; rcl_params_t * params_st = rcl_yaml_node_struct_init(rcl_get_default_allocator()); @@ -53,7 +53,8 @@ class TestPoseInstabilityDetector : public ::testing::Test } } - subject_ = std::make_shared(node_options); + subject_ = + std::make_shared(node_options); executor_.add_node(subject_); helper_ = std::make_shared(); @@ -69,7 +70,7 @@ class TestPoseInstabilityDetector : public ::testing::Test } rclcpp::executors::SingleThreadedExecutor executor_; - std::shared_ptr subject_; + std::shared_ptr subject_; std::shared_ptr helper_; }; diff --git a/localization/pose_instability_detector/test/test_message_helper_node.hpp b/localization/autoware_pose_instability_detector/test/test_message_helper_node.hpp similarity index 100% rename from localization/pose_instability_detector/test/test_message_helper_node.hpp rename to localization/autoware_pose_instability_detector/test/test_message_helper_node.hpp From 490cd186e7ce4443177234aa3574cfd79b51640e Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Fri, 23 Aug 2024 14:05:27 +0900 Subject: [PATCH 17/46] refactor(autoware_pointcloud_preprocessor): rework voxel grid outlier filter parameters (#8476) * feat: rework voxel grid outlier filter parameters Signed-off-by: vividf * chore: add boundary Signed-off-by: vividf --------- Signed-off-by: vividf --- .../CMakeLists.txt | 2 +- .../voxel_grid_outlier_filter_node.param.yaml | 6 +++ .../docs/voxel-grid-outlier-filter.md | 7 +-- ...hpp => voxel_grid_outlier_filter_node.hpp} | 8 +-- .../voxel_grid_outlier_filter_node.launch.xml | 16 ++++++ ...voxel_grid_outlier_filter_node.schema.json | 52 +++++++++++++++++++ ...cpp => voxel_grid_outlier_filter_node.cpp} | 12 ++--- 7 files changed, 86 insertions(+), 17 deletions(-) create mode 100644 sensing/autoware_pointcloud_preprocessor/config/voxel_grid_outlier_filter_node.param.yaml rename sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/{voxel_grid_outlier_filter_nodelet.hpp => voxel_grid_outlier_filter_node.hpp} (92%) create mode 100644 sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_outlier_filter_node.launch.xml create mode 100644 sensing/autoware_pointcloud_preprocessor/schema/voxel_grid_outlier_filter_node.schema.json rename sensing/autoware_pointcloud_preprocessor/src/outlier_filter/{voxel_grid_outlier_filter_nodelet.cpp => voxel_grid_outlier_filter_node.cpp} (90%) diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 56c0238f104ad..5d7aba4d0567b 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -71,7 +71,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/downsample_filter/approximate_downsample_filter_nodelet.cpp src/downsample_filter/pickup_based_voxel_grid_downsample_filter.cpp src/outlier_filter/ring_outlier_filter_nodelet.cpp - src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp + src/outlier_filter/voxel_grid_outlier_filter_node.cpp src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp src/outlier_filter/dual_return_outlier_filter_nodelet.cpp src/passthrough_filter/passthrough_filter_nodelet.cpp diff --git a/sensing/autoware_pointcloud_preprocessor/config/voxel_grid_outlier_filter_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/voxel_grid_outlier_filter_node.param.yaml new file mode 100644 index 0000000000000..2ff4c64d5f835 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/voxel_grid_outlier_filter_node.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + voxel_size_x: 0.3 + voxel_size_y: 0.3 + voxel_size_z: 0.1 + voxel_points_threshold: 2 diff --git a/sensing/autoware_pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md index d99b54ef09a73..920d550c2426b 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md @@ -23,12 +23,7 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Core Parameters -| Name | Type | Default Value | Description | -| ------------------------ | ------ | ------------- | ------------------------------------------ | -| `voxel_size_x` | double | 0.3 | the voxel size along x-axis [m] | -| `voxel_size_y` | double | 0.3 | the voxel size along y-axis [m] | -| `voxel_size_z` | double | 0.1 | the voxel size along z-axis [m] | -| `voxel_points_threshold` | int | 2 | the minimum number of points in each voxel | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/voxel_grid_outlier_filter_node.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_node.hpp similarity index 92% rename from sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_node.hpp index 4ec50c53c2393..c9c6d1fd2dbad 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_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__VOXEL_GRID_OUTLIER_FILTER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODE_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODE_HPP_ #include "autoware/pointcloud_preprocessor/filter.hpp" @@ -50,4 +50,4 @@ class VoxelGridOutlierFilterComponent : public autoware::pointcloud_preprocessor }; } // namespace autoware::pointcloud_preprocessor -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODELET_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODE_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_outlier_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_outlier_filter_node.launch.xml new file mode 100644 index 0000000000000..380a46eed3159 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_outlier_filter_node.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/schema/voxel_grid_outlier_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/voxel_grid_outlier_filter_node.schema.json new file mode 100644 index 0000000000000..586b577f8540d --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/voxel_grid_outlier_filter_node.schema.json @@ -0,0 +1,52 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Voxel Grid Outlier Filter Node", + "type": "object", + "definitions": { + "voxel_grid_outlier_filter": { + "type": "object", + "properties": { + "voxel_size_x": { + "type": "number", + "description": "the voxel size along x-axis [m]", + "default": "0.3", + "minimum": 0 + }, + "voxel_size_y": { + "type": "number", + "description": "the voxel size along y-axis [m]", + "default": "0.3", + "minimum": 0 + }, + "voxel_size_z": { + "type": "number", + "description": "the voxel size along z-axis [m]", + "default": "0.1", + "minimum": 0 + }, + "voxel_points_threshold": { + "type": "integer", + "description": "the minimum number of points in each voxel", + "default": "2", + "minimum": 1 + } + }, + "required": ["voxel_size_x", "voxel_size_y", "voxel_size_z", "voxel_points_threshold"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/voxel_grid_outlier_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_node.cpp similarity index 90% rename from sensing/autoware_pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_node.cpp index 94dabf7ee5d9b..1f5cdc80a9c2c 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/voxel_grid_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/voxel_grid_outlier_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_node.hpp" #include #include @@ -28,10 +28,10 @@ VoxelGridOutlierFilterComponent::VoxelGridOutlierFilterComponent( { // set initial parameters { - 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_points_threshold_ = static_cast(declare_parameter("voxel_points_threshold", 2)); + voxel_size_x_ = declare_parameter("voxel_size_x"); + voxel_size_y_ = declare_parameter("voxel_size_y"); + voxel_size_z_ = declare_parameter("voxel_size_z"); + voxel_points_threshold_ = declare_parameter("voxel_points_threshold"); } using std::placeholders::_1; From 27bc0a995b747ceacd179368d8ff61de8cb28ebf Mon Sep 17 00:00:00 2001 From: Hayate TOBA <105347690+bathteayo@users.noreply.github.com> Date: Fri, 23 Aug 2024 14:59:41 +0900 Subject: [PATCH 18/46] fix(diagnostic_graph_aggregator): fix unusedFunction (#8580) fix: unusedFunction Signed-off-by: bathteayo <105347690+bathteayo@users.noreply.github.com> Co-authored-by: kobayu858 <129580202+kobayu858@users.noreply.github.com> --- system/diagnostic_graph_aggregator/src/common/graph/graph.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/system/diagnostic_graph_aggregator/src/common/graph/graph.hpp b/system/diagnostic_graph_aggregator/src/common/graph/graph.hpp index 0e64f25860fda..14a27dae9a539 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/graph.hpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/graph.hpp @@ -34,7 +34,6 @@ class Graph void update(const rclcpp::Time & stamp); // cppcheck-suppress functionConst bool update(const rclcpp::Time & stamp, const DiagnosticStatus & status); const auto & nodes() const { return nodes_; } - const auto & diags() const { return diags_; } const auto & units() const { return units_; } DiagGraphStruct create_struct(const rclcpp::Time & stamp) const; DiagGraphStatus create_status(const rclcpp::Time & stamp) const; From e47f8097a8696426402168d5fa941f7ca6c8284d Mon Sep 17 00:00:00 2001 From: Hayate TOBA <105347690+bathteayo@users.noreply.github.com> Date: Fri, 23 Aug 2024 15:00:19 +0900 Subject: [PATCH 19/46] fix(duplicated_node_checker): fix unusedFunction (#8579) fix: unusedFunction Signed-off-by: bathteayo <105347690+bathteayo@users.noreply.github.com> Co-authored-by: kobayu858 <129580202+kobayu858@users.noreply.github.com> --- .../src/duplicated_node_checker_core.cpp | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp index c9bc07cffde72..5c698a0a01860 100644 --- a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp +++ b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp @@ -42,19 +42,6 @@ DuplicatedNodeChecker::DuplicatedNodeChecker(const rclcpp::NodeOptions & node_op this, get_clock(), period_ns, std::bind(&DuplicatedNodeChecker::onTimer, this)); } -std::string get_fullname_from_name_ns_pair(std::pair name_and_ns_pair) -{ - std::string full_name; - const std::string & name = name_and_ns_pair.first; - const std::string & ns = name_and_ns_pair.second; - if (ns.back() == '/') { - full_name = ns + name; - } else { - full_name = ns + "/" + name; - } - return full_name; -} - void DuplicatedNodeChecker::onTimer() { updater_.force_update(); From d913e318fe18ba8433ed8db205dd2383d392f348 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 23 Aug 2024 15:01:07 +0900 Subject: [PATCH 20/46] fix(control_performance_analysis): fix unusedFunction (#8557) fix:unusedFunction Signed-off-by: kobayu858 --- .../control_performance_analysis_utils.hpp | 4 ---- .../src/control_performance_analysis_utils.cpp | 10 ---------- 2 files changed, 14 deletions(-) diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp index 87f331c5ff2af..fb939d825b653 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp @@ -79,10 +79,6 @@ inline geometry_msgs::msg::Quaternion createOrientationMsgFromYaw(double yaw_ang // p-points a, b contains [x, y] coordinates. double determinant(std::array const & a, std::array const & b); -double triangleArea( - std::array const & a, std::array const & b, - std::array const & c); - double curvatureFromThreePoints( std::array const & a, std::array const & b, std::array const & c); diff --git a/control/control_performance_analysis/src/control_performance_analysis_utils.cpp b/control/control_performance_analysis/src/control_performance_analysis_utils.cpp index 467d26bbb69a0..f96aba2b54547 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_utils.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_utils.cpp @@ -25,15 +25,5 @@ double determinant(std::array const & a, std::array const return a[0] * b[1] - b[0] * a[1]; } -double triangleArea( - std::array const & a, std::array const & b, std::array const & c) -{ - double m1 = determinant(a, b); - double m2 = determinant(b, c); - double m3 = determinant(c, a); - - return 0.5 * (m1 + m2 + m3); -} - } // namespace utils } // namespace control_performance_analysis From efa1b4aa5992d7e6e1c858c84dc3cdfdd23aad25 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 23 Aug 2024 15:01:22 +0900 Subject: [PATCH 21/46] fix(autoware_compare_map_segmentation): fix unusedFunction (#8565) fix:unusedFunction Signed-off-by: kobayu858 --- .../src/voxel_grid_map_loader/voxel_grid_map_loader.cpp | 9 --------- .../src/voxel_grid_map_loader/voxel_grid_map_loader.hpp | 3 --- 2 files changed, 12 deletions(-) diff --git a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp index 4e3728be53171..195e09e4b406e 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp @@ -31,15 +31,6 @@ VoxelGridMapLoader::VoxelGridMapLoader( debug_ = node->declare_parameter("publish_debug_pcd"); } -bool VoxelGridMapLoader::is_close_points( - const pcl::PointXYZ point, const pcl::PointXYZ target_point, const double distance_threshold) -{ - if (distance3D(point, target_point) < distance_threshold * distance_threshold) { - return true; - } - return false; -} - void VoxelGridMapLoader::publish_downsampled_map( const pcl::PointCloud & downsampled_pc) { diff --git a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp index 6560ae82f8bce..1bcdff228f40b 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp @@ -88,7 +88,6 @@ class VoxelGridEx : public pcl::VoxelGrid inline Eigen::Vector4i get_max_b() const { return max_b_; } inline Eigen::Vector4i get_div_b() const { return div_b_; } inline Eigen::Array4f get_inverse_leaf_size() const { return inverse_leaf_size_; } - inline std::vector getLeafLayout() { return (leaf_layout_); } }; class VoxelGridMapLoader @@ -122,8 +121,6 @@ class VoxelGridMapLoader const double distance_threshold, const PointCloudPtr & map, VoxelGridPointXYZ & voxel) const; void publish_downsampled_map(const pcl::PointCloud & downsampled_pc); - static bool is_close_points( - const pcl::PointXYZ point, const pcl::PointXYZ target_point, const double distance_threshold); std::string * tf_map_input_frame_; }; From 3bc7897adcf2967980b95c996f60ae4935ce9e9d Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 23 Aug 2024 15:01:53 +0900 Subject: [PATCH 22/46] fix(autoware_ground_segmentation): fix unusedFunction (#8566) fix:unusedFunction Signed-off-by: kobayu858 --- .../src/scan_ground_filter/node.hpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp index 94ef16042ff33..e87fd0c341a6f 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp @@ -143,11 +143,6 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt float getMinHeight() const { return height_min; } - uint16_t getGridId() const { return grid_id; } - - pcl::PointIndices getIndices() const { return pcl_indices; } - std::vector getHeightList() const { return height_list; } - pcl::PointIndices & getIndicesRef() { return pcl_indices; } std::vector & getHeightListRef() { return height_list; } }; From e34d7edcb706da39328bc69349ff91e8fc73add4 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 23 Aug 2024 15:02:24 +0900 Subject: [PATCH 23/46] fix(autoware_multi_object_tracker): fix unusedFunction (#8573) fix:unusedFunction Signed-off-by: kobayu858 --- .../src/processor/input_manager.cpp | 11 ----------- .../src/processor/input_manager.hpp | 15 --------------- 2 files changed, 26 deletions(-) diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp index 5172cc5062450..decd2139c5b81 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp @@ -46,17 +46,6 @@ void InputStream::init(const InputChannel & input_channel) latest_message_time_ = node_.now(); } -bool InputStream::getTimestamps( - rclcpp::Time & latest_measurement_time, rclcpp::Time & latest_message_time) const -{ - if (!isTimeInitialized()) { - return false; - } - latest_measurement_time = latest_measurement_time_; - latest_message_time = latest_message_time_; - return true; -} - void InputStream::onMessage( const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg) { diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp index ee2c6910159b5..f8b221f733faa 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp @@ -46,7 +46,6 @@ class InputStream void init(const InputChannel & input_channel); - void setQueueSize(size_t que_size) { que_size_ = que_size; } void setTriggerFunction(std::function func_trigger) { func_trigger_ = func_trigger; @@ -60,15 +59,8 @@ class InputStream void getObjectsOlderThan( const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, ObjectsList & objects_list); - void getNames(std::string & long_name, std::string & short_name) const - { - long_name = long_name_; - short_name = short_name_; - } bool isSpawnEnabled() const { return is_spawn_enabled_; } - std::string getLongName() const { return long_name_; } - size_t getObjectsCount() const { return objects_que_.size(); } void getTimeStatistics( double & latency_mean, double & latency_var, double & interval_mean, double & interval_var) const @@ -78,13 +70,6 @@ class InputStream interval_mean = interval_mean_; interval_var = interval_var_; } - void getLatencyStatistics(double & latency_mean, double & latency_var) const - { - latency_mean = latency_mean_; - latency_var = latency_var_; - } - bool getTimestamps( - rclcpp::Time & latest_measurement_time, rclcpp::Time & latest_message_time) const; rclcpp::Time getLatestMeasurementTime() const { return latest_measurement_time_; } private: From 18063a5f8b30b9cb7804831e5886922774006fd4 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 23 Aug 2024 15:25:23 +0900 Subject: [PATCH 24/46] fix(autoware_traffic_light_fine_detector): fix unusedFunction (#8583) fix:unusedFunction Signed-off-by: kobayu858 --- .../src/traffic_light_fine_detector_node.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp index 381fa79ed090c..fec26b762dfe9 100644 --- a/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp +++ b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp @@ -49,11 +49,6 @@ float calWeightedIou( namespace autoware::traffic_light { -inline std::vector toFloatVector(const std::vector & double_vector) -{ - return std::vector(double_vector.begin(), double_vector.end()); -} - TrafficLightFineDetectorNode::TrafficLightFineDetectorNode(const rclcpp::NodeOptions & options) : Node("traffic_light_fine_detector_node", options) { From f656a122039adca416241857bef11eff2a8b59c0 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Fri, 23 Aug 2024 16:14:35 +0900 Subject: [PATCH 25/46] feat(localization): add `lidar_marker_localizer` (#5573) * Added lidar_marker_localizer Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * fix launch file Signed-off-by: yamato-ando * style(pre-commit): autofix * Removed subscriber_member_function.cpp Signed-off-by: Shintaro Sakoda * Renamed the package and the node Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Removed pose_array_interpolator Signed-off-by: Shintaro Sakoda * Removed unused files Signed-off-by: Shintaro Sakoda * Removed include dir Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Renamed wrong names Signed-off-by: Shintaro Sakoda * fix magic number Signed-off-by: yamato-ando * style(pre-commit): autofix * fix bug Signed-off-by: yamato-ando * parameterized Signed-off-by: yamato-ando * style(pre-commit): autofix * add base_covariance Signed-off-by: yamato-ando * style(pre-commit): autofix * Removed std::cerr Signed-off-by: Shintaro Sakoda * Removed unused code Signed-off-by: Shintaro Sakoda * Removed unnecessary publishers Signed-off-by: Shintaro Sakoda * Changed to use alias Signed-off-by: Shintaro Sakoda * Fixed result_base_link_on_map Signed-off-by: Shintaro Sakoda * Changed to use "using std::placeholders" Signed-off-by: Shintaro Sakoda * Refactored points_callback Signed-off-by: Shintaro Sakoda * Fixed as pointed out by linter Signed-off-by: Shintaro Sakoda * Refactored lidar_marker_localizer Signed-off-by: Shintaro Sakoda * Fixed const reference Signed-off-by: Shintaro Sakoda * Refactor point variables Signed-off-by: Shintaro Sakoda * Added detect_landmarks Signed-off-by: Shintaro Sakoda * rework filering params Signed-off-by: yamato-ando * fix marker position Signed-off-by: yamato-ando * style(pre-commit): autofix * fix build error Signed-off-by: yamato-ando * fix marker position Signed-off-by: yamato-ando * style(pre-commit): autofix * update readme Signed-off-by: yamato-ando * style(pre-commit): autofix * Added calculate_diff_pose Signed-off-by: Shintaro Sakoda * Fixed to pass linter Signed-off-by: Shintaro Sakoda * update package.xml Signed-off-by: yamato-ando * Fixed to use SmartPoseBuffer Signed-off-by: Shintaro Sakoda * Fixed function calculate_diff_pose to calculate_new_self_pose Signed-off-by: Shintaro Sakoda * Compatible with the latest landmark_manager Signed-off-by: Shintaro Sakoda * Fixed pub_marker Signed-off-by: Shintaro Sakoda * Fixed launch Signed-off-by: Shintaro Sakoda * Removed unused arg Signed-off-by: Shintaro Sakoda * Removed limit_distance_from_self_pose_to_marker_from_lanelet2 Signed-off-by: Shintaro Sakoda * Fixed parse_landmarks Signed-off-by: Shintaro Sakoda * Fixed parameter type Signed-off-by: Shintaro Sakoda * Fixed typo Signed-off-by: Shintaro Sakoda * rework diagnostics Signed-off-by: yamato-ando * style(pre-commit): autofix * rotate covariance Signed-off-by: yamato-ando * style(pre-commit): autofix * add json schema Signed-off-by: yamato-ando * style(pre-commit): autofix * parameterize marker name Signed-off-by: Yamato Ando * python to xml Signed-off-by: Yamato Ando * update launch files Signed-off-by: Yamato Ando * style(pre-commit): autofix * add debug/pose_with_covariance Signed-off-by: Yamato Ando * style(pre-commit): autofix * update readme Signed-off-by: Yamato Ando * update readme Signed-off-by: Yamato Ando * add depend Signed-off-by: Yamato Ando * add sample dataset Signed-off-by: Yamato Ando * add param marker_height_from_ground Signed-off-by: Yamato Ando * style(pre-commit): autofix * fix typo Signed-off-by: Yamato Ando * add includes Signed-off-by: Yamato Ando * add name to TODO comment Signed-off-by: Yamato Ando * style(pre-commit): autofix * rename lidar-marker Signed-off-by: Yamato Ando * modify sample dataset url Signed-off-by: Yamato Ando * add flowchat to readme Signed-off-by: Yamato Ando * fix callbackgroup Signed-off-by: Yamato Ando * add TODO comment Signed-off-by: Yamato Ando * fix throttle timer Signed-off-by: Yamato Ando * delete unused valriable Signed-off-by: Yamato Ando * delete unused line Signed-off-by: Yamato Ando * style(pre-commit): autofix * fix the duplicated code Signed-off-by: Yamato Ando * style(pre-commit): autofix * avoid division by zero Signed-off-by: Yamato Ando * fix TODO comment Signed-off-by: Yamato Ando * fix uncrustify failed Signed-off-by: Yamato Ando * style(pre-commit): autofix * Update localization/landmark_based_localizer/lidar_marker_localizer/src/lidar_marker_localizer.cpp Co-authored-by: SakodaShintaro * change lint_common Signed-off-by: Yamato Ando * update CMakeLists Signed-off-by: Yamato Ando * save intensity func Signed-off-by: Yamato Ando * style(pre-commit): autofix * style(pre-commit): autofix * fix build error Signed-off-by: Yamato Ando * style(pre-commit): autofix * apply PointXYZIRC Signed-off-by: Yamato Ando * add autoware prefix Signed-off-by: Yamato Ando * componentize Signed-off-by: Yamato Ando * move directory Signed-off-by: Yamato Ando * use localization_util's diagnostics lib Signed-off-by: Yamato Ando * style(pre-commit): autofix * applay linter Signed-off-by: Yamato Ando * style(pre-commit): autofix * to pass spell-check Signed-off-by: Yamato Ando * remove _ex Signed-off-by: Yamato Ando * refactor Signed-off-by: Yamato Ando * style(pre-commit): autofix * remove unused depend Signed-off-by: Yamato Ando * update readme Signed-off-by: Yamato Ando * fix typo Signed-off-by: Yamato Ando * fix json Signed-off-by: Yamato Ando * fix autoware prefix Signed-off-by: Yamato Ando * style(pre-commit): autofix --------- Signed-off-by: Shintaro Sakoda Signed-off-by: yamato-ando Signed-off-by: yamato-ando Signed-off-by: Yamato Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: yamato-ando Co-authored-by: Yamato Ando Co-authored-by: yamato-ando --- .../launch/localization.launch.xml | 5 + .../lidar_marker_localizer.launch.xml | 43 ++ .../pose_twist_estimator.launch.xml | 13 +- launch/tier4_localization_launch/package.xml | 1 + .../CMakeLists.txt | 35 + .../autoware_lidar_marker_localizer/README.md | 114 ++++ .../config/lidar_marker_localizer.param.yaml | 42 ++ .../doc_image/detection_algorithm.png | Bin 0 -> 77739 bytes .../launch/lidar_marker_localizer.launch.xml | 24 + .../package.xml | 40 ++ .../schema/lidar_marker_localizer.schema.json | 149 +++++ .../src/lidar_marker_localizer.cpp | 618 ++++++++++++++++++ .../src/lidar_marker_localizer.hpp | 148 +++++ 13 files changed, 1231 insertions(+), 1 deletion(-) create mode 100644 launch/tier4_localization_launch/launch/pose_twist_estimator/lidar_marker_localizer.launch.xml create mode 100755 localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/CMakeLists.txt create mode 100644 localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/README.md create mode 100644 localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/config/lidar_marker_localizer.param.yaml create mode 100644 localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/doc_image/detection_algorithm.png create mode 100644 localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/launch/lidar_marker_localizer.launch.xml create mode 100755 localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml create mode 100644 localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/schema/lidar_marker_localizer.schema.json create mode 100644 localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp create mode 100644 localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index cfc09459a5cb1..dd274ce247064 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -18,6 +18,11 @@ + + + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/lidar_marker_localizer.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/lidar_marker_localizer.launch.xml new file mode 100644 index 0000000000000..301e961a94012 --- /dev/null +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/lidar_marker_localizer.launch.xml @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml index f06c9f83efb35..00f2406aa82d6 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml @@ -9,7 +9,7 @@ - + @@ -18,6 +18,7 @@ + @@ -91,6 +92,16 @@ + + + + + + + + + + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 870fc66c1e2e4..70e09bb9072d8 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -21,6 +21,7 @@ autoware_ar_tag_based_localizer autoware_geo_pose_projector autoware_gyro_odometer + autoware_lidar_marker_localizer autoware_pointcloud_preprocessor autoware_pose_estimator_arbiter autoware_pose_instability_detector diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/CMakeLists.txt b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/CMakeLists.txt new file mode 100755 index 0000000000000..d352cb2f0fa96 --- /dev/null +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_lidar_marker_localizer) + +# find dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(PCL REQUIRED COMPONENTS common io) +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/lidar_marker_localizer.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${PCL_LIBRARIES} +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::lidar_marker_localizer::LidarMarkerLocalizer" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + config + launch +) diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/README.md b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/README.md new file mode 100644 index 0000000000000..15afefaf20ee4 --- /dev/null +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/README.md @@ -0,0 +1,114 @@ +# LiDAR Marker Localizer + +**LiDARMarkerLocalizer** is a detect-reflector-based localization node . + +## Inputs / Outputs + +### `lidar_marker_localizer` node + +#### Input + +| Name | Type | Description | +| :--------------------- | :---------------------------------------------- | :--------------- | +| `~/input/lanelet2_map` | `autoware_map_msgs::msg::HADMapBin` | Data of lanelet2 | +| `~/input/pointcloud` | `sensor_msgs::msg::PointCloud2` | PointCloud | +| `~/input/ekf_pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose | + +#### Output + +| Name | Type | Description | +| :------------------------------ | :---------------------------------------------- | :----------------------------------------------------------------- | +| `~/output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose | +| `~/debug/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] Estimated pose | +| `~/debug/marker_detected` | `geometry_msgs::msg::PoseArray` | [debug topic] Detected marker poses | +| `~/debug/marker_mapped` | `visualization_msgs::msg::MarkerArray` | [debug topic] Loaded landmarks to visualize in Rviz as thin boards | +| `~/debug/marker_pointcloud` | `sensor_msgs::msg::PointCloud2` | [debug topic] PointCloud of the detected marker | +| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostics outputs | + +## Parameters + +{{ json_to_markdown("localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/schema/lidar_marker_localizer.schema.json") }} + +## How to launch + +When launching Autoware, set `lidar-marker` for `pose_source`. + +```bash +ros2 launch autoware_launch ... \ + pose_source:=lidar-marker \ + ... +``` + +## Design + +### Flowchart + +```plantuml +@startuml + +group main process + start + if (Receive a map?) then (yes) + else (no) + stop + endif + + :Interpolate based on the received ego-vehicle's positions to align with sensor time; + + if (Could interpolate?) then (yes) + else (no) + stop + endif + + :Detect markers (see "Detection Algorithm"); + + :Calculate the distance from the ego-vehicle's positions to the nearest marker's position on the lanelet2 map; + + if (Find markers?) then (yes) + else (no) + if (the distance is nearby?) then (yes) + stop + note : Error. It should have been able to detect marker + else (no) + stop + note : Not Error. There are no markers around the ego-vehicle + endif + endif + + :Calculate the correction amount from the ego-vehicle's position; + + if (Is the found marker's position close to the one on the lanelet2 map?) then (yes) + else (no) + stop + note : Detected something that isn't a marker + endif + + :Publish result; + + stop +end group + +@enduml + +``` + +## Detection Algorithm + +![detection_algorithm](./doc_image/detection_algorithm.png) + +1. Split the LiDAR point cloud into rings along the x-axis of the base_link coordinate system at intervals of the `resolution` size. +2. Find the portion of intensity that matches the `intensity_pattern`. +3. Perform steps 1 and 2 for each ring, accumulate the matching indices, and detect portions where the count exceeds the `vote_threshold_for_detect_marker` as markers. + +## Sample Dataset + +- [Sample rosbag and map](https://drive.google.com/file/d/1FuGKbkWrvL_iKmtb45PO9SZl1vAaJFVG/view?usp=sharing) + +This dataset was acquired in National Institute for Land and Infrastructure Management, Full-scale tunnel experiment facility. +The reflectors were installed by [Taisei Corporation](https://www.taisei.co.jp/english/). + +## Collaborators + +- [TIER IV](https://tier4.jp/en/) +- [Taisei Corporation](https://www.taisei.co.jp/english/) +- [Yuri Shimizu](https://github.com/YuriShimizu824) diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/config/lidar_marker_localizer.param.yaml b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/config/lidar_marker_localizer.param.yaml new file mode 100644 index 0000000000000..1c04d51e5df0c --- /dev/null +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/config/lidar_marker_localizer.param.yaml @@ -0,0 +1,42 @@ +/**: + ros__parameters: + + # marker name + marker_name: "reflector" + + # for marker detection algorithm + resolution: 0.05 + # A sequence of high/low intensity to perform pattern matching. + # 1: high intensity (positive match), 0: not consider, -1: low intensity (negative match) + intensity_pattern: [-1, -1, 0, 1, 1, 1, 1, 1, 0, -1, -1] + match_intensity_difference_threshold: 20 + positive_match_num_threshold: 3 + negative_match_num_threshold: 3 + vote_threshold_for_detect_marker: 20 + marker_height_from_ground: 1.075 + + # for interpolate algorithm + self_pose_timeout_sec: 1.0 + self_pose_distance_tolerance_m: 1.0 + + # for validation + limit_distance_from_self_pose_to_nearest_marker: 2.0 + limit_distance_from_self_pose_to_marker: 2.0 + + # base_covariance + # [TBD] This value is dynamically scaled according to the distance at which markers are detected. + base_covariance: [0.04, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.04, 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.00007569, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.00007569, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.00030625] + + # for visualize the detected marker pointcloud + marker_width: 0.8 + + # for save log + enable_save_log: false + save_file_directory_path: detected_reflector_intensity + save_file_name: detected_reflector_intensity + save_frame_id: velodyne_top diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/doc_image/detection_algorithm.png b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/doc_image/detection_algorithm.png new file mode 100644 index 0000000000000000000000000000000000000000..905b432053078ac8a6de1209fa7a53988876b68a GIT binary patch literal 77739 zcmdSBby$;c_%}SoMB=Mb5+Z^kOuD2IkZ#!Mk_HD#_do>{1O%i@8a7hcXiyMokQf~z z9b@$9z4!3>5BQ!f zCBGHmU;Gd%B)D77xX zB$T-IsL)hhLN6$IO}H>Vr7*`~keSy(KP9+G@G}f0=L{!ze{sFAN-ke#lFI1usw1aP z5w#^wo$srw)$sEq7a2N-Y_Ytr&w@TIYd}TC_hWI)=(_|Jr9M%XkGuX#+8cLI+@#t) z?_?&!G_ zZUi&`d;6?-10XT+36hZgbN%1z!tKk_|K1)%|DQA@FQ0-Etl39no!KP+geGyViMe#p zEsQlbcMCUyJj{4VOg*&u+q(I$G9FI7}vt}e})&h zMjhG4lTc@?0GTZh{@Q0Mm|sUr94?ADTu_766IZ@MRgv7k%+SJ@F|7FBYY$0HDUj80 zEzYMptCZk{dswvSv*M<>sv5X*_`!QBQoN(q3r=p2ZntLD9Q5lZ-QeJxg7+gh{Y7(M zyZ8&YWDnZxOyOePvfG)+!E|KE=8h5wWes~E9GLB9b_>8B7LOxP>$QHj^t80T+VEV< zwU?=9_Uty@#Q0|FLtH#Foh^Vg?He_3ilXFQXMr7l z*YIz;0Rr*nh=zi(WI5*NqYT4ie7*>(BkNQ|hJ()Q2Z83JDrL~x^g$VeEE%o22)F*Kac++^wPSG)ohTIR!l?@AD7JtNwu+MJkpL74kT;1}wDqLp1 z#~J3^pnd*RZsiH`bM+KgK{l!Rie5SUQ$juY4)d-P&Q7t)%H~(Fcz7&VXU$kHj7Jds z%+vQKaBj^k0ORzS%-(07Fv@39mg{SG3PUW3he~K}(u?i1Z@8y!<+2y(bh9o1?!(FMQ87dT05XAv*}^v3=J}w4LcNWOycl?(s&k&KXVrlWMoZon&8#7#jyC!48|#BVw<#jz(DS|D<67 zBz`#wQJl*(CE?<6!PCli6xH8@GA6Xtb9zWe@gxL(jtcG9q|> zX&Z3j-icbNE61L!Hu~byNhVH?p+~9lvg~UnVm6W$~9U?iw1iWpfsD@fHcY;-%PdGl*LAFrZNR3#lN>H zl!dMj7vpBNaG7_(muTzs9KB*ag&56$=*Q;tq}QwP7yahKKMWhTQi?I#|Mg|w6eYL$ zO(#{HIE~bW@;dr^f4T673A_q^=(;jL?Re^)22(h(QA!|@UO!P^0Hn?znRy->P6*QjUa;_c+vPwB;71+2#; zZAU`S{O%h(($D&f#An=y^2K%33)SJmx~n#Rs84u#rk$l4bKu@>r%28F7hD!;ufetj ziLq`+^pTcH5O;2oa$x`@k*7&vB&%{SC z2wUw_*OeR+$B2a*x2L%Yl#|q(6dc3zS`OnK(F(S=1+B?!X9h~v+8ehm8%VHXTh8`h zX724Y4;5Re>13%2X&$~xng0*C9{*0T@~tkW+4)O92L}ywx8HI2(D0enc0}RZuftlROJ6lTaG_L^jhaFkk82a^AZxr-n-t&_r4PovJvj*jR{?T~lrq-N@S zE{jm1=H~dh2H>2G{;6l0HA^Hu|7=2Q{v0dIjhtR_^C}PH&zvl=!)TK}ZD(4*OySGjiu`guEgf|LT$6md|HmMu8@vTf06hPvn4wB@ zdS1Y-RNvcEV%Jo$v0){uDp1h9Fu=zC&ub^Q%BH(x-+IOC?@uZZKzsnixhCg(3G~Js zo*vj(5XLD@{4OcD5U{>~?(bQ4W5ghZ~Yk#|FQ7dN{R`PUY_759W$O=Uu)5>jrvE&PHwPE->*=Uk&~0F zd$5B*bh=BaF5p{1|Iu6q1q~D%CrS!+e!RORUE^Lqj}wnIcN*b+aWkm?EilFRy#yyI z86@VdF*>*TDDc2=#-D_!XcaFh4J{Mu#wnj%`u-5`RKD*)p)}@NY)6l097AS`bhmLu z>{p>nv}r*J2{a)-^U-7?&Qrf<&kw4LHm~JZ-~w=db@&;-f;xtzU+7ok*&(-RH9SP} zHEba>z>$zNqns+GUrBg49$)zY;?*FMdbOnZX}~5UGR}0;l<(Ok^ML($*{t8DS+=jJ^-?^Q%W7 zkwRXZQ$?E;`B{|Gr>_z97U}V8XA6Yso~ungM)e7nHUGCS=mpuidosLkn=T=$WB7VS z%isAsrBwNEQ4q*BIOF~*YV@KQd;TeYiI zUk3%jV&k_vF`IGJKDOsL!?W+OZ<8cJ5=7G~$dFy*)B8Yn&0m^t)iT_B*4MfwJYn++ zL#8C4@6fVFn#?j`Ol5zH3y()uq&227z725u9EeO`{?O;;Hy^jt?%eDqDREzcR}QKX z;m60JVb^DkDq2PWxl^fD`ha`X1U(sO5g(3fs$;dHaCuJGXqAZ$_?u;I&l8$U`>~pR^7jL8jvjazT|ztZe3l~)^5L{Bpt{$ zWud{lsHZPZANsiQ*zt6SGVs4-(I^_b5WXiQ3irK56DUW|We#G7hsI`j*wl9clxvD&}M52a{sig$2@%agB zbgn7$Qw~1n!^IRYN)RuYXnuUQ(+QiJ&<;s-x@uV&Y_4sd*@eeNmocW@U~wricph{E z6!%DU4_*9JiYgJ{y?>p8_{Q%Y_O7qd-_K59eCQ9HlId8}wEy)>7W7Zn))`8Rn;>S+ zzMLV(U}&y-uh#Hrfsd)_(u8q6lr&6zAGq9=w}GjH_<*HqPOSZaBDTXz&HE>8ZEE;H zYAE0oOU4^SVUs1);Jv(%THQ;a@^E6D;}eI+`ICBmN;IcWY&zpFe`G^in>?t;z8YGr zrEPA0>SFXz(0T%37x|0f>uvd=|7n~xmFK+sPWhcFuk@|4`&k`88h2RhHC=eLYsed@ z)YM8TQrF8gef5L{^x$8&m2OEtw$22WmRgO%lF%^Vzy=YHpoM~vOn7M?{7`N}7~FhP zyELr|UA+Y2r6#iAchk69-|~3J1{2I%s79#(Nf2-l9-|m-z8h+~LmXNR8PEq6YlnZ{ zULDGJ0R@eJByOiisq^lu`lYKqui7_{u3a*ytje>=6?>)~>9_WqMHjYc^wi!=KhfIt z2^Tn$1eDJ}WG7zMX`>2RfJaqQBf6uGKmvG6np@$Z1EaBlm;*n@M_;P_W+VYN{nt3h zhT^VpcVFBYKz_qp;YuSXH+tAQeKwX%Yk2*qRkqTR*`pifGYeaj)`f6@nziDWKt3VF zwaIM9^8x(*8OmqB{PrPqYIn>kHg{4rK`FLr-*>-y!pInZX{@BMOp~xRsA@P_awV|# z!14euSo;Am29G}c8{)s0{NSx(wLj9!UDUm`t~Q6qcq8MRr@S1oNC>TIvy^{heQBRN zwQ1==uL>;sDj%RV7>M>kP;acIGUCV8TWZyb^aqCfjU!O%#-n^^9z%W;zQauEv!a(s z{Yf-skICW*-mWAl065Rd%fjH`Srj<~o$oEs_h-bD1dSEfmugIVcsT4Hj^EQ^dnYE) zG&!CO;X5oDb*gK`q0kg~b|Lwn)7k)o=5cWumI+lNe@z&Q+Idl%xn5!ffnM7YZxtwx z?Vg6gO%77c%)SZXbi{U3Gt78mAE3qx&vJjNBUtEt&+?zweeqyUi2x(FrXyQ#T$TL&pG@!~45j1a32elj zHjPfCps{6{fsSG~+^nB=Sv=Bn+ZfNm`R}Sv@DcWhG}Tf2jnPs%+6uGR%zYhbK=ZGGEF3Sq3N^PPIopzyzF>21=&`L`;ei)(fc2zFcr`5#I*+OIJiGxAduxGf zMc$KoivSy2T~Ms&AzTz5_^d`Huv%cNvZe1}V{`J=8>co>nl>OlO8L)U;rE1){eqT_ z%sM(fx?Qd=U)p{q&ar!>KAiYbMFFu5FwY(0DT35$8?8T{&wsyUJgLg$D!$wm@6hNS zHhw;B_D%jBPa{=x8r_VO&AAlk4$$Q>k-I@^w>-nblCg$2F|n|`YyP&a9RQU(l>VJU zFp;Z3KFbHOk6K$JvAm}GzpsKo9X5SLv+d#u3b^p-fcO7>l=Hiq$4>6>kN~FfqKldU zFgWks{XM1i5oKevnuq4a*F$O-#-N4lw{K;|J^l<~6o9Z*b{hFqh4L)PbG|U=^6vxN z?2-2*TRi*o?y&riT$lnnq;*7A9vI=1Qb7MNzo#It+r&23(3xiwq6BkIO$muFe*;tk ziPoNiv)8d-iN1NfnU4*^m&AhR;TB0yvg7Vj%v_D_0_kZ_s5`r+Z9jXBXNmjY_q7MW zsQKYc5T9gRrA7wZ7RhV<3j6E8{HY?U*C{8&mouiReV|CnIO=ym%Q!5c{%>Ui>L0nw z58itm9zy{b-_*cX8TR*nB5|=U*1EJf-udC84H$H0rPMgFkr%Mf8hn5dd>s33>o47c z9(aurG~qtoglIe|=o2yY(4&y-wgH=YMUV>IBH6{GcFA2eDW9_d7k&kpZPw3Mp{Zq# zV?!%+;UxMSzn`KFa1n_tQ*2!M_&mONFZGjVMM)1&k_3y=wREQuHH{9y_utHr3)?Wh zDl)2c9lX2A7tI&Mp#Ui7a_)N>7!Gv{6f~~!hutIr1=WL(nhzJ0SJ%tt zpQom`+F}bgGy`Q0H!4RaE(LKA^?H(7SJ&SkmTOy#^(128*2wA2J(cGl9iM|hzvdS@ z4<#Clgk8`yhc^5tO8T&R9`ecrri_fS3V?pvao z0pv9x(fJy%!&!2`SjjDBfTsStUREi+#m_MKNUpZW2j8QjL2?96~k;>n*cq^b$aSX&TYUubq(_t#H$LpBRkUT3!UDDG!&Dgfu)47d6Xxt zLAsOlIe2&NGDzVC(JbxtWX8e9 z7elUR20Gcg$q|_#&?~+&q$(i$3t;PkYaF}ew$}o68u+JsWEpI1w&%VNS)Ut&z8?}< zkN4^|Rh2=`V>SIZQ3XJ~Hi~(a{HlNCb<|f% zkS!Zg7VUg5*>RobUm!~6dt8T|_02ac< z(a_AX1Z}GXK8Zb@v_8NsTkCTg9~(D#aH+If@HJh4p2ZJ{DItgA zG|=)(ySA^>n769NMuKHXrw&{A0UT3X zkqgVCD3-lqm)oR#YQ&mMq%W;<3+hvwcSLFUWeZazsq7p~NT|X%MF7?r zVEs%^XhM&ZW9}U)f1^4a_b~x<`9G714zTrpC;Ap$Hv@wa@kXbe1**rp=V08ijJ%AbOG*MBH3Tv=;#;l|7D)^(0N(a6vtnEvEis1 zd%l?yEeI&6Dwr)vbIVXc*koO!b>|j-!KTOG7CD^j{laXie0;MYLkHj(CewN_7#>Ya zT@6@rZ;48&$LNArefSY)t|fm(sh3P@vvDqy324|>UZUFd53^1%rlvG9ax0{MD(=ry zU?ytS;PsT=)Jnuk4+RhE#KRF^KJLUI67{sW(&7@e51>FUV!cTy9g-X3oY&)?%e&6- zgZYss#qZ_Z?#R8v#@@THqC+S#&YvI}9dNu0fjCz}lISlK_E1k6Sbr~ZcR37sT1-JWjf$iWOpXuZ?eDTmb{Z>=N5^7{I)nPUc+Eqn zE`xaYFLZZ)@^FaI#um}Y11wFUo6q71-zzb+D4-1&tZr_$U*kkTAi|HNyHD5= z@+KyHlVc>mnpJl?^`mILUkz>@PMfhdWAGO~*Ypzyi8ON2=#x5}1}rroXyr()p@LMX zr@!y0RnH>Y<+Y;Z)Hrn)#wz^8>icI)I{Kc|@#JYEf`5ds+sptrIhyM!eY(VqLFs&>=9>>yE(Remgb(mxlc-f+EkR@IS`%@NL;+U$aR< zsoduB-2s_t*SCq~;VHf)7NO6a;NI1;hWw3n2(coJHk& zGe`HNPo2uRO({6|qqg$)Vag#IKKG{)zeX7#_5N?48&#QJpV~$IGvf{)u_?+UNJ|NhYI3BCNlhEAI*~NJk^6ze= z-*qF`G8w{uvEg_ADvt$IiA{aYmlj=^Y*NM8#C^Nt7c?za6 z@8!u_;Z3di`PKb*zhv~z8ZATmytwCl^Mk zp{k6;&9{mKcax*;@n<&@Go2?DK`kVqvdkidYYcK&CUJ=9XqSfD6XZxKyaZi4pl69{ z9?H!Za29pEd73}y4fnc4T>JNhBy{ksH#Jw;z(}TkRNy-$nO+xR#5Sz?A&DibQ7bPc z$y_mT^`7Ditxoqz;EPA1BQVU22iL^jOPPTvTa^2#Q@aks35W6oa%KE6M!G^BCaJSf zsUQ?+l9RyRLy7X}G0&?rcaFFFG)X6kb@b2HIO$WMmaJ4257aN|gWq4jKlEEhYneNx z=+Oo1f;xDHJO?NyYd9gzgqpYTDMWZ}?91CILLJaCij`4pGJ8oNlr79(bGQR9AK5^m ztRTb1e2qP?zVA93ab&)}$f>rsKkxa1-A0p@(FQW%SI)x6la(do6!F%ILQWV6%5%PM z;em&l2UoJZ_$B1;{qhqsYcAjl&#a~W;gQDG$HUj` z20BFCet|n^fGfyn39$C7yYsvKwm0~3H}I(cJr~pR`;2wHkL}{G#lHS&`#^R#fY&*c zT8u&K&wKM=f#)HtSK5i>4oZeiTnn57(iwU{jvrU5lS2*;sJk!umDS~N;Y)7t*Mp{k z?26HFq2Y54q%5CuA16j~Ew*b-?@X(Ko7cO>@Laj;Y8nG_LG=r>0>NK-uKqUNw0Vho z_ua($cVdZP35FX*SP_pMZf@u{rGDqSaoIkQ=*R!C;pE~T`G$ZwN(rb)_~bwDZZiJ* z_HFj3c;_6)5^kO6&x6A`y})dp&t{94IaMJI-s4+RP#+b@X{);9A)8hFlvRIG%~zSL zl-LMqjf&3)5SqdV+Y{pfd^yYjgoJ0*9^z1;(%oo8*zzsG`=DL{(h3Y@rIg2p)8gvZ zMw>EAMg!W~>UhRCTh|x4cp%%2c!x9T%{YQ%sdy-=pH}+(Ewk_w-d}Fh2Wo#k@xw)2 zqu6V*j?+zM_-SK=O*}wuVwa%~}?6+%J%sGVW9qzqYP^ack1;J8D~PMdt(x zIqxBsL|~)}WSy(A%fY0Or(T`fPhRv_JK!~;op@IZSxEdZq5}BiE5x`5mUtlA#kt^D zWrT@7*3(}4oNakD3;TWC1tdWOV|D%-D5*mrol_%Km^XU_{;7YvK(>Gr?xANMup2K%7}aKS5J$|@ z`eIJn%phE`*6%UL5n|YSi5Vtw1>4w$)zPSBFOHqxW=w>9bMvd>9#gYLslK0fvn%>=+#9SCl#3IArp*}I9MUfOore&K zLRMGnC!V`&EOYN6q}}Dk0~fx$)V_M2rxD3CPqvheoj7pMIlZmy2H-K~9_@$quO1dC zx-SgPi_vakYQ63txy)hu^FxuOYO+hTfnKTf(QVAsqpW_Idm0kbnfLXVGyV24%+-j! z2nFnMkH3x`IZ{{+J zx@M@yx=z=-GzbDdRWefD%CGix=8fHLRO3MLR~BUoACcq7RrKW!(^#>T8>@`n`fbbN zYGRf8^-`)=oQ=~bw#oEVA3zq3qwLhkfHI>Y= z^L{_REJmZZEBv8@n7E-64II$63TLXQITeHnWLZ}gPH zp|h~!9}Y2xr!(@4$+|OkjziFW^3w_~DXOa9d3Bkik@RmxnTzy23@Z;jRAa2LYo8#U zB3b%Lx)2^g>nk!hL+<;4(*}1P|NMhq7!T_c4GL`>(c2Lp+c=R($P? z)0?7exG_g1EbPbJR#9NLJ(xjU??|nYbJkJo`-d)qVSTpdXcxMptX-OML`-BP(#T;s zu+9upc7n0>VXI>)nM7Qxw9I51>i4KNKi`zU#jvhd`K0}9QRl2|9fhl$77gQNQ26$9 z@H3wyD$m$>vUzZ1oB2N=XAcD1A7PKNk5pedbvycn6lmI^Y|xEBPzeZRs7?~IYPvA$ z{e6eI<;((DJ;baONYo@bx>zcP0YPfb$q`e1yE+#jLh$X__P`(!T?OPJ^e51_Caayw zbFEuoK;~zV=&mo%0U@FMXaEZ4%dsvh>9O*;2<^V|xynZM7_DzE#4KE5oXB)cPM++v zzM$-Pg0|^i!1uoTG}17i60b9BrqzAmagDg^AsXHi}u|P8ld= z9dZ^YDx1cPK|0&S(f8An1`IzXbYBmp9j8-2v_Ng1hfE{k;6 zjSN_D?=-OMQ7R(+9AjKZo0&3pO!s}5>A9}C6^+{`Lp@RG+v+M=q>0W~#qewW98}2O8?n6tvdedFxjtJcDx{NC zN;BucxrR+>7&GZ^03fS~_=3lo>s3|(<*s{HrnSl#RWB#6sI0}!MdZR$GR~J*Q21r? zordYcwMAj5$z?BI9N{#xDo$L0Jxw{8{UdY&ytdx$=u)2h)7|fuxY##zvZSGAC}WXj z3y=s=!*dBJ(V3Kgcc1;A(4@m;DQwI_Ot5yjqJ6M*76`*%#01cceg`Y{gcCTWd8XT9 zI5A@11})3D@b^2%tD6Ili;9}2feeu0^D*VI-i~(6+;$u_Z2pAgay);P=&w1&!uvsis>#YGUaoY} z40hZJ#U4UX5;Uz+$~s<1`V0rAcJlNciAH^{=CVZgeaP)wU`7pzmqggwy;ouj+~9bs z$k{UeLHWzgEjsai%;+Y+?q{;0TqsVhdZ|~>V6_bsvMqbj?Et)NS_9f#OM9w6= zI>APmMUpk=tJ20tz3JAerJby3U)X&W+_@}9uPi!Jaja#KWwS`J>grju>TO-%De0=$ zA3&qMwhsz?Z-SIeeUMd)kbti%*{sn~tCc$M%1VR7lx(yh+ zMv~L|@2rhE!Lo*zfrqNkpC9j|oYe8y3Qeg20t1;!Z*S{l(^O^q@J&yEm?PGVL&OB> zVuob?F}f=6(fXAWAaSXdS)7QQ&BTZdWla!Cj|HN1FqW|RCLrX>f^-=be#MI!0pc>> z-aZzXGa2-1I(m$h1aA~)lkD6KW&&A=rFaBq-oAg+L50@t`n9aq-W|n9oh57Wg* zGU~Hx@B5IRk^aiwZWGU-^J@|r&8e@#IcOee4H}f4o-ho%k9(k<1gPLTWqfxsFO`b(r-y3mbu8B2?Z;jntVsx71%BZBZtrqfvZ_@$iIWy%YJUtFmkmE()aE zywug4K&-E2CgOZFYL8R=79*I!U9em?ru$O<28LHy6H!jslh5GV8w!qukz&5tnWf%?eHeCP!)K>>uXn>gptjgloS&ZcXpAq6 zIp|GS&X^VBwFyj$!GCfZnZlA^0%=^_1(Bk)I#~b2G%vBv=zHM`+5De^Ql!c3+Zk#} z8pYof&*U}sS38~IWWzJY@?b9dtA{+Wh?S?D2}2yK`MLJZn}VF86D~hV3R_-MPx7&i z)3muhKH-nDqN}cL+vY?i4KqhF<5zf+pT8ia)EKM?ZjaIBgnG0^#niGy23PprhPc0s zW7ZIFbBk+tw13@WB}7##8f7=J8spNfafIIZG`n1Et~hBlyyuL3D?%PZ;i79g&YA8v z9TnmeG*!6q{nZ{e8lEO3?+v78^TTf1Q3RZqwJj}|*ECuy8~H)QxCtd_VeE0WS=lPO zxQ8;?p*&Zf7VQE65k>&{`7N|E6$Z#wHYeZ9;DBxk4KS9NdnW{LB&{}C^NW+qX*i9l z%#{aVlhY4~^zPL>iHS(hGBQkV#E#d#bvC>Q-nq;RgI*2%F^kpd#vIy=&hH~sQstiV z35q=K)Xw^Kce!d;L(a+SlF%1F6z&_sFT8Za)ZN{IL*Tei81InsQ&X zv_!SH%BE7bddfL$7^VbZ`$*`m-jK@8W&BM2B%3(pWf?s8Ciyp?nv9{p)c3D!0NS(+ zp^(|mLj$d8SE@H3d~HoVLjXuLU^?Tn;i|2|~uK7|k z!D`-A>8Agfx|GmbspcF;B}f+X?)i(wPNpMMtloom1WMo|@I!_y2b}=bv94CAYk!4Z znq>CDCLevc0u`5gq+Xz{kscBjErQrDc`d;voZID~TjBV8N%k%@^u5H%tAn%v=tOt1 zQu*KrxNPi1xiJ!RhhY99L?a=7buNqt~&NgZO%0~s5-{`Mc|681aIwj$<$`v1?PM!Cx zKKT#kvVJ60>7N(!LOtZP(L#P=v~hK4-}2nuYT82zFt~j%FR8ypI7FHig}vgC$-!FXGO&iD01{ty}efzJ6sB3PoE(XHrLZ z<{YbXYz{>`Hst=`)pzrXCH{vlNa2iX!sB zUpY2wCiQGP!Yu+P(E!g^q9{-fB*GMSujB<9TG|;CT}s|=b>BaB@~u1Y!P0~N%~NW@ zm;`b&Np`|b__4lX{RD-G1O7nF2fi|LjD?YsD{;C5F^~Ev>%M5aL(HiqZ{Rp2=eV6+ zD^DMqGZDEx$zWbZ)_=)9N_F2wxNG?fRRrQ}5$3pAF_@ta6)xQF^u9tbIr4Q4i|t~( zN55*XH+_IU{A{m}qOVR(?f?C>{q(0B%>{Vck0m00a`@2tp&HOW#as}a#NU?srdFL? zQ@HRfKUcPTEl{ZG_h_R{0h&9SvXHz%J7<|9Y!J^B!MO!Z&rp3|-RBWG5u?qr8qmQ| z))5ghe7e(A705xO{ilQ;=F5r*$8fC66oeWAQ4oTmjfDIsG;%Ik0cEbf8ge^^Q0;*@ zF)f=?Z8uW&y{05Nl8SZo{`MS}sCb_@r{1(4{!KX&zEkLH6=emK37AF8Df~S`_=*1q z@6G?n>F%9MFCA6A?we~g!%_uIT8V~;M9+s(V9qPdxir;^zdYXA7A_%p zGnUMn=P|k5*P726mjmUrr+DPD2acavW3MdFxfLy~atZ~GAU@pI>OQ%I^&gTQL%Aa< zPU~JE&B!`x#!EwKQx;FE0;j0M;#!N=;IM(x*YWh4iqzgtaS2QZg@2z%t1yN768trO zM8tUi7HZZJH%^84CAvNO-=o4~RWX1=oDMG?*c8ydIvb#Yq!8(79OeoeK)&{}_TFFW zg8ORvF*Dh%h>sSShT%E)R)kUJT${RJwi(lX8?xZ4G*`Ibdnzf!<)YK;a;Ni{%(P;37x`Go^KjVvA!21U z0I@CDSzD;rwx*B66q1D*qj1%Q=eNox#Q1So9qjpICi0OYlz8rlxi{uGTQy^`ThBK- z_eUV{sx~Ei_a%%XS0fPvwl9FJ9Uw!Dy82lfLtlzZc;jBDL+4V{Z{OCWG){G=;uitK9p&o9k7a=@O({AhV_6t z+O>-^1>`{EBN+LeL#dvPh^geW8A}xaXj+SP*p@?Fgx1jJ*vxe5sWsKelXQSdtFk_(C9`eSi6Wdsm4V{j<`> z{*#3a@QZp9M?$VTEc1xE!Lefwp|vkvy)s36T`%3qthF|N;zT*ZQ{rz_*|GS4+AFVQ zfC&3O4YcqaABbw+jB_ewJ)ih=kG{u0vbSWo)GCl^Oot;;PUk6N>nP~5oh)hl)Xo{= z#rcRYWXd?rJAi@Tc5TZ{F~l=)q9_4g3T?~w_ORJ=G?+`J0P;#%c#SmO?l>#?g$M-Z z&enRPIkZz?_BOns|8Nnv)eRS;gHP9R>YZugigqLLdXlXX4=*w+m6|_(1CO59Wr4`s zW%TPb+4W|*u0tn^J-hXPx!R-t}lXS+wOao@+0Ig0nx63kG7`6FVOyjybn zKVHpyBCO%R7+w3kevu3vLQ#^sh?G`s2WOO#Z*-xs-C}8$y_Zs!%Q41<&h=S1q+yJ; zUuXIKJGg~J;BM+{VRQ3r!VB@w%-6168y*?SM<6(16IBj~`gaa1D+4)>E-v~W9;l_h z-Z1K1i)sKlttBKRgeWRXUMmFx<+p9~-`=FC-7NdOel9+{Q|eZ?;A6q~$S{PWHbK8j zC z?h>uVLFM)PM3x45nFdm;O`SeF8WJsxk)ZWl>G)` zU_fH)DUegPxc0{0Fj+3E+wz4A8o6y3xnr)L`H9yTt-NoXLKUv;s#kcCt*t@`U-jHIi%jr-3-@6jw#1eo+e-jwKp@2`Mv*!~LU;P2J1C!= z27B=<0G;{&zU(-r2ft00tHHKWnDp8JW%S*Qri?%7+2YQ%33nE0c6hhopSC+g7fQ-~<%?tP_7`LMZD1 z;0nV@vN#wJVrxsV`jU(D^jz~RDRG!|<}oWKMnoLTyk{D!%C`WN;_-SxX38c2Q~0iT zF=qL5z;9;*yo{g2XSV<_DI%M}fAx{7r2(QRT+CWkO2JbSIy;Nz;(F2uAQ-Nh`wyU( zsQ{5Fcmbz)41l72o~^|FwVW-I3sXb2{ik;_c4S}#10!8`0pLgVJ|$11AmbCtgfI55 ze_S@5tQpWD5YH9B6#|%l1z>If%T=^;yg#=NgaLkE;L}7#JdjPhy(0NIKflg9h~vlh zV$x=_&2Qw0sAn|b@3gr8Nqhwsk^zMG5dfnGurBnsqzNVfbSJC|1{46~lQjQ`jZMqS zG@bhx4_X6uZRj>YB#wKV(b6d#$~jxy^C)Rm8h)&MwyX&k^Plee%NE=C+@aL7N6HzV zd80_z&?lXUQ{w`_vXh(yO|D?J^VUj z=e}~@BMn&JPjT+=3wfNs``Su;xizB1ES*Nd=%!MExHwW9025VJ<-Hi}(6RzQ7Xu0f zqKF$%<3v)s5VzNI6a4%|Y2esx|1rL&u^5X2W~8P0+HP|3fxzSv-eszMSncWh%KyRw zB&yY#;cd;Efu7$GSIb~iN<^$2)dg|U09Y>z!0lxS&l=!$HH!dPvz}j^j%VV z(0iAT`hgJBeDj$|x|qBD;l9!Es7MKk5V$!Lh_Y6d=dhY_CD|9$mqyL~0u~4&?p-O4 zjH)NY1i(pWplX7qd#(in)FuLU4;O!y%gJ8pxk7k@YvfE4A4QKk@jtD~X+b?1J$*|P z+RwAGLWIK`&GrOZ0KE5#2@rO!VHXVsPdI@4J*NQ9f{1ekbs+L9POi+T_yN8f*)mCf1pxQ~fMb@{ zHQW2^384&=jbD|PyufhPjWnQeEU5FM!h-CJ3AMnz;YZ86-`q56fdF*A#)PhBDAu6o zZJd*ZZ|X+YHk3q6{1Gxsmw*Op2-2H%K|v10f~%l=oNf>`@P?G(8Bja&3P$;TV;k2V zHLf3%XchRwWQgKZy(9zpS`iHpm$|?WZ{D#JM8Kc9DG>T28CoGez%mn%E3q3X?EQ`? z3ifhP@wXhYXDF_E&NKeWios@LG<>#M8f#Dn8~X}CuZ-4ML7)rZvR7}og_hIhOC0~v z%|NRx->n|-YrZT=`)vlmqPg@({hF=N15&+`$D29?fURG5VB8jAX7w`&;zB|+S;W~Kbc*RmhNT`YY+q){-@7g4E^hGD0#gDiB2pvN6mO-A!dsOUDJ*O5Yw+_GMc*W9x=SFA$BV6*t2&WDZQb6F>w_sm+v@;+69DRdzaXYs zqqP!#e{_2tsK&canIdio@Sk08N=$f@_2keZ8}RQ#l+DYG3$9ZzDrL6!cM%XhYf@(f z;J+8c(!!$rRgY7ng&zk^#q9B*~(SX|>(;Bs55=!}Id3pK#dbO#fF6(cU7X2ALX#DXy ztNufp+cEDix($<;v5i%-@9r+-aY_?fc0Hay0ET>XbVR$f9>Jp*?_hrJy4D}rKg1v+ zVRrlWz3lgyA3o$hX17})?C$Dn9=0v--{Y2Q5TPz>%s&bSX48r9y+ z%ElHp%Sd%Q`C^f^Q=c+s*)h_tVfq7Yld4{QU*`soNX$B=Zr_es1qubZPc@<`aX_FY zM831tJ#d+@qDUKhqctQo@mO3|-uBX?Q+auA_8jES`5g10aigcZ4tKf_{;%6%8}e+g z9sS`$M~}!U{z$LqW9r8v(lEja)}??p`a_3c7O?7$#h08*e>%Uohmu49Ef-m&39^H{ zcXw~u`Uc+#o0nzS_N=0>_+b65MFc3FQ|seN>Wlo1LFT{< zurnnLh=;Lv{%yi*=HA&mL8)Q3fwh{89`B~>dHkiP^QAg}ezv(?Tg)-X{?Ju)K!Tt6 ztCji-YSYKg#fUmhd7WJ15fC3GvA??MWI#0TF8D^ohg_wb(zh?37_|@VM}z7I(>?{U zfe&M8h%;V1`4Ok5F}t_IKjd0yph^Dw*BYRGk9LdZ|1^=erul#QVSuxdxHSII)kZw; zSs6j-Z$-ywlfnXeQpXV&om-bW{j|^AYdYz@IRfJ(z%}j>xH|Z$NI}kvP8{wpw=0%jJLmJU?hS7bA(FwG;i;-JVAFe^K|=L3K4vzbF;tW-MV$EPM!4!$%ZvE z-96ns-7~+QtwaJxTpXGL@TxI9!m(235J(CrX7G>qf95etT*c<-1u6yw*|Ew&Zw#^I zm7&{5D?WU$tj#IOTnUyK%T(=phYk(D#DfY4J;`t3Qn@!jx3c4F4^|`VSc%p~+up+n zX*F=KUjWRlo%|J0zgu5&GHf7@BS3|{$!eEHAVQ0A7y)pOTddAm?Y4itTO#RdVoR0J zdJ)x=cybcn$qh_fpdM+{jWr_&@6gK@<%@%LCYWwWEB_(Plj;ZebUtY+Fp)EU{z$41 z!VlfS-7@no`oH#5>yNtv@V)YHroA7A@VvJrHN5Z7fl1-or%BxiO-?zT)Qmro=!$WT zP@pN$klLKO9~n$#=5Yv?2hG8NaTm8MpZXn*|Dz}z`yIF@fE>%0e!ql~nYZ~M(HjE( zU^y!On>PBkm!3iRavrFpW@6CYu6HC3|5(0$ws?o0`?z79IqugT$$5Eo98PyPLd*vR z+CwYt>9uF-?6=Amg-*i(69K6I`p+oL)>afz-+WB;sPGfDq*hm(ZwfO|7-%I7Dy1+q z2hZjLvY6yw4j`eW7X|*KQwQq ztsXVsp*}ixqHGMMryuLHjb9Lu3G^uIGhuemUb*b6L6j5>n7IMn6Hom^MKm@rOTB`w z9DHb(OYfe9xufVhsl~qmsK@|CaUPL`L>{B8Q79llvB$PA4w~*POb3lIjg5_!Go-0o zGW4zquYA+CGfn`;qWS(1M`H=d4x&m#JbVv8CzZGINvI~PMQ z|Ild6H+`9%%CEXGC*Xj==h%);Ld6WYUh+Q-8|7WW5$9`l7`df2X5r&wsgr#O5sK=Q zgD6G{2=3NmGES+%xIRJLR#J6>fEP^s<3OW`*ChF!PqH7cg~c>pJYFOkb12nC z4>oEPq+v%Xq|_ga0#FYBI+0D5|HtII}va_cD)P? zgff`YQ^l(Y)DdcbG+-pa6F;Ae0~~_0z){PWX8{Ck8-Ri_r80?t4QBrMj5Qp!qg>(a zHq?R}UJ%r*4DGQvoi$D)mKc=oYy)r4xrlEDMgnviym~TNj`0Oo`WyTw_XIc@ zDOaL{qeEFh)w&8Fz+j{L;r0Oes-@oJ?-6FIVgqPx}EMF<_5^I>3X>lS;yLQD`a zEOF)1Qft|`FQ&j(H~(Xl>q+fMFTtx9cPoVXHNaG>=Ar;XOr)r9$a-F{z^1|>c({=2 z%-9P2X>=r?)OxK3wD1uix3a3rc5x(o?rzdM73e3QJ@ED^4)6IemMEk$pNSpO&$1ovx2vGR}f#pZC4e5UGh)?#!e5+6r}Rp2Qx9RhaX6I*uSNf{ zBf3p7;d+~QSdUpkSReNzucd|>0B))BdleKtG7WTrJrZ2}QXDBR$DrU~nm^_%HOPM$ z-sGNc77NTu7!t*An&qbnRn4)U>$K)-`_fG8bf`4`J-A2YNu}-T#Ep?z!G|Trb_-2! zUz`JO0BqoPefUTYz2DzjBz3<4KMsufQDxzJ1HAujVzD9LycK&ibIxQa`rL;wrH zEj>H;NE9DSi@~R`Iip4%5;*AS1W|o0DHcdT7#`npWT1Wbm`~!REchVi$!C8GGzq5R zz;E|gE)7f}v*F2H+|9`j?*Ci>gf4Yt;S@K~R&CaoR`cY9fYIbTZl)g1m=gvfHlNCD zSaEa}-UF7v@MHVOl{eAPL%+*;b4CTc@Pm6#1^S5#kXs&m_tPIcB(RM`tT zI56>J(@Km#uvr)`L@Jz-1?Ld|9hk(A2^cfJ0bZf85Ozp)uH$6ljD6_gtCp!8zD}EE*6ym`?xdNav$>@K3;wOk;s0 z<>`rX$OB^Kzk@!#1UyFnY4ZR0z%uiaipnnJ%lizApckVZEiEuGxiYCMm_TKITvpU} zQEqO~!^49`5bzciXTTqd;y=B@``jkmHC1Cxd3&+n=S0L2u|MAcntcgO60jI>^6>B^ z?7sj8WLR7)+JI?+jrosr5@qN$C@U z*l!MEhkz46))e*|28Ti`*QcAj?ib8-F(GyKn?@{<*4B4}DZK4-b@s0l%WsV*wnuYw zrOE6!`gIo?T~#Bg>FI-j&Zr7)Q+VwG&)8a&-Zz*g2<*yk;){h3=2J`8*zb&FOR82F zNl&Z*x)$=JYf;38UJK)@p7*UoLex0dDc8#DQ8AriuMvglPo|9%`Uy7r2Ubc?IpI?T z-BCvG%oF(8+zIzKsd9PkbK>hxPZOLwV^zae@9L0qWz*LRuIWmk9PT06r~dwTLAp9= z{PE`HDN87+LIe2r8#)7aWa$700DUT7*LmEiu@Q+4qTV#v-o5mI!0LZ>RrY!fQ2%%r znZkMVdR?uzw|9Zp@%PoYo=xv0#LclGUia!q*Sfld4Z}mCkQBl8L{8%bV{T4sD6hF~ zN+g)a^TuI9OB-xJ_s9Pk1(=sM#T^`2LG4zhRU=kcvzk}e#3-X5<3T#V^g2g+F&nP0 zE-7^$e+~3EpP;ISQQwJ!C?88wMYN$&0%0uXwjLMVF%Fb7orI4T&*IF<0Y1+*z1K`O zo#O5jzmUeU462bCQ*B;tY6e|bn3pWmH3*v9!c>wA^|uw4WNy{(mXmZ}P>c1Xf+A;6 zPAf=)E-#xmrDOt{Z`7PtT{ahmR@x(P>q?d&MP1ke7l$b~k&k`R%~vG$($kVQ=VXt< z9w*C>UC=v%jZLkXDxS77)0|>Z6!{O5wUG+T>mPPSpmd@JAdRvy)x#iR_0gDT7dvUeeVCaxytu1oh))CT7 zT=G;E{f*(Qv!*RtfhMatdULe+-xiFh*Mq4=D-Wr3z+Go)QXMYciz4ZT55ThV$o>PYNx!-OuRcS2F1!<+q_KJ!($^mZRm42|yQ-<8FGLSRN^ z32<*u49Am_`Bh_)El^QB$P-sJZlURZyl-J%n17$6-=Qz+odDH=dGvK^B=>3)C3;j( z&YPNkzZ<8NMCl|g5asqGF=t+s%d~bR^zp4vOEq1iETLR9D1=;1>sL%U#AC2qPCrve+>#~q85_4*->n`} z-3qVYvr*;$0^EcX@>7U9m=COvAZZ-0mot}uE`p7&W{!!!D%ELtBUPD8PF1p@86nZi zHod=(7DZ{czaFdrk*}37XYDL!u8{`tnaq5a1VE|p?_p;sm@=4Gvd@P@6lI+nXTU+Q z%*+{16{+>sTuZ{k5~J$p-{wHaG_|g}FYzOlGzF%~PRh3*P|TbI*&$qI%iDJKY0k2x z5<^~;sX=V3Sr2xL|LVW^N`Q993x^IdWgFlY50!n46sD1c&0W24ry2TGT25s0i{Wwj ziCsZC2j3s`u@76GO#lVQM>(!{J&AX$SbgT+pZfq|#E|h=zpsx#WdSU(4y$ zaQd;0ejkHh*75SL^;gJWh@Qk!wOf}OPdAd;+1aUFkYg^}pzhk)_w=%4WrHRrR{MH- zdtcJi$GW#aIVjDZm-KS)F<)l)^hw@6J zimslm5JlCfrrcuOjl@aYLZpu!3Nll1{!kgAcli^lq9!F`k!PvVz4~nUDs3xRY5APH zjJ3Ly7lQ6WAB)i6>AJ{N>0J5QQyrx}`EvXdJ6li+8Po0};;Id`p$vbYRmj1IeTYiu z@1I@+_ey=iJFPADox_)0&g9ZfZ>^Eg*T1L`vXJf!t*FQ8RuO)>h}4R{%<(;}LNBUY z2tB`fAF7Nm?;#0$A=0OEs)7sk(($%rUeNfp(0Dd3!BYmU)h#Q(ktU1u}{W3$?C;VZY!W=1?p|nw^ldlVmm=-R)lF@7=^C zH?J#l)isErowl`vMMp$A#A^BzuZ7?ckz77KODyI3X6vDW-dNdUO5; z54dJc(;)ZiLE5Drmr^~EZz9496Ma#&y2nVT)+P}+86occ- zW#`I}&`iAv;my`X)C~{9g9b^4rrC%-qb}rYliOUBQ02)bVq06|oUMF1GwU}WXuz|$ zjsBh z$hBdUeZ!M4W0}r&M;@ZK3O%%LvF9Nx z{Ae%dP=TvXK}64I;_883V$HP@`AnBPxs16|hl0Z1L-?eB4RT5suFI7BkS(E+FJoxJ z-CbaXhIqpdtYv}KSuv77){|DTNr~tMEL8S$Hl&3&)>%W92<7lf(NJzp-Ob3eAJ-&* zhra6;uDA8h#XcN1d6sKwh-#C8P(kZu^%GLWxO+K#gidHqVczD>g+_oRVlK&WZzO*9=H`P#qg-FT`b3%O;*?f@ zPnp+`sPat5vBnKrEW}eB`jXkXT5G1ZL~Gv%ET|>ALXn+(g*}|Tt&GuQcg%^aAmf?Q zvrco+@7hsW9=)#WHj!a7(1E%p4?)uF_g(^Q7A=~UY1O#6=n{VxIdF~|*Rrw0C-*}{ zH3*50)8~iJ^sR5{8@%Qvc1xXyT?O9WR%G)?VJ$4aU8iQDF4X8}EzmB?euc?1{Cxj5Y4LN}U0Zb+N#220p&KXVRgQODu2bULvXq{J@6;$O(cL4qggxn# zOkJpbP8aypE}e$g58E2bY)m*8;}~}qP~nXI-a>eK4=FE6R-=94O;`CM{Fo)FC9>s;N*K-u#hh zXw`6RFZfsoD69W_F90$ufd(dM)bWmJzu7dq$`YUz26iZ>*79v>{LHP{QMeb=h|olP zrGR1>J%UPT-UV6Kp$lQEyK|YjHNU3si+9$7*?l2&^q`4R+p>F%W@T(@cZ)DxMy7Pb(4M>gfz#;fRUIq~tpN)jo_Mx>d|qeLy<>^*V>%)QyiX{PmF$I#9V5e2C6Zh0SI?5oBii(B zznXr!{<84p4dI6j={`)5v9Ufd%oo&M`~UF3vP@!Rc7}agsZru!x|u~S^3#%XdT(5G|;zxjMGUQi z>j*lAr`ghBkac%$RA5bh;Q=rvmTc--pU?vPSR{qVW6c2ZgrMC8-HBIo96@LN*x(Yv z@@f~~>=|?aItWv0!#Vl=t&-}Eqi_7K$qvNJTD1Ygxxk(uLcgwW0*3+me5>a0s&GH~ z+{H@`V&5T@P&Th9wtKohCDk%ax1%M=arS1MhkD<|IY|HY*k|W>eT8Rf#IrkQN$I6+ zRdXUyf>KL33hmjRTEcUXOE8z$WtZD8<#4{lB<5NQw=Wx%c*`oP0qw|OrrN*ZEY0r5 zTb~J#e)}`aYz4s-3rktf12IPZ4`}z5RgU4)ZP!~&WAlc-AANEFV{7+W&N@JfxI$xJ zRFfRV70H)Ap(woz<;intyq$6}Zyu`ly9yXZjC>}#Nm>gEtfupA>=1=Wqv}YAE zm>ozwc^_Mg^UT~yuC6TUS5QHoR~}KHgVa?gkt^*Oji)Sqmy_(9*go9*4yk39GJF$i zSie?X(}rM7S&uZZI&rKsEk*jC$CWta=_rcz49BeHlzzA6&lw+05s!PIn|2 zMb9hN41T7qD@L>qekFtZ+ zS(o))ScT4+WoAUqwqUMglzRqw^$LaO~C zZY{E-g=LIIw1x@`sV1bHBA4;2Zo0V8;K(@TrZJ&Iie`xkWZWI-RU6g@E$f`K@~5h@ z(CBsfS`~?c?%$nYI0U$33XEk}E8LJcNC;|LExWSz5<)odD|yK1yY*2;F7GT@ZEv6I z2y>I!y^f}d2zSS`LX&HSaT&TNApt(EJK4m$!2jquW)*E;-Vj@x=j4}rJ0rV6vA@w- zFA?rNeAzSHmZ#+U;kghenN-sEviZr6?X&v|7O1-5md0vGF(!R!T=KUMuLTiFj{k>@fery;!8Xav0X?<3_Z+6_C*=# zo_VILcV#@Y3~84ZW*Qh>WRK^x)fo?NHt9%UKgXewRb5WzUfxhY=9ma!p&)x>04BlW zo4`JC9s3|&Qj&un<+6)b#iZma?m~zl3r-H3xDeMk4@=?h@xf2|bALXyi=d3j$V+MA z1rRALXIyOOw^-%R=bv~>B%^5hdR*tjcMkLkjx8_{qAkf8+#cq=iv|E&>7xofoRW4R-iu*A~}vs#^N#(>72%!u+$v0 z*lKpDbnYPd+UjS&z9k#xyP3U_q0ioEk88+J|J$VJ!>Ia8iEi<+M3!gY=F}zHRA(pD zj+~3K^7(;)Sh2FmGTWmUd|{Lnn%-(5@@-LF%CFMQZ&QUK%}*eAz1l2ku{2XRJ!nGP z{b=uOMG&hcm?!|#-PEm3a`aSMV0uJien&EXP3ExCs`dQ0(hOx$XH8b@L`{>6v->s= zN#MtXmIDIreXES)Tt%*?s@Rc44`$#z9eDbw=$I$A;0UnU7e9&LSJ0Yr6K7hkA|^rL zxCupEh4>(DdDpX|p@Q=K`R*?28lGyPWR;k?_zOnsJ59kRZjZB@`~_8x*zLN ze8Pt;C`evU8>59cnEt$6X_qxl@xo-W?%a&Gudo`=(=Vxs$f?m6KNdjc?(3AVSpI}e zjnj2&#_5Yx7(+kTtS&%zu~~!%(jo%Ddt}gBtzHEg$9d)B;W0%xiwxJz#uMzZK>z z#x%h8X4r{G%Eg;HcF}g_Mq1IVJNu6af(^#V0tZ5jQ;^vgq-1fi-71m_roOhG7z?_1 zkw~9>ah~{mo$|PX{v*RPH=f<6S7d!vJlgXu4;E39wiVayd0F51M;PxVyHYtx{{hIG ziN|H!m(jUrXm`*ORb})o_YBAe?(m`IV$lzSG4??qcyn=s?f)jQOYb7!5>*OQd?~^S z?t;=K(eZ&fVP^%=$L7f`na-5nj9cg2<`3b-z#HGKIa0M$-qo@?tUzm$ikr7H@3*H= z)-$-17SmRQhK^i4x%y71Bq)n>&+nb8yCB&|3v_A_gR3upqjVZtdv-z~Pd)qf+hsAV z#`%t(XjM}o-6X?IA2s76SN?CCQuMwVzG|U0gf@pKzk4t5s7OIa2C1^NTk4Um)(oBi zHHqVX8casbvX>KiUwJ44MR>cl&Jm}n{kD~5*W@GhHHSHi!%Da!J2BLWF9tpR;Bute zK9DXY8hL-}?fM491c!4-dSI27dNL^Zij{ey?B};%?sT7b47)tFEwrG)>2ca_qaUMZ z^5#B=&BB4-R|;rJ_l0|)THq0OBC?Y1yIgAh5bF|j&1TTULWxk$H|sk8L~F(A^KQw- zW=9W}!$}L&7U0&s4XTLwg)~3B%8x^1BM__ePw+Cj;S*8ru19F0i+MieL?cT>W@#}0 znW-=I*HoqbS*N8>j#z;yDy-x$Qx|R_so#F?_JWFiz7H5X6a`sX!qcckL=4*B)PJI( zhlLlnCB9vXVC3Ep?zY0BAi8$j7+?R^xsh?wT=%A4I7{S?>v816QXZvu4YB*f&&}@U z*NE(NO(s3edW>C7jB!1D!xDWEa4(iWWT5IptPQ6YO~7Rx$K0Y>$0TL~b1ld^+pV)_ zAiPPsk__T?cxCjA7B^*Zps^tOWJ+Sv*HYSLg!p+=V`#B#2jiej<)?cW^{&Dej^#Fw zRKyU`K>kC9tR6H#d=D-uuDs}!RfR<=spfeux+(S=D0gf{?N$Sw2L+jyjU8${VBj`& z=82G|ZNk~dwGqM6AQJ}dz;iRE!P{iy@KcH91)CyEChlDWJ-k2bNmM=uQj3!|iy}yD zv*h%?)~}q>{dq*a4aUCtkCg+G#wJ}jF?5T`$VMIX##-Mb@|Wz>ujWVC?YtNzpF0jI zt+4yre)s*AE2>DZTJ)h^_`X5#O1_bVO+%5aspX9am!%akZ^sbw(seG!#9c9A_oA{q zPGA2s3@#_HyjElYz>ICZQyX{O=cn@a(G|KLbZHmXnN_<5Zf7 z$X&WODSWSZ_*jkJ@XE!B_2L6I~CS~oa8h$ zi~bG%i^kHZ0+uurjXp`08ao`=CmbA*b$tuTneG=EYpRQF(IhqDC22XmA|X>xsnx(J zqVtEorH4|NF+v3BW7*zBXxLtUgx^g+!h)c$TQ)lHVTtt3pIbBy_0>L5S+#FY&l2@TF#>C?}ev=!35 zgoqbj>5*@z!cSX`lG5!)DzmVMyx{f4&<{%8ZHqO;<}22DV7l9-xvu9F8u#e$wBQW~ zMJ@Wsrv?yxvtU=KgDlN=O}xwu$_s29U&qDIB8$Os#m<;&>UGaqXV{6+JAz~l*w6%I zOSAOU%O!8w)95FbiH{Q!N3;=5n+`{c?|88{#;Ytm$NK6wJ%Ej!u{h=|s|@%CLmwSy zWNN*xa}y@@up|bTLh5@$E)T8+ZP8T(?kd6hu%*)j@i&w+)xK|LS)r+}BG1Rwt3SKf z=v(M*gLi-Hk#8h3<~2Hgez{azgL64b?s9|P*lf}D_{ni)+=aevFY~63F=>)}2?SSZ zQIugZ&eV=GHEqmbC!aDISVQR{%-VGm2+!}yLvd&@iAm=3C7Pvp%sKv31$Dq<1UjBo zVY{n6EJh(Q^;YibTOS()cGXum4Mrq~y+@mC8)_u!JKxtB*fQpmU)gi(s2+4||BOaO zLufdneBP<|bJ?ptssNwR+#&N;7{WEQ3kjxK)Embj<= zsLP5`hVn9J5FU>nePUT)*I^>({^rbuN~^xZFcc}2dmNR7+P)~oYNl1x-bBN_slWqX z0uxDX^eV2b;mbWbG086AOIH%nd$>5e9PM3T_cLXlOz*pBGjp!V3mrQ2HiVxOrV>Q0$3;B>nzSybegZdz^r-2f-)|y<6H! zg2E%Z*fJFX(=wM;t{n5Ic{rekvPJ6nemzEZf`uV#<-nxv*VH7~KIv2}$vSMP?70I- z?zGl8<(X3Xi{|KJbqU)JA!E^3Z@*8rSU}mQVg(4tn=i_CuO;UCSabr8nl~eq><%!o zy24k1-G~{!V=E`wE5x>rM24H&eTRnv$jK~Of;4b;AjIe7c6@L)V#@<2LWPXSjy=|= z?t5=0UAbCP-3q5E4DG_u(JNXCISHzVxPN&@r3;6|cXV*JGCz+keX-8_s{T@3jlXfM zG))Uc_BR+{HRkmP9({@kq5#O%7K$i+#4pXYmbx}y!-7H(&_PfZ;TAzMd=SeyA=i5^ zkB1lV*gaK*OKPT)Hc^&XLfB1PRT1{ZBnKDEm}i~PiJrjGN-D*huE-dxR zLPyJlbeM8Eep2^Q9W$_3G?u<+qG}pdE+^jx`EmF~;B>fWi2eKYNhoqcY4xhga)&L0 zz1%X^Ll&%`rza6M=Y)t8SzmXgtDdEd#bb=bX`@xZ-Bl#w_U%{;dt%v){!nuOh!a5f zd~kLKf#&NrMw(3EC@JsB>4k5;k}A)Km@YIHb=sb^TM0efBQdjSbl)*G1IM^+=?Z-j1F#p^NmS0uf)0@i0j(363?pM)Q&#rSLFuQr{O_(}%;bMSmB z5?0%5HKaxcV;O|0-@HK*TGGH;Xlo&C7cl2~YU91mq#DtFd%I{)NB@4>`#!9w>v*u) zvm=P|a+Uc(Nof_C;?`N_zWdRzv)Fq;V-#OCENk=XaXAL_5!ClM`q+LqXI@Sqb`=8y zoQ(VLYCeV(7NeFEbm!*E9Jl&<(>LqT{`b+d_Wh3Xbzk#cUs2~vDh^y)r~+qVSut~) zQ~stthpNro+OLoDM#(W$vzSGYvUw1`EmH)g+ zu{}v0<)?bQT$IJT;GaQpHG1Yg8x0JD%?8VVQwt!ukdV0Q{Z_`ik4AQki-Ke_ts>s; zo)zu&k}dfclj#oP&n2%P(V2aAB_IB#tWW50H<6sdP{t%~cU$W-tR1T?qu8K&d{&hB zjrZCaEI5i`FzCA8a+>khlr{7?AE7Q&6uK0P@r=W_ zD)8FLZpy|98X5YV08&wzN;XQA_9!4D9Z{lV?~(dwFkN`i57MeZRlsycv(=VomzSK@ z4|hdfwyZeIcXjst4SGUr25gYiDEkzC!d9*!G~Q`cgQGj%L6JfBNiG0sG2IZ!-_-OMFjzpzdv}8q@Qy@cVTX?^L5V)_+09Q4FeE{k8g!>VscD7e$L*)H0NL% zEdVFER)wrO(6qhqUrVZ}`2}j(=ENRMCGBt!u~J&)q2vXiB~g3bwybT}_QtsbyyINf zS`$P0Q9wWD9IxqeE^UdpQU3p_y~{&lgSe`n-svTOsAVe zn_=2_dP@3Nqv;PF(-p>LYQ_D%E1fFd-ogN;WCK;Z7Ezu6mk1sn-s8a)!`0Qbe@Ktx z;%s~L?DB(^6)ONBGMrce&@^|)k^cSZ!s(5T2$G}PRGUNTn;zPf0DOkcV;Mkqz%SiV zKC}Qr_^DP5pg{mw6FkfWbNYuBTa}{eh^*4mNPv{nPBx=U&+Apn$B#4|goWp*qf04# z^t%A|MEEhq*LT}aW`FY+Qz;Dy zY)Nr(YMgDytjyZQtTiPraI?4j(e1r{wPvNsZ~{+#eZ9-&yfbdKrjk-urbrkaUeM_F z8ifBK;S(9m;_bqv=v^}a{+gJW2$-dt>7JNSa&vQ|!|Qgr<>A91YKS}*s=dFM?LK?z z76-bCO2{fjBv$@FbPt6Qzir=kpz|2Q?TpIXPfJVtaUW$unJWogjI}3TyAO=j)iFFe z`axM)xouypnQgU7^V27GI!H-LNkF$&W;G|iSf*!+6xnK3ss&|C=WDmwr#@`MA+C(G zP>q$3MrYNlmcoj&T07Ob92kJHM)d*kaSqz91|5kXH8r5+izi}c9;CvkRrNZy9U!h2 zPx4@+!qv2ZOgd%KD3n{`YO-J7KLpIuH(-&nUB|qFgwXLvcfy9?M)p=qf9oJEEybnv z?vgZ<;Tl#_QsUldB76GHOqb+}^d!&B%nZPB2u|A}^ww&m8n=n97w2~I{=m80VsT=E zd-<}lu#m}gHU!5K=xl8A&s)$mDYB}ns^_a8CnmFmIakjeB_;cUA1}|&J}E25+suMJ z?@!Yo1x&T+C4A^+yVLL1==CpNtCATS8tOyFpJu^RP;*74(ga)hx>vTVBdTa0Amg>| zS2Q30igpP_1fDKKRwES0As$q&Y&o80hW0$(?D6yb=d(nbj&y`YFezgWcyiSBd0nNmvYmX7XNE9D)GBst$&d$c2;U4~S>M|>9G+SO?9?*Ma;F(&q4+g=l zcnaRYy^))h$>rR3kNF7bMrWDXIHOXr+MA7I15d$3eFF|#8ve9$Y_UvkoOuNahu0u2 zGN4EC!dNj$hgQC{-VUHaL4arKD~O+;UwB8<)_HWQaI_*k-3>F{>;oWG*`zyS%jLE& z;i6|!u6@}WGwn5^tju));Pru0VFnFS-<$2c;)C`l$`8yg$;*>`_8 zl7OEdjZB804CK%AzXS~SU(m7+0M-R8dUPw=kLTL3Kc_8JBdGCEV1L>yzn99{yC)HJ zJ)r?&I~8Vvek1#d6&}YOJmbNX<$}VJm7T&;07aMXb;FSEc|m--F|hiy7)-3NqxswL zNXf`xgb}gm14l%OH4*a*6)M`_*=pX z%USN5+gku!3P3J>-4E*i!spRGe|Z9*BP$ig4hd1})D_Yc+McCtWge~_sydB&>mJN8 z0dbD@)~hDUTh#_9mPrkJ04900l@;mocz<~fASam{Pkt439tRBxZ!SV8H$@v#X;t?k zA2^L&Kv+v`1S~5|s00Qqkc!Qric^0~8)g+B`{B%@PS~STN{^N*SJ&FTi=wWw+o{(j zH*JIfj+-BV2Lc}RO8*h&+IxG@j4b0VefpOV9<+U%W3mv+jmUI_auRvd@ z4cxCW=R5$SO4;BLF;qiKV6hCqF@#4cOW1!;9N8NP1){s`|6rB;Yfm}&|HUiom$ z@@%SD2}OeYcct}NiX$wE(t31^zVSW8WVUN|w`>Ef|BJ;s`RHf?zz3~utbL7XfaGYg zW7PcAjU{i$`nvK59`)5P+X)3a{#HyN{A;}QB4djc+xOq(^ONs`@ZE=RjxVcbCEJ}R zep*D~H`t#+XBe0l19IKRPTN_E`UYOo*M!Gu!T3YE~%Xvm_ zO=EM4qU{eVbe0+ZRPng(6am6pKkk7X4k9{nc(LSZN;q)I%L=xsde+y*E;`i`3}eI@ z*J$#|=(NN9x7|Qp*&$k+SW;fCv!G=YcS;R`?n|j9pY*TTUCqUTpZBi3%zzsO`A8r> z0AfbO6)}U_3$6(Q>t1SqM-G+Fy`CXyH$pRr-f7TRHTUKe$jZWbBD(b1j=zFR2`~7;z&5 zNeJKYyS`QPql1Oenqm~=Oe>B}y}gp`HTC4Jt(v9UeB>Z}Kgz44bar-#o5?F}q%@fw z<-!SBuo1=oh!a~DsHL&~ei9;s@?gc^?bp4vH`Q6vluE|G(8+Y!h2t=9Io8Zw&&Uk! zTT}wac+#PY2u}@`t#+F{4mhdJR#)tP`nt5>CJ_e%AXCH`O?Ys;OZ*vSze1Os7%j_O zCah?GkBR}fmsVv-I-$KWyjc#M@7G9GmHfL?_{byU!!qt-gsqrr`D(=TbZ1V2v6rQ} zuRs4{Iz)PYGh@0q+grAp+Gr+R@c*mf0&5uqur+2GXS{J%H zU9N{pGm)3Jd!Oj{51p|E0FE*FUc+aQzfrKbpxyo|vqNe6qOt&@=_>@eo|#^^%-wbQ zI>3+>h|5&hN=+zwvX?{hB)LtSwEP7@*||G@d?5QLxJIvP5J* z+CS%@La#1OtFOsopaymxD+T#?u6>@4!o1apr322LKRp&1?|ymsdDHMu(412)IiaAQH}$)!VkKTh%`G>m-Mp_lw()t;3R8%@JK1I~it`pm~p zzRxDJA_VHVZ%kaANoU>ZiJOr%an&#{HgOpi*`@v~kd#u^WRQ(E`MiB`((D zVg4+(hAxj%;~;F>3NT4cS;jFGY+^Cm-&87MYac{QyPb9GHe&#KIP^YEf|_)9oYW8$;%ry3a> zyDni}B#W^%@U`kswSFI8T{Q6K3@0pti7=MrV`3NsD~T~oNw3H;3jqlvFrx-{QO>6H zBh+=Lzt+l@_93b>v=af`a$vXou`;4Jv=ie8#(My;=2nDDu2?yZ{s|0lB=eXXpwzrhJfW`Ho{f`_)5(qn^bwj*E zc^@i}2@4jeVBvhH9^-hr(ALT41F07=01H7?Ct=gW;#=x$ zm=wzue*3cqvptxVPY;ZS2)UIfE4RS9d%-Q1*&FD2*SK;eMNss-fQ;?VLxoaWw<(rIM+ZC)|GBThQ3eLFr=98lYa8d*sz`@{& z6=-mGe~JZ&+giR!ZdHArlXd)&h{cs-r3;jfsOoS(*8hztdoy)U=fB+6PR(F0LsfA4 z3_6pH&S$cXm*&e&Urj5QXQxh30@z&;UDe9F7C%}xzFSH(oq(6yj6iK+Jn!FtAzBxR z5vMWUfpg$v;Ke*vpr3qNYTF&q;pPc9a_wofBlX-u0sDb;HvjP335ub)Wk*JUV^H3l zE{$}QqVW;_P6E_m8T=cRB;s&~T0>~_!U0_RDh=?@%wibHQV%0_aDYsC~0p&ln}6xM!&r+5u&lSWthF6ygVLjVUR@5KvL$w>ZFJrTvQa-+mh zqT*L@{>3OdyZ*iPn-Mpvo2JubLp<7Kk6xXmK}|wZC>jxf^9x{_4LlZ*JAJHher%N+ zcYQcaPvx?KgOxS^Z6yMV;Q5ei(={0So7lKH{>aft`d6KyUFu=%H~DqM;zHd#^5|oC zR}gMuhaXh|;T*Qb~$|xIreDA1l?Lufe3A z-DmNCeT(!Snd^C}Sghf!=S|AxbxJ~t*ZC$AOD>5njJrx&G= zhB_}&MhY@<_e&F5R)Z`h1uC)C*-Z2K5x7319XM*05UiK>n5j z^jTBPiIsG_Lh`U+DGagCeLW$;J$Oflj}M0`f|PO!CyV83-RZ?xo|HkVjg8S7n?*(A z!;`ZE>FyV{K)R%#lqf`|Md5w7N_idmc!l}U*Y;)ZYkHHcV*179X|g|3-R?#*gF{`p zQg?jyK(p5oY)!-bRY|juxnl|$*;<6~oo{J>`VcvV?8FX?$^K3or!|GIx>c_k`RwH( z{o;CNQ4w>YBZB`sJ#C8B>gGnaLdUa*O>5qh>Fz5DX!1B871HARsq8}xaGjmeU`pb+ z8Y^Y;)&si|jVLp0$+eb8SX08P#DT>MQk#k`J4&Mq>Hrhd5$Q{>7vQPT*Gv8FtLMv1 z{YN#wAh`P#uTkcV)4i!6B}l#Q(3uaLzsh2np{d)b@a-x!zl~QQ9TidL#>KZ{3W-1d1mX z(uei6y+YfUm)bFfY1r7<%pE-d=fXpV;nsZRch&NCVrNU(7A~m$-B}MdgdRt=uw916 zhKKBoT=17Sg-}WrAI^slFdkleM7I?xNG+c@dgH58&-r99Fzn8@`QgjuX0e}>)feQ+DfgN+> z1~edkK%j70f;~AstX5WIj)|l~oXGF$fI1`9x9Cg(z_eeEBS0 z!RoIf;7d(ork;pda!Ag@7`H9qdlIQ#e$amlTZI4b`uV?Yg8pCiV5Q#)7Lba3Z^I!J z)yFicKjAgCxg(j72ayl3%MM5_rD!rLIk~TDQVY1GS+9&J1Ptc`REruHL$bso?e?b1 zfWLpF3GUQWErpkP4F4+v4OT)K;|rlAv`O1`bkI+oR==Wgpdy$<^^e+W0JjEQ2SA}v zi+JFd@hyUvb20lkfNbc#Jkc42%ICTJlfscTdVeiD;XLZQX_|_mmRr2?#9z{X8{c)$5-(o1_ip@%6hNl zn3rVyUjL;=4rBeeEn9nXa%C3h9^G_%+}+pd<$XVgmnj_CJ5%ujm_G|38D8fp=xn^IrGhpknw^e z$hZoUrR~AD;=I!jNak)TKA#V`iBk<7nAI5ILR7#m|33faCRoQFB8pPd#TGOOX@xP2 z8%VIDdku6TCH=kP3ILqjIIs8d`R&@sk z?Mr`{?uzJUlF|83^)^Z@N~~HL%V}XSCI#i>QI_1!NUJ9%DjvA5pC&58Hs8*q2rRml z^!|2OyWEy2c@~XS&9c_M?y#1uG|Z;A$sZ#YJ)JpqgHyH-%Bi_)h$V{($`QWizePUr zkkLH5J{HzaJ5J;%X&L#}msQ547+`)cGC1a0<|v}G_w(Fa`7nOvVQ>O7w1rW(HKD|l zN9;D--IZS{plkj6p8+(RPk_46{KwdIb6_2tX!{B$r(gNAv0IEoe43BzKDgp$uIxvA zXGrAu(dM6Q$FEI!#7S_GC81rL*_YmOF6q{|!2KjE@L#O=m7kpHuD=(GTD6@mo!Zn6 znNw&t5$-0d5eg+npS)J(!7%3oYW+ecOq}aU#WV(ssc7dYOYZTC2OQZ#A|CdL>I#TD zS?{RvbjRwr2x*9+dfV&{*vW={AqC!-`1v90&(h1B;jf5#+CSLy8%kPu7X_YzzcIhM zdzOed2Qd;6a6DK+OKsH%*|jaOvZu_m{@N`EzHgQb_qf)*Vfdn;rflX`wSTk17KugO zP!~UlD^mDtl=*~#wwDbYEDpE^Q?<55>IR-QYYo#(~+Is=X7PV?u(0v=nQNrSW#Y(-+ z&Zxon8T8_lEe5nIcu`42XGU?x{1<-7_HgwZjK()p|DhqboE-91sshA^;IqGEpQphk zf&=(0?X3$0h)5^uGC#cpfTRyr2V3CEf<2dF0@_WuIJV6KAt~lU&&7_-fqyijJ# zX7gH}=iAphGp0LWJJKIxq?drN=pDR*8f!>$#T>g?B@up-`}2*1y~Xz$a$mp~LLB+a z$DnEA3tgS~#Ll%J`RDaAraRjyAUZ;pWWwzuyKj7^{;w|Hc{etNK? zI#w@p;+n*=+RfHmAl;P`$&f*n`K7WBzwiAu1CXGNlf1%$M!epyuf*b{u(9EjaDTXR zk5MX5AYetoLk1L8f@M!<=a?cJGDB~G#NaOi*T}fLfn%WsUG4ba@2GUF8r=Uj_}Kgo z`0_$!f7CAv^SkX;T4tJ~U-j;q5QSiEPCWD5G?6`yGmozW&sJ>Dt9RmZs=NDKo9u$H zb+2$|A5LP=Gl9s1^zUrrWT_Mc?w61J`i}hIeA}ZLL#MxowCZf|>zCbhaEjx04Y$UU zgZn9C8v19;JvfOK;+5{>rsyCNMzm^CWjP0scqi6iRbg-y=8$%?fAsqH7fHlvTEOIy=E zX)bN^GXPK_?-54C(C#d|ujVy2Ud0{?VTM!yRXxByMM6mlVjRFRDc&=?GB6B;{=__%laq~9^Qdo{Gx>JlUHxIxlCH24%{E8;tK9FWT~zeW?S8?{Mzu@ zKFW$r1NaMfz(7RG53AI%;-bcA9nAJU)Xn9!JU}oJ(X@t(%SUP&P~7#m>u|g2;`^0; zy9g-@h&d|oDD0S7=DcX-T)4I>9GwfRh-k_ywvO+_%wv( zyN_zNa+2gZ`z(1PG0yRbo}Yp z*bFczFSuRtYSJhv*dA3CGY=9CkVHM=U>-SdTp)hto(Y*Mhp)q?-f@D`eg)DM>Hm4_ zW%UG|(7KOJRbVBZfJ9GGVkCD5PlAEW>G{8>5jCjmi7=oYTD|P$(yX^5ez=T!=>Li? z7(837<^4#|G?)l52E6@mn#1z|FNqDQn8P*M z5c1-;3P^(dl)nsBM}H&1rT4eegI41&ZdmP+$llr3HK)?F*#eM~uArqPU?WZ7jV~O3 zVVR@mwj{IY6E!T;f^B;BUBR9=?Ac`T1x`7Q6s%uHlU>8Z!4r2Q#X$CsZ+CMmX`&+U zvoe=qkfol^7xUI2GJlpw3^%m*^5W^0V3C1UzE{U@+ZPxyA%<6tO z>pnyh>>FPWn2OFbN&%tqiznDB+Zq1R3JNnaHi1k^9K+PV@QDIENK(AgjWS_|PVtFZ zYIk8ps@$0Rnp}{d+f?UXHB0!*a(C~S$*Q+DdH&sY`LiAJ z5z;a+Bk@;;EHp3U3A-3Xm)ZvgqcCZn3X{vB-%J*tEOdLn%i-LLu)y6M)pm2WDOk@2 zL4DqmP_{6Maa(~^OweRd75U|6!PUuW#T1a z8wBvJo1X63_?@uDYc40^iZ%Ig4IS2K{)$)S0o12b@ z(O;_hZ3jpgfvj{S%gv913tIkl5>VM)ob6>;0uh&><_5dFVXPd!5qTuvEm z)#`ogqW)`>Q7gGQIbW>H23C%yUWVudz;(ooIq?9R@alx2pSj$i7cTJ<9BfGVBJ;W6 zlE&Va%bilE-m+MvaOTig`w(=tF4_fOrH$kUEmBzxE z$VZq$EA+K`Vq&B3A5+8rnu1IpAimkA==%&oE~(D-xmx)Ogv)DvxLSWh&oXnB)BHwTd_ALa;zSmWw{8zmv3z zO&lzcKHYEjRcrsHmf8p6w93g@*taTWc4 zI(l{oA8&!HwD*bs)dGm+lk*vwz!&F6@h+ZqH9g^jpf6@F{ikwes;z=96caW*+uK`f z{nNYhYo&3@aU$#6wln&2*al^B)=YlS4{AersbAn5bq!1QEa4AXN28q zGs_nGeyvjC>EA03SVU6sxL`_-hCh;DD$!n-A-X?~`7*@Druj;Q#LSZdP4#0$4g*{) zf!kYbOPbvUMRG?uHe)Co4mSE@QAdv%Ln|h5Yw9J{U3n2yiH0tm8I(gQxOBDYTP@ep z*SQz*nhw$Z7jFY}>epEw~LA>sKf>X)Q)Z^~OG6wVaULu<^!yx>`pM<-L$+Z0(e9V9SPUMEc7{RllMesIkxVpYF zHvsO6#q-oNlk7HZ8E^BNfg{EmalM`|M7LjYMP&!%c?P+}QTU!AQ3tzq(NS2iH%_yd z-OVB%v-R^q?$t8%k3aSUstDUp!L6Mrvyx>reC|*Rk|>i@sz;($oA~bTj)>1K9N=AJ zlad};ycaJXAFxMVZw4P1Pypa|{NlksI-J7BGIDU|&(6*+W?%pYgiX5c%dT3*2y2rH zLeJN(_scbii#Fdqs>Xn_;hQ&aqT}Kqtw<*x=~&eAYfg;-+35`_j3nae1dI#;??GTe zG%ZDI6rn&Q^(5+Ba}2&+bvQsjPDrZOO90AB|9z-&hwbv5{3uKB*6!Z(A!flQF5(FG zGOa16xQzL_TFxS)doE-Aha;A*6BrLgm7N7*(OH zGl{gl#!p#f9E*5P0zZv>k#-rd8R$y`m3ORHybPaVhx* zJ$V^6Q!t|l7kOsZags z??ukgTsnRjf0TEgtxb?wMykl{#WkKZbRtO5?dDau>>J%HZp&X?7TGInd)G~sdJi40 zi=4bAMlW8iF?x^MT!O(t`K6?_F@4+Odgp;hXvP{=;#UG?p5y4}cRNw6{UeTfAX2E6FIJceWMsd%y)OC_`j~!rd3!r!y@Qs^(g~vGU9VSkK*$haqND zZ3&UTWtr|N_4%Di-F~JiYP{Jm{kS){@aNW(j{!aXe$#50g9UkG&XKS7Rk4xbc7X_X z7qc=4&R@n%^j#Obioi^I*|@+38oP??5kVcv2TDeX?5x;DNHP#8>aaosE$$q#ZTa{t z=f^+Hr|cM^z>6v?n^QHbIf*YaNbR@WAn#}RRi#vL8*p!4dY}Yr23$y1ZW?K4(NS;M z6xV^62< z7WEoug?Cqg*T$THt&Of)h_wRs{t^EsH?H#`3PN>zH#90v8#LQa!>sJOLl}OLYie6) z{5w~ta$>O&_v-kK@U1Xj@mS7b4@W%Irohu2x%d?uCsMbzR&`Ej{+(g|;zFcmK+xj# zfD@VS2OUDZUGVoOd=h_pqz7kSglv1CFu^yRdMlblG#%6a=*X3{G6h=OBV(K2FP+=v zS&736v-n=MT-w7oc#X&Br=@+xgP|JQG$H8qIxF1Xx`V&h+?sa}4}Z__LCt0ktgF-| zZzo9v@4sj0gx=$xjvV-cc&>Z0c0K)Z|E32u7D_XF!F`)RDn|Iuv001X-*0A*n}@rf z5a=MN$A07R{{}0%k-eYSXiJ^u<-I?rCcC@iKmR2P)C2(OGV${AvJb56X2A7hdHv~s zpyH;wKYsP;wpnT9WdRP@|5pqwv(YI}x-;tm-0-{2ycuxsd=@uxw$1txrO&j7I|Ns@2C)1$0EabR65;ALcmGB+w=r&2bmMHe2`$ zeW@)V#l~E~(V3mb;-9`5mXTu?c~_OOcfO;unJK3kWYUe+7u=#8=r~4&MiM#Rn9Yt& z!Q(r1@Bu>dNI_o8=rn)5Mw~RtS~r=N$q*}%ce^ttT=c698x_Sl98+6!6}iobP!(!T z_=CR_<4zZV|NYKfu7*dZ5YXsltlKs5aVqiL;xDDhhhX9`06Y{Mw#^DaheCfqN;57T z0zNEyJ*E02*LNmIK6>?(TjdD7O;z-(+F*VpQVe_%U^@poL^Evsn3qkCgxOG6i*vT+ z;!^#2K5TW#FTiUiEwx+(by5lTtb*pwl^rn4i7&ID6mHxiXTG}^G(U}XD12>K>?xR+ zGH)`o_fAyj%$Kx8hi)*)z-|-s;{zQZ0xot`W_tuH5>neNfwnq4PFscPM_QMctb4+C z89&lcPfQ0LvG)W*Sm;+3mjkv_sD0nZ=ytffwU!N?JY1TT-Yhm($2H$?T5y?5vg;B> z+_hN*GT{N?Ue~&4)N^#s z>cb-$$EX8Njo05VcH~O5qp!z~6Sv1dOOtii&59Pj9ncf+s>b#h!-M^4^%h8*xl&n| zKH}7NSGa$LKF)w@<3o+z6fX#gNR+${PlXNaQXPNA;S@^L7UjH_KuB8=G@>DU6z=aer#zZ^0jI{V-l$=gB583;y>$yd%1LeY6&)#F4ON>dRUOz zX1MR|tojw8-elv{+0qa$zW~zStkOL8<b+uWcec+Icdk9548iL;ZZw|_XLdXuuOFLn{QdGFs^zW8f5+Ru7l2JG@Old!`JX`&i1gazm8M7P+ueUYG+k1yp8#e8P}8*?%aVo14O z%8OrNnF)K*BNN7=k77(s766wt+Iot1MFkb#Zp6)ZNNIQrJ6ttgeFISZt7)0__LjTJ z9oV2SX&NmMF=D${0ah5uoXN~+&44BI8|Pns_+zvQPBPvn)VaIDl_0Ejt-Uct@--Ku zRAF!hpj6zh!I?cib2}&@g(TfBKvw0wg+6_@FBEeA{=&f=HBFyq7I39Xsw%~OPNT=% zYb}qJl(Nuli~eIfRrX;9y$vj1TR0i!4_uoZ(2DoaF`=!C?f|?CI91lexg&sfJbRsa zL+}$q?OmK{J~D1c6f!0$kU&IWY}np&@GgOZOeGAs`lsO{Oe@Z}6luu($@0NeHe@kK zroq8LH411cpS*7sXD1;2xLD{zV`S1-GTtRctzv(9-bzl)es`(+^_4M7IlZ+`Vd|)$ zJ%ar@HqOTUs2eEvz=YFiPJa5Mc+A;pxJ*y(b zX`~ah)R`H@2kk$q4bECH^mN{`J9#$PQTUnf$%k#wu;tny*Z{q9h*j3Pmy&;wvem89 z<6z3fB;7U@dvv_`RaBJR!IV$pi{3`B#Es5(pXj~55Izb@KIYW4#d-47>@opUc`=CY zvU0QJs46!l8@BUL-%V4JsYE;#YV@KMu_>X+wR5*x;eem2srr}40yxG)7GaXPz5gqm zkmf-iYh8v2iNIZ5@oc0rNIi1n-6+56g_UDtxgKZd+K`3QwKLHF-LHy$^b+Hqx%boL zx4xaO9l@I+`DP~B%R3~NQwLAqOXR-!eMSswulCN+ZoChV9Ey8L6iN`DV};v-C@$x; zR13B2Tqv_J31bxQsxcgxByNj!_-dROA7oCFxp;etqr1)Ii)A1@_VB#YAOkiLa!+o! zgu+uTFOWpUFMsL*9uV70TUz4b5-odi8~P#iF?85?53gr}P`dH;hX#qpRp{@%5Kwp9 z`)>@9oBdkp&VH?}$Xo`k$fEbjesc8Yh!{)IYci1|Pug5)lt7V;H0b6J)SyC`Eu55s zUl5*U3INW;Rd0Zy5M)Wouu06&8COF>GaJF0jG{m4dnCnqSEYY;Z!8N!j_$ou4T>lY zpN?~8YBdM;a&_iPs5K;MOdg+76(xl`Momrj_7-Gi??!Nzu+4I?EnOZNh;{{P55AIY z0TyN_1?&!$o;er(%GZ5Z097tjJqMfX-}T%GsAE@ruwm;c8%JcpdG=FtvAq`@zd^FE zV()dGe4OYt!#^#iGuE$w{Q}xtZ0S+yc;K8)F_0aH;qHdW%(ZA#G~dB=G5Z2vUr`VTtFhmxznGzR%-)g8XFu%_%XiZa9|;Y|TphX6Z5! zkvYWV5E-lXk!H?)ik_X-XxUPb9&ytFp%8w%O4TG3K5dTW+nh3uj^_cl>?sqGQ&S)&wNh$(S5-FcnX)VYY0o4DH%X1yM$?h84jT4v4M0;Zbki_+B zSI#pqQ|@VxB3%i)Y)e>611h1lf3k%=P%YP#T7=5g)XuC8#n0pax|?*O%d#N1^LRwM zWP!Cp0oe5+t<&9mH^vPYb&5(3<0Dz)zsA4%G@i9F(~tE!=pk~m+9g#W;_dk--LF23 z?4Ih)vRY?K$wQAv95B%YP|F@1Ekm(mXvM9)hNfmG<-Nz|nB1WP5Q4&^i*-BMlSwWN zd8U=l$`xX>MeALMU1eq&<{1 zmJ$1c2U6mdmo}Fzzu*taIBc^kI8zK5-Jw@Dn7epU=$&(QuG+Cpr%lFU+-!Zq_XOps zf-5E;5xE<8XI+qjKZhMeS8jTU)CR=|-7e=&G5qV1`$TJfT7R}3r_ezcCgqB7-x)6a9YEN0DJQ~T~$Fr}y&IguxC({nwaDyM&n=Gxj^~&j^Oaskz zpdfPRImAM2uO~!kh*!`w2Zol2`B^;$Xm|>@lhee*)&}AtnX%tTr8X?pPebdZrcf_XA(As z=;>EQX@iK;D!H_q?+LQ@@w`1wKT{3byHzqP4zTf+Z!>DdxgW|$SrNAjR#Si1k}&fU z=e{}o$O#>FF|*;V1B@z6Frc9u1Y@rTG?3t2GAD!}&O;p?)-mU~=7j5=u@g30Ampg1 zbdpVJ$WP9|;vKXI za9rgam5MDW9B6cVv0WH8`~JA(^|a}9oa?jG&tl2>M{1|T^84p78L?fD?9d`y<<(1q zA?u%7EZ?@Tbadrlp)TLA)CJU{xUhI#t8KBCm&K+b)6b9xX>|Cv^??9p8hR?AnBkaQ zbmO*KxT_rgTTPW|%)!69U%}CfLX3id*w?@LXJVabjH-R}PH}HC)tuZ(M(dR5V&`6C z#lqpT&io9r;o8HB!==ZIE(KVVb`iKR`yvK&809lqf|g@nnr-$EW3NH7A5jQqV+H2e z;g(Ts))B?Yz(Zg971x?W_@+r?*L!(1?%0>H16z&t8LpFCQ?z6%sej>aqc3QwN&c4k zEnWz}+oK{Sv7m@n3f;ECeM;%Nl&)yTRu1*V<(h_AUyEyD&?*Z#YWC~?_VlyK>cAHZ z?FoTuS<`VW=PtIrav8B=3mGvH)w@)B{)QIhlVPU(`;7A!?trA@Ds+mg74m+st@d?N zK`@y=M&=cX^h}-cyx|-5irr~#9~CQgRj%nV4&^$K@~TgH{@iA#^P&jOsQWstUe}BV zZAAHu3hOr~SpXd2qa!>_uxwM4=&Pa~U*37J*>3XR<#F91nEPhizG*mNJlSL-x}oZH z!QajFzj&^ogXLGi27BH(DLOC_j{S7a_h-xqlWoq)ZtA!Ieh|YeMb;VqUU@7HFV&C? zOZI+l$|yh3kq>dx17Se%2j~s`99#qCejOv+`m{fTXaAN@>M*2(6pz>OX(T+3h=7cT zz5kctmjHqG6Q)g~{hN5Kmss1ERO%(cvh=TRO$m+_uO=Al9(Brf_T0ge2$qOl8ELcv2faA5XXidN*Z}fv>ki(|=Y$$Ue0!j5}EFv7*VuFS@*0TzG9qnXpO= zlylP)R<)(3W!7KH6B=V(G2g*TCnI*q>>?trGF8?;25PVb8pxPtHktDiW!<+$^FNV% zpgwos;P%A(w3Jev%wrC1xQGFg5SkciqBiaqCo54QT3}4ROVX1}A*X|C1$=(TFZfr^ zDKTd}0pL`_S~AuQ*}{7oG85L8cmYU6Rl#3LV;4g7PG@s>)+^TDs_)@}Yw!1RjYQUw z5dNc2C&9K&b}u(iQj-BDl(sfXLMQbkOe6!Y5sXT>@t*6qeA^C&6JC-m_I4oV2B3>o zpdyStUH#(=dUILmXU8`^w(z&C@WVK<<*#nuyhNHa^(N*Jh2QBOAdTIYno~5L&bT>; z4F|k<5ofZz3C%zCqQ|jknroPqcYkPQ?G1}_JWSVDhfGNI1TwGd6Q0Vod^4}Fy@3*N z)=vMLGsgh5pN}O8#8u4IF_y~el~(OagV_}?#xbnw2Jl{|Rj?^lLM zZqDH)LotNfUJ`nHAPwt&4Qo7n9Q8ymzH-I&B$=g>>E@v9BLI;L0EkSPj66rRBGjUR zyPaaNXMnWRpkv$_TS<&RyX?9Z-gYz+a?hl4^kf@vtv@Zy#O?`CV`Jer-cj9yO~B|3 z2hRlV_aqhe&dU4YZ_^~nLZL*A%|bBvJzz@bO_kFJGmg0{BP{RCCYJCxF^ znfB($SYfn)n5U6|2XZ+~la1iW{p-UtFL;qNJ<}S~rV|jidhy#5?Ty_BhPN0rE|X=)pbDed2;pKVOw#<2n|tJ zM{wOFtpEHwAaL3$GF7fK!s76zw_L!bJ{O@xt_QI9$wn%R@K%?imA)v5t^GQQfCiGx ze=gR5yY?5n2c2bNHgTLD`S3dFH^~yz8FBsTH1eQTgFt-H1fsyEAGO zG@_35xWS{qSi`JblI16V7f@(+I8v}>oWT6m1^q~@? z4z!UpYGcP&TDtS4`D+HQ+6pg`ovRbtHuFt{B?AGxA=8%Fmc2yrsC9sKMsW-Q5v{^( z2*=9G%EHQO1CU!mh7*9;r~3}~<8@+3q(lW~N?iz+PgP{1isvfmqFwW*y7I7El+C1v zjt=(+Nsz%}Jb{Q$zF2Z%7Gfn@z)Luu$1A2ab}AWYcha*Iwucwnde*UVaj$N}Qwn$D z+QW7GQ&_$N)pP9*_|{8I1W6vpnyijE>h}cS8?AI^FUS|UQXOEsIJMgv#0YJ)6{u=2u(z9c)g8fGtjMbL=F8;-m%@X)qCJtw- zqTE0~&utDdyMU71GT{pje~tHNQQ9HY8!k}fE&*~JBR0K;bm6C+fOypZPZ!r|W;adV zMF^`lP?7+Q`>&s%w=4N%Bntvez8VSN>qXqw%V#0bl9 z_Ft!}Z~2g;c*;q!C*>SSse=%Om^S)oUs*Z8uHI&*$CkU#JsV=etBw(C1|JFEA< zsW0#vqKq3*i3$n~l!qk}`uVi__cmvp9XfCM^)A4vvwS)Mm8`@29lm$G2h{)70=PqU z?I19997f%re-&mnGFC6^L|_Xqw3|1?nir7Vp5QeAWkVxT^8F*&Q+)!?XO)>YluP#G z7hca+zi<@#a^Fos=7jscqSn9`=1yqTiDU~{$QqngPL%l$Kfl<^vZZPel0O$tULS`9 z@(K4WcoFz%26E|Sc{!sb->Gre`FA!u3=fm1@Wm~4U{S;se&hQ{B=odeuRsFdl)o%k z7xvf*(;t79Q0_yyNGUtlcDLAe(t!vDf*ae)Pd*|vpj64j!vl05rRT{cDeLN{f0G7` zE}9&Lsxm%Hj#iIvXfGgIjX@&Vgg1kv2>Lwcb(Mzd;MO5vB#b&^6z;QQ58umIl75zE zYY+FFP2iA!J!-l6m@JPf?&254Lu8SkcP}WnWYSk*tKTvUnC!rfZ2coVpV@G)ZTooy+r!1)bhRV`IAwHY{nRxnfwNbc%PfJY_p%N ztb1Fa!`mLh(PB0B!Qm9z}maB62uY(kVdh1 z0WB%fY4)ri7$t=wSUpak@yJtGZ>QkRHmof;HJpqOIT({U<^64=bizkP^d=W=)v<>p z0J6w1K$&dW|2P}dh1FS*O~Z$Muh4XZMpRO0<1#3X`*gf`xb^0yq6p>(aVA&YfytZ~KzgnOaB*jRf``6th(dB%-42WDGuifLZ z08JY0kQxv&`+sjZjFQ4r$v&=Lu|=jCZ2s|9V7^)K98QkQ@@$GqCrVCH7(MYLs@~4i zfPQG(?l%gI=?hP@GqSK)bIJ@1)?NR1fRFpLoH24~Bs|ivZ12zE;um z?u_8$=m2%!E=6r8lW;5m40+s2vO2I3w!&G^9U>i<%}RkPwawpF9mZT>30<6Ld=Ffb zvbQ|WEy)ko=%N={Jo9H`L;~kRAAIf^qY1Euy8$K0yNnzyh$<+80MFBz^(T+{vfdW23Fmt>! zFvswHY@`iNku-XLy`_Qvp~u^}F957SbZ%dht{h>MNpls{Sr z{d@g?>TvuwgIz_Do#6SS%8s1cT>_}j8iAj$Rj)7`_*el&)NU!lPw0Y|pFU8;*7C}y zb3YM2!ZmQ%x;!bFW{H=5`dYv#VD+&x(WD`9EQ)v&9)xCA8@UW@x8>kL@I4WwX;ez(sMcoU&gbxLju27xatY(4($wdVA5Po(xm(iKkf3_o97jWz#t2;;7`Rn%;ogUrVeZg8se&UPIUa$L3R^e?NR1 z@cQz}zfIDBcJzN+NTkpF{%r{(ksc%b+Y~|i4F0$ES_J9#-`1zsBL8#m|9|b$xVj10 zJF|`y!O6$ z2n0g;bb0AfECc)s>+i4X>C29niA~QNq^i)}wbg?htEBpb?i6Pw6s8>dTob1qucxFM z^{|1_;e^;&SSDJ;`sErxTY}O#S6g92Fw)Ro^bfe;o$9D{nNmv+`M~|o9E+as?}St$ zY%@z@aMLoc??p*|B)*9yF$XH@X*z%5E)KLHcI$crdD<720EA%6U8J^!1#MzMA5Epr zw!GppwEB|B#QZUs_%dk(376KzI@-`4vo=5P{rnR>&)*4&(;!d-^W#PPzPiAO*wX8m zA;DdWtXyVOH0vm}zKU09A6doN&h1(yahhAy>CVZZa@TXc*8^QAubR_|}@a z`ds}^iv59sF;0MUFE_Nu?R^ohN{#BSTd+R*VQ8;m!mT#a|KlCyD(KU_$pqTJ+vb1V zbe~$!5^3F?0y^6;%7U*5x9XZmK!*)KaR2_`&{ME~8O4Z{s;*R4Z) zWQ;LGg{7bc2lT%Wz^7W2s3LUL;<8G$S5b;fKTLagyzf4G3YMwE%pJV0tP9~B%dyFn=Vva4-Jk~=^Rn=J)jkZA|w+&u2c=;IRlL9QdL^H4e5h1^aH;E!id1wF znXQws-plDzbNMlJbF-u#MaQ+h7de0t8Z2{eYoeZ-PbEfgtec7>r1q}9J_xOHo)W)k zCAoMW*j?tq1HXA5t)p&0j`{Zak^~4BoBUtgNeUEL5>g(H9@i}$5rT-ya5nw;`7CdR zfB~OT4eT&r(xBGJB6&t|P(-7h(@?f=Nuv%uM6S8^^^fJGlP?^28A?LxDOjYq`=q{T zRkI+^9=GuFTYU}f%@RSnd_j63X;dFrFv4NbGKr%3VRhwYeb5aU|CDIydyhMlnvrNc z=Eom{v{AZX)gfCCRS2aeWq=viuG5?!yC0<4XRxk#g!zA~j@FKb!WX)ks{6Pk-|-R~D!;K^L+qxw5>ij~4gT z$H%rUJHT{iA7Yma8`3u3m~n0KYBIpbS#9-sbb9Q|)g0Fmq>-i++zR*H-7Pu>jXe(> zr86-KkT?ciE0h_dNa1|@HSD=c7~Wj8&Z!xZoJ4(7lJC5op;c_4+PZcIvgrEU&i1~(r?p~iTXm1g?a#4!kXn;-P#q5R zmkxNQ&`#m=c0__{L(P?WSnc)qhtg5;1=ZP~z4aP2^Hfr9d4kH>y_*qM{Q3o@a0ys8 z@%_}${zSgPRv|m7fqmnis+xMyL>1k%Z;cwd>?yA?+g96Z;I9wrXfcrpu5L4wcTv{{^n>_ zh6(aRGzd*g?-gjR%Iv38luUId!^A=}7Vj$9?tuSyc{+6gg{L0f@}ya3h&yt8+cK^- z58Er%%X9u7SA56q7T0f_cYYyENY;!h9h2e&6=6~kMm5f*Y}YgmOZC1M2b=s;lJwu! zX-b7Tdh(^0lP?$=^SbV?4_<1Y_G1Eh$w2NP1UqB)&M}+w@I=k7%{O6Yc`>hiOZD{B zRhy#^9Nnt-lNi;vWR+%Sk2-YgMEXKU-C`09R_Oxt{Zu=lk6uSn5eHKn+VvEnaaP+jJ9@Wx z>k($1SOiVd%{%$4~|4G?52(6aNUDC-+<8sE= zfi(4>m({$h7R~0k^v6>qN~za}&sIgdC}(3@r|nuGS~^kEv3dhSR>CQTiIw zc?9jscXFek&shi6=k21^PiOHs)CRaN!LyBOv-Oxy&Jp0i;47aQNVP?~FM;a5@4R-4Qx6Qb88cv0Xl(MedXZ>NQ zkvLZ#ovlsutc|=l_Z>DtDQgQ^1n32x0SgGkNS<$5CxnKK9A}xzDyL#(WVBFkpLKD# z2>6a)jY-SMOorpLy#a&mjcP%k6meMScrU~1jxT~TPU*{K7?U1SDjk;@e;*E)H4x@d zdiop{$FaZuoHbGytkm&}2i`J?k71g!!n}3=U9-oVPE83=zKG_cbVaWxpp<9Nj1}E@ z4pe0~MctCq*eZoWyBJWKtD+(=zNLLONPrx2l)r$UT7eE9(WB@D(DeZW8qD@X>B}C! zTL9>V%13o|cJc}cuI>Y$yXkb0-WzsbHfGSjPh7v4wcx#&U$+BUfzGnI5|5bU1%`%& zsp;yL0R0|yQP+eVp|mg%2sa91V4fQoUN-|`i_BE zY}sDdn|5M6L{1v`J-@9-p8)863aP>k(jXE~^XTg(+&X>uvrjUg05Hrh zfX{|m>c^WlJ#ZEM6r2N&)#r2hbuzw-_paPTse-;u%F4>@ zMx6mNb6p^i%k;XRduH$1=*6Dpd5~rulDKaF9}XEjI=|DCiwgUdxV(kyu(Im&Ync7< zcZz~^av%6ioVJE{>Gchp66Ln#tx_IMSpDl?@Vy5d+y$M z3gSC8>fVuL;0ln+E9Y&w7A}~5;DYlBJYx<<$zXc}0y%W>j!w|Kl8jSJt=?bMy8^C5 z`;9x9lfZ=gmp35LSK0W9)1i6>PIO04kjy4t^hMoMrlT`7%U0-7QGHZS`p6NrTwqSvG{%JomOGzXEVi zd~)(n?<0st+r!;aEx`MZB(s=&h|qop($$Z}{YWl1gzxXyzXp+(+lUyF}h%l;_G0&ajx@he9*fNgR{qYx*&1+NJm_Ep!fV|Q zJR~~0#2^sFL8j~!dJ%DY))G@Z92B~>7i-e!{@e0I^qZ46rMMa0QTEg*GB^2~b#zei zDx)&YaQN0{AT?pHEqf(KP8*WnbY|8JWi7o?O2x6M+5c={Hb^U_A4Mnl6WWq_Ry!>y zL$5x1HsayqQk`h}yXe?3=oKig zI%u+g(8q#E4KdwRCw;rAmI5x?ht+Lb+W-oDI`4C|5&Z^Dnb{kru|sj4P3Y=(RQNR% z*8SgP2 zT0h99lllpKBN9mE#*~lv6->W!=>gYOEP7l%L zS|+~Z3G0qSu2bam2Vd!jQ)@)TzdavJ--Q&`6+6YYCo z{97odFaRSqRgZ2(`R7DkwhzS0I-b>lMr{|YVX-oDZ|kdDI*7pNi84TvP$aH!plCV% ziW0qXKJxRBO7grL8<50)61?7uQ&3e_&Y6>{X78MLz3k4LoKP(%2e;Ujz1+YT4Eima{}RE2)i#5N zGogpvv{u)Gz;FJORlNLJ%`OLuDgPIB?-|up_ci*Ws8|qefOM4(0wPViiqebpPUs~N zkS;Zd3JLER6=$JquVx6;Kq zYC)-t@l!>G7L~X0KT3NUV-x)3LB-@xM&Bw6{yRziZ=xKcVqz+>#%ZW;YCwg3O;jME zSVYA~jP%=Ab@*d*1yQHk?NQ8?B~%CupECIP#HkK-(pe$N-XWq+Z;n{>N|*502W3fE z`!WNcIl}gR#^zzO2c077UF*+|4=}O0_4Sft&LbJA=ks)=A5t1Vq!3~2b)TO_S=(vv zTFR2qjrDD2or$O&vfv+F-6H&_>r4O;aRhSIOTOFwj%mX%ONx91>;Y@g13eE)LiD;m8xPM21G!nuD|R@aj+j|AZ{i3WffitdNmi zeW*76A4v7_*8eTc^#2*T3cler(|p0V$y|A`P%Gy#EGIATb!lm7B4+l$Z*k|{V}yJx z0|P_hd0hb8GsO4UO%*AdFwf4;GROw}T=X(^Yo)0_9CzY}%>z}}4_F=?Mh(#-;^JLT zD3u5KYbULYn8}{pyA5Ew;BRk?j2ykLkp*A41#l?f%aet3oEQAfA~he*EUeXn^3AXn zO+P!yoebk!XxzFII6qzv@T9Xqc5Vo(Xu0NY68j~pfHL1en-Q3&~(GQhz zjeAk>x)X4uha`WgHEN>tA7B;WIrmYmV^=aN=r}KxvSnA2K_fF>>am(NGnJXwIVc-A zJ;0ol6e(M`Mo6E&8;E_EkAhE35} zpPYy(FbI-79W8}0FJBtp^6HX?4&<}&5NP8|AnZO zzy_3z@dS$~ecZ00S!ZxV`uKHI@P7y^{1v9A@4LldY|mXZ$Zf6}VXtQBGyy@Si81***MkWmFmV zBRM@OvjF?ndABxSoVeg8V^GNjjPMmG=Atj*Q7i~479Ma`)5zYIB)@v3zUQQFo|p4_ zcIL!dkbQ>x5(lr9XVslJ;(AtEe$ll7V9*#2Nl4S1nymgKTZ1mtpJlrtB3ducXm<{t zem;t&fQ>0K_UaRD~I6%6;OVDjw8iz5f;cpH^}d*7AP@QX-xI z*MXFGlBcrk}KVSFtp@PiohYDG$DkMYhoxeZ1s&CCAlx_HZ?>G3*wR~mzJay0Jg z8nO{9jayiMFN$8Co>;H((>4&8ce{I013*%w2=ty~sy<^}6*1T_5k_Xt|*cICouph4U;-G^_?!sDIX@sjDai#&M$v+}pBbg;jbhK%OMKZc`nCfe+H zNk2qDgvjf98=Izi%aNtLo~p5CMbvO~xT_DniTI-z5t+UC=wxACBg0(Tc1PBG+$38N zYdinM_fk}9ULnR8@|LQkrbLARt zek9bbR@Gtx<3_k+J3ZsS5tO5pHyxqtV*vcmkhYMjA(kuahVb2rLv%{cRytrm9#RuZo%`=GQe-RNRA_y=j zasH8T(BpyFMZT)J8i(pD#%;2y)Y2k++lxpoorHi7B)1F`ulI=;Lb?C(zX{X{B9C4g zn0X~2cOqR40d7c^@hUI>SWxt4^Q!JglBJVJyOyg!LeNZf#D=4a>gH^|e~nF^x(E26 zou%l<%LG={N*CnSPd*bG*|BltnA=;an#>CLCiM?h36PWAh1Z z-mj~AE;&~ny0Pb&XRrw3^N(q|Tef2fV`{y)? zvn}~HAB`{T&zssps_EVCqd9iyMQE4NuP)VBo{F&td5ZG<}-1=={=4hrH7s=49F@x=3=meeIGF22r6Itn}6 ziH14=$x~B>tV8oPv*7FShTgS7fRQxZLWvAl1{T=~PR=w4Vn1K%Y4G{-beWyc&9o}r zyJ5qdXVT7L9i^_<%ScCil|lP2p6ki2qfTJDrV59A1Q{9{66VDKtA5A{(Ugxd_doa5 zf?1D!(0T?&T40nX6!+O7(E?%J|0Pwr` zq$Deca59`lcJX|&O{ol+!(d0p7{}Dj3tH{a23e|o)9>wc>t3czkU(F+AZyy|mGWE8 z@DcIbcTmrgH3Rr-|4+C1$M9QZWXj>%}viC$`YzT(xK4pSk3!LbrsfxY@S3fB(+X&~r&K>s?W!xwq(~FSx{TL4X1< z$ATX^{{DFF1jsHV290S@a`i~rg_dieAtbER-zV&s8v`WXnn)civx+&OH>o|dO7ScDkkPGJ z*oz89#VmVoo0;A7iAyutir+OpNf-h_ePY}$oA%5t`CJQ%x^!h}AFpvKib6vcrUOC2^0Tc;+pB>?ST27ruz)*NA|Y8_I94IaoX} zv-CP2x0oa8g%!^CrrV#up6R2`4iE=G`n#kufWBUOmT(EtV)C0 zQJH%+qADqUE`!DN8p@%(7(w8^S0l5e^}gKArim$g4#Vy!x$_>((&>HsrG6G1b>8Nh zIWPyZVdqKbMD2T-78e&+zi?&toB2%T6&60HdwGHE$!CfGhiK`+_X-X^JoRy^=!p1m z&-Twn$+Z~g!>yzqWA3eKbzVR3FR?irv6!4Sp9sCGCPQ0R#_%q~AL{#J7eCuV1=NZ|yn40I;Mx$PdcLbks3IJ2J)e#8%g62|s+Iax>$Ylk zU-uJyG2`CK(F9PiSYdS?u{8B_^sB6_~PnldH3@6oad*R z!A*E+zIxwJDkcV(Og_ZopjtFt*B2z;@MCuLd8A*uiF5I}=!UgAyBY7dU+4E5;rFRD z>($a2lgho-1YZAq$yT;rFD3duQ1IB)|Mj`;#;sx7yrH(zGvI_C1=7 zEv6<2tOs7GToj%<+|Ka6>)yXZQg%*Gsld4kUp1O+`R|FGDK|0$%4_~4={EO~y~AOr zu&%tfLQ*3O9K#+sb&`h@9i-&iI(l-_*@!tZGBP%IWa;kH zp2|eDWtGSYsroSaq0~ZK@EnZDl1@F=zDMnixNTNr zI_)aAhI31!ZnG(@q&mUN8>3(dq_IW;0LmB`8NE*S9hFD6*!cPRfw83AnLzd={{NT6 z=p($ixHz~knnlJKNY1&1uV{~E#q7+tlM7@4K>Ci?B@@?)U7}e{!6Knz<2jt5jrqsk ze_r@-yCt}=xL7+VD5x2;ySrO|SrFLWG@$t#r;Lmlu)zaqdbKV#_6}sKUUa5j+;v)- zd;M9^>AQZ(tp=PdF^jYZtw-PHexRnNrVDI3SFj9-u@6u>3RAprpY%{^Mwu;d#rS_S=`6kRV2VR(4B64!j<`YRnM)CtMKo$hS55F}P zxVHW}g7&hh*c*@>e$s<{1oI&;DvNZwyqV?Oj#N=nS~*Msw(vCh^y$+D%v5z$jTc31 zO3K)#Fm$VKS=Y>L3Ni-RFZfjg&C2~dB~knP`{qzo?iNHEE0kM{MOXwj0pWYW>s#Zj zkaa3{Vc|*>W+b3plrAtupfiHl-Bgy zfeQY>f*%|jB0uyh>uYpk2CjI4Z-RR%A#rN1>sLqGfZ}}!RQtN^A(wB|A9$5XUaT%h z1Tz04i;LGKg6W+*cj^!RDM_;aP*HQ8O1FJ$ssY{K-wzoBV$?35c2md$yBx4mpO8CX zY$ryT{{o)n^Ws=fPb#+l{P`0M*?=(9!6M(Ut*A0Kj10J$(andgz9H=dK3KnB$l63P z@qjGIfucllChUR`U~bllr4ThVZ3wKLl67&<_RGgj}Pv>^wQJT9$bI$ z9oV84^B$z>b?KxRFs-;&qPfiusJ#LHyQM%Viq3s`x` z0|kDqUEOE1wX~dly}JHfPWuXB$Lc%u77L{u60|0HvfHx%hWQfUdA`4`C{LE8pX`{L zkA4mGVPyLEALm#JBuQS`_2zy*0%?U9`88~1CN_=c^h(OI6vl*-OU&)ORP>_Cm-IWi z&Cy@Div0S$X*egrpP_@X5Ol%R_&;oq?D|HF?|R8!$* z*v+xK74aVaCFsjQ-$CgI2zUjRNF3iyx`X!r&?r0kY)Lo*N z$|7QckA%7@LCa3YKv@UJ?$27a=BH--2^iSz`agOsjzPv)cmFY6t zJP~`k+e$UD0*hmnwF*wGo%oDD?cI94;m$qGxNky_WUCZ;jhY>VJUjVrGP)$m^h19& zhJH=diG?I@v}`6dBD+E?|333lVOaV$wI*xSm zYfkw!d0GUuv{)gS8nArDYhM2Y)y^zyuj_ZR8y9(hLg3e_#u9c{{KtM4`-zr>saf)W;)!3UJP@>~(_-65x$@fK6 z25c%iedSEDB~IR+Y=c>t*$Kcsvq_>j%8Y_QAHK!!lYs{^8#=Cc6IFrt1ce zb^WYLvgekah_fDuK4`@&_1J}}0#P}m75_fX5ik^^tk8JCOH$2Dv{l2z9~r)GZFB1E zJ~lr5tB`>4!3Md4zok~~X<@~ZP}+R+SsccsF6SQgF9sV2<>6VW@K16eSVmanA%jXL z)bPf#n%3!oR(QRT`H*lT4=I6&6x-MKH`9#&>rgasSdip&*3A223Ccy)bgS~OlKncnE6 zM4!ok#ys-5WySP$^f&hTJsffpYcXu-q*8-v84ppJ0g?o^2FU-3ZTTbo{^Up$$|_f( zqt0JFe!St%-`Im~-_a4lXNFY=X--607am4dBhMOzRJ1Vd;PdJ_TkuBj-wMDaL3F1F zk5x3ofeO+JkIaJC$(h1KP(G_+P!~*#oN3`FqrC*wdc12y!g@kR^_v@&71cEr>!~-< z$R>U2+Jj%fRqCnmW7WOBN|4g-$zh}|{SXb*I;_vm0D|vRP#-`3jH@&{JU!Ex5cJJ% zS;lJTY-*ky4j7VTC83VU8eIw;)BI3|F=6l18FGh}k7cZI_ipH+9BzV?NX*M$t|??~ zzeZm?wTNrT9reJ@lVKy_6IX-MS2B=HtP?PqMja<;N@-n@&bZ+G z-uzBKsxNcGybm>UpLitQ;EJNdI_12&EVnOM$<^00Mp*Qree#R>Kg(_YUI zv;IigM8F2fs<%mY&{834qwMsEayr|vS1(Nm;tZPH!<5VrrI%Rt^!ogliWeRrq?qhFpRNNGIIjs2$HJJ!dfq#J>%J4mT}KcIar1h;wA!S z1xRNocb@_V^ON_u<6I6{ce)81Ifg~JJpc^FHzBL7B2$E9-}FL`X2#&}B8dQDCLfw#AKWZ5s6Nd%rrldtvQ>%G!^`|e|(W}Hn7olV~tUN?Ys zOZ+-aFd?rcS#5!UzVH`7o$sHX+&ZexI*RIe3IadFpZ>Cc+I>H)#q+DOP!fCs!r~~V zKIAMqMF5F<5G_>}e6pTWXHu03K$T+3-6M|&55;@LBfQfn(tuwtw-&c3=zD}Didqp^c z(-+`@B}Yb}$e_cYK54N02^H^QflI>2K&Z7GfBOsRx_+vDhE za0JK!?x=^1oQ3<<`%Pgg4e?SuR(GZr$ex2Q^ z8`$Y&C1H1)+!S~0jioJ3X}fjDq5WZ`J+i8Tzxj(0&bQ8EZDzV+nDr}HiVW}y41N6Z8Cfh6pXvPS~> zZ?HbLHsL8I?pq&LtK=e;g>XhqJ(E5qXlI#Cq3C#|%~JO^PV<_?Qd$Pph2?W`!{{X> zx&lgCWb0j3;S%kvx~-3csy&(~CTp@J)Y=yw?-gwVZ@F^SAA3keoSt}|U)z4I5wW060N}a(+fN}9 zn?m@ebnD)IRv+yk^g-B1eGh!+JrX`3KZSt7pSH^n?QdZ1;PpdLN+mw$x#9 z-7vJ`o-gKx9l>#=JgK|nK4?2;$29o1{dsR-9dO~bi(Bw5E1TFlPgUyqityfyzbJ+o zwV^#f6S7z<`1L2@eft|?9z?F-yGhRLo)vy)4850wVrpgWkaXYurwQsRgsbT(q_IHH zZ@wEG!d+19v;&NJ2CvUm{pDm$y-TAG2`EiwD&a+OW)0)C+7wmd!xtl2xz4Dr7TnbrMj$>+O5^e`AitPH!19T9T=m4C-88SJM(PAO8Jz&osX(6!NE4qA` zMa6}W!3K6t(s4g`q&|Jr6Q9t~KtXjAk%~W3yxX(NKz3<>BOeo(mwSYQR^H|Zp^>m|$D$76Y+XfPH;dji3B^~Sm>TD)$>M%*1b)Vw7+`A%z zzb48ao;pj-$Kog4TtBs>8WD(#KK`Y;$#%7`HmjC)`J`p^-d_1`@4A%hj5yHhA3i5- z`Ufo4X?Zg==D1X$V| z%Et9iP0G*k!)K5Pl~nfD$GTh5)(r?xtHg?-jo4aqDN;itVO@yc*xSGc$~RJDPy?Ih z8$+e#EDAGE(YfXF;osL)s1`%5b80TvXMqI4_r*kC^IYI0si`RMPGR~pb3e(ct;u@q zc4uMW#O zNloSmX^%nSTAN(eyd)?+u;QRG_D&$qsThGNLJrsaq}4##J2JC32uoKCeR3;;o*7W3 zd4fqnXwN37**D^3X8eLv7N&GO6nT$bV#$Wj-wMCX#o124;f)X{i8c0VReOE?2l7Xm`d2AW2 zh8ZN)njdd+X@?#XG#NH`?$R9lmR?kwePFMan`?|_E!JupL}T9}7;&-*1`M$qDk;L^ zD}HRQX*u@@8VTv+1-nG;!}JKfU>jDKq$y6(4xaYE54w(anKPcA+{5ix>E%JWULP9N zbRAVGi1KU4$`Q+@m^Lj#2XtZbde!d!sh(?J*LRQTA>y z(~et^LGwIYJ)-~Xpbhbg1T#v5B^L>&_-ggRxB0FdX#GS2CnvoyhlLW4@}U$4R%026 z)t{Um(HZ2y;)^hFCfc`-4NaT=mW6?}_^qKcUhGMnUA}4Ke6z2pBlI|kK9;FyPn65> zNW>Dpr12MXM!l|w#DDYjAH3{>zZezm9mWJ9n8M3QsF0RM#(4V`Ace4kNt_!&G}J6F9}Mh9_ncWpTLL12N4`Eg=Tp1r|5d zpapM+g^D~?rpNbS_Wu~ZmJDFq{5mLVd`5sLEii26Cb3W)fAEfs(x+2MEp~uiGmx4MZ@i|5-SA&|`%z)gCcQAJov&jlJ_j;v zOQcZ4zS0Sp_L+?A1B8aL0=dPO5G0#{hF?`lpCb7Wuu;_KM+(51>orxsP z>b}fw`rEEI-~Er2nHB%Oj`B49xOM^!m_%(^f=0zC?W$o0pElQ8n6}!(!Z-QR8LYB$HtX;-2`T`+@!JTVxdx{eYXw~cW52hMsdr6VwX!ed6_*a~HdQ3A> zN;Wg?e#?;6r6V!1q5#~FVC-P-MBs$5>|{Pa>y>fCrWzTf+L&C2{+3hqL)vhoS|||Q z+6Aol4gO}I6$eXCq3SS!@$*$Aki3&BOHS_4*z{peaXos6e00!mtUgx+DNb1P{7`jt0;-V2z)cJDH03WKX`TU(4tMeP2K70}pe zmfK@@3%FYaI=BbMxoTD>eM^6w_iRKWwlKE?PrB)v^rUm?+t4f{vyO=e_sr!2s<0vP z-y6D-hpbyC;F1c~PW{FnAL<6yMQ3SowlY*`szD;(I{Y}*-yYd1e~|9KaF3%^Sv$^K zCJvFod{H0h5Ero-c_B?_ZT<~m5*j4Y6a`2mxE{Ikw||#IxuWY$M_gAts$jm?1%N=( z{Qmi-p&KfFz1JmGVF`0|f|&G}SPf@`AdMHxfrKm$Uc*#aB)MC3hhtZQzkTjPDY}iO ztE#4Ao&~rZN9{BQ24Ok)JM>0gry3PP+uIE}jti#X(9-9ZZJEIkpeF^{@%Vp$o@QcJ z?meBbUplI>-@uG#$xI#xWlZ}eNH>`tlcw0m;^K)d@!uL&4qhH2fa*EeCZ~qY;2w(Y z-UdkX=NCr>Vq~Vy<;FEGrBpHnY|K|C5QZXoNr91IkWoI5>jFW5M_LP(s+q>FSYpNI zq-P?CX7zyWJ?kyHVb0n3|XSMwMZF zG`4CZ$!L6+RO{3J!_Cw;q#f0DRd?q7Q?BIey+5o(f2MWMj!2YD%v%nNOJ--@AN<{t zFZ%wccwM9j{T&|5yAqc3na!gz5>I27lVdp9^%GS~C_9QhzG|hZPnwG*i8*HmjN_hw~Jf@+lICluoUz#QG>G_5jJ-+B2*XJvpU z76g~eg`TW!>u18Z-hauwZ?UiN_3Vb@9RGzYhgO4WMkQ2A(Gr}TKyK~U3Nw}oWz8C71AwlmMcJgOX`W* zgZqClOi!7y`HY zS_@^}y2VE1Emk$*{Yl#N9;-k1-=V@rS(3`FGIwht^ywb>IVUf4{dnCKzw$BartrS@ zB`EWZ;ez1eU+ksIy14h$L;KP*muWUO27SUWXW<@BTV>B$Mca0m*Ce0%S^D{zN8avl z3S1Vj>ClYVkRQ4#E&56_<-tkd-JXs(;*;0Ao~aw7lx;}vFE**a+D}bzrn=3~hTFH( zGCnejyk#!wl5n>ct8QTPL?u`r9TUyZNc>%YvlUCKKe(L)(f%pCfVYd<_{0>K7bjFr zHigThf5-V1k}~j!l@V}dL=S) z+MneBce$}0^$A#t^H6`APKC#-_U^-vw|sPe{X&8*B?QK6Z@PL9WiUyrk1Zg^pIz?v zIo@V8aW!-z3Ur?(RsOu0kz08!M094{x6MWJdufH*4XTk1=kI>3c2jFzzv@!iNhzOl=#PBTR=PIEGs?=~6VER9sJ75b)2+|XnKGNyJx-b!>=o8ePgvcf zU;H3kx%_VMwXj;i&8}0IA@_1blx0Gq=0i=_y`Vcu54T^2cB{Uhz}fLHo$&OpQ+7+~ zc{{DJF0cKrRqvisuqCd>s|R)e=5biTpKRmCjYVw&c}qb8z3O4(xcd%ww(p)RDMYDR z^XkJ_t(})oKaPrVI&Uyvv7JYWQvhz@kKOSa`=!$4VMyf*yjnEcsE;~8`b3JO;mS%E0%j>npQ*X?gVmZ*hG|;vx!|O0-fLdr~E>UIdC0KoT6WA zjaZEBjA6(bUf59Q25Jx&(SM*KIMu^gmR);f_w9rOxcz6T=NE&33wf8!`(lZ|FZX=k z31&_C{HP|6C(Tq$_3VV*WmBV@bc_9`J&a%akr@BK#f(1xw-(L?1WL1FjCn2fpYP{? z7*$}O;rd_af5aYLko)Ih{{5)AeeLMl|2i*^9G={~1XSta+&5y&1Nw>ASh}2?j>6xv zX1fJgb}#hn20@kO7nRou_pA~lyj;Hh5;ocuXSVZBer3M#&>QP_=ok~kuiT4%Vlw_^ z>o~MjYHdj=Q_Y%2|cUqp*7g-M#`sJbsWFG@v;#K{fs%Z~+_f!_`m{vnnWG zC6+b{N`7S$;3FkE(Hjm~1k07meU9TlT0OP+_y^e#`$zBCuIl(^MF-&H?o!YFmRBpG zhD%o19=*D1F9!H}ZoPDtdzyRirSn6E_wsIYVfs?g=UkqTdsfPCs>hF(1PEP``!aP5 zsikCGAEzX1BmPonz6GM=SMqU>-)hK++5|ULC2Yo0Km13-xMTJF&R>=*J00E$2ii?e z__yUuL|i)kcxxhA{$x>XJR9QK7{hlw$*~uX_rYNybiSI{Akz zB+!^Myc3ou?Lj_n;jO2g=BPuzG>IsUz}W+RuVft#-X;W&n27vRj{BULTT1D}zsmn? zVHyW4H+{%JCO&X&91Jyjq;_=N5v4S+diXW>eP+H+tYgF%O@u?H#tQ0?OM9XJzq^Y z__J&HUF7A(aZCs-S!Jv*|DAn8AkR0T*rFad^bA?BEpT4j98z?Ms2MoazTPnNjqt(1 z`wTbTB7=pn@5c%F^%?OcO}X>(482S|YoBn4@Q!($nC8W);Z)8vXRlxQM_h-=H#f4z zf$xjQ^u}}iAX&KsBV^z{F{#sa$y_rDB3QTkT2IYBVXY+X2mZF8YtwR9f;_-)z#&%QXBL9YI9@H=ThpY+W~SlBI{yB zNsYLPplMB)yowa1yU;&fd(8>%^eS=h01*`L*yMKx$YBKTRuuu$ z#ffrZ`{h>9gUnC-0$BH*GPKT5U73EA=G`VW;otT-vl7jt7Gc#% zy>JI*Ie+Nqk*9$st;@cT6n7J-jMEx}z%SAS46rJ?i>AHiD3(=@`4D19FeV6PFc^e2 z^nG782BF1z~QTYj&puZE^C2LlZY8hni-9MkmbEMmS_Z2DMk zDIpC@K(5Qna^~`&)lAiQ2M-vyd|Omq&oZcRl~%_$fH8+W9%w7xBg!;Fo_Xt`VA8zj zWyUC2To`5Edi`$Gwp+(?+}f~aQy;k|lU|10{#nxk>Vy^@XX4kmYMK>()Ol)-NhmLP zUA?h$#(diPx3hx)e#Jax5+qj-hs_8oCgozD4I8=np%*@{`HqT{8YJcEz4j*1q@RkB zlg73&cG1Na8F0^>*UUkW3%^!4it7&Dut(dZ^b`ml7Oe!H>^dH6)k^y3m>LDbHDqX$ z<;*jFQ0CakHG~ukiz45qL09#Wl4h$4uQD37lzo%Nyk8i%p|?HMZ|ou;@OxdL2@KTb zm%WP#n#iU!GialhLho>0T3fMiHlPXx9}};)_u-3Zty3WD z0mXYDTGo~~{F+{{MB6C))9u2OpQf*i>@qS~^uv4-R^^rk4H4N;Xw{sEx&Lz6^~XB2 zNDAXd?`#aL*E0nYmr?A9d3w-(weZkpRU%nG2SK|&0sp+#CCFPWSL5$h-CIC~YAC$< z_z+#Vc5J!Rck`)!=Pb@!-L3$y}PZFr_ZBL0uEcALKZi44!UZD=YPnG z`Y()${lH0-)U(1A(hL2m$@Ho4e?M_)u1jwSdaa8W%$auWj8~s2vebifn4Ld${FH(LQhu{_PYW?la!e@D;StST z)BP2Td@fFzhv_NcI9m8te~8YG9C>#$RWPYgQm+PfWO^Vyldu;;ND|#oh{L@~s*~$a zK(3cD7(+<>fIVXN?|3fuWTs%f{^{W>^p20>aCL`co=YTbz@`>lBuMp=h1vN2&-#P% zwP}M!$Emw|GDTXVK~Bc%n&vue9rL{P_GkHrH5cUahW8h8Q%~Ks&HO9hd6;71p>0il zoXZvUQTxSCYr{Fnh8XA1KZ8VcxuK^S@^Qdv|+ zrnJqYW$noMTYhiBIO!+1%2V0gaaWQE0P ziqepCUO=Ce;S`=qXIetqfB5HB?z7>ENBvr~x81QzB2n<(*?HQYc$P)`&7&{1v)r&`(@O(g(iADkIAJHTl%gi|)fH zQZcOa3J)#g@#}smS2>yLLPWBDi)Jsy=K#$NGhO7o$FKZ+&=lvhAlPWWz~Q?KbM?V% z8qxk)Q>j>|gNZ+qC!Mwvrwajf8`p~uv6*^VP>IrEgCX$g+&9US{(uyI#%-A08B1m# zXl8aEd^Gnn^-QH`RS;1W4RXzb`L_wQ-008XR(5qUWX`&Ik>NHP|9TZ!fNmGO6n>%Q zvt%Z3jnv`+#N)!zNo;*x=Uz#^qMl~?83SP)-O(+IKpcU!1J5*NJczo)7B1D0kBn~x zR1qM*Ca_%85EMJdiQ&hMK)$M*S>YQJpxAwx4Lk7zCUW4lhH3&=*jG z8pTRN)bFzn3oDPU1YBtPYPz8|gb)a+xu>yPc*w&=3|uPSo)7-U01lVx~KE?RT!AlmT%?n&xq@ntr_? zT8i#3^4^h@vM7cb#9+F*Rq8~uqiF4|9y6ULQgY&}V6#s@M6O1(>*?%BD^5S2!i#$C zKD17G@KJlMjfbXGa3tDC1>AgH*jrx!M^jU+QT!JU_+3-(=*er|qX3{Ax;@X!-g_HF z_hz#r@A4C(;MDX+O#OGsO#Zn1Jd=_fwFG1Dx3wzvSw$eux4E>Gw<0P0hF4<8m@rG@ z`Q;8}<_D7JPi*U$uG^w{??bo06nf0oj&$7FyPYQJfxT;c(Q}AxZ3Q1eR9yY4qvtNL zei0ZUisDoC*-+fQyF1M0_aB%RJ$u^fH_{IeRo#;g)b~2^%)=Gq?aZfY%g%~TT8f)~ zNu3Qq`HutgQrtlI>_3iNMh`sosy~5PZ3Zq@93Pd~!*5of9`3a^=&X$d%mj=DgGBcX zD39pnq~QvkrXhCZ_0x}$PXmV|b$IJxYEWM%!|J}n9M^R?^jYt$ zRa#_W9OyAI$ZxA&yPkL@y{j%b>qm%SKw}m5#9fIwGb5$(!C%mw{i&g%-o#a(Hf)CL z9_+@EI;^tq9YG|>GoXXDE?#?WZ|r1#UDw`zVoehM0)6#JZ#)ZzP#E3;Bug=B1*UUi ziH$9qSIa;B%mq=F%l$;1LgiMherEfuyq~_qwq$Zr=ZauuZZ0ohp60iSlbvrOs5`BF zw+5x4LvT(H7fd#Je<#2m10x7NwEV_*xzX2{>AcmaRIOJr7c@fmkFT2->&$aan%bFR zm|xV7%R2=7OSIIjxAql0W!E$bR@mNgdDJ$jUfpEIBw<4)FoxHTZ+!8&&LBHbrUh7F zvk6{2l9H!v@(zsKUw))kP^_ldIs|>h5tM&8tGb>RSueOnpE4mB)=FEsbYBh*3F(x?US^1R$Z{dcZn*`vLG(q_-U;Q zx~ArjB!A903_oX*bCLCk)05f%5jhgZ?T{a{fHDKx3K>ly0$Q4^J4!we)B|H#vJ*g! zk5J2U$IuBq5JY<~h5OkEIj0l94q(#K93XK%i~W8#(OQeH!x38cn#I*+b^J}?Dx~~& z9CBH2(FFZKD=o{_!`)%RyA$Lt+hi|{!NI`lcCTsFehXj*X1sc~qIH$&-KS|@{#+Mk z3V3%Ucd5=+l*X}9@8$G0Cc&o4j%{DeVif=*fM2(;t}p$KW|-8H!x_sn=7N8&8fXNc z>!tI9R(db@^EVG`xKm&}6sNabfMz*6_7ggwGkDr!+p8kfnT zpYC(xbT7A$s+`M3+b;8IvB~3pT@on!x15*v8iUY*D)l$B{fOoxc>gl(*N8E6BUbXk z2492Lp=@6%gb?DclkQjuv4v=akA2L*p*uC&Tx7;({Fe7mbJtx7_f=-bX50aA$-WY% z@s*W-?o6(#EHg|_IDii0Hsb|AGx66izo~ATm^G09+QzcIO#%GR3@~7K z(ESftvTw(Uy&VVLB-j)td@w@vuS1v{UM_3S?xxf>>4D55MtAlfS(XTtXApZXDjrcuItUr01RIc^ERW^ zo&UZ{!}yxox$>DkK3}`uqqyH<<+7`pn_mK7mws*%c{NSwi8Mp+BuSmiko}WI_RIj; zGXwPgDn$E<0TKr{c0f5d=c8ujveIVrU+}iskO0Yg%XBlkTXnV)h1q(bp?#~eONkllALhGIc*-L4Lufz#7WXW7~VMmB?(>EdPHE*`fYg?huU#I*(w9)!v z8oQujHbsdw2l3b6!%ht-SPZ=!Csk%v>WT^_Gv&5J8CEL~bN3Uq71gaxbqx|i2(jsH zhfA*zLfjcUbZ_7@11Y*XrMk}GdVGiOS$(a0Zd{S-^XRt=^Ix|s#K<%%H687mDs6s58yD4{r{kn>2=!9Vt(qQ z^nVa1Q0ZM*Ay5C`^?L8{ssXMii63nx^m?SS*=O}3LVGkamoj9s4*pAN@&%1hOSqUF zKP?A+Brc4RK9{F^Uxus$+JO+EEjmeSZFFW&nLXib@4$(jB=L&(Sa;~<6hqG~>fZ`E zMqh*B~K;5SyMX z+98Axt5xI2zuSAE_h@#MYQ744L)4o18vtB#7w%ko-CUN@*+)&kTO4Y9&yT+raPy30 zmwya1cOi=duq#00?E9x2i`<1Q*$a8>(pi>uCc%fh88|&atR8T4B>$M(h%6Nu`0w*x z&TnV4j4k)L+eC(uD_&^8A86dTQtHvX)o7Ke(nF&DKPxOi90!eG2`SAO^>t-&u z+_FI~*7+2v(>Og)y?Z3RebOVV_?AJ<90+SuW2u+0!Xfr_In10rN&Svd&gZU6_`j7r@B8H|QZfOWc;!ONii z$1vg2cR}tB(|RAc|2s+I^cd8bgqtf|uwWZJM-pi9%8BbK2Ftd2o6*m<{<@^@>?;Cr zam!`%`Uu(Og48RXXFrh^pSSY#heW#;dU+!5IfXH>%lhquT-$(LY~3ryBNG&x<8lP zYhA?8c#k^=ACHv;m<*!Nb`a3O{WL+x(G*tcrcRdrr;BB0MCkr`aQ?f!ZA5l!P{=cI zEJ5bx!aUd?^)Z-r0iDMCR~L|2b9wLUqWd8O3KoOUchG;Sbin0eojF3si7YM}fhW3& zZs_BW>nPJI*r|KZmbq4*d=ZlmRT_K)Va`4$J71g1F&@w zjudB4%Ercq4Tz-e{y@#+c&e&L$W=7p< z2V!*mmFBhiqn!);Jl7)iQjGXN--X3vs!aP{qqq99o1y=eWJzD*VxR9P_V6#r6c^NS z3yUIKw>i)L$GV6Zz|}#zo*!J;Urgi}c>0LfPc^g>YuSvC{2H|Wtk~v1?V;;fdQqDn zpyTN`%eVOz_8EIBhFJsl={Oy)E;9_#U5vX=&k1&8tP3*s|i#>z2r$-*gU_#f*ay8HIKn^U7vYp3XHPSd-0 zKLG>mu@SOk-tD~J9H;kh7Y5(7eccSany2mg6uCEI>_6=%xYvjNwM{OY#c}{WNaySQ zUchJWSOL0!*G=Z(1ljYwv<3$7|J8nc_tfH8IkJ~6+ydy$M~MIRDkE%bE^mYAi2+7$ z#OS|}Cf*WYpqPKh8YO$#TZPqpsE2_Uf*bzsq&1gm6$B0hF<#4KU+Jgy{tSV8H5#8A zrsv=~bG=b@wuX=9J_EkYle=Q$?lLhoYheHu0FA&4gY;Z& z<;eRMsh7fJ-VCxoU{YJK8M}}rZ@XZGIP!RC?zLWXkMe2(I{$0CH^T>DM~L{#G189@ zV*O*B_`wi;53Mp_q6G1u;9wa4Z*pGHp(#}P4AHtEp*j*~fbg!I)EUCn-a_cUVGPGbEjhc%&L7#dF1XGYy& z4Kcd^c986Ad93$3X#607zd?ryi~My5bQ9CCuxzkzjI&_n*V~w`I|TLWPjJTw9Oj^dW~Dr%EnrAoW7?< zX!~7`^h+TEZw1-!GvLcHM*deGrvTcX4*Fh=mb@x-9oL|m5baNg7cQ8g5sO7(K7X2z6o}o%!esb=dxHcI{pVc8TfFRMDx<} z)An^U{NVtd5Bkw@$bXvl-uHCLg1P{a7Y0ecTke?4SWkJ^g-D!EGw@6pzdnO?!OMT< zvJJa}#EuP|BOi>@_p=~I4Y)URWH02HtkTnX8&YShN+P&{bQ%m4bH$&%! z=-gYZKy#1sg8y$5bnjhQvGj-H#Aj#xRP<=CM(BZJP;qydaJk^=o-i#9#r!sh))2t< z)Bc;|M0}d}8h}e#vX;ZlR(luTeI1-SJwQj3iB@p1E@jAkn!~OM5<4+W-=nj0Be(MM z2s)u-!wkI?!CwbB7Wqp#@;AY3?quM@G(8V^VJADAUGzS~+n;CSp+1I_38MRq`F`r7 zEY_5c|DkRM&kYklu$e$h+$a_2MNb@VSIbPuLT?5lnq4nE&Qyv0Pr z#U9I&y=*Z#`wqdn0FCt;py3PnCD6Cs2UTa$}(b&UK3=!S{Ez6!Uh zQ#Sd}EF8_izuQEw_PzI%jdj(=cD*#4^%bt*cOlOHgZ<=df+R9&I(@}7U-f1Om_9$o zj@9kTov^U3+c-5kX1yO{wZ%7OlfP=?xS-V;_!^A053|=>-USEys)gl%UT0vgCoNU5 z<}zK+G3`8K#Th_8-v;D=y>)h@6&$QTS=eqZM!g@i!i7v=5O!PRukhSj&9Ev4XK zUCCn?bc_Z+<`y2+yEpTU-B?)PGcPvM`i6zp+{5S}J1SYYd2^bp(_qy5FzZ&?;#PT{ zUE5;px{X`2MeD!`;U+*7<44_^fXtxQy1sDg=IVNZ3A<+>Ai1n6=_-1 zHE1>ux2kymavRRoT>1MY*TMdOVC>pz6+s9gHa`jY2_b}7y>bCRA%xh@3O4qHfl)4! zSIE)Z93pWU%qNE#eKmB)lPB&B=cY~VuJQpj=X3{w_9RYCh@OkXbX55j3PK1W#9GKB zy+Q~fgb+fMWs`ofm0+ZUflG^*9B|0~CQjcKpvF(+P<0o^5u1m5rJs)G5Unq#m%N+P zy_%%^SPG{A`j4WtO0Z7|A;cyji*^Vhgb+dqQI?D2xXg?tInt8m&89G+Jtlg=CU<_A z^kr{V^1xF)967M59yS-%bZe4H*CzYBHuj$m($XBntb>{BS<)Y5v8DhcO#E1E>p#*h zgb-rekbs{MLI@#*5M`M}Ur%EM;v`;Ak^Lx1{-gPCH6uXuxn2@a24%s4ZA{x2XZVbt z#IarmKgyDNJ6rlKW=kiDSG$Qc+_fYMA%qZ1$UFRm5JIe;W0AdR;i6$42x7{F79qqp zMZv|ooFVs_%_JH->jMP$`7xv){6=x@JlP95tO*xQH}KydB2X`tKnNkk<|6?=A%qY@ z2qA literal 0 HcmV?d00001 diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/launch/lidar_marker_localizer.launch.xml b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/launch/lidar_marker_localizer.launch.xml new file mode 100644 index 0000000000000..78c5958df4775 --- /dev/null +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/launch/lidar_marker_localizer.launch.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + 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 new file mode 100755 index 0000000000000..bf60d96311ff1 --- /dev/null +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml @@ -0,0 +1,40 @@ + + + + autoware_lidar_marker_localizer + 0.0.0 + The autoware_lidar_marker_localizer package + Yamato Ando + Shintaro Sakoda + Apache License 2.0 + Eijiro Takeuchi + Yamato Ando + Shintaro Sakoda + + ament_cmake_auto + autoware_cmake + + autoware_landmark_manager + autoware_map_msgs + autoware_point_types + autoware_universe_utils + localization_util + pcl_conversions + pcl_ros + rclcpp + rclcpp_components + sensor_msgs + std_srvs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tf2_sensor_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/schema/lidar_marker_localizer.schema.json b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/schema/lidar_marker_localizer.schema.json new file mode 100644 index 0000000000000..31a1a8add8cab --- /dev/null +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/schema/lidar_marker_localizer.schema.json @@ -0,0 +1,149 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Lidar Marker Localizer Node", + "type": "object", + "definitions": { + "lidar_marker_localizer": { + "type": "object", + "properties": { + "marker_name": { + "type": "string", + "description": "The name of the markers listed in the HD map.", + "default": "reflector" + }, + "resolution": { + "type": "number", + "description": "Grid size for marker detection algorithm. [m]", + "default": 0.05, + "minimum": 0.0 + }, + "intensity_pattern": { + "type": "array", + "description": "A sequence of high/low intensity to perform pattern matching. 1: high intensity (positive match), 0: not consider, -1: low intensity (negative match)", + "default": [-1, -1, 0, 1, 1, 1, 1, 1, 0, -1, -1] + }, + "match_intensity_difference_threshold": { + "type": "number", + "description": "Threshold for determining high/low.", + "default": 20, + "minimum": 0 + }, + "positive_match_num_threshold": { + "type": "number", + "description": "Threshold for the number of required matches with the pattern.", + "default": 3, + "minimum": 0 + }, + "negative_match_num_threshold": { + "type": "number", + "description": "Threshold for the number of required matches with the pattern.", + "default": 3, + "minimum": 0 + }, + "vote_threshold_for_detect_marker": { + "type": "number", + "description": "Threshold for the number of rings matching the pattern needed to detect it as a marker.", + "default": 20, + "minimum": 0 + }, + "marker_height_from_ground": { + "type": "number", + "description": "Height from the ground to the center of the marker. [m]", + "default": 1.075 + }, + "self_pose_timeout_sec": { + "type": "number", + "description": "Timeout for self pose. [sec]", + "default": 1.0, + "minimum": 0.0 + }, + "self_pose_distance_tolerance_m": { + "type": "number", + "description": "Tolerance for the distance between two points when performing linear interpolation. [m]", + "default": 1.0, + "minimum": 0.0 + }, + "limit_distance_from_self_pose_to_nearest_marker": { + "type": "number", + "description": "Distance limit for the purpose of determining whether the node should detect a marker. [m]", + "default": 2.0, + "minimum": 0.0 + }, + "limit_distance_from_self_pose_to_marker": { + "type": "number", + "description": "Distance limit for avoiding miss detection. [m]", + "default": 2.0, + "minimum": 0.0 + }, + "base_covariance": { + "type": "array", + "description": "Output covariance in the base_link coordinate.", + "default": [ + 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 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.00007569, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00007569, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.00030625 + ] + }, + "marker_width": { + "type": "number", + "description": "Width of a marker. This param is used for visualizing the detected marker pointcloud[m]", + "default": 0.8, + "minimum": 0.0 + }, + "enable_save_log": { + "type": "boolean", + "description": "", + "default": false + }, + "save_file_directory_path": { + "type": "string", + "description": "", + "default": "$(env HOME)/detected_reflector_intensity" + }, + "save_file_name": { + "type": "string", + "description": "", + "default": "detected_reflector_intensity" + }, + "save_frame_id": { + "type": "string", + "description": "", + "default": "velodyne_top" + } + }, + "required": [ + "resolution", + "intensity_pattern", + "match_intensity_difference_threshold", + "positive_match_num_threshold", + "negative_match_num_threshold", + "vote_threshold_for_detect_marker", + "self_pose_timeout_sec", + "self_pose_distance_tolerance_m", + "limit_distance_from_self_pose_to_nearest_marker", + "limit_distance_from_self_pose_to_marker", + "base_covariance", + "marker_width", + "enable_save_log", + "save_file_directory_path", + "save_file_name", + "save_frame_id" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/lidar_marker_localizer" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} 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 new file mode 100644 index 0000000000000..83fcae2352f51 --- /dev/null +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp @@ -0,0 +1,618 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_marker_localizer.hpp" + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::lidar_marker_localizer +{ + +landmark_manager::Landmark get_nearest_landmark( + const geometry_msgs::msg::Pose & self_pose, + const std::vector & landmarks); +std::array rotate_covariance( + const std::array & src_covariance, const Eigen::Matrix3d & rotation); + +LidarMarkerLocalizer::LidarMarkerLocalizer(const rclcpp::NodeOptions & node_options) +: Node("lidar_marker_localizer", node_options), is_activated_(false) +{ + using std::placeholders::_1; + using std::placeholders::_2; + + param_.marker_name = this->declare_parameter("marker_name"); + param_.resolution = this->declare_parameter("resolution"); + param_.intensity_pattern = this->declare_parameter>("intensity_pattern"); + param_.match_intensity_difference_threshold = + this->declare_parameter("match_intensity_difference_threshold"); + param_.positive_match_num_threshold = + this->declare_parameter("positive_match_num_threshold"); + param_.negative_match_num_threshold = + this->declare_parameter("negative_match_num_threshold"); + param_.vote_threshold_for_detect_marker = + this->declare_parameter("vote_threshold_for_detect_marker"); + param_.marker_height_from_ground = this->declare_parameter("marker_height_from_ground"); + param_.self_pose_timeout_sec = this->declare_parameter("self_pose_timeout_sec"); + param_.self_pose_distance_tolerance_m = + this->declare_parameter("self_pose_distance_tolerance_m"); + param_.limit_distance_from_self_pose_to_nearest_marker = + this->declare_parameter("limit_distance_from_self_pose_to_nearest_marker"); + param_.limit_distance_from_self_pose_to_marker = + this->declare_parameter("limit_distance_from_self_pose_to_marker"); + std::vector base_covariance = + this->declare_parameter>("base_covariance"); + for (std::size_t i = 0; i < base_covariance.size(); ++i) { + param_.base_covariance[i] = base_covariance[i]; + } + param_.marker_width = this->declare_parameter("marker_width"); + param_.enable_save_log = this->declare_parameter("enable_save_log"); + param_.save_file_directory_path = + this->declare_parameter("save_file_directory_path"); + 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( + this->get_logger(), param_.self_pose_timeout_sec, param_.self_pose_distance_tolerance_m); + + rclcpp::CallbackGroup::SharedPtr points_callback_group; + points_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto points_sub_opt = rclcpp::SubscriptionOptions(); + points_sub_opt.callback_group = points_callback_group; + sub_points_ = this->create_subscription( + "~/input/pointcloud", rclcpp::QoS(1).best_effort(), + std::bind(&LidarMarkerLocalizer::points_callback, this, _1), points_sub_opt); + + rclcpp::CallbackGroup::SharedPtr self_pose_callback_group; + self_pose_callback_group = + this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto self_pose_sub_opt = rclcpp::SubscriptionOptions(); + self_pose_sub_opt.callback_group = self_pose_callback_group; + sub_self_pose_ = this->create_subscription( + "~/input/ekf_pose", rclcpp::QoS(1), + std::bind(&LidarMarkerLocalizer::self_pose_callback, this, _1), self_pose_sub_opt); + sub_map_bin_ = this->create_subscription( + "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal), + std::bind(&LidarMarkerLocalizer::map_bin_callback, this, _1)); + + pub_base_link_pose_with_covariance_on_map_ = + this->create_publisher("~/output/pose_with_covariance", 10); + rclcpp::QoS qos_marker = rclcpp::QoS(rclcpp::KeepLast(10)); + qos_marker.transient_local(); + qos_marker.reliable(); + pub_marker_mapped_ = this->create_publisher("~/debug/marker_mapped", qos_marker); + pub_marker_detected_ = this->create_publisher("~/debug/marker_detected", 10); + pub_debug_pose_with_covariance_ = + this->create_publisher("~/debug/pose_with_covariance", 10); + pub_marker_pointcloud_ = this->create_publisher("~/debug/marker_pointcloud", 10); + service_trigger_node_ = this->create_service( + "~/service/trigger_node_srv", + std::bind(&LidarMarkerLocalizer::service_trigger_node, this, _1, _2), + rclcpp::ServicesQoS().get_rmw_qos_profile(), points_callback_group); + + 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")); +} + +void LidarMarkerLocalizer::initialize_diagnostics() +{ + diagnostics_module_->clear(); + diagnostics_module_->add_key_value("is_received_map", false); + diagnostics_module_->add_key_value("is_received_self_pose", false); + diagnostics_module_->add_key_value("detect_marker_num", 0); + diagnostics_module_->add_key_value("distance_self_pose_to_nearest_marker", 0.0); + diagnostics_module_->add_key_value( + "limit_distance_from_self_pose_to_nearest_marker", + param_.limit_distance_from_self_pose_to_nearest_marker); + diagnostics_module_->add_key_value("distance_lanelet2_marker_to_detected_marker", 0.0); + diagnostics_module_->add_key_value( + "limit_distance_from_lanelet2_marker_to_detected_marker", + param_.limit_distance_from_self_pose_to_marker); +} + +void LidarMarkerLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & map_bin_msg_ptr) +{ + landmark_manager_.parse_landmarks(map_bin_msg_ptr, param_.marker_name); + const MarkerArray marker_msg = landmark_manager_.get_landmarks_as_marker_array_msg(); + pub_marker_mapped_->publish(marker_msg); +} + +void LidarMarkerLocalizer::self_pose_callback( + const PoseWithCovarianceStamped::ConstSharedPtr & self_pose_msg_ptr) +{ + // TODO(YamatoAndo) + // if (!is_activated_) return; + + if (self_pose_msg_ptr->header.frame_id == "map") { + ekf_pose_buffer_->push_back(self_pose_msg_ptr); + } else { + RCLCPP_ERROR_STREAM_THROTTLE( + get_logger(), *this->get_clock(), 1000, + "Received initial pose message with frame_id " + << self_pose_msg_ptr->header.frame_id << ", but expected map. " + << "Please check the frame_id in the input topic and ensure it is correct."); + } +} + +void LidarMarkerLocalizer::points_callback(const PointCloud2::ConstSharedPtr & points_msg_ptr) +{ + const auto sensor_ros_time = points_msg_ptr->header.stamp; + + initialize_diagnostics(); + + main_process(points_msg_ptr); + + diagnostics_module_->publish(sensor_ros_time); +} + +void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & points_msg_ptr) +{ + const builtin_interfaces::msg::Time sensor_ros_time = points_msg_ptr->header.stamp; + + // (1) check if the map have be received + const std::vector map_landmarks = landmark_manager_.get_landmarks(); + const bool is_received_map = !map_landmarks.empty(); + diagnostics_module_->add_key_value("is_received_map", is_received_map); + if (!is_received_map) { + std::stringstream message; + message << "Not receive the landmark information. Please check if the vector map is being " + << "published and if the landmark information is correctly specified."; + RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_module_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + return; + } + + // (2) get Self Pose + 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); + if (!is_received_self_pose) { + std::stringstream message; + message << "Could not get self_pose. Please check if the self pose is being published and if " + << "the timestamp of the self pose is correctly specified"; + RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_module_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + return; + } + + ekf_pose_buffer_->pop_old(sensor_ros_time); + const Pose self_pose = interpolate_result.value().interpolated_pose.pose.pose; + + // (3) detect marker + const std::vector detected_landmarks = + detect_landmarks(points_msg_ptr); + + const bool is_detected_marker = !detected_landmarks.empty(); + diagnostics_module_->add_key_value("detect_marker_num", detected_landmarks.size()); + + // (4) check distance to the nearest marker + const landmark_manager::Landmark nearest_marker = get_nearest_landmark(self_pose, map_landmarks); + const Pose nearest_marker_pose_on_base_link = + autoware::universe_utils::inverseTransformPose(nearest_marker.pose, self_pose); + + const double distance_from_self_pose_to_nearest_marker = + std::abs(nearest_marker_pose_on_base_link.position.x); + diagnostics_module_->add_key_value( + "distance_self_pose_to_nearest_marker", distance_from_self_pose_to_nearest_marker); + + const bool is_exist_marker_within_self_pose = + distance_from_self_pose_to_nearest_marker < + param_.limit_distance_from_self_pose_to_nearest_marker; + + if (!is_detected_marker) { + if (!is_exist_marker_within_self_pose) { + std::stringstream message; + message << "Could not detect marker, because the distance from self_pose to nearest_marker " + << "is too far (" << distance_from_self_pose_to_nearest_marker << " [m])."; + diagnostics_module_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::OK, message.str()); + } else { + std::stringstream message; + message << "Could not detect marker, although the distance from self_pose to nearest_marker " + << "is near (" << distance_from_self_pose_to_nearest_marker << " [m])."; + RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_module_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } + return; + } + + // for debug + if (pub_marker_detected_->get_subscription_count() > 0) { + PoseArray pose_array_msg; + pose_array_msg.header.stamp = sensor_ros_time; + pose_array_msg.header.frame_id = "map"; + for (const landmark_manager::Landmark & landmark : detected_landmarks) { + const Pose detected_marker_on_map = + autoware::universe_utils::transformPose(landmark.pose, self_pose); + pose_array_msg.poses.push_back(detected_marker_on_map); + } + pub_marker_detected_->publish(pose_array_msg); + } + + // (4) calculate diff pose + const Pose new_self_pose = + landmark_manager_.calculate_new_self_pose(detected_landmarks, self_pose, false); + + const double diff_x = new_self_pose.position.x - self_pose.position.x; + const double diff_y = new_self_pose.position.y - self_pose.position.y; + + const double diff_norm = std::hypot(diff_x, diff_y); + const bool is_exist_marker_within_lanelet2_map = + diff_norm < param_.limit_distance_from_self_pose_to_marker; + + diagnostics_module_->add_key_value("distance_lanelet2_marker_to_detected_marker", diff_norm); + if (!is_exist_marker_within_lanelet2_map) { + std::stringstream message; + message << "The distance from lanelet2 to the detect marker is too far(" << diff_norm + << " [m]). The limit is " << param_.limit_distance_from_self_pose_to_marker << "."; + RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_module_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + return; + } + + // (5) Apply diff pose to self pose + // only x and y is changed + PoseWithCovarianceStamped result; + result.header.stamp = sensor_ros_time; + result.header.frame_id = "map"; + result.pose.pose.position.x = new_self_pose.position.x; + result.pose.pose.position.y = new_self_pose.position.y; + result.pose.pose.position.z = self_pose.position.z; + result.pose.pose.orientation = self_pose.orientation; + + // set covariance + const Eigen::Quaterniond map_to_base_link_quat = Eigen::Quaterniond( + result.pose.pose.orientation.w, result.pose.pose.orientation.x, result.pose.pose.orientation.y, + result.pose.pose.orientation.z); + const Eigen::Matrix3d map_to_base_link_rotation = + map_to_base_link_quat.normalized().toRotationMatrix(); + result.pose.covariance = rotate_covariance(param_.base_covariance, map_to_base_link_rotation); + + pub_base_link_pose_with_covariance_on_map_->publish(result); + pub_debug_pose_with_covariance_->publish(result); + + // for debug + const landmark_manager::Landmark nearest_detected_landmark = + get_nearest_landmark(self_pose, detected_landmarks); + const auto marker_pointcloud_msg_ptr = + extract_marker_pointcloud(points_msg_ptr, nearest_detected_landmark.pose); + pub_marker_pointcloud_->publish(*marker_pointcloud_msg_ptr); + + // save log + if (param_.enable_save_log) { + save_detected_marker_log(marker_pointcloud_msg_ptr); + } +} + +void LidarMarkerLocalizer::service_trigger_node( + const SetBool::Request::SharedPtr req, SetBool::Response::SharedPtr res) +{ + is_activated_ = req->data; + if (is_activated_) { + ekf_pose_buffer_->clear(); + } + res->success = true; +} + +std::vector LidarMarkerLocalizer::detect_landmarks( + const PointCloud2::ConstSharedPtr & points_msg_ptr) +{ + // TODO(YamatoAndo) + // Transform sensor_frame to base_link + + pcl::PointCloud::Ptr points_ptr( + new pcl::PointCloud); + pcl::fromROSMsg(*points_msg_ptr, *points_ptr); + + if (points_ptr->empty()) { + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "No points!"); + return std::vector{}; + } + + std::vector> ring_points(128); + + float min_x = std::numeric_limits::max(); + float max_x = std::numeric_limits::lowest(); + for (const autoware_point_types::PointXYZIRC & point : points_ptr->points) { + ring_points[point.channel].push_back(point); + min_x = std::min(min_x, point.x); + max_x = std::max(max_x, point.x); + } + + // Check that the leaf size is not too small, given the size of the data + const int bin_num = static_cast((max_x - min_x) / param_.resolution + 1); + + // initialize variables + std::vector vote(bin_num, 0); + std::vector min_y(bin_num, std::numeric_limits::max()); + + // for each channel + for (const pcl::PointCloud & one_ring : ring_points) { + std::vector intensity_sum(bin_num, 0.0); + std::vector intensity_num(bin_num, 0); + std::vector average_intensity(bin_num, 0.0); + + for (const autoware_point_types::PointXYZIRC & point : one_ring.points) { + const int bin_index = static_cast((point.x - min_x) / param_.resolution); + intensity_sum[bin_index] += point.intensity; + intensity_num[bin_index]++; + min_y[bin_index] = std::min(min_y[bin_index], point.y); + } + + // calc average + for (int bin_index = 0; bin_index < bin_num; bin_index++) { + if (intensity_num[bin_index] == 0) { + continue; + } + + average_intensity[bin_index] = intensity_sum[bin_index] / intensity_num[bin_index]; + } + + // pattern matching + for (size_t i = 0; i <= bin_num - param_.intensity_pattern.size(); i++) { + int64_t pos = 0; + int64_t neg = 0; + double min_intensity = std::numeric_limits::max(); + double max_intensity = std::numeric_limits::lowest(); + + // find max_min + for (size_t j = 0; j < param_.intensity_pattern.size(); j++) { + if (intensity_num[i + j] == 0) { + continue; + } + + min_intensity = std::min(min_intensity, average_intensity[i + j]); + max_intensity = std::max(max_intensity, average_intensity[i + j]); + } + + if (max_intensity <= min_intensity) { + continue; + } + + const double center_intensity = (max_intensity - min_intensity) / 2.0 + min_intensity; + + for (size_t j = 0; j < param_.intensity_pattern.size(); j++) { + if (intensity_num[i + j] == 0) { + continue; + } + + if (param_.intensity_pattern[j] == 1) { + // check positive + if ( + average_intensity[i + j] > + center_intensity + static_cast(param_.match_intensity_difference_threshold)) { + pos++; + } + } else if (param_.intensity_pattern[j] == -1) { + // check negative + if ( + average_intensity[i + j] < + center_intensity - static_cast(param_.match_intensity_difference_threshold)) { + neg++; + } + } else { + // ignore param_.intensity_pattern[j] == 0 + } + } + + if ( + pos >= param_.positive_match_num_threshold && neg >= param_.negative_match_num_threshold) { + vote[i]++; + } + } + } + + std::vector detected_landmarks; + + for (size_t i = 0; i < bin_num - param_.intensity_pattern.size(); i++) { + if (vote[i] > param_.vote_threshold_for_detect_marker) { + const double bin_position = + static_cast(i) + static_cast(param_.intensity_pattern.size()) / 2.0; + Pose marker_pose_on_base_link; + marker_pose_on_base_link.position.x = bin_position * param_.resolution + min_x; + marker_pose_on_base_link.position.y = min_y[i]; + marker_pose_on_base_link.position.z = param_.marker_height_from_ground; + marker_pose_on_base_link.orientation = + autoware::universe_utils::createQuaternionFromRPY(M_PI_2, 0.0, 0.0); // TODO(YamatoAndo) + detected_landmarks.push_back(landmark_manager::Landmark{"0", marker_pose_on_base_link}); + } + } + + return detected_landmarks; +} + +landmark_manager::Landmark get_nearest_landmark( + const geometry_msgs::msg::Pose & self_pose, + const std::vector & landmarks) +{ + landmark_manager::Landmark nearest_landmark; + double min_distance = std::numeric_limits::max(); + + for (const auto & landmark : landmarks) { + const double curr_distance = + autoware::universe_utils::calcDistance3d(landmark.pose.position, self_pose.position); + + if (curr_distance > min_distance) { + continue; + } + + min_distance = curr_distance; + nearest_landmark = landmark; + } + return nearest_landmark; +} + +std::array rotate_covariance( + const std::array & src_covariance, const Eigen::Matrix3d & rotation) +{ + std::array ret_covariance = src_covariance; + + Eigen::Matrix3d src_cov; + src_cov << src_covariance[0], src_covariance[1], src_covariance[2], src_covariance[6], + src_covariance[7], src_covariance[8], src_covariance[12], src_covariance[13], + src_covariance[14]; + + Eigen::Matrix3d ret_cov; + ret_cov = rotation * src_cov * rotation.transpose(); + + for (Eigen::Index i = 0; i < 3; ++i) { + ret_covariance[i] = ret_cov(0, i); + ret_covariance[i + 6] = ret_cov(1, i); + ret_covariance[i + 12] = ret_cov(2, i); + } + + return ret_covariance; +} + +sensor_msgs::msg::PointCloud2::SharedPtr LidarMarkerLocalizer::extract_marker_pointcloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & points_msg_ptr, + const geometry_msgs::msg::Pose marker_pose) const +{ + // convert from ROSMsg to PCL + pcl::shared_ptr> points_ptr( + new pcl::PointCloud); + pcl::fromROSMsg(*points_msg_ptr, *points_ptr); + + pcl::shared_ptr> marker_points_ptr( + new pcl::PointCloud); + + // extract marker pointcloud + for (const autoware_point_types::PointXYZIRC & point : points_ptr->points) { + const double xy_distance = std::sqrt( + std::pow(point.x - marker_pose.position.x, 2.0) + + std::pow(point.y - marker_pose.position.y, 2.0)); + if (xy_distance < param_.marker_width / 2.0) { + marker_points_ptr->push_back(point); + } + } + + marker_points_ptr->width = marker_points_ptr->size(); + marker_points_ptr->height = 1; + marker_points_ptr->is_dense = false; + + sensor_msgs::msg::PointCloud2::SharedPtr marker_points_msg_ptr(new sensor_msgs::msg::PointCloud2); + pcl::toROSMsg(*marker_points_ptr, *marker_points_msg_ptr); + marker_points_msg_ptr->header = points_msg_ptr->header; + + return marker_points_msg_ptr; +} + +void LidarMarkerLocalizer::save_detected_marker_log( + const sensor_msgs::msg::PointCloud2::SharedPtr & points_msg_ptr) +{ + // transform input_frame to save_frame_id + sensor_msgs::msg::PointCloud2::SharedPtr marker_points_msg_sensor_frame_ptr( + new sensor_msgs::msg::PointCloud2); + transform_sensor_measurement( + param_.save_frame_id, points_msg_ptr->header.frame_id, points_msg_ptr, + marker_points_msg_sensor_frame_ptr); + + // convert from ROSMsg to PCL + pcl::shared_ptr> + marker_points_sensor_frame_ptr(new pcl::PointCloud); + pcl::fromROSMsg(*marker_points_msg_sensor_frame_ptr, *marker_points_sensor_frame_ptr); + + // to csv format + std::stringstream log_message; + log_message << "point.position.x,point.position.y,point.position.z,point.intensity,point.ring" + << std::endl; + for (const auto & point : marker_points_sensor_frame_ptr->points) { + log_message << point.x; + log_message << "," << point.y; + log_message << "," << point.z; + log_message << "," << point.intensity; + log_message << "," << point.channel; + log_message << std::endl; + } + + // create file name + const double times_seconds = rclcpp::Time(points_msg_ptr->header.stamp).seconds(); + double time_integer_tmp = 0.0; + double time_decimal = std::modf(times_seconds, &time_integer_tmp); + auto time_integer = static_cast(time_integer_tmp); + struct tm * time_info{}; + time_info = std::localtime(&time_integer); + std::stringstream file_name; + file_name << param_.save_file_name << std::put_time(time_info, "%Y%m%d-%H%M%S") << "-" + << std::setw(3) << std::setfill('0') << static_cast((time_decimal) * 1000) + << ".csv"; + + // write log_message to file + std::filesystem::path save_file_directory_path = param_.save_file_directory_path; + std::filesystem::create_directories(save_file_directory_path); + std::ofstream csv_file(save_file_directory_path.append(file_name.str())); + csv_file << log_message.str(); + csv_file.close(); +} + +void LidarMarkerLocalizer::transform_sensor_measurement( + const std::string & source_frame, const std::string & target_frame, + const sensor_msgs::msg::PointCloud2::SharedPtr & sensor_points_input_ptr, + sensor_msgs::msg::PointCloud2::SharedPtr & sensor_points_output_ptr) +{ + if (source_frame == target_frame) { + sensor_points_output_ptr = sensor_points_input_ptr; + return; + } + + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf_buffer_->lookupTransform(source_frame, target_frame, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + RCLCPP_WARN( + this->get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + // Since there is no clear error handling policy, temporarily return as is. + sensor_points_output_ptr = sensor_points_input_ptr; + return; + } + + 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); + pcl_ros::transformPointCloud( + base_to_sensor_matrix, *sensor_points_input_ptr, *sensor_points_output_ptr); +} + +} // namespace autoware::lidar_marker_localizer + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::lidar_marker_localizer::LidarMarkerLocalizer) 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 new file mode 100644 index 0000000000000..11a6e0b34abdc --- /dev/null +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp @@ -0,0 +1,148 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_MARKER_LOCALIZER_HPP_ +#define LIDAR_MARKER_LOCALIZER_HPP_ + +#include "localization_util/diagnostics_module.hpp" +#include "localization_util/smart_pose_buffer.hpp" + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#include +#else +#include + +#include +#include +#endif +#include + +#include + +#include +#include +#include + +namespace autoware::lidar_marker_localizer +{ + +class LidarMarkerLocalizer : public rclcpp::Node +{ + using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; + using MarkerArray = visualization_msgs::msg::MarkerArray; + using Pose = geometry_msgs::msg::Pose; + using PoseStamped = geometry_msgs::msg::PoseStamped; + using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using PoseArray = geometry_msgs::msg::PoseArray; + using TransformStamped = geometry_msgs::msg::TransformStamped; + using Vector3 = geometry_msgs::msg::Vector3; + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using SetBool = std_srvs::srv::SetBool; + using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; + + struct Param + { + std::string marker_name; + + double resolution; + std::vector intensity_pattern; + int64_t match_intensity_difference_threshold; + int64_t positive_match_num_threshold; + int64_t negative_match_num_threshold; + int64_t vote_threshold_for_detect_marker; + double marker_height_from_ground; + + double self_pose_timeout_sec; + double self_pose_distance_tolerance_m; + + double limit_distance_from_self_pose_to_nearest_marker; + double limit_distance_from_self_pose_to_marker; + std::array base_covariance; + + double marker_width; + + bool enable_save_log; + std::string save_file_directory_path; + std::string save_file_name; + std::string save_frame_id; + }; + +public: + explicit LidarMarkerLocalizer(const rclcpp::NodeOptions & node_options); + +private: + void self_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & self_pose_msg_ptr); + void points_callback(const PointCloud2::ConstSharedPtr & points_msg_ptr); + void map_bin_callback(const HADMapBin::ConstSharedPtr & map_bin_msg_ptr); + void service_trigger_node( + const SetBool::Request::SharedPtr req, SetBool::Response::SharedPtr res); + + void initialize_diagnostics(); + void main_process(const PointCloud2::ConstSharedPtr & points_msg_ptr); + std::vector detect_landmarks( + const PointCloud2::ConstSharedPtr & points_msg_ptr); + sensor_msgs::msg::PointCloud2::SharedPtr extract_marker_pointcloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & points_msg_ptr, + const geometry_msgs::msg::Pose marker_pose) const; + void save_detected_marker_log(const sensor_msgs::msg::PointCloud2::SharedPtr & points_msg_ptr); + + void transform_sensor_measurement( + const std::string & source_frame, const std::string & target_frame, + const sensor_msgs::msg::PointCloud2::SharedPtr & sensor_points_input_ptr, + sensor_msgs::msg::PointCloud2::SharedPtr & sensor_points_output_ptr); + + std::shared_ptr tf_listener_; + std::shared_ptr tf_buffer_; + + rclcpp::Subscription::SharedPtr sub_points_; + rclcpp::Subscription::SharedPtr sub_self_pose_; + rclcpp::Subscription::SharedPtr sub_map_bin_; + + rclcpp::Publisher::SharedPtr + pub_base_link_pose_with_covariance_on_map_; + rclcpp::Service::SharedPtr service_trigger_node_; + rclcpp::Publisher::SharedPtr pub_marker_mapped_; + rclcpp::Publisher::SharedPtr pub_marker_detected_; + rclcpp::Publisher::SharedPtr pub_debug_pose_with_covariance_; + rclcpp::Publisher::SharedPtr pub_marker_pointcloud_; + + std::shared_ptr diagnostics_module_; + + Param param_; + bool is_activated_; + std::unique_ptr ekf_pose_buffer_; + + landmark_manager::LandmarkManager landmark_manager_; +}; + +} // namespace autoware::lidar_marker_localizer + +#endif // LIDAR_MARKER_LOCALIZER_HPP_ From a079c20f8661db47cf6f2f6f0b9e38b68aac45b5 Mon Sep 17 00:00:00 2001 From: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com> Date: Fri, 23 Aug 2024 16:15:47 +0900 Subject: [PATCH 26/46] chore(cspell): update prefix and path in cspell (#8524) --- .cspell.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.cspell.json b/.cspell.json index 94b509370e688..b0d572db8726b 100644 --- a/.cspell.json +++ b/.cspell.json @@ -1,7 +1,7 @@ { "ignorePaths": [ - "perception/bytetrack/lib/**", - "planning/behavior_velocity_intersection_module/scripts/**" + "perception/autoware_bytetrack/lib/**", + "planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/scripts/**" ], "ignoreRegExpList": [], "words": ["dltype", "tvmgen", "fromarray"] From 23e13fde3617938ab7e65c968d98470b9ec20274 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Fri, 23 Aug 2024 16:17:00 +0900 Subject: [PATCH 27/46] refactor(ekf_localizer): rename dev to var in the Simple1DFilter class (#8576) rename dev to var Signed-off-by: Yamato Ando --- .../include/ekf_localizer/ekf_localizer.hpp | 30 ++++++++-------- .../ekf_localizer/src/ekf_localizer.cpp | 36 +++++++++---------- 2 files changed, 33 insertions(+), 33 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 7e091a0e88a19..f3830c303ab48 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -56,44 +56,44 @@ class Simple1DFilter { initialized_ = false; x_ = 0; - dev_ = 1e9; - proc_dev_x_c_ = 0.0; + var_ = 1e9; + proc_var_x_c_ = 0.0; }; - void init(const double init_obs, const double obs_dev, const rclcpp::Time & time) + void init(const double init_obs, const double obs_var, const rclcpp::Time & time) { x_ = init_obs; - dev_ = obs_dev; + var_ = obs_var; latest_time_ = time; initialized_ = true; }; - void update(const double obs, const double obs_dev, const rclcpp::Time & time) + void update(const double obs, const double obs_var, const rclcpp::Time & time) { if (!initialized_) { - init(obs, obs_dev, time); + init(obs, obs_var, time); return; } - // Prediction step (current stddev_) + // Prediction step (current variance) double dt = (time - latest_time_).seconds(); - double proc_dev_x_d = proc_dev_x_c_ * dt * dt; - dev_ = dev_ + proc_dev_x_d; + double proc_var_x_d = proc_var_x_c_ * dt * dt; + var_ = var_ + proc_var_x_d; // Update step - double kalman_gain = dev_ / (dev_ + obs_dev); + double kalman_gain = var_ / (var_ + obs_var); x_ = x_ + kalman_gain * (obs - x_); - dev_ = (1 - kalman_gain) * dev_; + var_ = (1 - kalman_gain) * var_; latest_time_ = time; }; - void set_proc_dev(const double proc_dev) { proc_dev_x_c_ = proc_dev; } + void set_proc_var(const double proc_var) { proc_var_x_c_ = proc_var; } [[nodiscard]] double get_x() const { return x_; } - [[nodiscard]] double get_dev() const { return dev_; } + [[nodiscard]] double get_var() const { return var_; } private: bool initialized_; double x_; - double dev_; - double proc_dev_x_c_; + double var_; + double proc_var_x_c_; rclcpp::Time latest_time_; }; diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 090395398026e..707d5e585cb0c 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -100,9 +100,9 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) ekf_module_ = std::make_unique(warning_, params_); logger_configure_ = std::make_unique(this); - z_filter_.set_proc_dev(params_.z_filter_proc_dev); - roll_filter_.set_proc_dev(params_.roll_filter_proc_dev); - pitch_filter_.set_proc_dev(params_.pitch_filter_proc_dev); + z_filter_.set_proc_var(params_.z_filter_proc_dev * params_.z_filter_proc_dev); + roll_filter_.set_proc_var(params_.roll_filter_proc_dev * params_.roll_filter_proc_dev); + pitch_filter_.set_proc_var(params_.pitch_filter_proc_dev * params_.pitch_filter_proc_dev); } /* @@ -367,9 +367,9 @@ void EKFLocalizer::publish_estimate_result( pose_cov.pose.covariance = ekf_module_->get_current_pose_covariance(); using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - pose_cov.pose.covariance[COV_IDX::Z_Z] = z_filter_.get_dev(); - pose_cov.pose.covariance[COV_IDX::ROLL_ROLL] = roll_filter_.get_dev(); - pose_cov.pose.covariance[COV_IDX::PITCH_PITCH] = pitch_filter_.get_dev(); + pose_cov.pose.covariance[COV_IDX::Z_Z] = z_filter_.get_var(); + pose_cov.pose.covariance[COV_IDX::ROLL_ROLL] = roll_filter_.get_var(); + pose_cov.pose.covariance[COV_IDX::PITCH_PITCH] = pitch_filter_.get_var(); pub_pose_cov_->publish(pose_cov); @@ -466,14 +466,14 @@ void EKFLocalizer::update_simple_1d_filters( const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - double z_dev = pose.pose.covariance[COV_IDX::Z_Z] * static_cast(smoothing_step); - double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL] * static_cast(smoothing_step); - double pitch_dev = + double z_var = pose.pose.covariance[COV_IDX::Z_Z] * static_cast(smoothing_step); + double roll_var = pose.pose.covariance[COV_IDX::ROLL_ROLL] * static_cast(smoothing_step); + double pitch_var = pose.pose.covariance[COV_IDX::PITCH_PITCH] * static_cast(smoothing_step); - z_filter_.update(z, z_dev, pose.header.stamp); - roll_filter_.update(rpy.x, roll_dev, pose.header.stamp); - pitch_filter_.update(rpy.y, pitch_dev, pose.header.stamp); + z_filter_.update(z, z_var, pose.header.stamp); + roll_filter_.update(rpy.x, roll_var, pose.header.stamp); + pitch_filter_.update(rpy.y, pitch_var, pose.header.stamp); } void EKFLocalizer::init_simple_1d_filters( @@ -484,13 +484,13 @@ void EKFLocalizer::init_simple_1d_filters( const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - double z_dev = pose.pose.covariance[COV_IDX::Z_Z]; - double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL]; - double pitch_dev = pose.pose.covariance[COV_IDX::PITCH_PITCH]; + double z_var = pose.pose.covariance[COV_IDX::Z_Z]; + double roll_var = pose.pose.covariance[COV_IDX::ROLL_ROLL]; + double pitch_var = pose.pose.covariance[COV_IDX::PITCH_PITCH]; - z_filter_.init(z, z_dev, pose.header.stamp); - roll_filter_.init(rpy.x, roll_dev, pose.header.stamp); - pitch_filter_.init(rpy.y, pitch_dev, pose.header.stamp); + z_filter_.init(z, z_var, pose.header.stamp); + roll_filter_.init(rpy.x, roll_var, pose.header.stamp); + pitch_filter_.init(rpy.y, pitch_var, pose.header.stamp); } /** From a6e0ca7cc973fec1adb25c5d79506ee6ba91e0c8 Mon Sep 17 00:00:00 2001 From: Mehmet Dogru <48479081+mehmetdogru@users.noreply.github.com> Date: Fri, 23 Aug 2024 11:09:32 +0300 Subject: [PATCH 28/46] fix(crosswalk): fix findEgoPassageDirectionAlongPath finding front and back point logic (#8459) * fix(crosswalk): fix findEgoPassageDirectionAlongPath finding front and back point logic Signed-off-by: Mehmet Dogru * define ego_crosswalk_passage_direction later Signed-off-by: Mehmet Dogru --------- Signed-off-by: Mehmet Dogru --- .../src/scene_crosswalk.cpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) 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 ca476481ddde5..f30ed9d7a68da 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 @@ -320,8 +320,6 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const double ego_vel = planner_data_->current_velocity->twist.linear.x; const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; - const std::optional ego_crosswalk_passage_direction = - findEgoPassageDirectionAlongPath(sparse_resample_path); const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose->position); @@ -357,6 +355,8 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( }; std::optional> nearest_stop_info; std::vector stop_factor_points; + const std::optional ego_crosswalk_passage_direction = + findEgoPassageDirectionAlongPath(sparse_resample_path); for (const auto & object : object_info_manager_.getObject()) { const auto & collision_point_opt = object.collision_point; if (collision_point_opt) { @@ -618,9 +618,17 @@ std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( if (!intersect_pt1 || !intersect_pt2) { return std::nullopt; } - const auto idx1 = intersect_pt1.value().first, idx2 = intersect_pt2.value().first; - const auto & front = idx1 > idx2 ? intersect_pt2.value().second : intersect_pt1.value().second; - const auto & back = idx1 > idx2 ? intersect_pt1.value().second : intersect_pt2.value().second; + + const auto idx1 = intersect_pt1.value().first; + const auto idx2 = intersect_pt2.value().first; + + const auto min_idx = std::min(idx1, idx2); + const auto dist1 = calcSignedArcLength(path.points, min_idx, intersect_pt1.value().second); + const auto dist2 = calcSignedArcLength(path.points, min_idx, intersect_pt2.value().second); + + const auto & front = dist1 > dist2 ? intersect_pt2.value().second : intersect_pt1.value().second; + const auto & back = dist1 > dist2 ? intersect_pt1.value().second : intersect_pt2.value().second; + return std::atan2(back.y - front.y, back.x - front.x); } From 18b5ee13c446cd8cb2e172214ac49ea6f29bbc84 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 23 Aug 2024 17:11:37 +0900 Subject: [PATCH 29/46] fix(map_tf_genrator): fix map launch prefix (#8598) fix(map_tf_geenrator): fix map launch prefix Signed-off-by: kosuke55 --- launch/tier4_map_launch/launch/map.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml index c58c0848eb5b3..051cf64d7752e 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -48,7 +48,7 @@ - + From b9b0fa9dbb4e141519dc8de13d37827763b481f2 Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Fri, 23 Aug 2024 17:51:51 +0900 Subject: [PATCH 30/46] feat(autoware_motion_utils): add interpolator (#8517) * feat(autoware_motion_utils): add interpolator Signed-off-by: Y.Hisaki * use int32_t instead of int Signed-off-by: Y.Hisaki * use int32_t instead of int Signed-off-by: Y.Hisaki * use int32_t instead of int Signed-off-by: Y.Hisaki * add const as much as possible and use `at()` in `vector` Signed-off-by: Y.Hisaki * fix directory name Signed-off-by: Y.Hisaki * refactor code and add example Signed-off-by: Y.Hisaki * update Signed-off-by: Y.Hisaki * remove unused include Signed-off-by: Y.Hisaki * refactor code Signed-off-by: Y.Hisaki --------- Signed-off-by: Y.Hisaki --- common/autoware_motion_utils/CMakeLists.txt | 30 ++++ .../examples/example_interpolator.cpp | 115 ++++++++++++++ .../trajectory_container/interpolator.hpp | 23 +++ .../interpolator/akima_spline.hpp | 98 ++++++++++++ .../interpolator/cubic_spline.hpp | 100 ++++++++++++ .../detail/interpolator_common_impl.hpp | 145 ++++++++++++++++++ .../detail/nearest_neighbor_common_impl.hpp | 80 ++++++++++ .../detail/stairstep_common_impl.hpp | 80 ++++++++++ .../interpolator/interpolator.hpp | 91 +++++++++++ .../interpolator/interpolator_creator.hpp | 77 ++++++++++ .../interpolator/linear.hpp | 90 +++++++++++ .../interpolator/nearest_neighbor.hpp | 83 ++++++++++ .../interpolator/stairstep.hpp | 82 ++++++++++ .../interpolator/akima_spline.cpp | 93 +++++++++++ .../interpolator/cubic_spline.cpp | 94 ++++++++++++ .../interpolator/linear.cpp | 62 ++++++++ .../test_interpolator.cpp | 75 +++++++++ 17 files changed, 1418 insertions(+) create mode 100644 common/autoware_motion_utils/examples/example_interpolator.cpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator_creator.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp create mode 100644 common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp create mode 100644 common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp create mode 100644 common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp create mode 100644 common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp diff --git a/common/autoware_motion_utils/CMakeLists.txt b/common/autoware_motion_utils/CMakeLists.txt index 4c36ef2f4e70d..e7f1f56a451f4 100644 --- a/common/autoware_motion_utils/CMakeLists.txt +++ b/common/autoware_motion_utils/CMakeLists.txt @@ -1,15 +1,22 @@ cmake_minimum_required(VERSION 3.14) project(autoware_motion_utils) +option(BUILD_EXAMPLES "Build examples" OFF) + 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) @@ -22,4 +29,27 @@ if(BUILD_TESTING) ) endif() +if(BUILD_EXAMPLES) + message(STATUS "Building examples") + + include(FetchContent) + fetchcontent_declare( + matplotlibcpp17 + GIT_REPOSITORY https://github.com/soblin/matplotlibcpp17.git + GIT_TAG master + ) + fetchcontent_makeavailable(matplotlibcpp17) + + file(GLOB_RECURSE example_files examples/*.cpp) + + foreach(example_file ${example_files}) + get_filename_component(example_name ${example_file} NAME_WE) + ament_auto_add_executable(${example_name} ${example_file}) + target_link_libraries(${example_name} + autoware_motion_utils + matplotlibcpp17::matplotlibcpp17 + ) + endforeach() +endif() + ament_auto_package() diff --git a/common/autoware_motion_utils/examples/example_interpolator.cpp b/common/autoware_motion_utils/examples/example_interpolator.cpp new file mode 100644 index 0000000000000..8c98143af7e28 --- /dev/null +++ b/common/autoware_motion_utils/examples/example_interpolator.cpp @@ -0,0 +1,115 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/motion_utils/trajectory_container/interpolator/linear.hpp" + +#include + +#include + +#include +#include + +int main() +{ + pybind11::scoped_interpreter guard{}; + 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 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) { + values.push_back(dist(engine)); + } + // Scatter Data + plt.scatter(Args(axis, values)); + + using autoware::motion_utils::trajectory_container::interpolator::Interpolator; + using autoware::motion_utils::trajectory_container::interpolator::InterpolatorCreator; + // Linear Interpolator + { + using autoware::motion_utils::trajectory_container::interpolator::Linear; + auto interpolator = *InterpolatorCreator().set_axis(axis).set_values(values).create(); + std::vector x; + std::vector y; + for (double i = axis.front(); i < axis.back(); i += 0.01) { + x.push_back(i); + y.push_back(interpolator.compute(i)); + } + plt.plot(Args(x, y), Kwargs("label"_a = "Linear")); + } + + // AkimaSpline Interpolator + { + using autoware::motion_utils::trajectory_container::interpolator::AkimaSpline; + auto interpolator = + *InterpolatorCreator().set_axis(axis).set_values(values).create(); + std::vector x; + std::vector y; + for (double i = axis.front(); i < axis.back(); i += 0.01) { + x.push_back(i); + y.push_back(interpolator.compute(i)); + } + plt.plot(Args(x, y), Kwargs("label"_a = "AkimaSpline")); + } + + // CubicSpline Interpolator + { + using autoware::motion_utils::trajectory_container::interpolator::CubicSpline; + auto interpolator = + *InterpolatorCreator().set_axis(axis).set_values(values).create(); + std::vector x; + std::vector y; + for (double i = axis.front(); i < axis.back(); i += 0.01) { + x.push_back(i); + y.push_back(interpolator.compute(i)); + } + plt.plot(Args(x, y), Kwargs("label"_a = "CubicSpline")); + } + + // NearestNeighbor Interpolator + { + using autoware::motion_utils::trajectory_container::interpolator::NearestNeighbor; + auto interpolator = + *InterpolatorCreator>().set_axis(axis).set_values(values).create(); + std::vector x; + std::vector y; + for (double i = axis.front(); i < axis.back(); i += 0.01) { + x.push_back(i); + y.push_back(interpolator.compute(i)); + } + plt.plot(Args(x, y), Kwargs("label"_a = "NearestNeighbor")); + } + + // Stairstep Interpolator + { + using autoware::motion_utils::trajectory_container::interpolator::Stairstep; + auto interpolator = + *InterpolatorCreator>().set_axis(axis).set_values(values).create(); + std::vector x; + std::vector y; + for (double i = axis.front(); i < axis.back(); i += 0.01) { + x.push_back(i); + y.push_back(interpolator.compute(i)); + } + plt.plot(Args(x, y), Kwargs("label"_a = "Stairstep")); + } + + plt.legend(); + plt.show(); + return 0; +} 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 new file mode 100644 index 0000000000000..a6542d1fc212f --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp @@ -0,0 +1,23 @@ +// 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_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_ +#include +#include +#include +#include +#include +#include +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_ 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 new file mode 100644 index 0000000000000..42cd1aa6759a3 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp @@ -0,0 +1,98 @@ +// 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__AKIMA_SPLINE_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_ + +#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" + +#include + +#include + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +/** + * @brief Class for Akima spline interpolation. + * + * This class provides methods to perform Akima spline interpolation on a set of data points. + */ +class AkimaSpline : public Interpolator +{ + template + friend class InterpolatorCreator; + +private: + Eigen::VectorXd a_, b_, c_, d_; ///< Coefficients for the Akima spline. + + AkimaSpline() = default; + + /** + * @brief Compute the spline parameters. + * + * This method computes the coefficients for the Akima spline. + * + * @param axis The axis values. + * @param values The values to interpolate. + */ + void compute_parameters( + const Eigen::Ref & axis, + const Eigen::Ref & values); + + /** + * @brief Build the interpolator with the given values. + * + * @param axis The axis values. + * @param values The values to interpolate. + */ + void build_impl( + const Eigen::Ref & axis, const std::vector & values) override; + + /** + * @brief Compute the interpolated value at the given point. + * + * @param s The point at which to compute the interpolated value. + * @return The interpolated value. + */ + [[nodiscard]] double compute_impl(const double & s) const override; + + /** + * @brief Compute the first derivative at the given point. + * + * @param s The point at which to compute the first derivative. + * @return The first derivative. + */ + [[nodiscard]] double compute_first_derivative_impl(const double & s) const override; + + /** + * @brief Compute the second derivative at the given point. + * + * @param s The point at which to compute the second derivative. + * @return The second derivative. + */ + [[nodiscard]] double compute_second_derivative_impl(const double & s) const override; + +public: + /** + * @brief Get the minimum number of required points for the interpolator. + * + * @return The minimum number of required points. + */ + [[nodiscard]] size_t minimum_required_points() const override { return 5; } +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_ 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 new file mode 100644 index 0000000000000..43f1102fd0f73 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp @@ -0,0 +1,100 @@ +// 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__CUBIC_SPLINE_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__CUBIC_SPLINE_HPP_ + +#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" + +#include + +#include + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +/** + * @brief Class for cubic spline interpolation. + * + * This class provides methods to perform cubic spline interpolation on a set of data points. + */ +class CubicSpline : public Interpolator +{ + template + friend class InterpolatorCreator; + +private: + Eigen::VectorXd a_, b_, c_, d_; ///< Coefficients for the cubic spline. + Eigen::VectorXd h_; ///< Interval sizes between axis points. + + CubicSpline() = default; + + /** + * @brief Compute the spline parameters. + * + * This method computes the coefficients for the cubic spline. + * + * @param axis The axis values. + * @param values The values to interpolate. + */ + void compute_parameters( + const Eigen::Ref & axis, + const Eigen::Ref & values); + + /** + * @brief Build the interpolator with the given values. + * + * @param axis The axis 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; + + /** + * @brief Compute the interpolated value at the given point. + * + * @param s The point at which to compute the interpolated value. + * @return The interpolated value. + */ + [[nodiscard]] double compute_impl(const double & s) const override; + + /** + * @brief Compute the first derivative at the given point. + * + * @param s The point at which to compute the first derivative. + * @return The first derivative. + */ + [[nodiscard]] double compute_first_derivative_impl(const double & s) const override; + + /** + * @brief Compute the second derivative at the given point. + * + * @param s The point at which to compute the second derivative. + * @return The second derivative. + */ + [[nodiscard]] double compute_second_derivative_impl(const double & s) const override; + +public: + /** + * @brief Get the minimum number of required points for the interpolator. + * + * @return The minimum number of required points. + */ + [[nodiscard]] size_t minimum_required_points() const override { return 4; } +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__CUBIC_SPLINE_HPP_ 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_impl.hpp new file mode 100644 index 0000000000000..f7de456b736a4 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp @@ -0,0 +1,145 @@ +// 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_COMMON_IMPL_HPP_ // NOLINT +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_IMPL_HPP_ // NOLINT +// clang-format on + +#include +#include + +#include + +#include + +namespace autoware::motion_utils::trajectory_container::interpolator::detail +{ +/** + * @brief Base class for interpolation implementations. + * + * This class provides the basic interface and common functionality for different types + * of interpolation. It is intended to be subclassed by specific interpolation algorithms. + * + * @tparam T The type of the values being interpolated. + */ +template +class InterpolatorCommonImpl +{ +protected: + Eigen::VectorXd axis_; ///< Axis values for the interpolation. + + /** + * @brief Get the start of the interpolation range. + */ + [[nodiscard]] double start() const { return axis_(0); } + + /** + * @brief Get the end of the interpolation range. + */ + [[nodiscard]] double end() const { return axis_(axis_.size() - 1); } + + /** + * @brief Compute the interpolated value at the given point. + * + * This method should be overridden by subclasses to provide the specific interpolation logic. + * + * @param s The point at which to compute the interpolated value. + * @return The interpolated value. + */ + [[nodiscard]] virtual T compute_impl(const double & s) const = 0; + + /** + * @brief Build the interpolator with the given values. + * + * This method should be overridden by subclasses to provide the specific build logic. + * + * @param axis The axis values. + * @param values The values to interpolate. + */ + virtual void build_impl( + const Eigen::Ref & axis, const std::vector & values) = 0; + + /** + * @brief Validate the input to the compute method. + * + * Checks that the interpolator has been built and that the input value is within range. + * + * @param s The input value. + * @throw std::runtime_error if the interpolator has not been built. + */ + void validate_compute_input(const double & s) const + { + if (s < start() || s > end()) { + RCLCPP_WARN( + rclcpp::get_logger("Interpolator"), + "Input value %f is outside the range of the interpolator [%f, %f].", s, start(), end()); + } + } + + [[nodiscard]] int32_t get_index(const double & s, bool end_inclusive = true) const + { + if (end_inclusive && s == end()) { + return static_cast(axis_.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; + } + +public: + /** + * @brief Build the interpolator with the given axis and values. + * + * @param axis The axis 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) + { + if (axis.size() != static_cast(values.size())) { + return false; + } + if (axis.size() < static_cast(minimum_required_points())) { + return false; + } + build_impl(axis, values); + return true; + } + + /** + * @brief Get the minimum number of required points for the interpolator. + * + * This method should be overridden by subclasses to return the specific requirement. + * + * @return The minimum number of required points. + */ + [[nodiscard]] virtual size_t minimum_required_points() const = 0; + + /** + * @brief Compute the interpolated value at the given point. + * + * @param s The point at which to compute the interpolated value. + * @return The interpolated value. + */ + [[nodiscard]] T compute(const double & s) const + { + validate_compute_input(s); + return compute_impl(s); + } +}; +} // namespace autoware::motion_utils::trajectory_container::interpolator::detail + +// clang-format off +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_IMPL_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 new file mode 100644 index 0000000000000..9fd451bcdcf6e --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp @@ -0,0 +1,80 @@ +// 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__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_ // NOLINT +#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 + +namespace autoware::motion_utils::trajectory_container::interpolator::detail +{ +/** + * @brief Common Implementation of nearest neighbor. + * + * This class implements the core functionality for nearest neighbor interpolation. + * + * @tparam T The type of the values being interpolated. + */ +template +class NearestNeighborCommonImpl : public Interpolator +{ +protected: + std::vector values_; ///< Interpolation values. + + /** + * @brief Compute the interpolated value at the given point. + * + * @param s The point at which to compute the interpolated value. + * @return The interpolated value. + */ + [[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])) + ? this->values_.at(idx) + : this->values_.at(idx + 1); + } + + /** + * @brief Build the interpolator with the given values. + * + * @param axis The axis 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 + { + this->axis_ = axis; + this->values_ = values; + } + +public: + /** + * @brief Get the minimum number of required points for the interpolator. + * + * @return The minimum number of required points. + */ + [[nodiscard]] size_t minimum_required_points() const override { return 1; } +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator::detail + +// 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 new file mode 100644 index 0000000000000..9ac5282429353 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp @@ -0,0 +1,80 @@ +// 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__STAIRSTEP_COMMON_IMPL_HPP_ // NOLINT +#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 + +namespace autoware::motion_utils::trajectory_container::interpolator::detail +{ + +/** + * @brief Base class for stairstep interpolation. + * + * This class implements the core functionality for stairstep interpolation. + * + * @tparam T The type of the values being interpolated. + */ +template +class StairstepCommonImpl : public Interpolator +{ +protected: + std::vector values_; ///< Interpolation values. + + /** + * @brief Compute the interpolated value at the given point. + * + * @param s The point at which to compute the interpolated value. + * @return The interpolated value. + */ + [[nodiscard]] T compute_impl(const double & s) const override + { + const int32_t idx = this->get_index(s, false); + return this->values_.at(idx); + } + /** + * @brief Build the interpolator with the given values. + * + * @param axis The axis values. + * @param values The values to interpolate. + */ + void build_impl( + const Eigen::Ref & axis, const std::vector & values) override + { + this->axis_ = axis; + this->values_ = values; + } + +public: + /** + * @brief Default constructor. + */ + StairstepCommonImpl() = default; + + /** + * @brief Get the minimum number of required points for the interpolator. + */ + [[nodiscard]] size_t minimum_required_points() const override { return 2; } +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator::detail + +// clang-format off +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_ // NOLINT +// clang-format on 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 new file mode 100644 index 0000000000000..18dce8b46c2c7 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp @@ -0,0 +1,91 @@ +// 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_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_HPP_ + +#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp" + +namespace autoware::motion_utils::trajectory_container::interpolator +{ +/** + * @brief Template class for interpolation. + * + * This class serves as the base class for specific interpolation types. + * + * @tparam T The type of the values being interpolated. (e.g. double, int, etc.) + */ +template +class Interpolator : public detail::InterpolatorCommonImpl +{ +}; + +/** + * @brief Specialization of Interpolator for double values. + * + * This class adds methods for computing first and second derivatives. + */ +template <> +class Interpolator : public detail::InterpolatorCommonImpl +{ +protected: + /** + * @brief Compute the first derivative at the given point. + * + * This method should be overridden by subclasses to provide the specific logic. + * + * @param s The point at which to compute the first derivative. + * @return The first derivative. + */ + [[nodiscard]] virtual double compute_first_derivative_impl(const double & s) const = 0; + + /** + * @brief Compute the second derivative at the given point. + * + * This method should be overridden by subclasses to provide the specific logic. + * + * @param s The point at which to compute the second derivative. + * @return The second derivative. + */ + [[nodiscard]] virtual double compute_second_derivative_impl(const double & s) const = 0; + +public: + /** + * @brief Compute the first derivative at the given point. + * + * @param s The point at which to compute the first derivative. + * @return The first derivative. + */ + [[nodiscard]] double compute_first_derivative(const double & s) const + { + this->validate_compute_input(s); + return compute_first_derivative_impl(s); + } + + /** + * @brief Compute the second derivative at the given point. + * + * @param s The point at which to compute the second derivative. + * @return The second derivative. + */ + [[nodiscard]] double compute_second_derivative(const double & s) const + { + this->validate_compute_input(s); + return compute_second_derivative_impl(s); + } +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_HPP_ 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 new file mode 100644 index 0000000000000..911adcb6545b6 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator_creator.hpp @@ -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. + +#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 new file mode 100644 index 0000000000000..df0806f285339 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp @@ -0,0 +1,90 @@ +// 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__LINEAR_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__LINEAR_HPP_ + +#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" + +#include + +#include + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +/** + * @brief Class for linear interpolation. + * + * This class provides methods to perform linear interpolation on a set of data points. + */ +class Linear : public Interpolator +{ + template + friend class InterpolatorCreator; + +private: + Eigen::VectorXd values_; ///< Interpolation values. + + /** + * @brief Default constructor. + */ + Linear() = default; + + /** + * @brief Build the interpolator with the given values. + * + * @param axis The axis 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; + + /** + * @brief Compute the interpolated value at the given point. + * + * @param s The point at which to compute the interpolated value. + * @return The interpolated value. + */ + [[nodiscard]] double compute_impl(const double & s) const override; + + /** + * @brief Compute the first derivative at the given point. + * + * @param s The point at which to compute the first derivative. + * @return The first derivative. + */ + [[nodiscard]] double compute_first_derivative_impl(const double & s) const override; + + /** + * @brief Compute the second derivative at the given point. + * + * @param s The point at which to compute the second derivative. + * @return The second derivative. + */ + [[nodiscard]] double compute_second_derivative_impl(const double &) const override; + +public: + /** + * @brief Get the minimum number of required points for the interpolator. + * + * @return The minimum number of required points. + */ + [[nodiscard]] size_t minimum_required_points() const override; +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__LINEAR_HPP_ 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 new file mode 100644 index 0000000000000..c433607902b38 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp @@ -0,0 +1,83 @@ +// 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__NEAREST_NEIGHBOR_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__NEAREST_NEIGHBOR_HPP_ + +#include "autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp" + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +/** + * @brief Template class for nearest neighbor interpolation. + * + * This class provides methods to perform nearest neighbor interpolation on a set of data points. + * + * @tparam T The type of the values being interpolated. + */ +template +class NearestNeighbor; + +/** + * @brief Template class for nearest neighbor interpolation. + * + * This class provides the interface for nearest neighbor interpolation. + * + * @tparam T The type of the values being interpolated. + */ +template +class NearestNeighbor : public detail::NearestNeighborCommonImpl +{ + template + friend class InterpolatorCreator; + +private: + NearestNeighbor() = default; +}; + +/** + * @brief Specialization of NearestNeighbor for double values. + * + * This class provides methods to perform nearest neighbor interpolation on double values. + */ +template <> +class NearestNeighbor : public detail::NearestNeighborCommonImpl +{ + template + friend class InterpolatorCreator; + +private: + NearestNeighbor() = default; + + /** + * @brief Compute the first derivative at the given point. + * + * @param s The point at which to compute the first derivative. + * @return The first derivative. + */ + [[nodiscard]] double compute_first_derivative_impl(const double &) const override { return 0.0; } + + /** + * @brief Compute the second derivative at the given point. + * + * @param s The point at which to compute the second derivative. + * @return The second derivative. + */ + [[nodiscard]] double compute_second_derivative_impl(const double &) const override { return 0.0; } +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__NEAREST_NEIGHBOR_HPP_ 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 new file mode 100644 index 0000000000000..e8c8f8c015b63 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp @@ -0,0 +1,82 @@ +// 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__STAIRSTEP_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__STAIRSTEP_HPP_ + +#include "autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp" + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +/** + * @brief Template class for stairstep interpolation. + * + * This class provides methods to perform stairstep interpolation on a set of data points. + * + * @tparam T The type of the values being interpolated. + */ +template +class Stairstep; + +/** + * @brief Template class for stairstep interpolation. + * + * This class provides the interface for stairstep interpolation. + * + * @tparam T The type of the values being interpolated. + */ +template +class Stairstep : public detail::StairstepCommonImpl +{ + template + friend class InterpolatorCreator; + +private: + Stairstep() = default; +}; + +/** + * @brief Specialization of Stairstep for double values. + * + * This class provides methods to perform stairstep interpolation on double values. + */ +template <> +class Stairstep : public detail::StairstepCommonImpl +{ + template + friend class InterpolatorCreator; + +private: + Stairstep() = default; + /** + * @brief Compute the first derivative at the given point. + * + * @param s The point at which to compute the first derivative. + * @return The first derivative. + */ + [[nodiscard]] double compute_first_derivative_impl(const double &) const override { return 0.0; } + + /** + * @brief Compute the second derivative at the given point. + * + * @param s The point at which to compute the second derivative. + * @return The second derivative. + */ + [[nodiscard]] double compute_second_derivative_impl(const double &) const override { return 0.0; } +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__STAIRSTEP_HPP_ 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 new file mode 100644 index 0000000000000..283f46dd5d3e9 --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp @@ -0,0 +1,93 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp" + +#include + +#include +#include + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +void AkimaSpline::compute_parameters( + const Eigen::Ref & axis, const Eigen::Ref & values) +{ + const auto n = static_cast(axis.size()); + + Eigen::VectorXd h = axis.tail(n - 1) - axis.head(n - 1); + + Eigen::VectorXd m(n - 1); + for (int32_t i = 0; i < n - 1; ++i) { + m[i] = (values[i + 1] - values[i]) / h[i]; + } + + Eigen::VectorXd s(n); + s[0] = m[0]; + s[1] = (m[0] + m[1]) / 2; + for (int32_t i = 2; i < n - 2; ++i) { + if ((std::abs(m[i + 1] - m[i]) + std::abs(m[i - 1] - m[i - 2])) == 0) { + s[i] = (m[i] + m[i - 1]) / 2; + } else { + s[i] = (std::abs(m[i + 1] - m[i]) * m[i - 1] + std::abs(m[i - 1] - m[i - 2]) * m[i]) / + (std::abs(m[i + 1] - m[i]) + std::abs(m[i - 1] - m[i - 2])); + } + } + s[n - 2] = (m[n - 2] + m[n - 3]) / 2; + s[n - 1] = m[n - 2]; + + a_.resize(n - 1); + b_.resize(n - 1); + c_.resize(n - 1); + d_.resize(n - 1); + for (int32_t i = 0; i < n - 1; ++i) { + a_[i] = values[i]; + b_[i] = s[i]; + c_[i] = (3 * m[i] - 2 * s[i] - s[i + 1]) / h[i]; + d_[i] = (s[i] + s[i + 1] - 2 * m[i]) / (h[i] * h[i]); + } +} + +void AkimaSpline::build_impl( + const Eigen::Ref & axis, const std::vector & values) +{ + this->axis_ = axis; + compute_parameters( + this->axis_, + 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]; + 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]; + 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]; + return 2 * c_[i] + 6 * d_[i] * dx; +} + +} // 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 new file mode 100644 index 0000000000000..9f262b6a702f9 --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp @@ -0,0 +1,94 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp" + +#include + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +void CubicSpline::compute_parameters( + const Eigen::Ref & axis, const Eigen::Ref & values) +{ + const int32_t n = static_cast(axis.size()) - 1; + + h_ = axis.tail(n) - axis.head(n); + a_ = values.transpose(); + + for (int32_t i = 0; i < n; ++i) { + h_(i) = axis(i + 1) - axis(i); + } + + Eigen::VectorXd alpha(n - 1); + for (int32_t i = 1; i < n; ++i) { + alpha(i - 1) = (3.0 / h_(i)) * (a_(i + 1) - a_(i)) - (3.0 / h_(i - 1)) * (a_(i) - a_(i - 1)); + } + + Eigen::VectorXd l(n + 1); + Eigen::VectorXd mu(n + 1); + Eigen::VectorXd z(n + 1); + l(0) = 1.0; + 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); + mu(i) = h_(i) / l(i); + z(i) = (alpha(i - 1) - h_(i - 1) * z(i - 1)) / l(i); + } + b_.resize(n); + d_.resize(n); + c_.resize(n + 1); + + l(n) = 1.0; + z(n) = c_(n) = 0.0; + + for (int32_t j = n - 1; j >= 0; --j) { + c_(j) = z(j) - mu(j) * c_(j + 1); + b_(j) = (a_(j + 1) - a_(j)) / h_(j) - h_(j) * (c_(j + 1) + 2.0 * c_(j)) / 3.0; + d_(j) = (c_(j + 1) - c_(j)) / (3.0 * h_(j)); + } +} + +void CubicSpline::build_impl( + const Eigen::Ref & axis, const std::vector & values) +{ + this->axis_ = axis; + compute_parameters( + this->axis_, + 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); + 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); + 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); + return 2 * c_(i) + 6 * d_(i) * dx; +} + +} // 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 new file mode 100644 index 0000000000000..4897ba4d2a56c --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp @@ -0,0 +1,62 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/motion_utils/trajectory_container/interpolator/linear.hpp" + +#include + +#include + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +void Linear::build_impl( + const Eigen::Ref & axis, const std::vector & values) +{ + this->axis_ = axis; + this->values_ = + Eigen::Map(values.data(), static_cast(values.size())); +} + +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 y0 = this->values_(idx); + const double y1 = this->values_(idx + 1); + return y0 + (y1 - y0) * (s - x0) / (x1 - x0); +} + +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 y0 = this->values_(idx); + const double y1 = this->values_(idx + 1); + return (y1 - y0) / (x1 - x0); +} + +double Linear::compute_second_derivative_impl(const double &) const +{ + return 0.0; +} + +size_t Linear::minimum_required_points() const +{ + return 2; +} + +} // 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 new file mode 100644 index 0000000000000..77f7af0eae93f --- /dev/null +++ b/common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp @@ -0,0 +1,75 @@ +// 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 + +#include +#include + +#include +#include +#include + +template +class TestInterpolator : public ::testing::Test +{ +public: + std::optional interpolator; + std::vector axis; + std::vector values; + + void SetUp() override + { + // generate random values -1 to 1 + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_real_distribution<> dis(-1, 1); + axis.resize(10); + values.resize(10); + for (size_t i = 0; i < axis.size(); ++i) { + axis[i] = static_cast(i); + values[i] = dis(gen); + } + } +}; + +using Interpolators = testing::Types< + autoware::motion_utils::trajectory_container::interpolator::CubicSpline, + autoware::motion_utils::trajectory_container::interpolator::AkimaSpline, + autoware::motion_utils::trajectory_container::interpolator::Linear, + autoware::motion_utils::trajectory_container::interpolator::NearestNeighbor, + autoware::motion_utils::trajectory_container::interpolator::Stairstep>; + +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); + } +} + +// Instantiate test cases for all interpolators +template class TestInterpolator< + autoware::motion_utils::trajectory_container::interpolator::CubicSpline>; +template class TestInterpolator< + autoware::motion_utils::trajectory_container::interpolator::AkimaSpline>; +template class TestInterpolator; +template class TestInterpolator< + autoware::motion_utils::trajectory_container::interpolator::NearestNeighbor>; +template class TestInterpolator< + autoware::motion_utils::trajectory_container::interpolator::Stairstep>; From 1bfb4c055cbe265849a400ba60bf0e7122c19685 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Mon, 26 Aug 2024 10:37:36 +0900 Subject: [PATCH 31/46] fix(lane_change): activate turn signal as soon as we have the intention to change lanes (#8571) * modify lane change requested condition Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: mohammad alqudah * modify lane change requested condition Signed-off-by: Muhammad Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> * style(pre-commit): autofix * fix docstring Signed-off-by: Muhammad Zulfaqar Azmi * modify LC turn signal logic Signed-off-by: mohammad alqudah * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * minor change Signed-off-by: mohammad alqudah --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: mohammad alqudah Co-authored-by: Muhammad Zulfaqar Azmi Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../scene.hpp | 4 +- .../utils/base_class.hpp | 29 +++++- .../utils/calculation.hpp | 2 +- .../src/scene.cpp | 98 ++++++++----------- .../src/utils/calculation.cpp | 2 +- 5 files changed, 73 insertions(+), 62 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 e3ef44f1081ed..24257e8855e33 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 @@ -109,7 +109,7 @@ class NormalLaneChange : public LaneChangeBase bool isStoppedAtRedTrafficLight() const override; - TurnSignalInfo get_current_turn_signal_info() override; + TurnSignalInfo get_current_turn_signal_info() const final; protected: lanelet::ConstLanelets getLaneChangeLanes( @@ -117,6 +117,8 @@ class NormalLaneChange : public LaneChangeBase int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; + TurnSignalInfo get_terminal_turn_signal_info() const final; + std::vector sampleLongitudinalAccValues( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; 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 6c7dc31e857e5..51d5dbe0f195c 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 @@ -213,7 +213,7 @@ class LaneChangeBase LaneChangeModuleType getModuleType() const { return type_; } - TurnSignalDecider getTurnSignalDecider() { return planner_data_->turn_signal_decider; } + TurnSignalDecider getTurnSignalDecider() const { return planner_data_->turn_signal_decider; } Direction getDirection() const { @@ -229,7 +229,7 @@ class LaneChangeBase void resetStopPose() { lane_change_stop_pose_ = std::nullopt; } - virtual TurnSignalInfo get_current_turn_signal_info() = 0; + virtual TurnSignalInfo get_current_turn_signal_info() const = 0; protected: virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0; @@ -245,6 +245,31 @@ class LaneChangeBase virtual lanelet::ConstLanelets getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, Direction direction) const = 0; + virtual TurnSignalInfo get_terminal_turn_signal_info() const = 0; + + TurnSignalInfo get_turn_signal(const Pose & start, const Pose & end) const + { + TurnSignalInfo turn_signal; + switch (direction_) { + case Direction::LEFT: + turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + break; + case Direction::RIGHT: + turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + break; + default: + turn_signal.turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + break; + } + + turn_signal.desired_start_point = start; + turn_signal.desired_end_point = end; + turn_signal.required_start_point = turn_signal.desired_start_point; + turn_signal.required_end_point = turn_signal.desired_end_point; + + return turn_signal; + } + LaneChangeStatus status_{}; PathShifter path_shifter_{}; 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 66c454f795ebb..819cccd9365b6 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 @@ -73,7 +73,7 @@ double calc_stopping_distance(const LCParamPtr & lc_param_ptr); * @return distance to last fit width position along the lane */ double calc_dist_to_last_fit_width( - const lanelet::ConstLanelets lanelets, const Pose & src_pose, + const lanelet::ConstLanelets & lanelets, const Pose & src_pose, const BehaviorPathPlannerParameters & bpp_param, const double margin = 0.1); /** 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 c6d05f90fe138..01bdbe77f1c99 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 @@ -181,81 +181,64 @@ bool NormalLaneChange::isStoppedAtRedTrafficLight() const status_.lane_change_path.info.length.sum()); } -TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() +TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() const { const auto original_turn_signal_info = prev_module_output_.turn_signal_info; - const auto & current_lanes = get_current_lanes(); - const auto is_valid = getLaneChangeStatus().is_valid_path; - const auto & lane_change_path = getLaneChangeStatus().lane_change_path; - const auto & lane_change_param = getLaneChangeParam(); - - if (getModuleType() != LaneChangeModuleType::NORMAL || current_lanes.empty() || !is_valid) { + if (getModuleType() != LaneChangeModuleType::NORMAL || get_current_lanes().empty()) { return original_turn_signal_info; } - // check direction - TurnSignalInfo current_turn_signal_info; - const auto & current_pose = getEgoPose(); - const auto direction = getDirection(); - if (direction == Direction::LEFT) { - current_turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - } else if (direction == Direction::RIGHT) { - current_turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + if (direction_ != Direction::LEFT && direction_ != Direction::RIGHT) { + return original_turn_signal_info; } const auto & path = prev_module_output_.path; - if (path.points.empty()) { - current_turn_signal_info.desired_start_point = current_pose; - current_turn_signal_info.required_start_point = current_pose; - current_turn_signal_info.desired_end_point = lane_change_path.info.lane_changing_end; - current_turn_signal_info.required_end_point = lane_change_path.info.lane_changing_end; - return current_turn_signal_info; + const auto & original_command = original_turn_signal_info.turn_signal.command; + if ( + !path.points.empty() && original_command != TurnIndicatorsCommand::DISABLE && + original_command != TurnIndicatorsCommand::NO_COMMAND) { + return get_terminal_turn_signal_info(); } - const auto min_length_for_turn_signal_activation = - lane_change_param.min_length_for_turn_signal_activation; - const auto & route_handler = getRouteHandler(); - const auto & common_parameter = getCommonParam(); + return get_turn_signal(getEgoPose(), getLaneChangePath().info.lane_changing_end); +} + +TurnSignalInfo NormalLaneChange::get_terminal_turn_signal_info() const +{ + const auto & lane_change_param = getLaneChangeParam(); + const auto & common_param = getCommonParam(); + const auto & current_pose = getEgoPose(); + const auto & path = prev_module_output_.path; + + const auto original_turn_signal_info = prev_module_output_.turn_signal_info; + const auto shift_intervals = - route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); + getRouteHandler()->getLateralIntervalsToPreferredLane(get_current_lanes().back()); const double next_lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength(lane_change_param, shift_intervals); - const double nearest_dist_threshold = common_parameter.ego_nearest_dist_threshold; - const double nearest_yaw_threshold = common_parameter.ego_nearest_yaw_threshold; - const double base_to_front = common_parameter.base_link2front; - const double buffer = - next_lane_change_buffer + min_length_for_turn_signal_activation + base_to_front; + const double buffer = next_lane_change_buffer + + lane_change_param.min_length_for_turn_signal_activation + + common_param.base_link2front; const double path_length = autoware::motion_utils::calcArcLength(path.points); - const size_t current_nearest_seg_idx = - autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - const double dist_to_terminal = utils::getDistanceToEndOfLane(current_pose, current_lanes); const auto start_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path.points, 0, std::max(path_length - buffer, 0.0)); - if (dist_to_terminal - base_to_front < buffer && start_pose) { - // modify turn signal - current_turn_signal_info.desired_start_point = *start_pose; - current_turn_signal_info.desired_end_point = lane_change_path.info.lane_changing_end; - current_turn_signal_info.required_start_point = current_turn_signal_info.desired_start_point; - current_turn_signal_info.required_end_point = current_turn_signal_info.desired_end_point; - - const auto & original_command = original_turn_signal_info.turn_signal.command; - if ( - original_command == TurnIndicatorsCommand::DISABLE || - original_command == TurnIndicatorsCommand::NO_COMMAND) { - return current_turn_signal_info; - } - // check the priority of turn signals - return getTurnSignalDecider().overwrite_turn_signal( - path, current_pose, current_nearest_seg_idx, original_turn_signal_info, - current_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold); - } + if (!start_pose) return original_turn_signal_info; + + const auto terminal_turn_signal_info = + get_turn_signal(*start_pose, getLaneChangePath().info.lane_changing_end); + + const double nearest_dist_threshold = common_param.ego_nearest_dist_threshold; + const double nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; + const size_t current_nearest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - // not in the vicinity of the end of the path. return original - return original_turn_signal_info; + return getTurnSignalDecider().overwrite_turn_signal( + path, current_pose, current_nearest_seg_idx, original_turn_signal_info, + terminal_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold); } LaneChangePath NormalLaneChange::getLaneChangePath() const @@ -319,14 +302,15 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() output.path = abort_path_->path; insertStopPoint(get_current_lanes(), output.path); } else { - output.path = getLaneChangePath().path; + output.path = status_.lane_change_path.path; const auto found_extended_path = extendPath(); if (found_extended_path) { output.path = utils::combinePath(output.path, *found_extended_path); } output.reference_path = getReferencePath(); - output.turn_signal_info = updateOutputTurnSignal(); + output.turn_signal_info = + get_turn_signal(getEgoPose(), status_.lane_change_path.info.lane_changing_end); if (isStopState()) { const auto current_velocity = getEgoVelocity(); 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 9585449b8e0e2..ef1648103ddde 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 @@ -58,7 +58,7 @@ double calc_stopping_distance(const LCParamPtr & lc_param_ptr) } double calc_dist_to_last_fit_width( - const lanelet::ConstLanelets lanelets, const Pose & src_pose, + const lanelet::ConstLanelets & lanelets, const Pose & src_pose, const BehaviorPathPlannerParameters & bpp_param, const double margin) { if (lanelets.empty()) return 0.0; From c9bb1e1a043c85a278aabf8c7c104029e5268131 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Mon, 26 Aug 2024 11:20:01 +0900 Subject: [PATCH 32/46] feat(out_of_lane): redesign to improve accuracy and performance (#8453) Signed-off-by: Maxime CLEMENT --- .../motion_utils/trajectory/trajectory.hpp | 9 + .../src/trajectory/trajectory.cpp | 23 ++ .../src/obstacle_velocity_limiter_module.cpp | 1 - .../README.md | 37 +- .../config/out_of_lane.param.yaml | 25 +- .../package.xml | 1 - .../src/calculate_slowdown_points.cpp | 131 ++++-- .../src/calculate_slowdown_points.hpp | 45 +- .../src/debug.cpp | 280 ++++++------- .../src/debug.hpp | 8 +- .../src/decisions.cpp | 389 ------------------ .../src/decisions.hpp | 116 ------ .../src/filter_predicted_objects.cpp | 46 +-- .../src/filter_predicted_objects.hpp | 4 +- .../src/footprint.cpp | 23 +- .../src/footprint.hpp | 12 +- .../src/lanelets_selection.cpp | 116 ++++-- .../src/lanelets_selection.hpp | 37 +- .../src/out_of_lane_collisions.cpp | 166 ++++++++ .../src/out_of_lane_collisions.hpp | 55 +++ .../src/out_of_lane_module.cpp | 317 +++++++------- .../src/out_of_lane_module.hpp | 18 +- .../src/overlapping_range.cpp | 127 ------ .../src/overlapping_range.hpp | 63 --- .../src/types.hpp | 216 +++------- .../CMakeLists.txt | 17 +- .../collision_checker.hpp | 69 ++++ .../planner_data.hpp | 29 +- .../package.xml | 1 - .../src/collision_checker.cpp | 70 ++++ .../test/test_collision_checker.cpp | 176 ++++++++ .../package.xml | 1 - .../src/node.cpp | 35 +- 33 files changed, 1236 insertions(+), 1427 deletions(-) delete mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp delete mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp delete mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp delete mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/collision_checker.hpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/collision_checker.cpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp index 126e66772920a..0ce42fbe1080f 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp @@ -2504,6 +2504,15 @@ extern template bool isTargetPointFront & trajectory, + const geometry_msgs::msg::Point & current_ego_point, const float min_velocity = 1.0f); + } // namespace autoware::motion_utils #endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ diff --git a/common/autoware_motion_utils/src/trajectory/trajectory.cpp b/common/autoware_motion_utils/src/trajectory/trajectory.cpp index ce26952c3d634..080d8ca8c7437 100644 --- a/common/autoware_motion_utils/src/trajectory/trajectory.cpp +++ b/common/autoware_motion_utils/src/trajectory/trajectory.cpp @@ -599,4 +599,27 @@ template bool isTargetPointFront & trajectory, + const geometry_msgs::msg::Point & current_ego_point, const float min_velocity) +{ + const auto nearest_segment_idx = findNearestSegmentIndex(trajectory, current_ego_point); + if (nearest_segment_idx + 1 == trajectory.size()) { + return; + } + for (auto & p : trajectory) { + p.time_from_start = rclcpp::Duration::from_seconds(0); + } + // TODO(Maxime): some points can have very low velocities which introduce huge time errors + // Temporary solution: use a minimum velocity + for (auto idx = nearest_segment_idx + 1; idx < trajectory.size(); ++idx) { + const auto & from = trajectory[idx - 1]; + const auto velocity = std::max(min_velocity, from.longitudinal_velocity_mps); + if (velocity != 0.0) { + auto & to = trajectory[idx]; + const auto t = universe_utils::calcDistance2d(from, to) / velocity; + to.time_from_start = rclcpp::Duration::from_seconds(t) + from.time_from_start; + } + } +} } // namespace autoware::motion_utils diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index 86bcf9df318bd..acf1dc1878087 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp @@ -25,7 +25,6 @@ #include #include #include -#include #include #include #include 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 99396eb0edf22..54b7cebb7c8ff 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 @@ -129,33 +129,24 @@ Moreover, parameter `action.distance_buffer` adds an extra distance between the | -------------------- | ------ | ---------------------------------------------------------------- | | `time_threshold` | double | [s] consider objects that will reach an overlap within this time | -| Parameter /intervals | Type | Description | -| --------------------- | ------ | ------------------------------------------------------- | -| `ego_time_buffer` | double | [s] extend the ego time interval by this buffer | -| `objects_time_buffer` | double | [s] extend the time intervals of objects by this buffer | - | Parameter /ttc | Type | Description | | -------------- | ------ | ------------------------------------------------------------------------------------------------------ | | `threshold` | double | [s] consider objects with an estimated time to collision bellow this value while ego is on the overlap | -| Parameter /objects | Type | Description | -| ------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `minimum_velocity` | double | [m/s] ignore objects with a velocity lower than this value | -| `predicted_path_min_confidence` | double | [-] minimum confidence required for a predicted path to be considered | -| `use_predicted_paths` | bool | [-] if true, use the predicted paths to estimate future positions; if false, assume the object moves at constant velocity along _all_ lanelets it currently is located in | - -| Parameter /overlap | Type | Description | -| ------------------ | ------ | ---------------------------------------------------------------------------------------------------- | -| `minimum_distance` | double | [m] minimum distance inside a lanelet for an overlap to be considered | -| `extra_length` | double | [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) | - -| Parameter /action | Type | Description | -| ----------------------------- | ------ | ---------------------------------------------------------------------------------------------- | -| `skip_if_over_max_decel` | bool | [-] if true, do not take an action that would cause more deceleration than the maximum allowed | -| `distance_buffer` | double | [m] buffer distance to try to keep between the ego footprint and lane | -| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap | -| `slowdown.velocity` | double | [m] slow down velocity | -| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | +| Parameter /objects | Type | Description | +| ------------------------------- | ------ | --------------------------------------------------------------------- | +| `minimum_velocity` | double | [m/s] ignore objects with a velocity lower than this value | +| `predicted_path_min_confidence` | double | [-] minimum confidence required for a predicted path to be considered | + +| Parameter /overlap | Type | Description | +| ------------------ | ------ | --------------------------------------------------------------------- | +| `minimum_distance` | double | [m] minimum distance inside a lanelet for an overlap to be considered | + +| Parameter /action | Type | Description | +| ----------------------------- | ------ | --------------------------------------------------------------------- | +| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap | +| `slowdown.velocity` | double | [m] slow down velocity | +| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | | Parameter /ego | Type | Description | | -------------------- | ------ | ---------------------------------------------------- | 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 a0cf69ee71027..67db597d80752 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 @@ -1,32 +1,23 @@ /**: ros__parameters: 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", "intervals", or "ttc" + mode: ttc # mode used to consider a conflict with an object. "threshold", or "ttc" skip_if_already_overlapping: true # 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: time_threshold: 5.0 # [s] consider objects that will reach an overlap within this time - intervals: # consider objects if their estimated time interval spent on the overlap intersect the estimated time interval of ego - ego_time_buffer: 0.5 # [s] extend the ego time interval by this buffer - objects_time_buffer: 0.5 # [s] extend the time intervals of objects by this buffer ttc: threshold: 3.0 # [s] consider objects with an estimated time to collision bellow this value while on the overlap objects: minimum_velocity: 0.5 # [m/s] objects lower than this velocity will be ignored - use_predicted_paths: true # if true, use the predicted paths to estimate future positions. - # if false, assume the object moves at constant velocity along *all* lanelets it currently is located in. predicted_path_min_confidence : 0.1 # when using predicted paths, ignore the ones whose confidence is lower than this value. - distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane cut_predicted_paths_beyond_red_lights: true # if true, predicted paths are cut beyond the stop line of red traffic lights ignore_behind_ego: true # if true, objects behind the ego vehicle are ignored - overlap: - minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered - extra_length: 0.0 # [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) - action: # action to insert in the trajectory if an object causes a conflict at an overlap - skip_if_over_max_decel: true # if true, do not take an action that would cause more deceleration than the maximum allowed precision: 0.1 # [m] precision when inserting a stop pose in the trajectory longitudinal_distance_buffer: 1.5 # [m] safety distance buffer to keep in front of the ego vehicle lateral_distance_buffer: 1.0 # [m] safety distance buffer to keep on the side of the ego vehicle @@ -38,8 +29,8 @@ distance_threshold: 15.0 # [m] insert a stop when closer than this distance from an overlap ego: - min_assumed_velocity: 2.0 # [m/s] minimum velocity used to calculate the enter and exit times of ego - extra_front_offset: 0.0 # [m] extra front distance - extra_rear_offset: 0.0 # [m] extra rear distance - extra_right_offset: 0.0 # [m] extra right distance - extra_left_offset: 0.0 # [m] extra left distance + # extra footprint offsets to calculate out of lane collisions + extra_front_offset: 0.0 # [m] extra footprint front distance + extra_rear_offset: 0.0 # [m] extra footprint rear distance + extra_right_offset: 0.0 # [m] extra footprint right distance + extra_left_offset: 0.0 # [m] extra footprint left distance 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 4908dbb4467be..3336278c761c6 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 @@ -15,7 +15,6 @@ autoware_cmake - autoware_lanelet2_extension autoware_motion_utils autoware_motion_velocity_planner_common autoware_perception_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp index 8b03fa66eab55..b7cd55ce2e642 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp @@ -15,74 +15,117 @@ #include "calculate_slowdown_points.hpp" #include "footprint.hpp" +#include "types.hpp" #include #include +#include -#include +#include +#include +#include +#include + +#include #include +#include +#include +#include + namespace autoware::motion_velocity_planner::out_of_lane { -bool can_decelerate( - const EgoData & ego_data, const TrajectoryPoint & point, const double target_vel) +std::optional calculate_last_avoiding_pose( + const std::vector & trajectory, + const universe_utils::Polygon2d & footprint, const lanelet::BasicPolygons2d & polygons_to_avoid, + const double min_arc_length, const double max_arc_length, const double precision) { - // TODO(Maxime): use the library function - const auto dist_ahead_of_ego = autoware::motion_utils::calcSignedArcLength( - ego_data.trajectory_points, ego_data.pose.position, point.pose.position); - const auto acc_to_target_vel = - (ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego); - return acc_to_target_vel < std::abs(ego_data.max_decel); + geometry_msgs::msg::Pose interpolated_pose{}; + bool is_avoiding_pose = false; + + auto from = min_arc_length; + auto to = max_arc_length; + while (to - from > precision) { + auto l = from + 0.5 * (to - from); + interpolated_pose = motion_utils::calcInterpolatedPose(trajectory, l); + const auto interpolated_footprint = project_to_pose(footprint, interpolated_pose); + is_avoiding_pose = + std::all_of(polygons_to_avoid.begin(), polygons_to_avoid.end(), [&](const auto & polygon) { + return boost::geometry::disjoint(interpolated_footprint, polygon); + }); + if (is_avoiding_pose) { + from = l; + } else { + to = l; + } + } + if (is_avoiding_pose) { + return interpolated_pose; + } + return std::nullopt; } -std::optional calculate_last_in_lane_pose( - const EgoData & ego_data, const Slowdown & decision, - const autoware::universe_utils::Polygon2d & footprint, - const std::optional & prev_slowdown_point, const PlannerParam & params) +std::optional calculate_pose_ahead_of_collision( + const EgoData & ego_data, const OutOfLanePoint & point_to_avoid, + const universe_utils::Polygon2d & footprint, const double precision) { - const auto from_arc_length = autoware::motion_utils::calcSignedArcLength( - ego_data.trajectory_points, 0, ego_data.first_trajectory_idx); - const auto to_arc_length = autoware::motion_utils::calcSignedArcLength( - ego_data.trajectory_points, 0, decision.target_trajectory_idx); - TrajectoryPoint interpolated_point; - for (auto l = to_arc_length - params.precision; l > from_arc_length; l -= params.precision) { - // TODO(Maxime): binary search - interpolated_point.pose = - autoware::motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l); - const auto respect_decel_limit = - !params.skip_if_over_max_decel || prev_slowdown_point || - can_decelerate(ego_data, interpolated_point, decision.velocity); - const auto interpolated_footprint = project_to_pose(footprint, interpolated_point.pose); - const auto is_overlap_lane = boost::geometry::overlaps( - interpolated_footprint, decision.lane_to_avoid.polygon2d().basicPolygon()); - const auto is_overlap_extra_lane = - prev_slowdown_point && - boost::geometry::overlaps( - interpolated_footprint, - prev_slowdown_point->slowdown.lane_to_avoid.polygon2d().basicPolygon()); - if (respect_decel_limit && !is_overlap_lane && !is_overlap_extra_lane) - return interpolated_point; + const auto first_avoid_arc_length = motion_utils::calcSignedArcLength( + ego_data.trajectory_points, 0UL, point_to_avoid.trajectory_index); + for (auto l = first_avoid_arc_length - precision; l >= ego_data.min_stop_arc_length; + l -= precision) { + 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)) { + return interpolated_pose; + } } return std::nullopt; } -std::optional calculate_slowdown_point( - const EgoData & ego_data, const std::vector & decisions, - const std::optional & prev_slowdown_point, PlannerParam params) +std::optional calculate_slowdown_point( + const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, PlannerParam params) { + const auto point_to_avoid_it = std::find_if( + out_of_lane_data.outside_points.cbegin(), out_of_lane_data.outside_points.cend(), + [&](const auto & p) { return p.to_avoid; }); + if (point_to_avoid_it == out_of_lane_data.outside_points.cend()) { + return std::nullopt; + } + const auto raw_footprint = make_base_footprint(params, true); // ignore extra footprint offsets + const auto base_footprint = make_base_footprint(params); params.extra_front_offset += params.lon_dist_buffer; params.extra_right_offset += params.lat_dist_buffer; params.extra_left_offset += params.lat_dist_buffer; - const auto base_footprint = make_base_footprint(params); + const auto expanded_footprint = make_base_footprint(params); // with added distance buffers + lanelet::BasicPolygons2d polygons_to_avoid; + for (const auto & ll : point_to_avoid_it->overlapped_lanelets) { + polygons_to_avoid.push_back(ll.polygon2d().basicPolygon()); + } + // points are ordered by trajectory index so the first one has the smallest index and arc length + const auto first_outside_idx = out_of_lane_data.outside_points.front().trajectory_index; + const auto first_outside_arc_length = + motion_utils::calcSignedArcLength(ego_data.trajectory_points, 0UL, first_outside_idx); + std::optional slowdown_point; // search for the first slowdown decision for which a stop point can be inserted - for (const auto & decision : decisions) { - const auto last_in_lane_pose = - calculate_last_in_lane_pose(ego_data, decision, base_footprint, prev_slowdown_point, params); - if (last_in_lane_pose) return SlowdownToInsert{decision, *last_in_lane_pose}; + // we first try to use the expanded footprint (distance buffers + extra footprint offsets) + for (const auto & footprint : {expanded_footprint, base_footprint, raw_footprint}) { + slowdown_point = calculate_last_avoiding_pose( + ego_data.trajectory_points, footprint, polygons_to_avoid, ego_data.min_stop_arc_length, + first_outside_arc_length, params.precision); + if (slowdown_point) { + break; + } } - return std::nullopt; + // fallback to simply stopping ahead of the collision to avoid (regardless of being out of lane or + // not) + if (!slowdown_point) { + slowdown_point = calculate_pose_ahead_of_collision( + ego_data, *point_to_avoid_it, expanded_footprint, params.precision); + } + return slowdown_point; } } // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp index 145f21c94c0d0..394334d434558 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -19,40 +19,39 @@ #include +#include + #include -#include namespace autoware::motion_velocity_planner::out_of_lane { - -/// @brief estimate whether ego can decelerate without breaking the deceleration limit -/// @details assume ego wants to reach the target point at the target velocity -/// @param [in] ego_data ego data -/// @param [in] point target point -/// @param [in] target_vel target_velocity -bool can_decelerate( - const EgoData & ego_data, const TrajectoryPoint & point, const double target_vel); - -/// @brief calculate the last pose along the trajectory where ego does not overlap the lane to avoid +/// @brief calculate the last pose along the trajectory where ego does not go out of lane /// @param [in] ego_data ego data -/// @param [in] decision the input decision (i.e., which lane to avoid and at what speed) /// @param [in] footprint the ego footprint -/// @param [in] prev_slowdown_point previous slowdown point. If set, ignore deceleration limits -/// @param [in] params parameters +/// @param [in] min_arc_length minimum arc length for the search +/// @param [in] max_arc_length maximum arc length for the search +/// @param [in] precision [m] search precision /// @return the last pose that is not out of lane (if found) -std::optional calculate_last_in_lane_pose( - const EgoData & ego_data, const Slowdown & decision, - const autoware::universe_utils::Polygon2d & footprint, - const std::optional & prev_slowdown_point, const PlannerParam & params); +std::optional calculate_last_in_lane_pose( + const EgoData & ego_data, const autoware::universe_utils::Polygon2d & footprint, + const double min_arc_length, const double max_arc_length, const double precision); + +/// @brief calculate the slowdown pose just ahead of a point to avoid +/// @param ego_data ego data (trajectory, velocity, etc) +/// @param point_to_avoid the point to avoid +/// @param footprint the ego footprint +/// @param params parameters +/// @return optional slowdown point to insert in the trajectory +std::optional calculate_pose_ahead_of_collision( + const EgoData & ego_data, const OutOfLanePoint & point_to_avoid, + const universe_utils::Polygon2d & footprint, const double precision); /// @brief calculate the slowdown point to insert in the trajectory /// @param ego_data ego data (trajectory, velocity, etc) -/// @param decisions decision (before which point to stop, what lane to avoid entering, etc) -/// @param prev_slowdown_point previously calculated slowdown point +/// @param out_of_lane_data data about out of lane areas /// @param params parameters /// @return optional slowdown point to insert in the trajectory -std::optional calculate_slowdown_point( - const EgoData & ego_data, const std::vector & decisions, - const std::optional & prev_slowdown_point, PlannerParam params); +std::optional calculate_slowdown_point( + const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, PlannerParam params); } // namespace autoware::motion_velocity_planner::out_of_lane #endif // CALCULATE_SLOWDOWN_POINTS_HPP_ 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 d9a85e266f790..cb29f9b3b42ea 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 @@ -16,12 +16,22 @@ #include "types.hpp" +#include +#include +#include #include +#include +#include #include +#include + +#include +#include +#include + #include -#include namespace autoware::motion_velocity_planner::out_of_lane::debug { @@ -36,209 +46,167 @@ visualization_msgs::msg::Marker get_base_marker() base_marker.id = 0; base_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; base_marker.action = visualization_msgs::msg::Marker::ADD; - base_marker.pose.position = autoware::universe_utils::createMarkerPosition(0.0, 0.0, 0); - base_marker.pose.orientation = autoware::universe_utils::createMarkerOrientation(0, 0, 0, 1.0); - base_marker.scale = autoware::universe_utils::createMarkerScale(0.1, 0.1, 0.1); - base_marker.color = autoware::universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); + base_marker.pose.position = universe_utils::createMarkerPosition(0.0, 0.0, 0); + base_marker.pose.orientation = universe_utils::createMarkerOrientation(0, 0, 0, 1.0); + base_marker.scale = universe_utils::createMarkerScale(0.1, 0.1, 0.1); + base_marker.color = universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); return base_marker; } -void add_footprint_markers( +void add_polygons_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::BasicPolygons2d & footprints, const double z, const size_t prev_nb) + const lanelet::BasicPolygons2d & polygons, const double z, const std::string & ns) { auto debug_marker = get_base_marker(); - debug_marker.ns = "footprints"; - for (const auto & f : footprints) { - debug_marker.points.clear(); - for (const auto & p : f) - debug_marker.points.push_back( - autoware::universe_utils::createMarkerPosition(p.x(), p.y(), z + 0.5)); - debug_marker.points.push_back(debug_marker.points.front()); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; - debug_marker.points.clear(); + debug_marker.ns = ns; + debug_marker.type = visualization_msgs::msg::Marker::LINE_LIST; + for (const auto & f : polygons) { + boost::geometry::for_each_segment(f, [&](const auto & s) { + const auto & [p1, p2] = s; + debug_marker.points.push_back(universe_utils::createMarkerPosition(p1.x(), p1.y(), z + 0.5)); + debug_marker.points.push_back(universe_utils::createMarkerPosition(p2.x(), p2.y(), z + 0.5)); + }); } - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) - debug_marker_array.markers.push_back(debug_marker); + debug_marker_array.markers.push_back(debug_marker); } void add_current_overlap_marker( visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::BasicPolygon2d & current_footprint, - const lanelet::ConstLanelets & current_overlapped_lanelets, const double z, const size_t prev_nb) + const lanelet::BasicPolygon2d & current_footprint, const double z) { auto debug_marker = get_base_marker(); debug_marker.ns = "current_overlap"; debug_marker.points.clear(); for (const auto & p : current_footprint) - debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition(p.x(), p.y(), z)); + debug_marker.points.push_back(universe_utils::createMarkerPosition(p.x(), p.y(), z)); if (!debug_marker.points.empty()) debug_marker.points.push_back(debug_marker.points.front()); - if (current_overlapped_lanelets.empty()) - debug_marker.color = autoware::universe_utils::createMarkerColor(0.1, 1.0, 0.1, 0.5); - else - debug_marker.color = autoware::universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); + debug_marker.color = universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); debug_marker_array.markers.push_back(debug_marker); debug_marker.id++; - for (const auto & ll : current_overlapped_lanelets) { - debug_marker.points.clear(); - for (const auto & p : ll.polygon3d()) - debug_marker.points.push_back( - autoware::universe_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.5)); - debug_marker.points.push_back(debug_marker.points.front()); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; - } - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) - debug_marker_array.markers.push_back(debug_marker); -} - -void add_lanelet_markers( - visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::ConstLanelets & lanelets, const std::string & ns, - const std_msgs::msg::ColorRGBA & color, const size_t prev_nb) -{ - auto debug_marker = get_base_marker(); - debug_marker.ns = ns; - debug_marker.color = color; - for (const auto & ll : lanelets) { - debug_marker.points.clear(); - - // add a small z offset to draw above the lanelet map - for (const auto & p : ll.polygon3d()) - debug_marker.points.push_back( - autoware::universe_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.1)); - debug_marker.points.push_back(debug_marker.points.front()); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; - } - debug_marker.action = debug_marker.DELETE; - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) - debug_marker_array.markers.push_back(debug_marker); } -void add_range_markers( - visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, - const std::vector & trajectory_points, - const size_t first_ego_idx, const double z, const size_t prev_nb) +void add_ttc_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, const EgoData & ego_data, + const OutOfLaneData & out_of_lane_data, const size_t prev_nb) { auto debug_marker = get_base_marker(); - debug_marker.ns = "ranges"; - debug_marker.color = autoware::universe_utils::createMarkerColor(0.2, 0.9, 0.1, 0.5); - for (const auto & range : ranges) { - debug_marker.points.clear(); - debug_marker.points.push_back( - trajectory_points[first_ego_idx + range.entering_trajectory_idx].pose.position); - debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( - range.entering_point.x(), range.entering_point.y(), z)); - for (const auto & overlap : range.debug.overlaps) { - debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( - overlap.min_overlap_point.x(), overlap.min_overlap_point.y(), z)); - debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( - overlap.max_overlap_point.x(), overlap.max_overlap_point.y(), z)); + debug_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + debug_marker.scale.z = 0.5; + debug_marker.ns = "ttcs"; + for (const auto & p : out_of_lane_data.outside_points) { + if (p.to_avoid) { + debug_marker.color.r = 1.0; + debug_marker.color.g = 0.0; + } else { + debug_marker.color.r = 0.0; + debug_marker.color.g = 1.0; + } + if (p.ttc) { + debug_marker.pose = ego_data.trajectory_points[p.trajectory_index].pose; + debug_marker.text = std::to_string(*p.ttc); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; } - debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( - range.exiting_point.x(), range.exiting_point.y(), z)); - debug_marker.points.push_back( - trajectory_points[first_ego_idx + range.exiting_trajectory_idx].pose.position); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; } - debug_marker.action = debug_marker.DELETE; - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker.action = visualization_msgs::msg::Marker::DELETE; + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) { debug_marker_array.markers.push_back(debug_marker); + } } - -void add_decision_markers( - visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, +size_t add_stop_line_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, const StopLinesRtree & rtree, const double z, const size_t prev_nb) { auto debug_marker = get_base_marker(); - debug_marker.action = debug_marker.ADD; - debug_marker.id = 0; - debug_marker.ns = "decisions"; - debug_marker.color = autoware::universe_utils::createMarkerColor(0.9, 0.1, 0.1, 1.0); - debug_marker.points.clear(); - for (const auto & range : ranges) { - debug_marker.type = debug_marker.LINE_STRIP; - if (range.debug.decision) { - debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( - range.entering_point.x(), range.entering_point.y(), z)); - debug_marker.points.push_back( - range.debug.object->kinematics.initial_pose_with_covariance.pose.position); + debug_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + debug_marker.ns = "stop_lines"; + const auto & add_lanelets_markers = [&](const auto & lanelets) { + for (const auto & ll : lanelets) { + debug_marker.points.clear(); + for (const auto & p : ll.polygon2d().basicPolygon()) { + debug_marker.points.push_back(universe_utils::createMarkerPosition(p.x(), p.y(), z + 0.5)); + } + debug_marker.points.push_back(debug_marker.points.front()); + debug_marker_array.markers.push_back(debug_marker); + ++debug_marker.id; } - debug_marker_array.markers.push_back(debug_marker); + }; + for (const auto & [_, stop_line] : rtree) { debug_marker.points.clear(); - debug_marker.id++; - - debug_marker.type = debug_marker.TEXT_VIEW_FACING; - debug_marker.pose.position.x = range.entering_point.x(); - debug_marker.pose.position.y = range.entering_point.y(); - debug_marker.pose.position.z = z; - std::stringstream ss; - ss << "Ego: " << range.debug.times.ego.enter_time << " - " << range.debug.times.ego.exit_time - << "\n"; - if (range.debug.object) { - debug_marker.pose.position.x += - range.debug.object->kinematics.initial_pose_with_covariance.pose.position.x; - debug_marker.pose.position.y += - range.debug.object->kinematics.initial_pose_with_covariance.pose.position.y; - debug_marker.pose.position.x /= 2; - debug_marker.pose.position.y /= 2; - ss << "Obj: " << range.debug.times.object.enter_time << " - " - << range.debug.times.object.exit_time << "\n"; + debug_marker.color.r = 1.0; + for (const auto & p : stop_line.stop_line) { + debug_marker.points.push_back(universe_utils::createMarkerPosition(p.x(), p.y(), z + 0.5)); } - debug_marker.scale.z = 1.0; - debug_marker.text = ss.str(); debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; + ++debug_marker.id; + debug_marker.color.r = 0.0; + add_lanelets_markers(stop_line.lanelets); } - debug_marker.action = debug_marker.DELETE; + const auto max_id = debug_marker.id; + debug_marker.action = visualization_msgs::msg::Marker::DELETE; for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) { debug_marker_array.markers.push_back(debug_marker); } + return max_id; } } // namespace -visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & debug_data) + +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) { - constexpr auto z = 0.0; + const auto z = ego_data.pose.position.z; visualization_msgs::msg::MarkerArray debug_marker_array; - debug::add_footprint_markers( - debug_marker_array, debug_data.footprints, z, debug_data.prev_footprints); - debug::add_current_overlap_marker( - debug_marker_array, debug_data.current_footprint, debug_data.current_overlapped_lanelets, z, - debug_data.prev_current_overlapped_lanelets); - debug::add_lanelet_markers( - debug_marker_array, debug_data.trajectory_lanelets, "trajectory_lanelets", - autoware::universe_utils::createMarkerColor(0.1, 0.1, 1.0, 0.5), - debug_data.prev_trajectory_lanelets); - debug::add_lanelet_markers( - debug_marker_array, debug_data.ignored_lanelets, "ignored_lanelets", - autoware::universe_utils::createMarkerColor(0.7, 0.7, 0.2, 0.5), - debug_data.prev_ignored_lanelets); - debug::add_lanelet_markers( - debug_marker_array, debug_data.other_lanelets, "other_lanelets", - autoware::universe_utils::createMarkerColor(0.4, 0.4, 0.7, 0.5), - debug_data.prev_other_lanelets); - debug::add_range_markers( - debug_marker_array, debug_data.ranges, debug_data.trajectory_points, - debug_data.first_trajectory_idx, z, debug_data.prev_ranges); - debug::add_decision_markers(debug_marker_array, debug_data.ranges, z, debug_data.prev_ranges); + // add_polygons_markers(debug_marker_array, ego_data.trajectory_footprints, z, "footprints"); + + lanelet::BasicPolygons2d drivable_lane_polygons; + for (const auto & poly : ego_data.drivable_lane_polygons) { + drivable_lane_polygons.push_back(poly.outer); + } + add_polygons_markers(debug_marker_array, drivable_lane_polygons, z, "ego_lane"); + + 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); + } + add_polygons_markers(debug_marker_array, out_of_lane_areas, z, "out_of_lane_areas"); + + lanelet::BasicPolygons2d object_polygons; + 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) { + const auto poly = universe_utils::toPolygon2d(pose, o.shape).outer(); + lanelet::BasicPolygon2d ll_poly(poly.begin(), poly.end()); + object_polygons.push_back(ll_poly); + } + } + } + } + add_polygons_markers(debug_marker_array, object_polygons, z, "objects"); + + add_current_overlap_marker(debug_marker_array, ego_data.current_footprint, z); + + add_ttc_markers(debug_marker_array, ego_data, out_of_lane_data, debug_data.prev_ttcs); + debug_data.prev_ttcs = out_of_lane_data.outside_points.size(); + + debug_data.prev_stop_line = add_stop_line_markers( + debug_marker_array, ego_data.stop_lines_rtree, z, debug_data.prev_stop_line); + return debug_marker_array; } -autoware::motion_utils::VirtualWalls create_virtual_walls( - const DebugData & debug_data, const PlannerParam & params) +motion_utils::VirtualWalls create_virtual_walls( + const geometry_msgs::msg::Pose & pose, const bool stop, const PlannerParam & params) { - autoware::motion_utils::VirtualWalls virtual_walls; - autoware::motion_utils::VirtualWall wall; + motion_utils::VirtualWalls virtual_walls; + motion_utils::VirtualWall wall; wall.text = "out_of_lane"; wall.longitudinal_offset = params.front_offset; - wall.style = autoware::motion_utils::VirtualWallType::slowdown; - for (const auto & slowdown : debug_data.slowdowns) { - wall.pose = slowdown.point.pose; - virtual_walls.push_back(wall); - } + wall.style = stop ? motion_utils::VirtualWallType::stop : motion_utils::VirtualWallType::slowdown; + wall.pose = pose; + virtual_walls.push_back(wall); return virtual_walls; } } // namespace autoware::motion_velocity_planner::out_of_lane::debug diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp index ea225442443c8..dc1f942655612 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp @@ -23,9 +23,11 @@ namespace autoware::motion_velocity_planner::out_of_lane::debug { -visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & debug_data); -autoware::motion_utils::VirtualWalls create_virtual_walls( - const DebugData & debug_data, const PlannerParam & params); +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); +motion_utils::VirtualWalls create_virtual_walls( + const geometry_msgs::msg::Pose & pose, const bool stop, const PlannerParam & params); } // namespace autoware::motion_velocity_planner::out_of_lane::debug #endif // DEBUG_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp deleted file mode 100644 index 2be4c89b02779..0000000000000 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp +++ /dev/null @@ -1,389 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "decisions.hpp" - -#include -#include -#include -#include - -#include - -#include -#include -#include - -#include -#include -#include -#include -namespace autoware::motion_velocity_planner::out_of_lane -{ -double distance_along_trajectory(const EgoData & ego_data, const size_t target_idx) -{ - return autoware::motion_utils::calcSignedArcLength( - ego_data.trajectory_points, ego_data.pose.position, ego_data.first_trajectory_idx + target_idx); -} - -double time_along_trajectory( - const EgoData & ego_data, const size_t target_idx, const double min_velocity) -{ - const auto dist = distance_along_trajectory(ego_data, target_idx); - const auto v = std::max( - std::max(ego_data.velocity, min_velocity), - ego_data.trajectory_points[ego_data.first_trajectory_idx + target_idx] - .longitudinal_velocity_mps * - 0.5); - return dist / v; -} - -bool object_is_incoming( - const lanelet::BasicPoint2d & object_position, - const std::shared_ptr route_handler, - const lanelet::ConstLanelet & lane) -{ - const auto lanelets = route_handler->getPrecedingLaneletSequence(lane, 50.0); - if (boost::geometry::within(object_position, lane.polygon2d().basicPolygon())) return true; - for (const auto & lls : lanelets) - for (const auto & ll : lls) - if (boost::geometry::within(object_position, ll.polygon2d().basicPolygon())) return true; - return false; -} - -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const std::shared_ptr route_handler, const double dist_buffer, - const rclcpp::Logger & logger) -{ - // skip the dynamic object if it is not in a lane preceding the overlapped lane - // lane changes are intentionally not considered - const auto object_point = lanelet::BasicPoint2d( - object.kinematics.initial_pose_with_covariance.pose.position.x, - object.kinematics.initial_pose_with_covariance.pose.position.y); - if (!object_is_incoming(object_point, route_handler, range.lane)) return {}; - - const auto max_deviation = object.shape.dimensions.y + range.inside_distance + dist_buffer; - const auto max_lon_deviation = object.shape.dimensions.x / 2.0; - auto worst_enter_time = std::optional(); - auto worst_exit_time = std::optional(); - - for (const auto & predicted_path : object.kinematics.predicted_paths) { - const auto unique_path_points = - autoware::motion_utils::removeOverlapPoints(predicted_path.path); - if (unique_path_points.size() < 2) continue; - const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds(); - const auto enter_point = - geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y()); - const auto enter_segment_idx = - autoware::motion_utils::findNearestSegmentIndex(unique_path_points, enter_point); - const auto enter_offset = autoware::motion_utils::calcLongitudinalOffsetToSegment( - unique_path_points, enter_segment_idx, enter_point); - const auto enter_lat_dist = std::abs(autoware::motion_utils::calcLateralOffset( - unique_path_points, enter_point, enter_segment_idx)); - const auto enter_segment_length = autoware::universe_utils::calcDistance2d( - unique_path_points[enter_segment_idx], unique_path_points[enter_segment_idx + 1]); - const auto enter_offset_ratio = enter_offset / enter_segment_length; - const auto enter_time = - static_cast(enter_segment_idx) * time_step + enter_offset_ratio * time_step; - - const auto exit_point = - geometry_msgs::msg::Point().set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); - const auto exit_segment_idx = - autoware::motion_utils::findNearestSegmentIndex(unique_path_points, exit_point); - const auto exit_offset = autoware::motion_utils::calcLongitudinalOffsetToSegment( - unique_path_points, exit_segment_idx, exit_point); - const auto exit_lat_dist = std::abs( - autoware::motion_utils::calcLateralOffset(unique_path_points, exit_point, exit_segment_idx)); - const auto exit_segment_length = autoware::universe_utils::calcDistance2d( - unique_path_points[exit_segment_idx], unique_path_points[exit_segment_idx + 1]); - const auto exit_offset_ratio = exit_offset / static_cast(exit_segment_length); - const auto exit_time = - static_cast(exit_segment_idx) * time_step + exit_offset_ratio * time_step; - - RCLCPP_DEBUG( - logger, "\t\t\tPredicted path (time step = %2.2fs): enter @ %2.2fs, exit @ %2.2fs", time_step, - enter_time, exit_time); - // predicted path is too far from the overlapping range to be relevant - const auto is_far_from_entering_point = enter_lat_dist > max_deviation; - const auto is_far_from_exiting_point = exit_lat_dist > max_deviation; - if (is_far_from_entering_point && is_far_from_exiting_point) { - RCLCPP_DEBUG( - logger, - " * far_from_enter (%d) = %2.2fm | far_from_exit (%d) = %2.2fm | max_dev = %2.2fm\n", - is_far_from_entering_point, enter_lat_dist, is_far_from_exiting_point, exit_lat_dist, - max_deviation); - continue; - } - const auto is_last_predicted_path_point = - (exit_segment_idx + 2 == unique_path_points.size() || - enter_segment_idx + 2 == unique_path_points.size()); - const auto does_not_reach_overlap = - is_last_predicted_path_point && (std::min(exit_offset, enter_offset) > max_lon_deviation); - if (does_not_reach_overlap) { - RCLCPP_DEBUG( - logger, " * does not reach the overlap = %2.2fm | max_dev = %2.2fm\n", - std::min(exit_offset, enter_offset), max_lon_deviation); - continue; - } - - const auto same_driving_direction_as_ego = enter_time < exit_time; - if (same_driving_direction_as_ego) { - worst_enter_time = worst_enter_time ? std::min(*worst_enter_time, enter_time) : enter_time; - worst_exit_time = worst_exit_time ? std::max(*worst_exit_time, exit_time) : exit_time; - } else { - worst_enter_time = worst_enter_time ? std::max(*worst_enter_time, enter_time) : enter_time; - worst_exit_time = worst_exit_time ? std::min(*worst_exit_time, exit_time) : exit_time; - } - } - if (worst_enter_time && worst_exit_time) { - RCLCPP_DEBUG( - logger, "\t\t\t * found enter/exit time [%2.2f, %2.2f]\n", *worst_enter_time, - *worst_exit_time); - return std::make_pair(*worst_enter_time, *worst_exit_time); - } - RCLCPP_DEBUG(logger, "\t\t\t * enter/exit time not found\n"); - return {}; -} - -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const DecisionInputs & inputs, const rclcpp::Logger & logger) -{ - const auto & p = object.kinematics.initial_pose_with_covariance.pose.position; - const auto object_point = lanelet::BasicPoint2d(p.x, p.y); - const auto half_size = object.shape.dimensions.x / 2.0; - lanelet::ConstLanelets object_lanelets; - for (const auto & ll : inputs.lanelets) - if (boost::geometry::within(object_point, ll.polygon2d().basicPolygon())) - object_lanelets.push_back(ll); - - geometry_msgs::msg::Pose pose; - pose.position.set__x(range.entering_point.x()).set__y(range.entering_point.y()); - const auto range_enter_length = lanelet::utils::getArcCoordinates({range.lane}, pose).length; - pose.position.set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); - const auto range_exit_length = lanelet::utils::getArcCoordinates({range.lane}, pose).length; - const auto range_size = std::abs(range_enter_length - range_exit_length); - auto worst_enter_dist = std::optional(); - auto worst_exit_dist = std::optional(); - for (const auto & lane : object_lanelets) { - const auto path = inputs.route_handler->getRoutingGraphPtr()->shortestPath(lane, range.lane); - RCLCPP_DEBUG( - logger, "\t\t\tPath ? %d [from %ld to %ld]\n", path.has_value(), lane.id(), range.lane.id()); - if (path) { - lanelet::ConstLanelets lls; - for (const auto & ll : *path) lls.push_back(ll); - pose.position.set__x(object_point.x()).set__y(object_point.y()); - const auto object_curr_length = lanelet::utils::getArcCoordinates(lls, pose).length; - pose.position.set__x(range.entering_point.x()).set__y(range.entering_point.y()); - const auto enter_dist = - lanelet::utils::getArcCoordinates(lls, pose).length - object_curr_length; - pose.position.set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); - const auto exit_dist = - lanelet::utils::getArcCoordinates(lls, pose).length - object_curr_length; - RCLCPP_DEBUG( - logger, "\t\t\t%2.2f -> [%2.2f(%2.2f, %2.2f) - %2.2f(%2.2f, %2.2f)]\n", object_curr_length, - enter_dist, range.entering_point.x(), range.entering_point.y(), exit_dist, - range.exiting_point.x(), range.exiting_point.y()); - const auto already_entered_range = std::abs(enter_dist - exit_dist) > range_size * 2.0; - if (already_entered_range) continue; - // multiple paths to the overlap -> be conservative and use the "worst" case - // "worst" = min/max arc length depending on if the lane is running opposite to the ego - // trajectory - const auto is_opposite = enter_dist > exit_dist; - if (!worst_enter_dist) - worst_enter_dist = enter_dist; - else if (is_opposite) - worst_enter_dist = std::max(*worst_enter_dist, enter_dist); - else - worst_enter_dist = std::min(*worst_enter_dist, enter_dist); - if (!worst_exit_dist) - worst_exit_dist = exit_dist; - else if (is_opposite) - worst_exit_dist = std::max(*worst_exit_dist, exit_dist); - else - worst_exit_dist = std::min(*worst_exit_dist, exit_dist); - } - } - if (worst_enter_dist && worst_exit_dist) { - const auto v = object.kinematics.initial_twist_with_covariance.twist.linear.x; - return std::make_pair((*worst_enter_dist - half_size) / v, (*worst_exit_dist + half_size) / v); - } - return {}; -} - -bool threshold_condition(const RangeTimes & range_times, const PlannerParam & params) -{ - const auto enter_within_threshold = - range_times.object.enter_time > 0.0 && range_times.object.enter_time < params.time_threshold; - const auto exit_within_threshold = - range_times.object.exit_time > 0.0 && range_times.object.exit_time < params.time_threshold; - return enter_within_threshold || exit_within_threshold; -} - -bool intervals_condition( - const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) -{ - const auto opposite_way_condition = [&]() -> bool { - const auto ego_exits_before_object_enters = - range_times.ego.exit_time + params.intervals_ego_buffer < - range_times.object.enter_time + params.intervals_obj_buffer; - RCLCPP_DEBUG( - logger, - "\t\t\t[Intervals] (opposite way) ego exit %2.2fs < obj enter %2.2fs ? -> should not " - "enter = %d\n", - range_times.ego.exit_time + params.intervals_ego_buffer, - range_times.object.enter_time + params.intervals_obj_buffer, ego_exits_before_object_enters); - return ego_exits_before_object_enters; - }; - const auto same_way_condition = [&]() -> bool { - const auto object_enters_during_overlap = - range_times.ego.enter_time - params.intervals_ego_buffer < - range_times.object.enter_time + params.intervals_obj_buffer && - range_times.object.enter_time - params.intervals_obj_buffer - range_times.ego.exit_time < - range_times.ego.exit_time + params.intervals_ego_buffer; - const auto object_exits_during_overlap = - range_times.ego.enter_time - params.intervals_ego_buffer < - range_times.object.exit_time + params.intervals_obj_buffer && - range_times.object.exit_time - params.intervals_obj_buffer - range_times.ego.exit_time < - range_times.ego.exit_time + params.intervals_ego_buffer; - RCLCPP_DEBUG( - logger, - "\t\t\t[Intervals] obj enters during overlap ? %d OR obj exits during overlap %d ? -> should " - "not " - "enter = %d\n", - object_enters_during_overlap, object_exits_during_overlap, - object_enters_during_overlap || object_exits_during_overlap); - return object_enters_during_overlap || object_exits_during_overlap; - }; - - const auto object_is_going_same_way = - range_times.object.enter_time < range_times.object.exit_time; - return (object_is_going_same_way && same_way_condition()) || - (!object_is_going_same_way && opposite_way_condition()); -} - -bool ttc_condition( - const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) -{ - const auto ttc_at_enter = range_times.ego.enter_time - range_times.object.enter_time; - const auto ttc_at_exit = range_times.ego.exit_time - range_times.object.exit_time; - const auto collision_during_overlap = (ttc_at_enter < 0.0) != (ttc_at_exit < 0.0); - const auto ttc_is_bellow_threshold = - std::min(std::abs(ttc_at_enter), std::abs(ttc_at_exit)) <= params.ttc_threshold; - RCLCPP_DEBUG( - logger, "\t\t\t[TTC] (%2.2fs - %2.2fs) -> %d\n", ttc_at_enter, ttc_at_exit, - (collision_during_overlap || ttc_is_bellow_threshold)); - return collision_during_overlap || ttc_is_bellow_threshold; -} - -bool will_collide_on_range( - const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) -{ - RCLCPP_DEBUG( - logger, " enter at %2.2fs, exits at %2.2fs\n", range_times.object.enter_time, - range_times.object.exit_time); - return (params.mode == "threshold" && threshold_condition(range_times, params)) || - (params.mode == "intervals" && intervals_condition(range_times, params, logger)) || - (params.mode == "ttc" && ttc_condition(range_times, params, logger)); -} - -bool should_not_enter( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger) -{ - RangeTimes range_times{}; - range_times.ego.enter_time = - time_along_trajectory(inputs.ego_data, range.entering_trajectory_idx, params.ego_min_velocity); - range_times.ego.exit_time = - time_along_trajectory(inputs.ego_data, range.exiting_trajectory_idx, params.ego_min_velocity); - RCLCPP_DEBUG( - logger, "\t[%lu -> %lu] %ld | ego enters at %2.2f, exits at %2.2f\n", - range.entering_trajectory_idx, range.exiting_trajectory_idx, range.lane.id(), - range_times.ego.enter_time, range_times.ego.exit_time); - for (const auto & object : inputs.objects.objects) { - RCLCPP_DEBUG( - logger, "\t\t[%s] going at %2.2fm/s", - autoware::universe_utils::toHexString(object.object_id).c_str(), - object.kinematics.initial_twist_with_covariance.twist.linear.x); - if (object.kinematics.initial_twist_with_covariance.twist.linear.x < params.objects_min_vel) { - RCLCPP_DEBUG(logger, " SKIP (velocity bellow threshold %2.2fm/s)\n", params.objects_min_vel); - continue; // skip objects with velocity bellow a threshold - } - // skip objects that are already on the interval - const auto enter_exit_time = - params.objects_use_predicted_paths - ? object_time_to_range( - object, range, inputs.route_handler, params.objects_dist_buffer, logger) - : object_time_to_range(object, range, inputs, logger); - if (!enter_exit_time) { - RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n"); - continue; // skip objects that are not driving towards the overlapping range - } - - range_times.object.enter_time = enter_exit_time->first; - range_times.object.exit_time = enter_exit_time->second; - if (will_collide_on_range(range_times, params, logger)) { - range.debug.times = range_times; - range.debug.object = object; - return true; - } - } - range.debug.times = range_times; - return false; -} - -void set_decision_velocity( - std::optional & decision, const double distance, const PlannerParam & params) -{ - if (distance < params.stop_dist_threshold) { - decision->velocity = 0.0; - } else if (distance < params.slow_dist_threshold) { - decision->velocity = params.slow_velocity; - } else { - decision.reset(); - } -} - -std::optional calculate_decision( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger) -{ - std::optional decision; - if (should_not_enter(range, inputs, params, logger)) { - decision.emplace(); - decision->target_trajectory_idx = inputs.ego_data.first_trajectory_idx + - range.entering_trajectory_idx; // add offset from curr pose - decision->lane_to_avoid = range.lane; - const auto ego_dist_to_range = - distance_along_trajectory(inputs.ego_data, range.entering_trajectory_idx); - set_decision_velocity(decision, ego_dist_to_range, params); - } - return decision; -} - -std::vector calculate_decisions( - const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger) -{ - std::vector decisions; - for (const auto & range : inputs.ranges) { - if (range.entering_trajectory_idx == 0UL) continue; // skip if we already entered the range - const auto optional_decision = calculate_decision(range, inputs, params, logger); - range.debug.decision = optional_decision; - if (optional_decision) decisions.push_back(*optional_decision); - } - return decisions; -} - -} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp deleted file mode 100644 index 455cee272f7be..0000000000000 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp +++ /dev/null @@ -1,116 +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 DECISIONS_HPP_ -#define DECISIONS_HPP_ - -#include "types.hpp" - -#include -#include - -#include - -#include - -#include -#include -#include -#include - -namespace autoware::motion_velocity_planner::out_of_lane -{ -/// @brief calculate the distance along the ego trajectory between ego and some target trajectory -/// index -/// @param [in] ego_data data related to the ego vehicle -/// @param [in] target_idx target ego trajectory index -/// @return distance between ego and the target [m] -double distance_along_trajectory(const EgoData & ego_data, const size_t target_idx); -/// @brief estimate the time when ego will reach some target trajectory index -/// @param [in] ego_data data related to the ego vehicle -/// @param [in] target_idx target ego trajectory index -/// @param [in] min_velocity minimum ego velocity used to estimate the time -/// @return time taken by ego to reach the target [s] -double time_along_trajectory(const EgoData & ego_data, const size_t target_idx); -/// @brief use an object's predicted paths to estimate the times it will reach the enter and exit -/// points of an overlapping range -/// @details times when the predicted paths of the object enters/exits the range are calculated -/// but may not exist (e.g,, predicted path ends before reaching the end of the range) -/// @param [in] object dynamic object -/// @param [in] range overlapping range -/// @param [in] route_handler route handler used to estimate the path of the dynamic object -/// @param [in] logger ros logger -/// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in -/// the opposite direction, time at enter > time at exit -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const std::shared_ptr route_handler, const double dist_buffer, - const rclcpp::Logger & logger); -/// @brief use the lanelet map to estimate the times when an object will reach the enter and exit -/// points of an overlapping range -/// @param [in] object dynamic object -/// @param [in] range overlapping range -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] dist_buffer extra distance used to estimate if a collision will occur on the range -/// @param [in] logger ros logger -/// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in -/// the opposite direction, time at enter > time at exit. -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const DecisionInputs & inputs, const rclcpp::Logger & logger); -/// @brief decide whether an object is coming in the range at the same time as ego -/// @details the condition depends on the mode (threshold, intervals, ttc) -/// @param [in] range_times times when ego and the object enter/exit the range -/// @param [in] params parameters -/// @param [in] logger ros logger -bool will_collide_on_range( - const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger); -/// @brief check whether we should avoid entering the given range -/// @param [in] range the range to check -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] params parameters -/// @param [in] logger ros logger -/// @return return true if we should avoid entering the range -bool should_not_enter( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger); -/// @brief set the velocity of a decision (or unset it) based on the distance away from the range -/// @param [out] decision decision to update (either set its velocity or unset the decision) -/// @param [in] distance distance between ego and the range corresponding to the decision -/// @param [in] params parameters -void set_decision_velocity( - std::optional & decision, const double distance, const PlannerParam & params); -/// @brief calculate the decision to slowdown or stop before an overlapping range -/// @param [in] range the range to check -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] params parameters -/// @param [in] logger ros logger -/// @return return an optional decision to slowdown or stop -std::optional calculate_decision( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger); -/// @brief calculate decisions to slowdown or stop before some overlapping ranges -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] params parameters -/// @param [in] logger ros logger -/// @return return the calculated decisions to slowdown or stop -std::vector calculate_decisions( - const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger); -} // namespace autoware::motion_velocity_planner::out_of_lane - -#endif // DECISIONS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index a53129f372e3d..79bdefee4c4c7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -53,7 +54,7 @@ void cut_predicted_path_beyond_line( auto cut_idx = stop_line_idx; double arc_length = 0; while (cut_idx > 0 && arc_length < object_front_overhang) { - arc_length += autoware::universe_utils::calcDistance2d( + arc_length += universe_utils::calcDistance2d( predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]); --cut_idx; } @@ -102,12 +103,11 @@ void cut_predicted_path_beyond_red_lights( } autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( - const std::shared_ptr planner_data, const EgoData & ego_data, - const PlannerParam & params) + const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params) { autoware_perception_msgs::msg::PredictedObjects filtered_objects; - filtered_objects.header = planner_data->predicted_objects.header; - for (const auto & object : planner_data->predicted_objects.objects) { + filtered_objects.header = planner_data.predicted_objects.header; + for (const auto & object : planner_data.predicted_objects.objects) { const auto is_pedestrian = std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; @@ -125,10 +125,10 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( auto filtered_object = object; const auto is_invalid_predicted_path = [&](const auto & predicted_path) { const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; - const auto no_overlap_path = autoware::motion_utils::removeOverlapPoints(predicted_path.path); + const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path); if (no_overlap_path.size() <= 1) return true; - const auto lat_offset_to_current_ego = std::abs( - autoware::motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); + const auto lat_offset_to_current_ego = + std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); const auto is_crossing_ego = lat_offset_to_current_ego <= object.shape.dimensions.y / 2.0 + std::max( @@ -136,23 +136,21 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( params.right_offset + params.extra_right_offset); return is_low_confidence || is_crossing_ego; }; - if (params.objects_use_predicted_paths) { - auto & predicted_paths = filtered_object.kinematics.predicted_paths; - const auto new_end = - std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path); - predicted_paths.erase(new_end, predicted_paths.end()); - if (params.objects_cut_predicted_paths_beyond_red_lights) - for (auto & predicted_path : predicted_paths) - cut_predicted_path_beyond_red_lights( - predicted_path, ego_data, filtered_object.shape.dimensions.x); - predicted_paths.erase( - std::remove_if( - predicted_paths.begin(), predicted_paths.end(), - [](const auto & p) { return p.path.empty(); }), - predicted_paths.end()); - } + auto & predicted_paths = filtered_object.kinematics.predicted_paths; + const auto new_end = + std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path); + predicted_paths.erase(new_end, predicted_paths.end()); + if (params.objects_cut_predicted_paths_beyond_red_lights) + for (auto & predicted_path : predicted_paths) + cut_predicted_path_beyond_red_lights( + predicted_path, ego_data, filtered_object.shape.dimensions.x); + predicted_paths.erase( + std::remove_if( + predicted_paths.begin(), predicted_paths.end(), + [](const auto & p) { return p.path.empty(); }), + predicted_paths.end()); - if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty()) + if (!filtered_object.kinematics.predicted_paths.empty()) filtered_objects.objects.push_back(filtered_object); } return filtered_objects; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp index baf07e4b06ea5..3b2ae11718810 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -19,7 +19,6 @@ #include -#include #include namespace autoware::motion_velocity_planner::out_of_lane @@ -52,8 +51,7 @@ void cut_predicted_path_beyond_red_lights( /// @param [in] params parameters /// @return filtered predicted objects autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( - const std::shared_ptr planner_data, const EgoData & ego_data, - const PlannerParam & params); + const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params); } // namespace autoware::motion_velocity_planner::out_of_lane #endif // FILTER_PREDICTED_OBJECTS_HPP_ 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 f5d68c4fa9bba..564bf5de7ef2e 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 @@ -21,15 +21,13 @@ #include #include -#include #include namespace autoware::motion_velocity_planner::out_of_lane { -autoware::universe_utils::Polygon2d make_base_footprint( - const PlannerParam & p, const bool ignore_offset) +universe_utils::Polygon2d make_base_footprint(const PlannerParam & p, const bool ignore_offset) { - autoware::universe_utils::Polygon2d base_footprint; + universe_utils::Polygon2d base_footprint; const auto front_offset = ignore_offset ? 0.0 : p.extra_front_offset; const auto rear_offset = ignore_offset ? 0.0 : p.extra_rear_offset; const auto right_offset = ignore_offset ? 0.0 : p.extra_right_offset; @@ -43,10 +41,10 @@ autoware::universe_utils::Polygon2d make_base_footprint( } lanelet::BasicPolygon2d project_to_pose( - const autoware::universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) + const universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) { const auto angle = tf2::getYaw(pose.orientation); - const auto rotated_footprint = autoware::universe_utils::rotatePolygon(base_footprint, angle); + const auto rotated_footprint = universe_utils::rotatePolygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; for (const auto & p : rotated_footprint.outer()) footprint.emplace_back(p.x() + pose.position.x, p.y() + pose.position.y); @@ -59,22 +57,15 @@ std::vector calculate_trajectory_footprints( const auto base_footprint = make_base_footprint(params); std::vector trajectory_footprints; trajectory_footprints.reserve(ego_data.trajectory_points.size()); - double length = 0.0; - const auto range = std::max(params.slow_dist_threshold, params.stop_dist_threshold) + - params.front_offset + params.extra_front_offset; - for (auto i = ego_data.first_trajectory_idx; - i < ego_data.trajectory_points.size() && length < range; ++i) { + for (auto i = ego_data.first_trajectory_idx; i < ego_data.trajectory_points.size(); ++i) { const auto & trajectory_pose = ego_data.trajectory_points[i].pose; const auto angle = tf2::getYaw(trajectory_pose.orientation); - const auto rotated_footprint = autoware::universe_utils::rotatePolygon(base_footprint, angle); + const auto rotated_footprint = universe_utils::rotatePolygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; for (const auto & p : rotated_footprint.outer()) footprint.emplace_back( p.x() + trajectory_pose.position.x, p.y() + trajectory_pose.position.y); trajectory_footprints.push_back(footprint); - if (i + 1 < ego_data.trajectory_points.size()) - length += autoware::universe_utils::calcDistance2d( - trajectory_pose, ego_data.trajectory_points[i + 1].pose); } return trajectory_footprints; } @@ -84,7 +75,7 @@ lanelet::BasicPolygon2d calculate_current_ego_footprint( { const auto base_footprint = make_base_footprint(params, ignore_offset); const auto angle = tf2::getYaw(ego_data.pose.orientation); - const auto rotated_footprint = autoware::universe_utils::rotatePolygon(base_footprint, angle); + const auto rotated_footprint = universe_utils::rotatePolygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; for (const auto & p : rotated_footprint.outer()) footprint.emplace_back(p.x() + ego_data.pose.position.x, p.y() + ego_data.pose.position.y); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp index ebe7ab0fa9d7f..88baeefba6f40 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp @@ -21,24 +21,21 @@ #include -namespace autoware::motion_velocity_planner -{ -namespace out_of_lane +namespace autoware::motion_velocity_planner::out_of_lane { /// @brief create the base footprint of ego /// @param [in] p parameters used to create the footprint /// @param [in] ignore_offset optional parameter, if true, ignore the "extra offsets" to build the /// footprint /// @return base ego footprint -autoware::universe_utils::Polygon2d make_base_footprint( +universe_utils::Polygon2d make_base_footprint( const PlannerParam & p, const bool ignore_offset = false); /// @brief project a footprint to the given pose /// @param [in] base_footprint footprint to project /// @param [in] pose projection pose /// @return footprint projected to the given pose lanelet::BasicPolygon2d project_to_pose( - const autoware::universe_utils::Polygon2d & base_footprint, - const geometry_msgs::msg::Pose & pose); + const universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); /// @brief calculate the trajectory footprints /// @details the resulting polygon follows the format used by the lanelet library: clockwise order /// and implicit closing edge @@ -54,7 +51,6 @@ std::vector calculate_trajectory_footprints( /// footprint lanelet::BasicPolygon2d calculate_current_ego_footprint( const EgoData & ego_data, const PlannerParam & params, const bool ignore_offset = false); -} // namespace out_of_lane -} // namespace autoware::motion_velocity_planner +} // namespace autoware::motion_velocity_planner::out_of_lane #endif // FOOTPRINT_HPP_ 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 853f2bb3ec9d8..f520a564519f0 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 @@ -14,36 +14,46 @@ #include "lanelets_selection.hpp" +#include "types.hpp" + #include #include +#include +#include #include -#include #include #include -#include namespace autoware::motion_velocity_planner::out_of_lane { +namespace +{ +bool is_road_lanelet(const lanelet::ConstLanelet & lanelet) +{ + return lanelet.hasAttribute(lanelet::AttributeName::Subtype) && + lanelet.attribute(lanelet::AttributeName::Subtype) == lanelet::AttributeValueString::Road; +} +} // namespace + lanelet::ConstLanelets consecutive_lanelets( - const std::shared_ptr route_handler, - const lanelet::ConstLanelet & lanelet) + const route_handler::RouteHandler & route_handler, const lanelet::ConstLanelet & lanelet) { - lanelet::ConstLanelets consecutives = route_handler->getRoutingGraphPtr()->following(lanelet); - const auto previous = route_handler->getRoutingGraphPtr()->previous(lanelet); + lanelet::ConstLanelets consecutives = route_handler.getRoutingGraphPtr()->following(lanelet); + const auto previous = route_handler.getRoutingGraphPtr()->previous(lanelet); consecutives.insert(consecutives.end(), previous.begin(), previous.end()); return consecutives; } lanelet::ConstLanelets get_missing_lane_change_lanelets( const lanelet::ConstLanelets & trajectory_lanelets, - const std::shared_ptr route_handler) + const route_handler::RouteHandler & route_handler) { lanelet::ConstLanelets missing_lane_change_lanelets; - const auto & routing_graph = *route_handler->getRoutingGraphPtr(); + const auto & routing_graph = *route_handler.getRoutingGraphPtr(); lanelet::ConstLanelets adjacents; lanelet::ConstLanelets consecutives; for (const auto & ll : trajectory_lanelets) { @@ -67,9 +77,9 @@ lanelet::ConstLanelets get_missing_lane_change_lanelets( } lanelet::ConstLanelets calculate_trajectory_lanelets( - const EgoData & ego_data, const std::shared_ptr route_handler) + const EgoData & ego_data, const route_handler::RouteHandler & route_handler) { - const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); + const auto lanelet_map_ptr = route_handler.getLaneletMapPtr(); lanelet::ConstLanelets trajectory_lanelets; lanelet::BasicLineString2d trajectory_ls; for (const auto & p : ego_data.trajectory_points) @@ -77,7 +87,9 @@ lanelet::ConstLanelets calculate_trajectory_lanelets( const auto candidates = lanelet_map_ptr->laneletLayer.search(lanelet::geometry::boundingBox2d(trajectory_ls)); for (const auto & ll : candidates) { - if (!boost::geometry::disjoint(trajectory_ls, ll.polygon2d().basicPolygon())) { + if ( + is_road_lanelet(ll) && + !boost::geometry::disjoint(trajectory_ls, ll.polygon2d().basicPolygon())) { trajectory_lanelets.push_back(ll); } } @@ -89,43 +101,65 @@ lanelet::ConstLanelets calculate_trajectory_lanelets( } lanelet::ConstLanelets calculate_ignored_lanelets( - const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, - const std::shared_ptr route_handler, - const PlannerParam & params) + const lanelet::ConstLanelets & trajectory_lanelets, + const route_handler::RouteHandler & route_handler) { lanelet::ConstLanelets ignored_lanelets; - // ignore lanelets directly behind ego - const auto behind = - autoware::universe_utils::calcOffsetPose(ego_data.pose, params.rear_offset, 0.0, 0.0); - const lanelet::BasicPoint2d behind_point(behind.position.x, behind.position.y); - const auto behind_lanelets = lanelet::geometry::findWithin2d( - route_handler->getLaneletMapPtr()->laneletLayer, behind_point, 0.0); - for (const auto & l : behind_lanelets) { - const auto is_trajectory_lanelet = contains_lanelet(trajectory_lanelets, l.second.id()); - if (!is_trajectory_lanelet) ignored_lanelets.push_back(l.second); + // ignore lanelets directly preceding a trajectory lanelet + for (const auto & trajectory_lanelet : trajectory_lanelets) { + for (const auto & ll : route_handler.getPreviousLanelets(trajectory_lanelet)) { + const auto is_trajectory_lanelet = contains_lanelet(trajectory_lanelets, ll.id()); + if (!is_trajectory_lanelet) ignored_lanelets.push_back(ll); + } } return ignored_lanelets; } -lanelet::ConstLanelets calculate_other_lanelets( - const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, - const lanelet::ConstLanelets & ignored_lanelets, - const std::shared_ptr route_handler, - const PlannerParam & params) +void calculate_drivable_lane_polygons( + EgoData & ego_data, const route_handler::RouteHandler & route_handler) { - lanelet::ConstLanelets other_lanelets; - const lanelet::BasicPoint2d ego_point(ego_data.pose.position.x, ego_data.pose.position.y); - const auto lanelets_within_range = lanelet::geometry::findWithin2d( - route_handler->getLaneletMapPtr()->laneletLayer, ego_point, - std::max(params.slow_dist_threshold, params.stop_dist_threshold) + params.front_offset + - params.extra_front_offset); - for (const auto & ll : lanelets_within_range) { - if (std::string(ll.second.attributeOr(lanelet::AttributeName::Subtype, "none")) != "road") - continue; - const auto is_trajectory_lanelet = contains_lanelet(trajectory_lanelets, ll.second.id()); - const auto is_ignored_lanelet = contains_lanelet(ignored_lanelets, ll.second.id()); - if (!is_trajectory_lanelet && !is_ignored_lanelet) other_lanelets.push_back(ll.second); + 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; + } +} + +void calculate_overlapped_lanelets( + OutOfLanePoint & out_of_lane_point, const route_handler::RouteHandler & route_handler) +{ + 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); + } + } +} + +void calculate_overlapped_lanelets( + OutOfLaneData & out_of_lane_data, const route_handler::RouteHandler & route_handler) +{ + 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; + } } - return other_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 94023ae08ad99..a7729deb539b6 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 @@ -44,6 +44,7 @@ inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lane /// @return lanelets crossed by the ego vehicle lanelet::ConstLanelets calculate_trajectory_lanelets( const EgoData & ego_data, 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 /// @param [in] trajectory_lanelets lanelets driven by the ego vehicle @@ -52,29 +53,27 @@ lanelet::ConstLanelets calculate_trajectory_lanelets( /// trajectory_lanelets) lanelet::ConstLanelets get_missing_lane_change_lanelets( const lanelet::ConstLanelets & trajectory_lanelets, - const std::shared_ptr route_handler); + const std::shared_ptr & route_handler); + /// @brief calculate lanelets that should be ignored -/// @param [in] ego_data data about the ego vehicle -/// @param [in] trajectory_lanelets lanelets driven by the ego vehicle +/// @param [in] trajectory_lanelets lanelets followed by the ego vehicle /// @param [in] route_handler route handler -/// @param [in] params parameters /// @return lanelets to ignore lanelet::ConstLanelets calculate_ignored_lanelets( - const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, - const std::shared_ptr route_handler, - const PlannerParam & params); -/// @brief calculate lanelets that should be checked by the module -/// @param [in] ego_data data about the ego vehicle -/// @param [in] trajectory_lanelets lanelets driven by the ego vehicle -/// @param [in] ignored_lanelets lanelets to ignore -/// @param [in] route_handler route handler -/// @param [in] params parameters -/// @return lanelets to check for overlaps -lanelet::ConstLanelets calculate_other_lanelets( - const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, - const lanelet::ConstLanelets & ignored_lanelets, - const std::shared_ptr route_handler, - const PlannerParam & params); + const lanelet::ConstLanelets & trajectory_lanelets, + const std::shared_ptr & route_handler); + +/// @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); + +/// @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); } // 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 new file mode 100644 index 0000000000000..c375bcc35c1ee --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp @@ -0,0 +1,166 @@ +// 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 "out_of_lane_collisions.hpp" + +#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 +{ + +void update_collision_times( + OutOfLaneData & out_of_lane_data, const std::unordered_set & potential_collision_indexes, + const universe_utils::Polygon2d & object_footprint, const double time) +{ + for (const auto index : potential_collision_indexes) { + 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()); + if (has_collision) { + out_of_lane_point.collision_times.insert(time); + } + } + } +} + +void calculate_object_path_time_collisions( + OutOfLaneData & out_of_lane_data, + const autoware_perception_msgs::msg::PredictedPath & object_path, + const autoware_perception_msgs::msg::Shape & object_shape) +{ + const auto time_step = rclcpp::Duration(object_path.time_step).seconds(); + auto time = time_step; + for (const auto & object_pose : object_path.path) { + time += time_step; + const auto object_footprint = universe_utils::toPolygon2d(object_pose, object_shape); + std::vector query_results; + out_of_lane_data.outside_areas_rtree.query( + boost::geometry::index::intersects(object_footprint.outer()), + std::back_inserter(query_results)); + std::unordered_set potential_collision_indexes; + for (const auto & [_, index] : query_results) { + potential_collision_indexes.insert(index); + } + update_collision_times(out_of_lane_data, potential_collision_indexes, object_footprint, time); + } +} + +void calculate_objects_time_collisions( + OutOfLaneData & out_of_lane_data, + const std::vector & objects) +{ + for (const auto & object : objects) { + for (const auto & path : object.kinematics.predicted_paths) { + calculate_object_path_time_collisions(out_of_lane_data, path, object.shape); + } + } +} + +void calculate_min_max_arrival_times( + OutOfLanePoint & out_of_lane_point, + const std::vector & trajectory) +{ + auto min_time = std::numeric_limits::infinity(); + auto max_time = -std::numeric_limits::infinity(); + for (const auto & t : out_of_lane_point.collision_times) { + min_time = std::min(t, min_time); + max_time = std::max(t, max_time); + } + if (min_time <= max_time) { + out_of_lane_point.min_object_arrival_time = min_time; + out_of_lane_point.max_object_arrival_time = max_time; + const auto & ego_time = + rclcpp::Duration(trajectory[out_of_lane_point.trajectory_index].time_from_start).seconds(); + if (ego_time >= min_time && ego_time <= max_time) { + out_of_lane_point.ttc = 0.0; + } else { + out_of_lane_point.ttc = + std::min(std::abs(ego_time - min_time), std::abs(ego_time - max_time)); + } + } +}; + +void calculate_collisions_to_avoid( + OutOfLaneData & out_of_lane_data, + const std::vector & trajectory, + const PlannerParam & params) +{ + for (auto & out_of_lane_point : out_of_lane_data.outside_points) { + calculate_min_max_arrival_times(out_of_lane_point, trajectory); + } + for (auto & p : out_of_lane_data.outside_points) { + if (params.mode == "ttc") { + p.to_avoid = p.ttc && p.ttc <= params.ttc_threshold; + } else { + p.to_avoid = p.min_object_arrival_time && p.min_object_arrival_time <= params.time_threshold; + } + } +} + +OutOfLaneData calculate_outside_points(const EgoData & ego_data) +{ + OutOfLaneData out_of_lane_data; + out_of_lane::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]; + out_of_lane::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_data.outside_points.push_back(p); + } + } + } + return out_of_lane_data; +} + +OutOfLaneData calculate_out_of_lane_areas(const EgoData & ego_data) +{ + auto out_of_lane_data = calculate_outside_points(ego_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); + } + out_of_lane_data.outside_areas_rtree = {rtree_nodes.begin(), rtree_nodes.end()}; + return out_of_lane_data; +} + +} // namespace autoware::motion_velocity_planner::out_of_lane 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 new file mode 100644 index 0000000000000..b9048cc358a07 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp @@ -0,0 +1,55 @@ +// 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 OUT_OF_LANE_COLLISIONS_HPP_ +#define OUT_OF_LANE_COLLISIONS_HPP_ + +#include "types.hpp" + +#include + +#include +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +/// @brief calculate the times and points where ego collides with an object's path outside of its +/// lane +void calculate_object_path_time_collisions( + OutOfLaneData & out_of_lane_data, + const autoware_perception_msgs::msg::PredictedPath & object_path, + const autoware_perception_msgs::msg::Shape & object_shape); + +/// @brief calculate the times and points where ego collides with an object outside of its lane +void calculate_objects_time_collisions( + OutOfLaneData & out_of_lane_data, + const std::vector & objects); + +/// @brief calculate the collisions to avoid +/// @details either uses the time to collision or just the time when the object will arrive at the +/// point +void calculate_collisions_to_avoid( + OutOfLaneData & out_of_lane_data, + const std::vector & trajectory, + const PlannerParam & params); + +/// @brief calculate the areas where ego will drive outside of its lane +/// @details the OutOfLaneData points and rtree are filled +OutOfLaneData calculate_out_of_lane_areas(const EgoData & ego_data); +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // OUT_OF_LANE_COLLISIONS_HPP_ 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 a6b34a1566e19..ca7ed3b9dc7bd 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 @@ -16,11 +16,10 @@ #include "calculate_slowdown_points.hpp" #include "debug.hpp" -#include "decisions.hpp" #include "filter_predicted_objects.hpp" #include "footprint.hpp" #include "lanelets_selection.hpp" -#include "overlapping_range.hpp" +#include "out_of_lane_collisions.hpp" #include "types.hpp" #include @@ -34,11 +33,13 @@ #include #include +#include #include #include #include +#include #include #include #include @@ -56,65 +57,54 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) logger_ = node.get_logger(); clock_ = node.get_clock(); init_parameters(node); - velocity_factor_interface_.init(autoware::motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + velocity_factor_interface_.init(motion_utils::PlanningBehavior::ROUTE_OBSTACLE); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); - processing_diag_publisher_ = std::make_shared( + processing_diag_publisher_ = std::make_shared( &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); processing_time_publisher_ = node.create_publisher( "~/debug/" + ns_ + "/processing_time_ms", 1); } void OutOfLaneModule::init_parameters(rclcpp::Node & node) { - using autoware::universe_utils::getOrDeclareParameter; + using universe_utils::getOrDeclareParameter; auto & pp = params_; pp.mode = getOrDeclareParameter(node, ns_ + ".mode"); pp.skip_if_already_overlapping = getOrDeclareParameter(node, ns_ + ".skip_if_already_overlapping"); + pp.ignore_lane_changeable_lanelets = + getOrDeclareParameter(node, ns_ + ".ignore_overlaps_over_lane_changeable_lanelets"); + pp.max_arc_length = getOrDeclareParameter(node, ns_ + ".max_arc_length"); pp.time_threshold = getOrDeclareParameter(node, ns_ + ".threshold.time_threshold"); - pp.intervals_ego_buffer = getOrDeclareParameter(node, ns_ + ".intervals.ego_time_buffer"); - pp.intervals_obj_buffer = - getOrDeclareParameter(node, ns_ + ".intervals.objects_time_buffer"); pp.ttc_threshold = getOrDeclareParameter(node, ns_ + ".ttc.threshold"); pp.objects_min_vel = getOrDeclareParameter(node, ns_ + ".objects.minimum_velocity"); - pp.objects_use_predicted_paths = - getOrDeclareParameter(node, ns_ + ".objects.use_predicted_paths"); pp.objects_min_confidence = getOrDeclareParameter(node, ns_ + ".objects.predicted_path_min_confidence"); - pp.objects_dist_buffer = getOrDeclareParameter(node, ns_ + ".objects.distance_buffer"); pp.objects_cut_predicted_paths_beyond_red_lights = getOrDeclareParameter(node, ns_ + ".objects.cut_predicted_paths_beyond_red_lights"); pp.objects_ignore_behind_ego = getOrDeclareParameter(node, ns_ + ".objects.ignore_behind_ego"); - pp.overlap_min_dist = getOrDeclareParameter(node, ns_ + ".overlap.minimum_distance"); - pp.overlap_extra_length = getOrDeclareParameter(node, ns_ + ".overlap.extra_length"); - - pp.skip_if_over_max_decel = - getOrDeclareParameter(node, ns_ + ".action.skip_if_over_max_decel"); pp.precision = getOrDeclareParameter(node, ns_ + ".action.precision"); pp.min_decision_duration = getOrDeclareParameter(node, ns_ + ".action.min_duration"); pp.lon_dist_buffer = getOrDeclareParameter(node, ns_ + ".action.longitudinal_distance_buffer"); pp.lat_dist_buffer = getOrDeclareParameter(node, ns_ + ".action.lateral_distance_buffer"); pp.slow_velocity = getOrDeclareParameter(node, ns_ + ".action.slowdown.velocity"); - pp.slow_dist_threshold = - getOrDeclareParameter(node, ns_ + ".action.slowdown.distance_threshold"); pp.stop_dist_threshold = getOrDeclareParameter(node, ns_ + ".action.stop.distance_threshold"); - pp.ego_min_velocity = getOrDeclareParameter(node, ns_ + ".ego.min_assumed_velocity"); pp.extra_front_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_front_offset"); pp.extra_rear_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_rear_offset"); pp.extra_left_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_left_offset"); pp.extra_right_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_right_offset"); - const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); + const auto vehicle_info = vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); pp.front_offset = vehicle_info.max_longitudinal_offset_m; pp.rear_offset = vehicle_info.min_longitudinal_offset_m; pp.left_offset = vehicle_info.max_lateral_offset_m; @@ -123,44 +113,82 @@ void OutOfLaneModule::init_parameters(rclcpp::Node & node) void OutOfLaneModule::update_parameters(const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using universe_utils::updateParam; auto & pp = params_; updateParam(parameters, ns_ + ".mode", pp.mode); updateParam(parameters, ns_ + ".skip_if_already_overlapping", pp.skip_if_already_overlapping); + updateParam(parameters, ns_ + ".max_arc_length", pp.max_arc_length); + updateParam( + parameters, ns_ + ".ignore_overlaps_over_lane_changeable_lanelets", + pp.ignore_lane_changeable_lanelets); updateParam(parameters, ns_ + ".threshold.time_threshold", pp.time_threshold); - updateParam(parameters, ns_ + ".intervals.ego_time_buffer", pp.intervals_ego_buffer); - updateParam(parameters, ns_ + ".intervals.objects_time_buffer", pp.intervals_obj_buffer); updateParam(parameters, ns_ + ".ttc.threshold", pp.ttc_threshold); updateParam(parameters, ns_ + ".objects.minimum_velocity", pp.objects_min_vel); - updateParam(parameters, ns_ + ".objects.use_predicted_paths", pp.objects_use_predicted_paths); updateParam( parameters, ns_ + ".objects.predicted_path_min_confidence", pp.objects_min_confidence); - updateParam(parameters, ns_ + ".objects.distance_buffer", pp.objects_dist_buffer); updateParam( parameters, ns_ + ".objects.cut_predicted_paths_beyond_red_lights", pp.objects_cut_predicted_paths_beyond_red_lights); updateParam(parameters, ns_ + ".objects.ignore_behind_ego", pp.objects_ignore_behind_ego); - updateParam(parameters, ns_ + ".overlap.minimum_distance", pp.overlap_min_dist); - updateParam(parameters, ns_ + ".overlap.extra_length", pp.overlap_extra_length); - updateParam(parameters, ns_ + ".action.skip_if_over_max_decel", pp.skip_if_over_max_decel); updateParam(parameters, ns_ + ".action.precision", pp.precision); updateParam(parameters, ns_ + ".action.min_duration", pp.min_decision_duration); updateParam(parameters, ns_ + ".action.longitudinal_distance_buffer", pp.lon_dist_buffer); updateParam(parameters, ns_ + ".action.lateral_distance_buffer", pp.lat_dist_buffer); updateParam(parameters, ns_ + ".action.slowdown.velocity", pp.slow_velocity); - updateParam(parameters, ns_ + ".action.slowdown.distance_threshold", pp.slow_dist_threshold); updateParam(parameters, ns_ + ".action.stop.distance_threshold", pp.stop_dist_threshold); - updateParam(parameters, ns_ + ".ego.min_assumed_velocity", pp.ego_min_velocity); updateParam(parameters, ns_ + ".ego.extra_front_offset", pp.extra_front_offset); updateParam(parameters, ns_ + ".ego.extra_rear_offset", pp.extra_rear_offset); updateParam(parameters, ns_ + ".ego.extra_left_offset", pp.extra_left_offset); updateParam(parameters, ns_ + ".ego.extra_right_offset", pp.extra_right_offset); } +void OutOfLaneModule::limit_trajectory_size( + out_of_lane::EgoData & ego_data, + const std::vector & ego_trajectory_points, + const double max_arc_length) +{ + ego_data.first_trajectory_idx = + motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position); + ego_data.longitudinal_offset_to_first_trajectory_index = + motion_utils::calcLongitudinalOffsetToSegment( + ego_trajectory_points, ego_data.first_trajectory_idx, ego_data.pose.position); + auto l = -ego_data.longitudinal_offset_to_first_trajectory_index; + ego_data.trajectory_points.push_back(ego_trajectory_points[ego_data.first_trajectory_idx]); + for (auto i = ego_data.first_trajectory_idx + 1; i < ego_trajectory_points.size(); ++i) { + l += universe_utils::calcDistance2d(ego_trajectory_points[i - 1], ego_trajectory_points[i]); + if (l >= max_arc_length) { + break; + } + ego_data.trajectory_points.push_back(ego_trajectory_points[i]); + } +} + +void OutOfLaneModule::calculate_min_stop_and_slowdown_distances( + out_of_lane::EgoData & ego_data, const PlannerData & planner_data, + std::optional & previous_slowdown_pose_, const double slow_velocity) +{ + ego_data.min_stop_distance = planner_data.calculate_min_deceleration_distance(0.0).value_or(0.0); + ego_data.min_slowdown_distance = + planner_data.calculate_min_deceleration_distance(slow_velocity).value_or(0.0); + if (previous_slowdown_pose_) { + // Ensure we do not remove the previous slowdown point due to the min distance limit + const auto previous_slowdown_pose_arc_length = motion_utils::calcSignedArcLength( + ego_data.trajectory_points, ego_data.first_trajectory_idx, previous_slowdown_pose_->position); + ego_data.min_stop_distance = + std::min(previous_slowdown_pose_arc_length, ego_data.min_stop_distance); + ego_data.min_slowdown_distance = + std::min(previous_slowdown_pose_arc_length, ego_data.min_slowdown_distance); + } + ego_data.min_stop_arc_length = motion_utils::calcSignedArcLength( + ego_data.trajectory_points, 0UL, ego_data.first_trajectory_idx) + + ego_data.longitudinal_offset_to_first_trajectory_index + + ego_data.min_stop_distance; +} + void prepare_stop_lines_rtree( out_of_lane::EgoData & ego_data, const PlannerData & planner_data, const double search_distance) { @@ -202,163 +230,140 @@ VelocityPlanningResult OutOfLaneModule::plan( const std::shared_ptr planner_data) { VelocityPlanningResult result; - autoware::universe_utils::StopWatch stopwatch; + universe_utils::StopWatch stopwatch; stopwatch.tic(); + + stopwatch.tic("preprocessing"); out_of_lane::EgoData ego_data; ego_data.pose = planner_data->current_odometry.pose.pose; - ego_data.trajectory_points = ego_trajectory_points; - ego_data.first_trajectory_idx = - autoware::motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position); - ego_data.velocity = planner_data->current_odometry.twist.twist.linear.x; - ego_data.max_decel = planner_data->velocity_smoother_->getMinDecel(); - stopwatch.tic("preprocessing"); - prepare_stop_lines_rtree(ego_data, *planner_data, 100.0); + limit_trajectory_size(ego_data, ego_trajectory_points, params_.max_arc_length); + calculate_min_stop_and_slowdown_distances( + ego_data, *planner_data, previous_slowdown_pose_, params_.slow_velocity); + prepare_stop_lines_rtree(ego_data, *planner_data, params_.max_arc_length); const auto preprocessing_us = stopwatch.toc("preprocessing"); stopwatch.tic("calculate_trajectory_footprints"); - const auto current_ego_footprint = + ego_data.current_footprint = out_of_lane::calculate_current_ego_footprint(ego_data, params_, true); - const auto trajectory_footprints = - out_of_lane::calculate_trajectory_footprints(ego_data, params_); + ego_data.trajectory_footprints = out_of_lane::calculate_trajectory_footprints(ego_data, params_); const auto calculate_trajectory_footprints_us = stopwatch.toc("calculate_trajectory_footprints"); - // Calculate lanelets to ignore and consider + stopwatch.tic("calculate_lanelets"); - const auto trajectory_lanelets = - out_of_lane::calculate_trajectory_lanelets(ego_data, planner_data->route_handler); - const auto ignored_lanelets = out_of_lane::calculate_ignored_lanelets( - ego_data, trajectory_lanelets, planner_data->route_handler, params_); - const auto other_lanelets = out_of_lane::calculate_other_lanelets( - ego_data, trajectory_lanelets, ignored_lanelets, planner_data->route_handler, params_); + out_of_lane::calculate_drivable_lane_polygons(ego_data, *planner_data->route_handler); const auto calculate_lanelets_us = stopwatch.toc("calculate_lanelets"); - debug_data_.reset_data(); - debug_data_.trajectory_points = ego_trajectory_points; - debug_data_.footprints = trajectory_footprints; - debug_data_.trajectory_lanelets = trajectory_lanelets; - debug_data_.ignored_lanelets = ignored_lanelets; - debug_data_.other_lanelets = other_lanelets; - debug_data_.first_trajectory_idx = ego_data.first_trajectory_idx; - - if (params_.skip_if_already_overlapping) { - debug_data_.current_footprint = current_ego_footprint; - const auto overlapped_lanelet_it = - std::find_if(other_lanelets.begin(), other_lanelets.end(), [&](const auto & ll) { - return boost::geometry::intersects(ll.polygon2d().basicPolygon(), current_ego_footprint); - }); - if (overlapped_lanelet_it != other_lanelets.end()) { - debug_data_.current_overlapped_lanelets.push_back(*overlapped_lanelet_it); - RCLCPP_DEBUG(logger_, "Ego is already overlapping a lane, skipping the module\n"); - return result; - } - } - // Calculate overlapping ranges - stopwatch.tic("calculate_overlapping_ranges"); - const auto ranges = out_of_lane::calculate_overlapping_ranges( - trajectory_footprints, trajectory_lanelets, other_lanelets, params_); - const auto calculate_overlapping_ranges_us = stopwatch.toc("calculate_overlapping_ranges"); - // Calculate stop and slowdown points - out_of_lane::DecisionInputs inputs; - inputs.ranges = ranges; - inputs.ego_data = ego_data; + stopwatch.tic("calculate_out_of_lane_areas"); + auto out_of_lane_data = calculate_out_of_lane_areas(ego_data); + out_of_lane::calculate_overlapped_lanelets(out_of_lane_data, *planner_data->route_handler); + const auto calculate_out_of_lane_areas_us = stopwatch.toc("calculate_out_of_lane_areas"); + stopwatch.tic("filter_predicted_objects"); - inputs.objects = out_of_lane::filter_predicted_objects(planner_data, ego_data, params_); + const auto objects = out_of_lane::filter_predicted_objects(*planner_data, ego_data, params_); const auto filter_predicted_objects_us = stopwatch.toc("filter_predicted_objects"); - inputs.route_handler = planner_data->route_handler; - inputs.lanelets = other_lanelets; - stopwatch.tic("calculate_decisions"); - const auto decisions = out_of_lane::calculate_decisions(inputs, params_, logger_); - const auto calculate_decisions_us = stopwatch.toc("calculate_decisions"); - stopwatch.tic("calc_slowdown_points"); + + stopwatch.tic("calculate_time_collisions"); + out_of_lane::calculate_objects_time_collisions(out_of_lane_data, objects.objects); + const auto calculate_time_collisions_us = stopwatch.toc("calculate_time_collisions"); + + stopwatch.tic("calculate_times"); + // calculate times + 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)) { + 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_)); + return result; + } + if ( // reset the previous inserted point if the timer expired - prev_inserted_point_ && - (clock_->now() - prev_inserted_point_time_).seconds() > params_.min_decision_duration) - prev_inserted_point_.reset(); - auto point_to_insert = - out_of_lane::calculate_slowdown_point(ego_data, decisions, prev_inserted_point_, params_); - const auto calc_slowdown_points_us = stopwatch.toc("calc_slowdown_points"); - stopwatch.tic("insert_slowdown_points"); - debug_data_.slowdowns.clear(); - if ( // reset the timer if there is no previous inserted point or if we avoid the same lane - point_to_insert && - (!prev_inserted_point_ || prev_inserted_point_->slowdown.lane_to_avoid.id() == - point_to_insert->slowdown.lane_to_avoid.id())) - prev_inserted_point_time_ = clock_->now(); - // reuse previous stop point if there is no new one or if its velocity is not higher than the new + previous_slowdown_pose_ && + (clock_->now() - previous_slowdown_time_).seconds() > params_.min_decision_duration) { + previous_slowdown_pose_.reset(); + } + + stopwatch.tic("calculate_slowdown_point"); + auto slowdown_pose = out_of_lane::calculate_slowdown_point(ego_data, out_of_lane_data, params_); + const auto calculate_slowdown_point_us = stopwatch.toc("calculate_slowdown_point"); + + if ( // reset the timer if there is no previous inserted point + slowdown_pose && (!previous_slowdown_pose_)) { + previous_slowdown_time_ = clock_->now(); + } + // reuse previous stop pose if there is no new one or if its velocity is not higher than the new // one and its arc length is lower - const auto should_use_prev_inserted_point = [&]() { - if ( - point_to_insert && prev_inserted_point_ && - prev_inserted_point_->slowdown.velocity <= point_to_insert->slowdown.velocity) { - const auto arc_length = autoware::motion_utils::calcSignedArcLength( - ego_trajectory_points, 0LU, point_to_insert->point.pose.position); - const auto prev_arc_length = autoware::motion_utils::calcSignedArcLength( - ego_trajectory_points, 0LU, prev_inserted_point_->point.pose.position); + const auto should_use_previous_pose = [&]() { + if (slowdown_pose && previous_slowdown_pose_) { + const auto arc_length = + motion_utils::calcSignedArcLength(ego_trajectory_points, 0LU, slowdown_pose->position); + const auto prev_arc_length = motion_utils::calcSignedArcLength( + ego_trajectory_points, 0LU, previous_slowdown_pose_->position); return prev_arc_length < arc_length; } - return !point_to_insert && prev_inserted_point_; + return !slowdown_pose && previous_slowdown_pose_; }(); - if (should_use_prev_inserted_point) { + if (should_use_previous_pose) { // if the trajectory changed the prev point is no longer on the trajectory so we project it - const auto insert_arc_length = autoware::motion_utils::calcSignedArcLength( - ego_trajectory_points, 0LU, prev_inserted_point_->point.pose.position); - prev_inserted_point_->point.pose = - autoware::motion_utils::calcInterpolatedPose(ego_trajectory_points, insert_arc_length); - point_to_insert = prev_inserted_point_; + const auto new_arc_length = motion_utils::calcSignedArcLength( + ego_trajectory_points, ego_data.first_trajectory_idx, previous_slowdown_pose_->position); + slowdown_pose = motion_utils::calcInterpolatedPose(ego_trajectory_points, new_arc_length); } - if (point_to_insert) { - prev_inserted_point_ = point_to_insert; - RCLCPP_DEBUG(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id()); - debug_data_.slowdowns = {*point_to_insert}; - if (point_to_insert->slowdown.velocity == 0.0) - result.stop_points.push_back(point_to_insert->point.pose.position); - else + if (slowdown_pose) { + const auto arc_length = + motion_utils::calcSignedArcLength( + ego_trajectory_points, ego_data.first_trajectory_idx, slowdown_pose->position) - + ego_data.longitudinal_offset_to_first_trajectory_index; + const auto slowdown_velocity = + arc_length <= params_.stop_dist_threshold ? 0.0 : params_.slow_velocity; + previous_slowdown_pose_ = slowdown_pose; + if (slowdown_velocity == 0.0) { + result.stop_points.push_back(slowdown_pose->position); + } else { result.slowdown_intervals.emplace_back( - point_to_insert->point.pose.position, point_to_insert->point.pose.position, - point_to_insert->slowdown.velocity); - - const auto is_approaching = autoware::motion_utils::calcSignedArcLength( - ego_trajectory_points, ego_data.pose.position, - point_to_insert->point.pose.position) > 0.1 && - ego_data.velocity > 0.1; - const auto status = is_approaching ? autoware::motion_utils::VelocityFactor::APPROACHING - : autoware::motion_utils::VelocityFactor::STOPPED; + slowdown_pose->position, slowdown_pose->position, slowdown_velocity); + } + + const auto is_approaching = + motion_utils::calcSignedArcLength( + ego_trajectory_points, ego_data.pose.position, slowdown_pose->position) > 0.1 && + planner_data->current_odometry.twist.twist.linear.x > 0.1; + const auto status = is_approaching ? motion_utils::VelocityFactor::APPROACHING + : motion_utils::VelocityFactor::STOPPED; velocity_factor_interface_.set( - ego_trajectory_points, ego_data.pose, point_to_insert->point.pose, status, "out_of_lane"); + ego_trajectory_points, ego_data.pose, *slowdown_pose, status, "out_of_lane"); result.velocity_factor = velocity_factor_interface_.get(); - } else if (!decisions.empty()) { - RCLCPP_WARN(logger_, "Could not insert stop point (would violate max deceleration limits)"); + virtual_wall_marker_creator.add_virtual_walls( + out_of_lane::debug::create_virtual_walls(*slowdown_pose, slowdown_velocity == 0.0, params_)); + virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now())); + } else if (std::any_of( + out_of_lane_data.outside_points.begin(), out_of_lane_data.outside_points.end(), + [](const auto & p) { return p.to_avoid; })) { + RCLCPP_WARN( + logger_, "[out_of_lane] Could not insert slowdown point because of deceleration limits"); } - const auto insert_slowdown_points_us = stopwatch.toc("insert_slowdown_points"); - debug_data_.ranges = inputs.ranges; + stopwatch.tic("gen_debug"); + const auto markers = + out_of_lane::debug::create_debug_marker_array(ego_data, out_of_lane_data, objects, debug_data_); + const auto markers_us = stopwatch.toc("gen_debug"); + stopwatch.tic("pub"); + debug_publisher_->publish(markers); + const auto pub_markers_us = stopwatch.toc("pub"); const auto total_time_us = stopwatch.toc(); - RCLCPP_DEBUG( - logger_, - "Total time = %2.2fus\n" - "\tpreprocessing = %2.0fus\n" - "\tcalculate_lanelets = %2.0fus\n" - "\tcalculate_trajectory_footprints = %2.0fus\n" - "\tcalculate_overlapping_ranges = %2.0fus\n" - "\tfilter_pred_objects = %2.0fus\n" - "\tcalculate_decisions = %2.0fus\n" - "\tcalc_slowdown_points = %2.0fus\n" - "\tinsert_slowdown_points = %2.0fus\n", - preprocessing_us, total_time_us, calculate_lanelets_us, calculate_trajectory_footprints_us, - calculate_overlapping_ranges_us, filter_predicted_objects_us, calculate_decisions_us, - calc_slowdown_points_us, insert_slowdown_points_us); - debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array(debug_data_)); - virtual_wall_marker_creator.add_virtual_walls( - out_of_lane::debug::create_virtual_walls(debug_data_, params_)); - virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now())); std::map processing_times; processing_times["preprocessing"] = preprocessing_us / 1000; processing_times["calculate_lanelets"] = calculate_lanelets_us / 1000; processing_times["calculate_trajectory_footprints"] = calculate_trajectory_footprints_us / 1000; - processing_times["calculate_overlapping_ranges"] = calculate_overlapping_ranges_us / 1000; + processing_times["calculate_out_of_lane_areas"] = calculate_out_of_lane_areas_us / 1000; processing_times["filter_pred_objects"] = filter_predicted_objects_us / 1000; - processing_times["calculate_decision"] = calculate_decisions_us / 1000; - processing_times["calc_slowdown_points"] = calc_slowdown_points_us / 1000; - processing_times["insert_slowdown_points"] = insert_slowdown_points_us / 1000; + processing_times["calculate_time_collisions"] = calculate_time_collisions_us / 1000; + processing_times["calculate_times"] = calculate_times_us / 1000; + processing_times["calculate_slowdown_point"] = calculate_slowdown_point_us / 1000; + processing_times["generate_markers"] = markers_us / 1000; + processing_times["publish_markers"] = pub_markers_us / 1000; processing_times["Total"] = total_time_us / 1000; processing_diag_publisher_->publish(processing_times); tier4_debug_msgs::msg::Float64Stamped processing_time_msg; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp index 310a73c04d643..5225a6dd4abd9 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -46,13 +47,24 @@ class OutOfLaneModule : public PluginModuleInterface private: void init_parameters(rclcpp::Node & node); + /// @brief resize the trajectory to start from the segment closest to ego and to have at most the + /// given length + static void limit_trajectory_size( + out_of_lane::EgoData & ego_data, + const std::vector & ego_trajectory_points, + const double max_arc_length); + /// @brief calculate the minimum stop and slowdown distances of ego + static void calculate_min_stop_and_slowdown_distances( + out_of_lane::EgoData & ego_data, const PlannerData & planner_data, + std::optional & previous_slowdown_pose_, const double slow_velocity); + out_of_lane::PlannerParam params_; inline static const std::string ns_ = "out_of_lane"; std::string module_name_; - std::optional prev_inserted_point_{}; - rclcpp::Clock::SharedPtr clock_{}; - rclcpp::Time prev_inserted_point_time_{}; + rclcpp::Clock::SharedPtr clock_; + std::optional previous_slowdown_pose_; + rclcpp::Time previous_slowdown_time_; protected: // Debug diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp deleted file mode 100644 index 097e71a51bfaf..0000000000000 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp +++ /dev/null @@ -1,127 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "overlapping_range.hpp" - -#include -#include - -#include - -#include -#include - -#include - -namespace autoware::motion_velocity_planner::out_of_lane -{ - -Overlap calculate_overlap( - const lanelet::BasicPolygon2d & trajectory_footprint, - const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet) -{ - Overlap overlap; - const auto & left_bound = lanelet.leftBound2d().basicLineString(); - const auto & right_bound = lanelet.rightBound2d().basicLineString(); - // TODO(Maxime): these intersects (for each trajectory footprint, for each lanelet) are too - // expensive - const auto overlap_left = boost::geometry::intersects(trajectory_footprint, left_bound); - const auto overlap_right = boost::geometry::intersects(trajectory_footprint, right_bound); - - lanelet::BasicPolygons2d overlapping_polygons; - if (overlap_left || overlap_right) - boost::geometry::intersection( - trajectory_footprint, lanelet.polygon2d().basicPolygon(), overlapping_polygons); - for (const auto & overlapping_polygon : overlapping_polygons) { - for (const auto & point : overlapping_polygon) { - if (overlap_left && overlap_right) - overlap.inside_distance = boost::geometry::distance(left_bound, right_bound); - else if (overlap_left) - overlap.inside_distance = - std::max(overlap.inside_distance, boost::geometry::distance(point, left_bound)); - else if (overlap_right) - overlap.inside_distance = - std::max(overlap.inside_distance, boost::geometry::distance(point, right_bound)); - geometry_msgs::msg::Pose p; - p.position.x = point.x(); - p.position.y = point.y(); - const auto length = lanelet::utils::getArcCoordinates(trajectory_lanelets, p).length; - if (length > overlap.max_arc_length) { - overlap.max_arc_length = length; - overlap.max_overlap_point = point; - } - if (length < overlap.min_arc_length) { - overlap.min_arc_length = length; - overlap.min_overlap_point = point; - } - } - } - return overlap; -} - -OverlapRanges calculate_overlapping_ranges( - const std::vector & trajectory_footprints, - const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet, - const PlannerParam & params) -{ - OverlapRanges ranges; - OtherLane other_lane(lanelet); - std::vector overlaps; - for (auto i = 0UL; i < trajectory_footprints.size(); ++i) { - const auto overlap = calculate_overlap(trajectory_footprints[i], trajectory_lanelets, lanelet); - const auto has_overlap = overlap.inside_distance > params.overlap_min_dist; - if (has_overlap) { // open/update the range - overlaps.push_back(overlap); - if (!other_lane.range_is_open) { - other_lane.first_range_bound.index = i; - other_lane.first_range_bound.point = overlap.min_overlap_point; - other_lane.first_range_bound.arc_length = - overlap.min_arc_length - params.overlap_extra_length; - other_lane.first_range_bound.inside_distance = overlap.inside_distance; - other_lane.range_is_open = true; - } - other_lane.last_range_bound.index = i; - other_lane.last_range_bound.point = overlap.max_overlap_point; - other_lane.last_range_bound.arc_length = overlap.max_arc_length + params.overlap_extra_length; - other_lane.last_range_bound.inside_distance = overlap.inside_distance; - } else if (other_lane.range_is_open) { // !has_overlap: close the range if it is open - ranges.push_back(other_lane.close_range()); - ranges.back().debug.overlaps = overlaps; - overlaps.clear(); - } - } - // close the range if it is still open - if (other_lane.range_is_open) { - ranges.push_back(other_lane.close_range()); - ranges.back().debug.overlaps = overlaps; - overlaps.clear(); - } - return ranges; -} - -OverlapRanges calculate_overlapping_ranges( - const std::vector & trajectory_footprints, - const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelets & lanelets, - const PlannerParam & params) -{ - OverlapRanges ranges; - for (auto & lanelet : lanelets) { - const auto lanelet_ranges = - calculate_overlapping_ranges(trajectory_footprints, trajectory_lanelets, lanelet, params); - ranges.insert(ranges.end(), lanelet_ranges.begin(), lanelet_ranges.end()); - } - return ranges; -} - -} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp deleted file mode 100644 index bc5872db16e03..0000000000000 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp +++ /dev/null @@ -1,63 +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 OVERLAPPING_RANGE_HPP_ -#define OVERLAPPING_RANGE_HPP_ - -#include "types.hpp" - -#include - -#include - -#include - -namespace autoware::motion_velocity_planner::out_of_lane -{ - -/// @brief calculate the overlap between the given footprint and lanelet -/// @param [in] path_footprint footprint used to calculate the overlap -/// @param [in] trajectory_lanelets trajectory lanelets used to calculate arc length along the ego -/// trajectory -/// @param [in] lanelet lanelet used to calculate the overlap -/// @return the found overlap between the footprint and the lanelet -Overlap calculate_overlap( - const lanelet::BasicPolygon2d & trajectory_footprint, - const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet); -/// @brief calculate the overlapping ranges between the trajectory footprints and a lanelet -/// @param [in] trajectory_footprints footprints used to calculate the overlaps -/// @param [in] trajectory_lanelets trajectory lanelets used to calculate arc length along the ego -/// trajectory -/// @param [in] lanelet lanelet used to calculate the overlaps -/// @param [in] params parameters -/// @return the overlapping ranges found between the footprints and the lanelet -OverlapRanges calculate_overlapping_ranges( - const std::vector & trajectory_footprints, - const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet, - const PlannerParam & params); -/// @brief calculate the overlapping ranges between the trajectory footprints and some lanelets -/// @param [in] trajectory_footprints footprints used to calculate the overlaps -/// @param [in] trajectory_lanelets trajectory lanelets used to calculate arc length along the ego -/// trajectory -/// @param [in] lanelets lanelets used to calculate the overlaps -/// @param [in] params parameters -/// @return the overlapping ranges found between the footprints and the lanelets, sorted by -/// increasing arc length along the trajectory -OverlapRanges calculate_overlapping_ranges( - const std::vector & trajectory_footprints, - const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelets & lanelets, - const PlannerParam & params); -} // namespace autoware::motion_velocity_planner::out_of_lane - -#endif // OVERLAPPING_RANGE_HPP_ 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 96c54064e296c..8067d9e8b3410 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 @@ -28,52 +28,46 @@ #include #include +#include -#include -#include -#include #include +#include #include #include #include namespace autoware::motion_velocity_planner::out_of_lane { -using autoware_planning_msgs::msg::TrajectoryPoint; +using Polygons = boost::geometry::model::multi_polygon; -/// @brief parameters for the "out of lane" module +/// @brief parameters for the out_of_lane module struct PlannerParam { std::string mode; // mode used to consider a conflict with an object bool skip_if_already_overlapping; // if true, do not run the module when ego already overlaps // another lane + double max_arc_length; // [m] maximum arc length along the trajectory to check for collision + bool ignore_lane_changeable_lanelets; // if true, ignore overlaps on lane changeable lanelets - double time_threshold; // [s](mode="threshold") objects time threshold - double intervals_ego_buffer; // [s](mode="intervals") buffer to extend the ego time range - double intervals_obj_buffer; // [s](mode="intervals") buffer to extend the objects time range + double time_threshold; // [s](mode="threshold") objects time threshold double ttc_threshold; // [s](mode="ttc") threshold on time to collision between ego and an object - double ego_min_velocity; // [m/s] minimum velocity of ego used to calculate its ttc or time range - bool objects_use_predicted_paths; // whether to use the objects' predicted paths bool objects_cut_predicted_paths_beyond_red_lights; // whether to cut predicted paths beyond red // lights' stop lines - double objects_min_vel; // [m/s] objects lower than this velocity will be ignored - double objects_min_confidence; // minimum confidence to consider a predicted path - double objects_dist_buffer; // [m] distance buffer used to determine if a collision will occur in - // the other lane + double objects_min_vel; // [m/s] objects lower than this velocity will be ignored + double objects_min_confidence; // minimum confidence to consider a predicted path bool objects_ignore_behind_ego; // if true, objects behind the ego vehicle are ignored - double overlap_extra_length; // [m] extra length to add around an overlap range - double overlap_min_dist; // [m] min distance inside another lane to consider an overlap - // action to insert in the trajectory if an object causes a conflict at an overlap - bool skip_if_over_max_decel; // if true, skip the action if it causes more than the max decel - double lon_dist_buffer; // [m] safety distance buffer to keep in front of the ego vehicle - double lat_dist_buffer; // [m] safety distance buffer to keep on the side of the ego vehicle - double slow_velocity; - double slow_dist_threshold; - double stop_dist_threshold; - double precision; // [m] precision when inserting a stop pose in the trajectory - double min_decision_duration; // [s] minimum duration needed a decision can be canceled + // action to insert in the trajectory if an object causes a collision at an overlap + double lon_dist_buffer; // [m] safety distance buffer to keep in front of the ego vehicle + double lat_dist_buffer; // [m] safety distance buffer to keep on the side of the ego vehicle + double slow_velocity; // [m/s] slowdown velocity + double stop_dist_threshold; // [m] if a collision is detected bellow this distance ahead of ego, + // try to insert a stop point + double precision; // [m] precision when inserting a stop pose in the trajectory + double + min_decision_duration; // [s] duration needed before a stop or slowdown point can be removed + // ego dimensions used to create its polygon footprint double front_offset; // [m] front offset (from vehicle info) double rear_offset; // [m] rear offset (from vehicle info) @@ -85,98 +79,6 @@ struct PlannerParam double extra_left_offset; // [m] extra left distance }; -struct EnterExitTimes -{ - double enter_time{}; - double exit_time{}; -}; -struct RangeTimes -{ - EnterExitTimes ego{}; - EnterExitTimes object{}; -}; - -/// @brief action taken by the "out of lane" module -struct Slowdown -{ - size_t target_trajectory_idx{}; // we want to slowdown before this trajectory index - double velocity{}; // desired slow down velocity - lanelet::ConstLanelet lane_to_avoid{}; // we want to slowdown before entering this lane -}; -/// @brief slowdown to insert in a trajectory -struct SlowdownToInsert -{ - Slowdown slowdown{}; - autoware_planning_msgs::msg::TrajectoryPoint point{}; -}; - -/// @brief bound of an overlap range (either the first, or last bound) -struct RangeBound -{ - size_t index{}; - lanelet::BasicPoint2d point{}; - double arc_length{}; - double inside_distance{}; -}; - -/// @brief representation of an overlap between the ego footprint and some other lane -struct Overlap -{ - double inside_distance = 0.0; ///!< distance inside the overlap - double min_arc_length = std::numeric_limits::infinity(); - double max_arc_length = 0.0; - lanelet::BasicPoint2d min_overlap_point{}; ///!< point with min arc length - lanelet::BasicPoint2d max_overlap_point{}; ///!< point with max arc length -}; - -/// @brief range along the trajectory where ego overlaps another lane -struct OverlapRange -{ - lanelet::ConstLanelet lane{}; - size_t entering_trajectory_idx{}; - size_t exiting_trajectory_idx{}; - lanelet::BasicPoint2d entering_point{}; // pose of the overlapping point closest along the lane - lanelet::BasicPoint2d exiting_point{}; // pose of the overlapping point furthest along the lane - double inside_distance{}; // [m] how much ego footprint enters the lane - mutable struct - { - std::vector overlaps{}; - std::optional decision{}; - RangeTimes times{}; - std::optional object{}; - } debug; -}; -using OverlapRanges = std::vector; -/// @brief representation of a lane and its current overlap range -struct OtherLane -{ - bool range_is_open = false; - RangeBound first_range_bound{}; - RangeBound last_range_bound{}; - lanelet::ConstLanelet lanelet{}; - lanelet::BasicPolygon2d polygon{}; - - explicit OtherLane(lanelet::ConstLanelet ll) : lanelet(std::move(ll)) - { - polygon = lanelet.polygon2d().basicPolygon(); - } - - [[nodiscard]] OverlapRange close_range() - { - OverlapRange range; - range.lane = lanelet; - range.entering_trajectory_idx = first_range_bound.index; - range.entering_point = first_range_bound.point; - range.exiting_trajectory_idx = last_range_bound.index; - range.exiting_point = last_range_bound.point; - range.inside_distance = - std::max(first_range_bound.inside_distance, last_range_bound.inside_distance); - range_is_open = false; - last_range_bound = {}; - return range; - } -}; - namespace bgi = boost::geometry::index; struct StopLine { @@ -185,68 +87,52 @@ struct StopLine }; using StopLineNode = std::pair; using StopLinesRtree = bgi::rtree>; -using OutAreaRtree = bgi::rtree, bgi::rstar<16>>; +using OutAreaNode = std::pair; +using OutAreaRtree = bgi::rtree>; /// @brief data related to the ego vehicle struct EgoData { - std::vector trajectory_points{}; + std::vector trajectory_points; + geometry_msgs::msg::Pose pose; size_t first_trajectory_idx{}; - double velocity{}; // [m/s] - double max_decel{}; // [m/s²] - geometry_msgs::msg::Pose pose{}; + double longitudinal_offset_to_first_trajectory_index{}; + double min_stop_distance{}; + double min_slowdown_distance{}; + double min_stop_arc_length{}; + + Polygons drivable_lane_polygons; + + lanelet::BasicPolygon2d current_footprint; + std::vector trajectory_footprints; + StopLinesRtree stop_lines_rtree; }; -/// @brief data needed to make decisions -struct DecisionInputs +struct OutOfLanePoint { - OverlapRanges ranges; - EgoData ego_data; - autoware_perception_msgs::msg::PredictedObjects objects; - std::shared_ptr route_handler; - lanelet::ConstLanelets lanelets; + size_t trajectory_index; + lanelet::BasicPolygon2d outside_ring; + std::set collision_times; + std::optional min_object_arrival_time; + std::optional max_object_arrival_time; + std::optional ttc; + lanelet::ConstLanelets overlapped_lanelets; + bool to_avoid = false; +}; +struct OutOfLaneData +{ + std::vector outside_points; + OutAreaRtree outside_areas_rtree; }; /// @brief debug data struct DebugData { - std::vector footprints; - std::vector slowdowns; - geometry_msgs::msg::Pose ego_pose; - OverlapRanges ranges; - lanelet::BasicPolygon2d current_footprint; - lanelet::ConstLanelets current_overlapped_lanelets; - lanelet::ConstLanelets trajectory_lanelets; - lanelet::ConstLanelets ignored_lanelets; - lanelet::ConstLanelets other_lanelets; - std::vector trajectory_points; - size_t first_trajectory_idx; - - size_t prev_footprints = 0; - size_t prev_slowdowns = 0; - size_t prev_ranges = 0; - size_t prev_current_overlapped_lanelets = 0; - size_t prev_ignored_lanelets = 0; - size_t prev_trajectory_lanelets = 0; - size_t prev_other_lanelets = 0; - void reset_data() - { - prev_footprints = footprints.size(); - footprints.clear(); - prev_slowdowns = slowdowns.size(); - slowdowns.clear(); - prev_ranges = ranges.size(); - ranges.clear(); - prev_current_overlapped_lanelets = current_overlapped_lanelets.size(); - current_overlapped_lanelets.clear(); - prev_ignored_lanelets = ignored_lanelets.size(); - ignored_lanelets.clear(); - prev_trajectory_lanelets = trajectory_lanelets.size(); - trajectory_lanelets.clear(); - prev_other_lanelets = other_lanelets.size(); - other_lanelets.clear(); - } + size_t prev_out_of_lane_areas = 0; + size_t prev_ttcs = 0; + size_t prev_objects = 0; + size_t prev_stop_line = 0; }; } // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt index f1eb7ff047fdc..ffc06aa8cc2f8 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt @@ -5,9 +5,20 @@ find_package(autoware_cmake REQUIRED) autoware_package() -# ament_auto_add_library(${PROJECT_NAME}_lib SHARED -# DIRECTORY src -# ) +ament_auto_add_library(${PROJECT_NAME}_lib SHARED + DIRECTORY src +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_collision_checker.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + gtest_main + ${PROJECT_NAME}_lib + ) + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() ament_auto_package(INSTALL_TO_SHARE include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/collision_checker.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/collision_checker.hpp new file mode 100644 index 0000000000000..bf471c62af969 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/collision_checker.hpp @@ -0,0 +1,69 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__COLLISION_CHECKER_HPP_ +#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__COLLISION_CHECKER_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +namespace bgi = boost::geometry::index; +using RtreeNode = std::pair; +using Rtree = bgi::rtree>; + +/// @brief collision as a trajectory index and corresponding collision points +struct Collision +{ + size_t trajectory_index{}; + autoware::universe_utils::MultiPoint2d collision_points; +}; + +/// @brief collision checker for a trajectory as a sequence of 2D footprints +class CollisionChecker +{ + const autoware::universe_utils::MultiPolygon2d trajectory_footprints_; + std::shared_ptr rtree_; + +public: + /// @brief construct the collisions checker + /// @param trajectory_footprints footprints of the trajectory to be used for collision checking + /// @details internally, the rtree is built with the packing algorithm + explicit CollisionChecker(autoware::universe_utils::MultiPolygon2d trajectory_footprints); + + /// @brief efficiently calculate collisions with a geometry + /// @tparam Geometry boost geometry type + /// @param geometry geometry to check for collisions + /// @return collisions between the trajectory footprints and the input geometry + template + [[nodiscard]] std::vector get_collisions(const Geometry & geometry) const; + + /// @brief direct access to the rtree storing the polygon footprints + /// @return rtree of the polygon footprints + [[nodiscard]] std::shared_ptr get_rtree() const { return rtree_; } + + /// @brief get the size of the trajectory used by this collision checker + [[nodiscard]] size_t trajectory_size() const { return trajectory_footprints_.size(); } +}; +} // namespace autoware::motion_velocity_planner + +#endif // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__COLLISION_CHECKER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp index 5dbb872d99ca9..a96587c4465d6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp @@ -15,7 +15,10 @@ #ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ #define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#include +#include #include +#include #include #include @@ -37,12 +40,9 @@ #include #include -#include -#include #include #include #include -#include namespace autoware::motion_velocity_planner { @@ -59,11 +59,11 @@ struct PlannerData } // msgs from callbacks that are used for data-ready - nav_msgs::msg::Odometry current_odometry{}; - geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration{}; - autoware_perception_msgs::msg::PredictedObjects predicted_objects{}; - pcl::PointCloud no_ground_pointcloud{}; - nav_msgs::msg::OccupancyGrid occupancy_grid{}; + nav_msgs::msg::Odometry current_odometry; + geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration; + autoware_perception_msgs::msg::PredictedObjects predicted_objects; + pcl::PointCloud no_ground_pointcloud; + nav_msgs::msg::OccupancyGrid occupancy_grid; std::shared_ptr route_handler; // nearest search @@ -79,7 +79,7 @@ struct PlannerData tier4_v2x_msgs::msg::VirtualTrafficLightStateArray virtual_traffic_light_states; // velocity smoother - std::shared_ptr velocity_smoother_{}; + std::shared_ptr velocity_smoother_; // parameters autoware::vehicle_info_utils::VehicleInfo vehicle_info_; @@ -88,7 +88,7 @@ struct PlannerData *@brief queries the traffic signal information of given Id. if keep_last_observation = true, *recent UNKNOWN observation is overwritten as the last non-UNKNOWN observation */ - std::optional get_traffic_signal( + [[nodiscard]] std::optional get_traffic_signal( const lanelet::Id id, const bool keep_last_observation = false) const { const auto & traffic_light_id_map = @@ -98,6 +98,15 @@ struct PlannerData } return std::make_optional(traffic_light_id_map.at(id)); } + + [[nodiscard]] std::optional calculate_min_deceleration_distance( + const double target_velocity) const + { + return motion_utils::calcDecelDistWithJerkAndAccConstraints( + current_odometry.twist.twist.linear.x, target_velocity, + current_acceleration.accel.accel.linear.x, velocity_smoother_->getMinDecel(), + std::abs(velocity_smoother_->getMinJerk()), velocity_smoother_->getMinJerk()); + } }; } // namespace autoware::motion_velocity_planner 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 062e6a4cb2fc0..515a30348cbee 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 @@ -16,7 +16,6 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common - autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs autoware_planning_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/collision_checker.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/collision_checker.cpp new file mode 100644 index 0000000000000..6e84b63a3c7fc --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/collision_checker.cpp @@ -0,0 +1,70 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/motion_velocity_planner_common/collision_checker.hpp" + +namespace autoware::motion_velocity_planner +{ +CollisionChecker::CollisionChecker(autoware::universe_utils::MultiPolygon2d trajectory_footprints) +: trajectory_footprints_(std::move(trajectory_footprints)) +{ + std::vector nodes; + nodes.reserve(trajectory_footprints_.size()); + for (auto i = 0UL; i < trajectory_footprints_.size(); ++i) { + nodes.emplace_back( + boost::geometry::return_envelope(trajectory_footprints_[i]), + i); + } + rtree_ = std::make_shared(nodes.begin(), nodes.end()); +} + +template +std::vector CollisionChecker::get_collisions(const Geometry & geometry) const +{ + std::vector collisions; + std::vector approximate_results; + autoware::universe_utils::MultiPoint2d intersections; + ; + rtree_->query(bgi::intersects(geometry), std::back_inserter(approximate_results)); + for (const auto & result : approximate_results) { + intersections.clear(); + boost::geometry::intersection(trajectory_footprints_[result.second], geometry, intersections); + if (!intersections.empty()) { + Collision c; + c.trajectory_index = result.second; + c.collision_points = intersections; + collisions.push_back(c); + } + } + return collisions; +} + +template std::vector CollisionChecker::get_collisions( + const autoware::universe_utils::Point2d & geometry) const; +template std::vector CollisionChecker::get_collisions( + const autoware::universe_utils::Line2d & geometry) const; +template std::vector +CollisionChecker::get_collisions( + const autoware::universe_utils::MultiPolygon2d & geometry) const; + +// @warn Multi geometries usually lead to very inefficient queries +template std::vector +CollisionChecker::get_collisions( + const autoware::universe_utils::MultiPoint2d & geometry) const; +template std::vector +CollisionChecker::get_collisions( + const autoware::universe_utils::MultiLineString2d & geometry) const; +template std::vector CollisionChecker::get_collisions< + autoware::universe_utils::Polygon2d>(const autoware::universe_utils::Polygon2d & geometry) const; +} // namespace autoware::motion_velocity_planner 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 new file mode 100644 index 0000000000000..df813db336a9d --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp @@ -0,0 +1,176 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/motion_velocity_planner_common/collision_checker.hpp" + +#include + +#include + +#include +#include + +using autoware::motion_velocity_planner::CollisionChecker; +using autoware::universe_utils::Line2d; +using autoware::universe_utils::MultiLineString2d; +using autoware::universe_utils::MultiPoint2d; +using autoware::universe_utils::MultiPolygon2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; + +Point2d random_point() +{ + static std::random_device r; + static std::default_random_engine e1(r()); + static std::uniform_real_distribution uniform_dist(-100, 100); + return {uniform_dist(e1), uniform_dist(e1)}; +} + +Line2d random_line() +{ + const auto point = random_point(); + const auto point2 = Point2d{point.x() + 1, point.y() + 1}; + const auto point3 = Point2d{point2.x() - 1, point2.y() + 1}; + const auto point4 = Point2d{point3.x() + 1, point3.y() + 1}; + return {point, point2, point3, point4}; +} + +Polygon2d random_polygon() +{ + Polygon2d polygon; + const auto point = random_point(); + const auto point2 = Point2d{point.x() + 1, point.y() + 4}; + const auto point3 = Point2d{point.x() + 4, point.y() + 4}; + const auto point4 = Point2d{point.x() + 3, point.y() + 1}; + polygon.outer() = {point, point2, point3, point4, point}; + return polygon; +} + +bool all_within(const MultiPoint2d & pts1, const MultiPoint2d & pts2) +{ + // results from the collision checker and the direct checks can have some small precision errors + constexpr auto eps = 1e-3; + for (const auto & p1 : pts1) { + bool found = false; + for (const auto & p2 : pts2) { + if (boost::geometry::comparable_distance(p1, p2) < eps) { + found = true; + break; + } + } + if (!found) return false; + } + return true; +} + +TEST(TestCollisionChecker, Benchmark) +{ + constexpr auto nb_ego_footprints = 1000; + constexpr auto nb_obstacles = 1000; + MultiPolygon2d ego_footprints; + ego_footprints.reserve(nb_ego_footprints); + for (auto i = 0; i < nb_ego_footprints; ++i) { + ego_footprints.push_back(random_polygon()); + } + const auto cc_constr_start = std::chrono::system_clock::now(); + CollisionChecker collision_checker(ego_footprints); + const auto cc_constr_end = std::chrono::system_clock::now(); + const auto cc_constr_ns = + std::chrono::duration_cast(cc_constr_end - cc_constr_start).count(); + std::printf( + "Collision checker construction (with %d footprints): %ld ns\n", nb_ego_footprints, + cc_constr_ns); + MultiPolygon2d poly_obstacles; + MultiPoint2d point_obstacles; + MultiLineString2d line_obstacles; + for (auto i = 0; i < nb_obstacles; ++i) { + poly_obstacles.push_back(random_polygon()); + point_obstacles.push_back(random_point()); + line_obstacles.push_back(random_line()); + } + const auto check_obstacles_one_by_one = [&](const auto & obstacles) { + std::chrono::nanoseconds collision_checker_ns{}; + std::chrono::nanoseconds naive_ns{}; + for (const auto & obs : obstacles) { + const auto cc_start = std::chrono::system_clock::now(); + const auto collisions = collision_checker.get_collisions(obs); + MultiPoint2d cc_collision_points; + for (const auto & c : collisions) + cc_collision_points.insert( + cc_collision_points.end(), c.collision_points.begin(), c.collision_points.end()); + const auto cc_end = std::chrono::system_clock::now(); + const auto naive_start = std::chrono::system_clock::now(); + MultiPoint2d naive_collision_points; + for (const auto & ego_footprint : ego_footprints) { + MultiPoint2d points; + boost::geometry::intersection(ego_footprint, obs, points); + naive_collision_points.insert(naive_collision_points.end(), points.begin(), points.end()); + } + const auto naive_end = std::chrono::system_clock::now(); + const auto equal = all_within(cc_collision_points, naive_collision_points) && + all_within(naive_collision_points, cc_collision_points); + EXPECT_TRUE(equal); + if (!equal) { + std::cout << "cc: " << boost::geometry::wkt(cc_collision_points) << std::endl; + std::cout << "naive: " << boost::geometry::wkt(naive_collision_points) << std::endl; + } + collision_checker_ns += + std::chrono::duration_cast(cc_end - cc_start); + naive_ns += std::chrono::duration_cast(naive_end - naive_start); + } + std::printf("%20s%10ld ns\n", "collision checker : ", collision_checker_ns.count()); + std::printf("%20s%10ld ns\n", "naive : ", naive_ns.count()); + }; + const auto check_obstacles = [&](const auto & obstacles) { + std::chrono::nanoseconds collision_checker_ns{}; + std::chrono::nanoseconds naive_ns{}; + const auto cc_start = std::chrono::system_clock::now(); + const auto collisions = collision_checker.get_collisions(obstacles); + MultiPoint2d cc_collision_points; + for (const auto & c : collisions) + cc_collision_points.insert( + cc_collision_points.end(), c.collision_points.begin(), c.collision_points.end()); + const auto cc_end = std::chrono::system_clock::now(); + const auto naive_start = std::chrono::system_clock::now(); + MultiPoint2d naive_collision_points; + boost::geometry::intersection(ego_footprints, obstacles, naive_collision_points); + const auto naive_end = std::chrono::system_clock::now(); + const auto equal = all_within(cc_collision_points, naive_collision_points) && + all_within(naive_collision_points, cc_collision_points); + EXPECT_TRUE(equal); + if (!equal) { + std::cout << "cc: " << boost::geometry::wkt(cc_collision_points) << std::endl; + std::cout << "naive: " << boost::geometry::wkt(naive_collision_points) << std::endl; + } + collision_checker_ns += std::chrono::duration_cast(cc_end - cc_start); + naive_ns += std::chrono::duration_cast(naive_end - naive_start); + std::printf("%20s%10ld ns\n", "collision checker : ", collision_checker_ns.count()); + std::printf("%20s%10ld ns\n", "naive : ", naive_ns.count()); + }; + + std::cout << "* check one by one\n"; + std::printf("%d Polygons:\n", nb_obstacles); + check_obstacles_one_by_one(poly_obstacles); + std::printf("%d Lines:\n", nb_obstacles); + check_obstacles_one_by_one(line_obstacles); + std::printf("%d Points:\n", nb_obstacles); + check_obstacles_one_by_one(point_obstacles); + std::cout << "* check all at once\n"; + std::printf("%d Polygons:\n", nb_obstacles); + check_obstacles(poly_obstacles); + std::printf("%d Lines:\n", nb_obstacles); + check_obstacles(line_obstacles); + std::printf("%d Points:\n", nb_obstacles); + check_obstacles(point_obstacles); +} 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 373eebbbd93b5..c1e68a232c55d 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 @@ -17,7 +17,6 @@ rosidl_default_generators - autoware_lanelet2_extension autoware_map_msgs autoware_motion_utils autoware_motion_velocity_planner_common diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index be9eab56d2320..9a08616b8fe7b 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -85,6 +84,8 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & "~/output/velocity_factors", 1); processing_time_publisher_ = this->create_publisher("~/debug/processing_time_ms", 1); + debug_viz_pub_ = + this->create_publisher("~/debug/markers", 1); diagnostics_pub_ = this->create_publisher("/diagnostics", 10); // Parameters @@ -99,7 +100,7 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & // Initialize PlannerManager for (const auto & name : declare_parameter>("launch_modules")) { // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. - if (name == "") { + if (name.empty()) { break; } planner_manager_.load_module_plugin(*this, name); @@ -135,7 +136,7 @@ bool MotionVelocityPlannerNode::update_planner_data() const auto check_with_log = [&](const auto ptr, const auto & log) { constexpr auto throttle_duration = 3000; // [ms] if (!ptr) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, log); + RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, "%s", log); is_ready = false; return false; } @@ -278,7 +279,6 @@ void MotionVelocityPlannerNode::on_trajectory( autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points{ input_trajectory_msg->points.begin(), input_trajectory_msg->points.end()}; - auto output_trajectory_msg = generate_trajectory(input_trajectory_points); output_trajectory_msg.header = input_trajectory_msg->header; processing_times["generate_trajectory"] = stop_watch.toc(true); @@ -333,7 +333,8 @@ void MotionVelocityPlannerNode::insert_slowdown( autoware::motion_utils::insertTargetPoint(to_seg_idx, slowdown_interval.to, trajectory.points); if (from_insert_idx && to_insert_idx) { for (auto idx = *from_insert_idx; idx <= *to_insert_idx; ++idx) - trajectory.points[idx].longitudinal_velocity_mps = slowdown_interval.velocity; + trajectory.points[idx].longitudinal_velocity_mps = + static_cast(slowdown_interval.velocity); } else { RCLCPP_WARN(get_logger(), "Failed to insert slowdown point"); } @@ -368,16 +369,14 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s autoware::motion_velocity_planner::TrajectoryPoints clipped; autoware::motion_velocity_planner::TrajectoryPoints traj_smoothed; clipped.insert( - clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); + clipped.end(), std::next(traj_resampled.begin(), static_cast(traj_resampled_closest)), + traj_resampled.end()); if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories, false)) { RCLCPP_ERROR(get_logger(), "failed to smooth"); } - traj_smoothed.insert( - traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); - if (external_v_limit) { autoware::velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( - traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); + 0LU, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); } return traj_smoothed; } @@ -385,13 +384,21 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_trajectory( autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points) { + universe_utils::StopWatch stop_watch; autoware_planning_msgs::msg::Trajectory output_trajectory_msg; output_trajectory_msg.points = {input_trajectory_points.begin(), input_trajectory_points.end()}; - if (smooth_velocity_before_planning_) + if (smooth_velocity_before_planning_) { + stop_watch.tic("smooth"); input_trajectory_points = smooth_trajectory(input_trajectory_points, planner_data_); - const auto resampled_trajectory = - autoware::motion_utils::resampleTrajectory(output_trajectory_msg, 0.5); - + RCLCPP_DEBUG(get_logger(), "smooth: %2.2f us", stop_watch.toc("smooth")); + } + autoware_planning_msgs::msg::Trajectory smooth_velocity_trajectory; + smooth_velocity_trajectory.points = { + input_trajectory_points.begin(), input_trajectory_points.end()}; + auto resampled_trajectory = + autoware::motion_utils::resampleTrajectory(smooth_velocity_trajectory, 0.5); + motion_utils::calculate_time_from_start( + resampled_trajectory.points, planner_data_.current_odometry.pose.pose.position); const auto planning_results = planner_manager_.plan_velocities( resampled_trajectory.points, std::make_shared(planner_data_)); From 87cfc7fb587298bb3363895c5aff4ca2b0d422fd Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Mon, 26 Aug 2024 12:05:19 +0900 Subject: [PATCH 33/46] chore(autoware_pointcloud_preprocessor): change unnecessary warning message to debug (#8525) --- .../src/concatenate_data/concatenate_and_time_sync_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index 2410a9dccebea..0a051d780f5f7 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -273,7 +273,7 @@ PointCloudConcatenateDataSynchronizerComponent::computeTransformToAdjustForOldTi // return identity if old_stamp is newer than new_stamp if (old_stamp > new_stamp) { - RCLCPP_WARN_STREAM_THROTTLE( + RCLCPP_DEBUG_STREAM_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(), "old_stamp is newer than new_stamp,"); return Eigen::Matrix4f::Identity(); From 43efc6fd61994086d03b4adca9fa90fb7f6e9e0c Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Mon, 26 Aug 2024 12:07:48 +0900 Subject: [PATCH 34/46] fix(simple_planning_simulator): fix acc output for the model sim_model_delay_steer_acc_geared_wo_fall_guard (#8607) fix acceleration output Signed-off-by: Yuki Takagi --- .../sim_model_delay_steer_acc_geared_wo_fall_guard.hpp | 1 + .../sim_model_delay_steer_acc_geared_wo_fall_guard.cpp | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp index d7a6a8284d704..5d9c3ec74115a 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp @@ -64,6 +64,7 @@ class SimModelDelaySteerAccGearedWoFallGuard : public SimModelInterface VX, STEER, PEDAL_ACCX, + ACCX, }; enum IDX_U { PEDAL_ACCX_DES = 0, GEAR, SLOPE_ACCX, STEER_DES }; 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 c699d704724f5..69a311b780ad8 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 @@ -64,7 +64,7 @@ double SimModelDelaySteerAccGearedWoFallGuard::getVy() } double SimModelDelaySteerAccGearedWoFallGuard::getAx() { - return state_(IDX::PEDAL_ACCX); + return state_(IDX::ACCX); } double SimModelDelaySteerAccGearedWoFallGuard::getWz() { @@ -103,6 +103,8 @@ void SimModelDelaySteerAccGearedWoFallGuard::update(const double & dt) // stop condition is satisfied state_(IDX::VX) = 0.0; } + + state_(IDX::ACCX) = (state_(IDX::VX) - prev_state(IDX::VX)) / dt; } void SimModelDelaySteerAccGearedWoFallGuard::initializeInputQueue(const double & dt) From b35ad4b2868f47b33c58af493093e5d0a078fc9a Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Mon, 26 Aug 2024 14:26:00 +0900 Subject: [PATCH 35/46] fix(autonomous_emergency_braking): fix debug marker visual bug (#8611) fix bug by using the collision data keeper Signed-off-by: Daniel Sanchez --- .../autonomous_emergency_braking/node.hpp | 5 +--- .../src/node.cpp | 25 +++++++++++-------- 2 files changed, 16 insertions(+), 14 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 24eaa8516a732..c29247a978c73 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 @@ -156,10 +156,7 @@ class CollisionDataKeeper * @brief Get the closest object data * @return Object data of the closest object */ - [[nodiscard]] ObjectData get() const - { - return (closest_object_.has_value()) ? closest_object_.value() : ObjectData(); - } + [[nodiscard]] std::optional get() const { return closest_object_; } /** * @brief Get the previous closest object data diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index dc8c876bd56f6..e8744d85dc11a 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -393,12 +393,15 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) const auto diag_level = DiagnosticStatus::ERROR; stat.summary(diag_level, error_msg); const auto & data = collision_data_keeper_.get(); - stat.addf("RSS", "%.2f", data.rss); - stat.addf("Distance", "%.2f", data.distance_to_object); - stat.addf("Object Speed", "%.2f", data.velocity); - if (publish_debug_markers_) { - addCollisionMarker(data, debug_markers); + if (data.has_value()) { + stat.addf("RSS", "%.2f", data.value().rss); + stat.addf("Distance", "%.2f", data.value().distance_to_object); + stat.addf("Object Speed", "%.2f", data.value().velocity); + if (publish_debug_markers_) { + addCollisionMarker(data.value(), debug_markers); + } } + addVirtualStopWallMarker(info_markers); } else { const std::string error_msg = "[AEB]: No Collision"; @@ -476,17 +479,19 @@ bool AEB::checkCollision(MarkerArray & debug_markers) return std::make_optional(*closest_object_point_itr); }); + const bool has_collision = (closest_object_point.has_value()) + ? hasCollision(current_v, closest_object_point.value()) + : false; + // Add debug markers if (publish_debug_markers_) { const auto [color_r, color_g, color_b, color_a] = debug_colors; addMarker( - this->get_clock()->now(), path, ego_polys, objects, closest_object_point, color_r, color_g, - color_b, color_a, debug_ns, debug_markers); + this->get_clock()->now(), path, ego_polys, objects, collision_data_keeper_.get(), color_r, + color_g, color_b, color_a, debug_ns, debug_markers); } // check collision using rss distance - return (closest_object_point.has_value()) - ? hasCollision(current_v, closest_object_point.value()) - : false; + return has_collision; }; // step3. make function to check collision with ego path created with sensor data From f021d06ee5a16e625d7d2646b2497d9d69f0afd1 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 26 Aug 2024 14:36:34 +0900 Subject: [PATCH 36/46] fix(goal_planner): remove time keeper in non main thread (#8610) Signed-off-by: kosuke55 --- .../src/goal_planner_module.cpp | 4 ---- 1 file changed, 4 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 ff248fbe02a1c..c67983993de01 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 @@ -1206,8 +1206,6 @@ bool GoalPlannerModule::hasDecidedPath( const std::shared_ptr & safety_check_params, const std::shared_ptr goal_searcher) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - return checkDecidingPathStatus( planner_data, occupancy_grid_map, parameters, ego_predicted_path_params, objects_filtering_params, safety_check_params, goal_searcher) @@ -1240,8 +1238,6 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus( const std::shared_ptr & safety_check_params, const std::shared_ptr goal_searcher) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - 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; From c80eddc26daf25d975dd294c5cb428d42ed7e600 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Mon, 26 Aug 2024 15:05:12 +0900 Subject: [PATCH 37/46] fix(lane_change): update rtc status for some failure condition (#8604) update rtc status for some failure condition Signed-off-by: Go Sakayori --- .../src/interface.cpp | 6 ++++++ 1 file changed, 6 insertions(+) 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 a7cf89193b37f..979004d439bdc 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 @@ -258,11 +258,17 @@ bool LaneChangeInterface::canTransitFailureState() if (!module_type_->isValidPath()) { log_debug_throttled("Transit to failure state due not to find valid path"); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); return true; } if (module_type_->isAbortState() && module_type_->hasFinishedAbort()) { log_debug_throttled("Abort process has completed."); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); return true; } From 1db4a65d5c6963b0430df7d7f24ec34eeed6bc3d Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Mon, 26 Aug 2024 15:26:26 +0900 Subject: [PATCH 38/46] fix(freespace_planner): fix free space planner spamming message (#8614) check data availability only when scenario is active Signed-off-by: mohammad alqudah --- .../freespace_planner_node.cpp | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp index 8e8a962ed1071..267fd9932e781 100644 --- a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp @@ -416,7 +416,6 @@ void FreespacePlannerNode::onOdometry(const Odometry::ConstSharedPtr msg) void FreespacePlannerNode::updateData() { occupancy_grid_ = occupancy_grid_sub_.takeData(); - scenario_ = scenario_sub_.takeData(); { auto msgs = odom_sub_.takeData(); @@ -440,11 +439,6 @@ bool FreespacePlannerNode::isDataReady() is_ready = false; } - if (!scenario_) { - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "Waiting for scenario."); - is_ready = false; - } - if (!odom_) { RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "Waiting for odometry."); is_ready = false; @@ -455,14 +449,15 @@ bool FreespacePlannerNode::isDataReady() void FreespacePlannerNode::onTimer() { - updateData(); - - if (!isDataReady()) { + scenario_ = scenario_sub_.takeData(); + if (!isActive(scenario_)) { + reset(); return; } - if (!isActive(scenario_)) { - reset(); + updateData(); + + if (!isDataReady()) { return; } From 3f015666774c42ed9bdeb3568ab34a563595448d Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Mon, 26 Aug 2024 16:29:11 +0900 Subject: [PATCH 39/46] refactor(ekf_localizer): fix tf2 listener (#8584) * fix tf2 listener Signed-off-by: Yamato Ando * style(pre-commit): autofix --------- Signed-off-by: Yamato Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../include/ekf_localizer/ekf_localizer.hpp | 5 +++++ .../ekf_localizer/src/ekf_localizer.cpp | 19 +++++++------------ 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index f3830c303ab48..650eca26d4e96 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -39,6 +39,7 @@ #include #include +#include #include #include @@ -141,6 +142,10 @@ class EKFLocalizer : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_tf_; //!< @brief tf broadcaster std::shared_ptr tf_br_; + //!< @brief tf buffer + tf2_ros::Buffer tf2_buffer_; + //!< @brief tf listener + tf2_ros::TransformListener tf2_listener_; //!< @brief logger configure module std::unique_ptr logger_configure_; diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 707d5e585cb0c..f1e4d0c958c77 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -44,6 +44,8 @@ using std::placeholders::_1; EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) : rclcpp::Node("ekf_localizer", node_options), warning_(std::make_shared(this)), + tf2_buffer_(this->get_clock()), + tf2_listener_(tf2_buffer_), params_(this), ekf_dt_(params_.ekf_dt), pose_queue_(params_.pose_smoothing_steps), @@ -285,21 +287,14 @@ bool EKFLocalizer::get_transform_from_tf( std::string parent_frame, std::string child_frame, geometry_msgs::msg::TransformStamped & transform) { - tf2::BufferCore tf_buffer; - tf2_ros::TransformListener tf_listener(tf_buffer); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - parent_frame = erase_leading_slash(parent_frame); child_frame = erase_leading_slash(child_frame); - for (int i = 0; i < 50; ++i) { - try { - transform = tf_buffer.lookupTransform(parent_frame, child_frame, tf2::TimePointZero); - return true; - } catch (tf2::TransformException & ex) { - warning_->warn(ex.what()); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } + try { + transform = tf2_buffer_.lookupTransform(parent_frame, child_frame, tf2::TimePointZero); + return true; + } catch (tf2::TransformException & ex) { + warning_->warn(ex.what()); } return false; } From f64c4ed448c88c83e200d45f6c4e1eb196142578 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Mon, 26 Aug 2024 16:29:26 +0900 Subject: [PATCH 40/46] fix(ndt_scan_matcher): fix timestamp in service_ndt_align func (#8599) fix timestamp Signed-off-by: Yamato Ando --- localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 3e8b9b99737c0..b59686c211b53 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -961,7 +961,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 - const auto initial_pose_msg_in_map_frame = transform(req->pose_with_covariance, transform_s2t); + auto initial_pose_msg_in_map_frame = 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_); From f99aa3a79b30ea39c1b160a565223d455fef3fb6 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Mon, 26 Aug 2024 17:18:30 +0900 Subject: [PATCH 41/46] fix(ekf_localizer): change roll, pitch proc dev (#8619) change roll, pitch proc dev Signed-off-by: Yamato Ando --- localization/ekf_localizer/config/ekf_localizer.param.yaml | 4 ++-- .../schema/sub/simple_1d_filter_parameters.sub_schema.json | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index 4a7696ec9e65e..13319521e35ab 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml @@ -30,8 +30,8 @@ simple_1d_filter_parameters: #Simple1DFilter parameters z_filter_proc_dev: 1.0 - roll_filter_proc_dev: 0.01 - pitch_filter_proc_dev: 0.01 + roll_filter_proc_dev: 0.1 + pitch_filter_proc_dev: 0.1 diagnostics: # for diagnostics diff --git a/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json b/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json index ad2f638a18d5f..00679ee92ad24 100644 --- a/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json +++ b/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json @@ -13,12 +13,12 @@ "roll_filter_proc_dev": { "type": "number", "description": "Simple1DFilter - Roll filter process deviation", - "default": 0.01 + "default": 0.1 }, "pitch_filter_proc_dev": { "type": "number", "description": "Simple1DFilter - Pitch filter process deviation", - "default": 0.01 + "default": 0.1 } }, "required": ["z_filter_proc_dev", "roll_filter_proc_dev", "pitch_filter_proc_dev"], From fe953a40cd4a2c09a7cd82d6f25c1d09b2a9d86c Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Mon, 26 Aug 2024 17:21:19 +0900 Subject: [PATCH 42/46] docs(autoware_autonomous_emergency_braking): improve AEB module's README (#8612) * docs: improve AEB module's README Signed-off-by: Kyoichi Sugahara * update rss distance length Signed-off-by: Kyoichi Sugahara --------- Signed-off-by: Kyoichi Sugahara --- .../README.md | 8 +- .../image/rss_check.drawio.svg | 183 ++++++------------ 2 files changed, 65 insertions(+), 126 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index 1382a24d98b0a..66d4483fbdf84 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -8,8 +8,6 @@ This module has following assumptions. -- It is used when driving at low speeds (about 15 km/h). - - The predicted path of the ego vehicle can be made from either the path created from sensors or the path created from a control module, or both. - The current speed and angular velocity can be obtained from the sensors of the ego vehicle, and it uses points as obstacles. @@ -25,7 +23,7 @@ The pros and cons of both approaches are: IMU angular velocity: - (+) Usually, it has high accuracy -- (-)Vehicle vibration might introduce noise. +- (-) Vehicle vibration might introduce noise. Steering angle: @@ -186,7 +184,7 @@ The AEB module can also prevent collisions when the ego vehicle is moving backwa When vehicle odometry information is faulty, it is possible that the MPC fails to predict a correct path for the ego vehicle. If the MPC predicted path is wrong, collision avoidance will not work as intended on the planning modules. However, the AEB’s IMU path does not depend on the MPC and could be able to predict a collision when the other modules cannot. As an example you can see a figure of a hypothetical case in which the MPC path is wrong and only the AEB’s IMU path detects a collision. -![backward driving](./image/wrong-mpc.drawio.svg) +![wrong mpc](./image/wrong-mpc.drawio.svg) ## Parameters @@ -217,6 +215,8 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t ## Limitations +- The distance required to stop after collision detection depends on the ego vehicle's speed and deceleration performance. To avoid collisions, it's necessary to increase the detection distance and set a higher deceleration rate. However, this creates a trade-off as it may also increase the number of unnecessary activations. Therefore, it's essential to consider what role this module should play and adjust the parameters accordingly. + - AEB might not be able to react with obstacles that are close to the ground. It depends on the performance of the pre-processing methods applied to the point cloud. - Longitudinal acceleration information obtained from sensors is not used due to the high amount of noise. diff --git a/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg b/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg index 2d1716519631d..1d3dd824df5f0 100644 --- a/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg +++ b/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg @@ -1,134 +1,73 @@ - - - - - - - + + + + + + + + + + + + + + + + +
+
+ + Closest point distance +
+ +
+
+ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- - Closest Point Distance -
-
-
- - - - - - - - - - - - - - - - - -
-
-
RSS distance
-
-
- - - - + + + + + + +
+
+ RSS distance +
+
+ From 1a58707d3a6fda9ad32d84a7940a1544a71f7af4 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Mon, 26 Aug 2024 19:32:54 +0900 Subject: [PATCH 43/46] fix(autoware_traffic_light_map_based_detector): output from screen to both (#8411) --- .../launch/traffic_light_map_based_detector.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/autoware_traffic_light_map_based_detector/launch/traffic_light_map_based_detector.launch.xml b/perception/autoware_traffic_light_map_based_detector/launch/traffic_light_map_based_detector.launch.xml index 9f035e7f1ae6c..39026c85c1cef 100644 --- a/perception/autoware_traffic_light_map_based_detector/launch/traffic_light_map_based_detector.launch.xml +++ b/perception/autoware_traffic_light_map_based_detector/launch/traffic_light_map_based_detector.launch.xml @@ -12,7 +12,7 @@ - + From a24e2069fa255f5bb3e35a5c6ab2dcad73eb8739 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Mon, 26 Aug 2024 19:33:34 +0900 Subject: [PATCH 44/46] fix(autoware_multi_object_tracker): output from screen to both (#8407) --- .../launch/multi_object_tracker.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/autoware_multi_object_tracker/launch/multi_object_tracker.launch.xml b/perception/autoware_multi_object_tracker/launch/multi_object_tracker.launch.xml index db76e181a6afa..88f63e00ed511 100644 --- a/perception/autoware_multi_object_tracker/launch/multi_object_tracker.launch.xml +++ b/perception/autoware_multi_object_tracker/launch/multi_object_tracker.launch.xml @@ -6,7 +6,7 @@ - + From ac95a1fc6492d874113e3ae3f70ab8adf70a3086 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Mon, 26 Aug 2024 19:39:58 +0900 Subject: [PATCH 45/46] fix(autoware_map_based_prediction): output from screen to both (#8408) --- .../launch/map_based_prediction.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/autoware_map_based_prediction/launch/map_based_prediction.launch.xml b/perception/autoware_map_based_prediction/launch/map_based_prediction.launch.xml index b65f0de892dcc..2c668639c2a56 100644 --- a/perception/autoware_map_based_prediction/launch/map_based_prediction.launch.xml +++ b/perception/autoware_map_based_prediction/launch/map_based_prediction.launch.xml @@ -7,7 +7,7 @@ - + From a3cf7bda38e1dba1c5b2aecab081ca8fd1398671 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Tue, 27 Aug 2024 07:58:40 +0900 Subject: [PATCH 46/46] fix(traffic_light_utils): fix unusedFunction (#8605) * fix:unusedFunction Signed-off-by: kobayu858 * fix:unusedFunction Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- .../traffic_light_utils.hpp | 7 ---- .../src/traffic_light_utils.cpp | 23 ----------- .../test/test_traffic_light_utils.cpp | 40 ------------------- 3 files changed, 70 deletions(-) diff --git a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp index cf4ecdd9774b8..3a40c0c3b1d51 100644 --- a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp +++ b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp @@ -30,13 +30,6 @@ namespace traffic_light_utils { -bool isRoiValid( - const tier4_perception_msgs::msg::TrafficLightRoi & roi, uint32_t width, uint32_t height); - -void setRoiInvalid(tier4_perception_msgs::msg::TrafficLightRoi & roi); - -bool isSignalUnknown(const tier4_perception_msgs::msg::TrafficLight & signal); - void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float confidence = -1); /** diff --git a/common/traffic_light_utils/src/traffic_light_utils.cpp b/common/traffic_light_utils/src/traffic_light_utils.cpp index 8aea884510ec2..a3da21c1bb578 100644 --- a/common/traffic_light_utils/src/traffic_light_utils.cpp +++ b/common/traffic_light_utils/src/traffic_light_utils.cpp @@ -17,29 +17,6 @@ namespace traffic_light_utils { -bool isRoiValid( - const tier4_perception_msgs::msg::TrafficLightRoi & roi, uint32_t width, uint32_t height) -{ - uint32_t x1 = roi.roi.x_offset; - uint32_t x2 = roi.roi.x_offset + roi.roi.width; - uint32_t y1 = roi.roi.y_offset; - uint32_t y2 = roi.roi.y_offset + roi.roi.height; - return roi.roi.width > 0 && roi.roi.height > 0 && x1 < width && y1 < height && x2 < width && - y2 < height; -} - -void setRoiInvalid(tier4_perception_msgs::msg::TrafficLightRoi & roi) -{ - roi.roi.height = roi.roi.width = 0; -} - -bool isSignalUnknown(const tier4_perception_msgs::msg::TrafficLight & signal) -{ - return signal.elements.size() == 1 && - signal.elements[0].shape == tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN && - signal.elements[0].color == tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; -} - void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float confidence) { signal.elements.resize(1); diff --git a/common/traffic_light_utils/test/test_traffic_light_utils.cpp b/common/traffic_light_utils/test/test_traffic_light_utils.cpp index a80510693c487..a9e601370945d 100644 --- a/common/traffic_light_utils/test/test_traffic_light_utils.cpp +++ b/common/traffic_light_utils/test/test_traffic_light_utils.cpp @@ -18,46 +18,6 @@ namespace traffic_light_utils { -TEST(isRoiValid, roi_validity) -{ - tier4_perception_msgs::msg::TrafficLightRoi test_roi; - test_roi.roi.x_offset = 300; - test_roi.roi.y_offset = 200; - test_roi.roi.width = 340; - test_roi.roi.height = 200; - uint32_t img_width = 640; - uint32_t img_heigh = 480; - EXPECT_FALSE(isRoiValid(test_roi, img_width, img_heigh)); - test_roi.roi.width = 339; - EXPECT_TRUE(isRoiValid(test_roi, img_width, img_heigh)); -} - -TEST(setRoiInvalid, set_roi_size) -{ - tier4_perception_msgs::msg::TrafficLightRoi test_roi; - test_roi.roi.x_offset = 300; - test_roi.roi.y_offset = 200; - test_roi.roi.width = 300; - test_roi.roi.height = 200; - EXPECT_EQ(test_roi.roi.width, (uint32_t)300); - EXPECT_EQ(test_roi.roi.height, (uint32_t)200); - setRoiInvalid(test_roi); - EXPECT_EQ(test_roi.roi.width, (uint32_t)0); - EXPECT_EQ(test_roi.roi.height, (uint32_t)0); -} - -TEST(isSignalUnknown, signal_element) -{ - tier4_perception_msgs::msg::TrafficLight test_signal; - tier4_perception_msgs::msg::TrafficLightElement element; - element.color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; - element.shape = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; - test_signal.elements.push_back(element); - EXPECT_TRUE(isSignalUnknown(test_signal)); - test_signal.elements[0].color = tier4_perception_msgs::msg::TrafficLightElement::RED; - EXPECT_FALSE(isSignalUnknown(test_signal)); -} - TEST(setSignalUnknown, set_signal_element) { tier4_perception_msgs::msg::TrafficLight test_signal;