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 01/10] 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 02/10] 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 03/10] 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 04/10] 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 05/10] 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 06/10] 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 07/10] 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 08/10] 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 09/10] 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 10/10] 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";