diff --git a/codecov.yaml b/codecov.yaml index 7bec89dd5a552..726b675d7ff1d 100644 --- a/codecov.yaml +++ b/codecov.yaml @@ -122,7 +122,6 @@ component_management: - component_id: localization-tier-iv-maintained-packages name: Localization TIER IV Maintained Packages paths: - - localization/autoware_ekf_localizer/** - localization/autoware_gyro_odometer/** - localization/autoware_localization_error_monitor/** - localization/autoware_localization_util/** diff --git a/localization/autoware_ekf_localizer/CHANGELOG.rst b/localization/autoware_ekf_localizer/CHANGELOG.rst deleted file mode 100644 index b8e74c530e059..0000000000000 --- a/localization/autoware_ekf_localizer/CHANGELOG.rst +++ /dev/null @@ -1,63 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_ekf_localizer -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - localization (`#9567 `_) -* fix(autoware_ekf_localizer): publish `processing_time_ms` (`#9443 `_) - Fixed to publish processing_time_ms -* 0.39.0 -* update changelog -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* fix(autoware_ekf_localizer): remove `timer_tf\_` (`#9244 `_) - Removed timer_tf\_ -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, SakodaShintaro, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* fix(autoware_ekf_localizer): remove `timer_tf\_` (`#9244 `_) - Removed timer_tf\_ -* Contributors: Esteve Fernandez, SakodaShintaro, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(localization_util)!: prefix package and namespace with autoware (`#8922 `_) - add autoware prefix to localization_util -* refactor(ekf_localizer)!: prefix package and namespace with autoware (`#8888 `_) - * import lanelet2_map_preprocessor - * move headers to include/autoware/efk_localier - --------- -* Contributors: Masaki Baba, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/localization/autoware_ekf_localizer/CMakeLists.txt b/localization/autoware_ekf_localizer/CMakeLists.txt deleted file mode 100644 index 6ace0b413d7a6..0000000000000 --- a/localization/autoware_ekf_localizer/CMakeLists.txt +++ /dev/null @@ -1,82 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_ekf_localizer) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Eigen3 REQUIRED) - -include_directories( - SYSTEM - ${EIGEN3_INCLUDE_DIR} -) - -ament_auto_find_build_dependencies() - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/ekf_localizer.cpp - src/covariance.cpp - src/diagnostics.cpp - src/mahalanobis.cpp - src/measurement.cpp - src/state_transition.cpp - src/warning_message.cpp - src/ekf_module.cpp -) - -rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "autoware::ekf_localizer::EKFLocalizer" - EXECUTABLE ${PROJECT_NAME}_node - EXECUTOR SingleThreadedExecutor -) - -target_link_libraries(${PROJECT_NAME} Eigen3::Eigen) - -function(add_testcase filepath) - get_filename_component(filename ${filepath} NAME) - string(REGEX REPLACE ".cpp" "" test_name ${filename}) - - ament_add_gtest(${test_name} ${filepath}) - target_link_libraries("${test_name}" ${PROJECT_NAME}) - ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) -endfunction() - -if(BUILD_TESTING) - add_launch_test( - test/test_ekf_localizer_launch.py - TIMEOUT "30" - ) - add_launch_test( - test/test_ekf_localizer_mahalanobis.py - TIMEOUT "30" - ) - find_package(ament_cmake_gtest REQUIRED) - - file(GLOB_RECURSE TEST_FILES test/*.cpp) - - foreach(filepath ${TEST_FILES}) - add_testcase(${filepath}) - endforeach() -endif() - - -# if(BUILD_TESTING) -# find_package(ament_cmake_ros REQUIRED) -# ament_add_ros_isolated_gtest(ekf_localizer-test test/test_ekf_localizer.test -# test/src/test_ekf_localizer.cpp -# src/ekf_localizer.cpp -# src/kalman_filter/kalman_filter.cpp -# src/kalman_filter/time_delay_kalman_filter.cpp -# ) -# target_include_directories(ekf_localizer-test -# PRIVATE -# include -# ) -# ament_target_dependencies(ekf_localizer-test geometry_msgs rclcpp tf2 tf2_ros) -# endif() - -ament_auto_package( - INSTALL_TO_SHARE - config - launch -) diff --git a/localization/autoware_ekf_localizer/README.md b/localization/autoware_ekf_localizer/README.md deleted file mode 100644 index a46ea7181315f..0000000000000 --- a/localization/autoware_ekf_localizer/README.md +++ /dev/null @@ -1,211 +0,0 @@ -# Overview - -The **Extend Kalman Filter Localizer** estimates robust and less noisy robot pose and twist by integrating the 2D vehicle dynamics model with input ego-pose and ego-twist messages. The algorithm is designed especially for fast-moving robots such as autonomous driving systems. - -## Flowchart - -The overall flowchart of the autoware_ekf_localizer is described below. - -

- -

- -## Features - -This package includes the following features: - -- **Time delay compensation** for input messages, which enables proper integration of input information with varying time delays. This is important especially for high-speed moving robots, such as autonomous driving vehicles. (see the following figure). -- **Automatic estimation of yaw bias** prevents modeling errors caused by sensor mounting angle errors, which can improve estimation accuracy. -- **Mahalanobis distance gate** enables probabilistic outlier detection to determine which inputs should be used or ignored. -- **Smooth update**, the Kalman Filter measurement update is typically performed when a measurement is obtained, but it can cause large changes in the estimated value, especially for low-frequency measurements. Since the algorithm can consider the measurement time, the measurement data can be divided into multiple pieces and integrated smoothly while maintaining consistency (see the following figure). -- **Calculation of vertical correction amount from pitch** mitigates localization instability on slopes. For example, when going uphill, it behaves as if it is buried in the ground (see the left side of the "Calculate delta from pitch" figure) because EKF only considers 3DoF(x,y,yaw). Therefore, EKF corrects the z-coordinate according to the formula (see the right side of the "Calculate delta from pitch" figure). - -

- -

- -

- -

- -

- -

- -## Node - -### Subscribed Topics - -| Name | Type | Description | -| -------------------------------- | ------------------------------------------------ | ---------------------------------------------------------------------------------------------------------------------------------------- | -| `measured_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Input pose source with the measurement covariance matrix. | -| `measured_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | Input twist source with the measurement covariance matrix. | -| `initialpose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Initial pose for EKF. The estimated pose is initialized with zeros at the start. It is initialized with this message whenever published. | - -### Published Topics - -| Name | Type | Description | -| --------------------------------- | --------------------------------------------------- | ----------------------------------------------------- | -| `ekf_odom` | `nav_msgs::msg::Odometry` | Estimated odometry. | -| `ekf_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose. | -| `ekf_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance. | -| `ekf_biased_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose including the yaw bias | -| `ekf_biased_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance including the yaw bias | -| `ekf_twist` | `geometry_msgs::msg::TwistStamped` | Estimated twist. | -| `ekf_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | The estimated twist with covariance. | -| `diagnostics` | `diagnostics_msgs::msg::DiagnosticArray` | The diagnostic information. | -| `debug/processing_time_ms` | `autoware_internal_debug_msgs::msg::Float64Stamped` | The processing time [ms]. | - -### Published TF - -- base_link - TF from `map` coordinate to estimated pose. - -## Functions - -### Predict - -The current robot state is predicted from previously estimated data using a given prediction model. This calculation is called at a constant interval (`predict_frequency [Hz]`). The prediction equation is described at the end of this page. - -### Measurement Update - -Before the update, the Mahalanobis distance is calculated between the measured input and the predicted state, the measurement update is not performed for inputs where the Mahalanobis distance exceeds the given threshold. - -The predicted state is updated with the latest measured inputs, measured_pose, and measured_twist. The updates are performed with the same frequency as prediction, usually at a high frequency, in order to enable smooth state estimation. - -## Parameter description - -The parameters are set in `launch/ekf_localizer.launch` . - -### For Node - -{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/node.sub_schema.json") }} - -### For pose measurement - -{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/pose_measurement.sub_schema.json") }} - -### For twist measurement - -{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/twist_measurement.sub_schema.json") }} - -### For process noise - -{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/process_noise.sub_schema.json") }} - -note: process noise for positions x & y are calculated automatically from nonlinear dynamics. - -### Simple 1D Filter Parameters - -{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json") }} - -### For diagnostics - -{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/diagnostics.sub_schema.json") }} - -### Misc - -{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/misc.sub_schema.json") }} - -## How to tune EKF parameters - -### 0. Preliminaries - -- Check header time in pose and twist message is set to sensor time appropriately, because time delay is calculated from this value. If it is difficult to set an appropriate time due to the timer synchronization problem, use `twist_additional_delay` and `pose_additional_delay` to correct the time. -- Check if the relation between measurement pose and twist is appropriate (whether the derivative of the pose has a similar value to twist). This discrepancy is caused mainly by unit error (such as confusing radian/degree) or bias noise, and it causes large estimation errors. - -### 1. Tune sensor parameters - -Set standard deviation for each sensor. The `pose_measure_uncertainty_time` is for the uncertainty of the header timestamp data. -You can also tune a number of steps for smoothing for each observed sensor data by tuning `*_smoothing_steps`. -Increasing the number will improve the smoothness of the estimation, but may have an adverse effect on the estimation performance. - -- `pose_measure_uncertainty_time` -- `pose_smoothing_steps` -- `twist_smoothing_steps` - -### 2. Tune process model parameters - -- `proc_stddev_vx_c` : set to maximum linear acceleration -- `proc_stddev_wz_c` : set to maximum angular acceleration -- `proc_stddev_yaw_c` : This parameter describes the correlation between the yaw and yaw rate. A large value means the change in yaw does not correlate to the estimated yaw rate. If this is set to 0, it means the change in estimated yaw is equal to yaw rate. Usually, this should be set to 0. -- `proc_stddev_yaw_bias_c` : This parameter is the standard deviation for the rate of change in yaw bias. In most cases, yaw bias is constant, so it can be very small, but must be non-zero. - -### 3. Tune gate parameters - -EKF performs gating using Mahalanobis distance before updating by observation. The gate size is determined by the `pose_gate_dist` parameter and the `twist_gate_dist`. If the Mahalanobis distance is larger than this value, the observation is ignored. - -This gating process is based on a statistical test using the chi-square distribution. As modeled, we assume that the Mahalanobis distance follows a chi-square distribution with 3 degrees of freedom for pose and 2 degrees of freedom for twist. - -Currently, the accuracy of covariance estimation itself is not very good, so it is recommended to set the significance level to a very small value to reduce rejection due to false positives. - -| Significance level | Threshold for 2 dof | Threshold for 3 dof | -| ------------------ | ------------------- | ------------------- | -| $10 ^ {-2}$ | 9.21 | 11.3 | -| $10 ^ {-3}$ | 13.8 | 16.3 | -| $10 ^ {-4}$ | 18.4 | 21.1 | -| $10 ^ {-5}$ | 23.0 | 25.9 | -| $10 ^ {-6}$ | 27.6 | 30.7 | -| $10 ^ {-7}$ | 32.2 | 35.4 | -| $10 ^ {-8}$ | 36.8 | 40.1 | -| $10 ^ {-9}$ | 41.4 | 44.8 | -| $10 ^ {-10}$ | 46.1 | 49.5 | - -## Kalman Filter Model - -### kinematics model in update function - - - -where, $\theta_k$ represents the vehicle's heading angle, including the mounting angle bias. -$b_k$ is a correction term for the yaw bias, and it is modeled so that $(\theta_k+b_k)$ becomes the heading angle of the base_link. -The pose_estimator is expected to publish the base_link in the map coordinate system. However, the yaw angle may be offset due to calibration errors. This model compensates this error and improves estimation accuracy. - -### time delay model - -The measurement time delay is handled by an augmented state [1] (See, Section 7.3 FIXED-LAG SMOOTHING). - - - -Note that, although the dimension gets larger since the analytical expansion can be applied based on the specific structures of the augmented states, the computational complexity does not significantly change. - -## Test Result with Autoware NDT - -

- -

- -## Diagnostics - -

- -

- -

- -

-

- -

- -### The conditions that result in a WARN state - -- The node is not in the activate state. -- The initial pose is not set. -- The number of consecutive no measurement update via the Pose/Twist topic exceeds the `pose_no_update_count_threshold_warn`/`twist_no_update_count_threshold_warn`. -- The timestamp of the Pose/Twist topic is beyond the delay compensation range. -- The Pose/Twist topic is beyond the range of Mahalanobis distance for covariance estimation. -- The covariance ellipse is bigger than threshold `warn_ellipse_size` for long axis or `warn_ellipse_size_lateral_direction` for lateral_direction. - -### The conditions that result in an ERROR state - -- The number of consecutive no measurement update via the Pose/Twist topic exceeds the `pose_no_update_count_threshold_error`/`twist_no_update_count_threshold_error`. -- The covariance ellipse is bigger than threshold `error_ellipse_size` for long axis or `error_ellipse_size_lateral_direction` for lateral_direction. - -## Known issues - -- If multiple pose_estimators are used, the input to the EKF will include multiple yaw biases corresponding to each source. However, the current EKF assumes the existence of only one yaw bias. Therefore, yaw bias `b_k` in the current EKF state would not make any sense and cannot correctly handle these multiple yaw biases. Thus, future work includes introducing yaw bias for each sensor with yaw estimation. - -## reference - -[1] Anderson, B. D. O., & Moore, J. B. (1979). Optimal filtering. Englewood Cliffs, NJ: Prentice-Hall. diff --git a/localization/autoware_ekf_localizer/config/ekf_localizer.param.yaml b/localization/autoware_ekf_localizer/config/ekf_localizer.param.yaml deleted file mode 100644 index e00d5e1c10ae2..0000000000000 --- a/localization/autoware_ekf_localizer/config/ekf_localizer.param.yaml +++ /dev/null @@ -1,50 +0,0 @@ -/**: - ros__parameters: - node: - show_debug_info: false - enable_yaw_bias_estimation: true - predict_frequency: 50.0 - tf_rate: 50.0 - extend_state_step: 50 - - pose_measurement: - # for Pose measurement - pose_additional_delay: 0.0 - pose_measure_uncertainty_time: 0.01 - pose_smoothing_steps: 5 - pose_gate_dist: 49.5 # corresponds to significance level = 10^-10 - - twist_measurement: - # for twist measurement - twist_additional_delay: 0.0 - twist_smoothing_steps: 2 - twist_gate_dist: 46.1 # corresponds to significance level = 10^-10 - - process_noise: - # for process model - proc_stddev_yaw_c: 0.005 - proc_stddev_vx_c: 10.0 - proc_stddev_wz_c: 5.0 - - simple_1d_filter_parameters: - #Simple1DFilter parameters - z_filter_proc_dev: 1.0 - roll_filter_proc_dev: 0.1 - pitch_filter_proc_dev: 0.1 - - diagnostics: - # for diagnostics - pose_no_update_count_threshold_warn: 50 - pose_no_update_count_threshold_error: 100 - twist_no_update_count_threshold_warn: 50 - twist_no_update_count_threshold_error: 100 - ellipse_scale: 3.0 - error_ellipse_size: 1.5 - warn_ellipse_size: 1.2 - error_ellipse_size_lateral_direction: 0.3 - warn_ellipse_size_lateral_direction: 0.25 - - misc: - # for velocity measurement limitation (Set 0.0 if you want to ignore) - threshold_observable_velocity_mps: 0.0 # [m/s] - pose_frame_id: "map" diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/aged_object_queue.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/aged_object_queue.hpp deleted file mode 100644 index 2b6f209c7121e..0000000000000 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/aged_object_queue.hpp +++ /dev/null @@ -1,71 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__EKF_LOCALIZER__AGED_OBJECT_QUEUE_HPP_ -#define AUTOWARE__EKF_LOCALIZER__AGED_OBJECT_QUEUE_HPP_ - -#include -#include - -namespace autoware::ekf_localizer -{ - -template -class AgedObjectQueue -{ -public: - explicit AgedObjectQueue(const size_t max_age) : max_age_(max_age) {} - - [[nodiscard]] bool empty() const { return this->size() == 0; } - - [[nodiscard]] size_t size() const { return objects_.size(); } - - Object back() const { return objects_.back(); } - - void push(const Object & object) - { - objects_.push(object); - ages_.push(0); - } - - Object pop_increment_age() - { - const Object object = objects_.front(); - const size_t age = ages_.front() + 1; - objects_.pop(); - ages_.pop(); - - if (age < max_age_) { - objects_.push(object); - ages_.push(age); - } - - return object; - } - - void clear() - { - objects_ = std::queue(); - ages_ = std::queue(); - } - -private: - const size_t max_age_; - std::queue objects_; - std::queue ages_; -}; - -} // namespace autoware::ekf_localizer - -#endif // AUTOWARE__EKF_LOCALIZER__AGED_OBJECT_QUEUE_HPP_ diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/covariance.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/covariance.hpp deleted file mode 100644 index 018b93f86d816..0000000000000 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/covariance.hpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__EKF_LOCALIZER__COVARIANCE_HPP_ -#define AUTOWARE__EKF_LOCALIZER__COVARIANCE_HPP_ - -#include "autoware/ekf_localizer/matrix_types.hpp" - -namespace autoware::ekf_localizer -{ - -std::array ekf_covariance_to_pose_message_covariance(const Matrix6d & P); -std::array ekf_covariance_to_twist_message_covariance(const Matrix6d & P); - -} // namespace autoware::ekf_localizer - -#endif // AUTOWARE__EKF_LOCALIZER__COVARIANCE_HPP_ diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/diagnostics.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/diagnostics.hpp deleted file mode 100644 index b194c3e956341..0000000000000 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/diagnostics.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__EKF_LOCALIZER__DIAGNOSTICS_HPP_ -#define AUTOWARE__EKF_LOCALIZER__DIAGNOSTICS_HPP_ - -#include - -#include -#include - -namespace autoware::ekf_localizer -{ - -diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_activated); -diagnostic_msgs::msg::DiagnosticStatus check_set_initialpose(const bool is_set_initialpose); - -diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated( - const std::string & measurement_type, const size_t no_update_count, - const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error); -diagnostic_msgs::msg::DiagnosticStatus check_measurement_queue_size( - const std::string & measurement_type, const size_t queue_size); -diagnostic_msgs::msg::DiagnosticStatus check_measurement_delay_gate( - const std::string & measurement_type, const bool is_passed_delay_gate, const double delay_time, - const double delay_time_threshold); -diagnostic_msgs::msg::DiagnosticStatus check_measurement_mahalanobis_gate( - const std::string & measurement_type, const bool is_passed_mahalanobis_gate, - const double mahalanobis_distance, const double mahalanobis_distance_threshold); -diagnostic_msgs::msg::DiagnosticStatus check_covariance_ellipse( - const std::string & name, const double curr_size, const double warn_threshold, - const double error_threshold); - -diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status( - const std::vector & stat_array); - -} // namespace autoware::ekf_localizer - -#endif // AUTOWARE__EKF_LOCALIZER__DIAGNOSTICS_HPP_ diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp deleted file mode 100644 index 84018c043cc14..0000000000000 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp +++ /dev/null @@ -1,199 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__EKF_LOCALIZER__EKF_LOCALIZER_HPP_ -#define AUTOWARE__EKF_LOCALIZER__EKF_LOCALIZER_HPP_ - -#include "autoware/ekf_localizer/aged_object_queue.hpp" -#include "autoware/ekf_localizer/ekf_module.hpp" -#include "autoware/ekf_localizer/hyper_parameters.hpp" -#include "autoware/ekf_localizer/warning.hpp" - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -namespace autoware::ekf_localizer -{ - -class EKFLocalizer : public rclcpp::Node -{ -public: - explicit EKFLocalizer(const rclcpp::NodeOptions & options); - - // This function is only used in static tools to know when timer callbacks are triggered. - std::chrono::nanoseconds time_until_trigger() const - { - return timer_control_->time_until_trigger(); - } - -private: - const std::shared_ptr warning_; - - //!< @brief ekf estimated pose publisher - rclcpp::Publisher::SharedPtr pub_pose_; - //!< @brief estimated ekf pose with covariance publisher - rclcpp::Publisher::SharedPtr pub_pose_cov_; - //!< @brief estimated ekf odometry publisher - rclcpp::Publisher::SharedPtr pub_odom_; - //!< @brief ekf estimated twist publisher - rclcpp::Publisher::SharedPtr pub_twist_; - //!< @brief ekf estimated twist with covariance publisher - rclcpp::Publisher::SharedPtr pub_twist_cov_; - //!< @brief ekf estimated yaw bias publisher - rclcpp::Publisher::SharedPtr pub_yaw_bias_; - //!< @brief ekf estimated yaw bias publisher - rclcpp::Publisher::SharedPtr pub_biased_pose_; - //!< @brief ekf estimated yaw bias publisher - rclcpp::Publisher::SharedPtr pub_biased_pose_cov_; - //!< @brief diagnostics publisher - rclcpp::Publisher::SharedPtr pub_diag_; - //!< @brief processing_time publisher - rclcpp::Publisher::SharedPtr - pub_processing_time_; - //!< @brief initial pose subscriber - rclcpp::Subscription::SharedPtr sub_initialpose_; - //!< @brief measurement pose with covariance subscriber - rclcpp::Subscription::SharedPtr sub_pose_with_cov_; - //!< @brief measurement twist with covariance subscriber - rclcpp::Subscription::SharedPtr - sub_twist_with_cov_; - //!< @brief time for ekf calculation callback - rclcpp::TimerBase::SharedPtr timer_control_; - //!< @brief last predict time - std::shared_ptr last_predict_time_; - //!< @brief trigger_node service - rclcpp::Service::SharedPtr service_trigger_node_; - - //!< @brief tf broadcaster - std::shared_ptr tf_br_; - //!< @brief tf buffer - tf2_ros::Buffer tf2_buffer_; - //!< @brief tf listener - tf2_ros::TransformListener tf2_listener_; - - //!< @brief logger configure module - std::unique_ptr logger_configure_; - - //!< @brief extended kalman filter instance. - std::unique_ptr ekf_module_; - - const HyperParameters params_; - - double ekf_dt_; - - bool is_activated_; - bool is_set_initialpose_; - - EKFDiagnosticInfo pose_diag_info_; - EKFDiagnosticInfo twist_diag_info_; - - AgedObjectQueue pose_queue_; - AgedObjectQueue twist_queue_; - - /** - * @brief computes update & prediction of EKF for each ekf_dt_[s] time - */ - void timer_callback(); - - /** - * @brief set pose with covariance measurement - */ - void callback_pose_with_covariance(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - - /** - * @brief set twist with covariance measurement - */ - void callback_twist_with_covariance( - geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); - - /** - * @brief set initial_pose to current EKF pose - */ - void callback_initial_pose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - - /** - * @brief update predict frequency - */ - void update_predict_frequency(const rclcpp::Time & current_time); - - /** - * @brief get transform from frame_id - */ - bool get_transform_from_tf( - std::string parent_frame, std::string child_frame, - geometry_msgs::msg::TransformStamped & transform); - - /** - * @brief publish current EKF estimation result - */ - void publish_estimate_result( - const geometry_msgs::msg::PoseStamped & current_ekf_pose, - const geometry_msgs::msg::PoseStamped & current_biased_ekf_pose, - const geometry_msgs::msg::TwistStamped & current_ekf_twist); - - /** - * @brief publish diagnostics message - */ - void publish_diagnostics( - const geometry_msgs::msg::PoseStamped & current_ekf_pose, const rclcpp::Time & current_time); - - /** - * @brief publish diagnostics message for return - */ - void publish_callback_return_diagnostics( - const std::string & callback_name, const rclcpp::Time & current_time); - - /** - * @brief trigger node - */ - void service_trigger_node( - const std_srvs::srv::SetBool::Request::SharedPtr req, - std_srvs::srv::SetBool::Response::SharedPtr res); - - autoware::universe_utils::StopWatch stop_watch_; - autoware::universe_utils::StopWatch stop_watch_timer_cb_; - - friend class EKFLocalizerTestSuite; // for test code -}; - -} // namespace autoware::ekf_localizer - -#endif // AUTOWARE__EKF_LOCALIZER__EKF_LOCALIZER_HPP_ diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_module.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_module.hpp deleted file mode 100644 index 36fc9053ef4d6..0000000000000 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_module.hpp +++ /dev/null @@ -1,156 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__EKF_LOCALIZER__EKF_MODULE_HPP_ -#define AUTOWARE__EKF_LOCALIZER__EKF_MODULE_HPP_ - -#include "autoware/ekf_localizer/hyper_parameters.hpp" -#include "autoware/ekf_localizer/state_index.hpp" -#include "autoware/ekf_localizer/warning.hpp" - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include - -#include -#include - -namespace autoware::ekf_localizer -{ -using autoware::kalman_filter::TimeDelayKalmanFilter; - -struct EKFDiagnosticInfo -{ - size_t no_update_count{0}; - size_t queue_size{0}; - bool is_passed_delay_gate{true}; - double delay_time{0.0}; - double delay_time_threshold{0.0}; - bool is_passed_mahalanobis_gate{true}; - double mahalanobis_distance{0.0}; -}; - -class Simple1DFilter -{ -public: - Simple1DFilter() - { - initialized_ = false; - x_ = 0; - var_ = 1e9; - proc_var_x_c_ = 0.0; - }; - void init(const double init_obs, const double obs_var) - { - x_ = init_obs; - var_ = obs_var; - initialized_ = true; - }; - void update(const double obs, const double obs_var, const double dt) - { - if (!initialized_) { - init(obs, obs_var); - return; - } - - // Prediction step (current variance) - double proc_var_x_d = proc_var_x_c_ * dt * dt; - var_ = var_ + proc_var_x_d; - - // Update step - double kalman_gain = var_ / (var_ + obs_var); - x_ = x_ + kalman_gain * (obs - x_); - var_ = (1 - kalman_gain) * var_; - }; - void set_proc_var(const double proc_var) { proc_var_x_c_ = proc_var; } - [[nodiscard]] double get_x() const { return x_; } - [[nodiscard]] double get_var() const { return var_; } - -private: - bool initialized_; - double x_; - double var_; - double proc_var_x_c_; -}; - -class EKFModule -{ -private: - using PoseWithCovariance = geometry_msgs::msg::PoseWithCovarianceStamped; - using TwistWithCovariance = geometry_msgs::msg::TwistWithCovarianceStamped; - using Pose = geometry_msgs::msg::PoseStamped; - using Twist = geometry_msgs::msg::TwistStamped; - -public: - EKFModule(std::shared_ptr warning, const HyperParameters & params); - - void initialize( - const PoseWithCovariance & initial_pose, - const geometry_msgs::msg::TransformStamped & transform); - - [[nodiscard]] geometry_msgs::msg::PoseStamped get_current_pose( - const rclcpp::Time & current_time, bool get_biased_yaw) const; - [[nodiscard]] geometry_msgs::msg::TwistStamped get_current_twist( - const rclcpp::Time & current_time) const; - [[nodiscard]] double get_yaw_bias() const; - [[nodiscard]] std::array get_current_pose_covariance() const; - [[nodiscard]] std::array get_current_twist_covariance() const; - - [[nodiscard]] size_t find_closest_delay_time_index(double target_value) const; - void accumulate_delay_time(const double dt); - - void predict_with_delay(const double dt); - bool measurement_update_pose( - const PoseWithCovariance & pose, const rclcpp::Time & t_curr, - EKFDiagnosticInfo & pose_diag_info); - bool measurement_update_twist( - const TwistWithCovariance & twist, const rclcpp::Time & t_curr, - EKFDiagnosticInfo & twist_diag_info); - geometry_msgs::msg::PoseWithCovarianceStamped compensate_rph_with_delay( - const PoseWithCovariance & pose, tf2::Vector3 last_angular_velocity, const double delay_time); - -private: - void update_simple_1d_filters( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step); - - TimeDelayKalmanFilter kalman_filter_; - - std::shared_ptr warning_; - const int dim_x_; - std::vector accumulated_delay_times_; - const HyperParameters params_; - - Simple1DFilter z_filter_; - Simple1DFilter roll_filter_; - Simple1DFilter pitch_filter_; - - /** - * @brief last angular velocity for compensating rph with delay - */ - tf2::Vector3 last_angular_velocity_; - - double ekf_dt_; -}; - -} // namespace autoware::ekf_localizer - -#endif // AUTOWARE__EKF_LOCALIZER__EKF_MODULE_HPP_ diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/hyper_parameters.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/hyper_parameters.hpp deleted file mode 100644 index 02958199cf2a1..0000000000000 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/hyper_parameters.hpp +++ /dev/null @@ -1,108 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__EKF_LOCALIZER__HYPER_PARAMETERS_HPP_ -#define AUTOWARE__EKF_LOCALIZER__HYPER_PARAMETERS_HPP_ - -#include - -#include -#include - -namespace autoware::ekf_localizer -{ - -class HyperParameters -{ -public: - explicit HyperParameters(rclcpp::Node * node) - : show_debug_info(node->declare_parameter("node.show_debug_info")), - ekf_rate(node->declare_parameter("node.predict_frequency")), - ekf_dt(1.0 / std::max(ekf_rate, 0.1)), - tf_rate_(node->declare_parameter("node.tf_rate")), - enable_yaw_bias_estimation(node->declare_parameter("node.enable_yaw_bias_estimation")), - extend_state_step(node->declare_parameter("node.extend_state_step")), - pose_frame_id(node->declare_parameter("misc.pose_frame_id")), - pose_additional_delay( - node->declare_parameter("pose_measurement.pose_additional_delay")), - pose_gate_dist(node->declare_parameter("pose_measurement.pose_gate_dist")), - pose_smoothing_steps(node->declare_parameter("pose_measurement.pose_smoothing_steps")), - twist_additional_delay( - node->declare_parameter("twist_measurement.twist_additional_delay")), - twist_gate_dist(node->declare_parameter("twist_measurement.twist_gate_dist")), - twist_smoothing_steps(node->declare_parameter("twist_measurement.twist_smoothing_steps")), - proc_stddev_vx_c(node->declare_parameter("process_noise.proc_stddev_vx_c")), - proc_stddev_wz_c(node->declare_parameter("process_noise.proc_stddev_wz_c")), - proc_stddev_yaw_c(node->declare_parameter("process_noise.proc_stddev_yaw_c")), - z_filter_proc_dev( - node->declare_parameter("simple_1d_filter_parameters.z_filter_proc_dev")), - roll_filter_proc_dev( - node->declare_parameter("simple_1d_filter_parameters.roll_filter_proc_dev")), - pitch_filter_proc_dev( - node->declare_parameter("simple_1d_filter_parameters.pitch_filter_proc_dev")), - pose_no_update_count_threshold_warn( - node->declare_parameter("diagnostics.pose_no_update_count_threshold_warn")), - pose_no_update_count_threshold_error( - node->declare_parameter("diagnostics.pose_no_update_count_threshold_error")), - twist_no_update_count_threshold_warn( - node->declare_parameter("diagnostics.twist_no_update_count_threshold_warn")), - twist_no_update_count_threshold_error( - node->declare_parameter("diagnostics.twist_no_update_count_threshold_error")), - ellipse_scale(node->declare_parameter("diagnostics.ellipse_scale")), - error_ellipse_size(node->declare_parameter("diagnostics.error_ellipse_size")), - warn_ellipse_size(node->declare_parameter("diagnostics.warn_ellipse_size")), - error_ellipse_size_lateral_direction( - node->declare_parameter("diagnostics.error_ellipse_size_lateral_direction")), - warn_ellipse_size_lateral_direction( - node->declare_parameter("diagnostics.warn_ellipse_size_lateral_direction")), - threshold_observable_velocity_mps( - node->declare_parameter("misc.threshold_observable_velocity_mps")) - { - } - - const bool show_debug_info; - const double ekf_rate; - const double ekf_dt; - const double tf_rate_; - const bool enable_yaw_bias_estimation; - const size_t extend_state_step; - const std::string pose_frame_id; - const double pose_additional_delay; - const double pose_gate_dist; - const size_t pose_smoothing_steps; - const double twist_additional_delay; - const double twist_gate_dist; - const size_t twist_smoothing_steps; - const double proc_stddev_vx_c; //!< @brief vx process noise - const double proc_stddev_wz_c; //!< @brief wz process noise - const double proc_stddev_yaw_c; //!< @brief yaw process noise - const double z_filter_proc_dev; - const double roll_filter_proc_dev; - const double pitch_filter_proc_dev; - const size_t pose_no_update_count_threshold_warn; - const size_t pose_no_update_count_threshold_error; - const size_t twist_no_update_count_threshold_warn; - const size_t twist_no_update_count_threshold_error; - double ellipse_scale; - double error_ellipse_size; - double warn_ellipse_size; - double error_ellipse_size_lateral_direction; - double warn_ellipse_size_lateral_direction; - - const double threshold_observable_velocity_mps; -}; - -} // namespace autoware::ekf_localizer - -#endif // AUTOWARE__EKF_LOCALIZER__HYPER_PARAMETERS_HPP_ diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/mahalanobis.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/mahalanobis.hpp deleted file mode 100644 index c1538a72f56b6..0000000000000 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/mahalanobis.hpp +++ /dev/null @@ -1,31 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__EKF_LOCALIZER__MAHALANOBIS_HPP_ -#define AUTOWARE__EKF_LOCALIZER__MAHALANOBIS_HPP_ - -#include -#include - -namespace autoware::ekf_localizer -{ - -double squared_mahalanobis( - const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C); - -double mahalanobis(const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C); - -} // namespace autoware::ekf_localizer - -#endif // AUTOWARE__EKF_LOCALIZER__MAHALANOBIS_HPP_ diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/matrix_types.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/matrix_types.hpp deleted file mode 100644 index fc75dfe72bb9d..0000000000000 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/matrix_types.hpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__EKF_LOCALIZER__MATRIX_TYPES_HPP_ -#define AUTOWARE__EKF_LOCALIZER__MATRIX_TYPES_HPP_ - -#include - -namespace autoware::ekf_localizer -{ - -using Vector6d = Eigen::Matrix; -using Matrix6d = Eigen::Matrix; - -} // namespace autoware::ekf_localizer - -#endif // AUTOWARE__EKF_LOCALIZER__MATRIX_TYPES_HPP_ diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/measurement.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/measurement.hpp deleted file mode 100644 index b8db07015ec46..0000000000000 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/measurement.hpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__EKF_LOCALIZER__MEASUREMENT_HPP_ -#define AUTOWARE__EKF_LOCALIZER__MEASUREMENT_HPP_ - -#include - -namespace autoware::ekf_localizer -{ - -Eigen::Matrix pose_measurement_matrix(); -Eigen::Matrix twist_measurement_matrix(); -Eigen::Matrix3d pose_measurement_covariance( - const std::array & covariance, const size_t smoothing_step); -Eigen::Matrix2d twist_measurement_covariance( - const std::array & covariance, const size_t smoothing_step); - -} // namespace autoware::ekf_localizer - -#endif // AUTOWARE__EKF_LOCALIZER__MEASUREMENT_HPP_ diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/numeric.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/numeric.hpp deleted file mode 100644 index 253da4eb7f6dc..0000000000000 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/numeric.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__EKF_LOCALIZER__NUMERIC_HPP_ -#define AUTOWARE__EKF_LOCALIZER__NUMERIC_HPP_ - -#include - -#include - -namespace autoware::ekf_localizer -{ - -inline bool has_inf(const Eigen::MatrixXd & v) -{ - return v.array().isInf().any(); -} - -inline bool has_nan(const Eigen::MatrixXd & v) -{ - return v.array().isNaN().any(); -} - -} // namespace autoware::ekf_localizer - -#endif // AUTOWARE__EKF_LOCALIZER__NUMERIC_HPP_ diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/state_index.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/state_index.hpp deleted file mode 100644 index 86fe83ec4aa8b..0000000000000 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/state_index.hpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__EKF_LOCALIZER__STATE_INDEX_HPP_ -#define AUTOWARE__EKF_LOCALIZER__STATE_INDEX_HPP_ - -namespace autoware::ekf_localizer -{ - -enum IDX { - X = 0, - Y = 1, - YAW = 2, - YAWB = 3, - VX = 4, - WZ = 5, -}; - -} // namespace autoware::ekf_localizer - -#endif // AUTOWARE__EKF_LOCALIZER__STATE_INDEX_HPP_ diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/state_transition.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/state_transition.hpp deleted file mode 100644 index e6ce18e76a3a4..0000000000000 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/state_transition.hpp +++ /dev/null @@ -1,31 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__EKF_LOCALIZER__STATE_TRANSITION_HPP_ -#define AUTOWARE__EKF_LOCALIZER__STATE_TRANSITION_HPP_ - -#include "autoware/ekf_localizer/matrix_types.hpp" - -namespace autoware::ekf_localizer -{ - -double normalize_yaw(const double & yaw); -Vector6d predict_next_state(const Vector6d & X_curr, const double dt); -Matrix6d create_state_transition_matrix(const Vector6d & X_curr, const double dt); -Matrix6d process_noise_covariance( - const double proc_cov_yaw_d, const double proc_cov_vx_d, const double proc_cov_wz_d); - -} // namespace autoware::ekf_localizer - -#endif // AUTOWARE__EKF_LOCALIZER__STATE_TRANSITION_HPP_ diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/string.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/string.hpp deleted file mode 100644 index 0146176b7ddc1..0000000000000 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/string.hpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__EKF_LOCALIZER__STRING_HPP_ -#define AUTOWARE__EKF_LOCALIZER__STRING_HPP_ - -#include - -namespace autoware::ekf_localizer -{ - -inline std::string erase_leading_slash(const std::string & s) -{ - std::string a = s; - if (a.front() == '/') { - a.erase(0, 1); - } - return a; -} - -} // namespace autoware::ekf_localizer - -#endif // AUTOWARE__EKF_LOCALIZER__STRING_HPP_ diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/warning.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/warning.hpp deleted file mode 100644 index f43c5e3413213..0000000000000 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/warning.hpp +++ /dev/null @@ -1,48 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__EKF_LOCALIZER__WARNING_HPP_ -#define AUTOWARE__EKF_LOCALIZER__WARNING_HPP_ - -#include - -#include - -namespace autoware::ekf_localizer -{ - -class Warning -{ -public: - explicit Warning(rclcpp::Node * node) : node_(node) {} - - void warn(const std::string & message) const - { - RCLCPP_WARN(node_->get_logger(), "%s", message.c_str()); - } - - void warn_throttle(const std::string & message, const int duration_milliseconds) const - { - RCLCPP_WARN_THROTTLE( - node_->get_logger(), *(node_->get_clock()), - std::chrono::milliseconds(duration_milliseconds).count(), "%s", message.c_str()); - } - -private: - rclcpp::Node * node_; -}; - -} // namespace autoware::ekf_localizer - -#endif // AUTOWARE__EKF_LOCALIZER__WARNING_HPP_ diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/warning_message.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/warning_message.hpp deleted file mode 100644 index b379e35763f54..0000000000000 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/warning_message.hpp +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__EKF_LOCALIZER__WARNING_MESSAGE_HPP_ -#define AUTOWARE__EKF_LOCALIZER__WARNING_MESSAGE_HPP_ - -#include - -namespace autoware::ekf_localizer -{ - -std::string pose_delay_step_warning_message( - const double delay_time, const double delay_time_threshold); -std::string twist_delay_step_warning_message( - const double delay_time, const double delay_time_threshold); -std::string pose_delay_time_warning_message(const double delay_time); -std::string twist_delay_time_warning_message(const double delay_time); -std::string mahalanobis_warning_message(const double distance, const double max_distance); - -} // namespace autoware::ekf_localizer - -#endif // AUTOWARE__EKF_LOCALIZER__WARNING_MESSAGE_HPP_ diff --git a/localization/autoware_ekf_localizer/launch/ekf_localizer.launch.xml b/localization/autoware_ekf_localizer/launch/ekf_localizer.launch.xml deleted file mode 100644 index 76ac35bcba38e..0000000000000 --- a/localization/autoware_ekf_localizer/launch/ekf_localizer.launch.xml +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/localization/autoware_ekf_localizer/media/calculation_delta_from_pitch.png b/localization/autoware_ekf_localizer/media/calculation_delta_from_pitch.png deleted file mode 100644 index 0fa459f96dd71..0000000000000 Binary files a/localization/autoware_ekf_localizer/media/calculation_delta_from_pitch.png and /dev/null differ diff --git a/localization/autoware_ekf_localizer/media/delay_model_eq.png b/localization/autoware_ekf_localizer/media/delay_model_eq.png deleted file mode 100644 index 41777d756b974..0000000000000 Binary files a/localization/autoware_ekf_localizer/media/delay_model_eq.png and /dev/null differ diff --git a/localization/autoware_ekf_localizer/media/ekf_autoware_res.png b/localization/autoware_ekf_localizer/media/ekf_autoware_res.png deleted file mode 100644 index c771e3620b161..0000000000000 Binary files a/localization/autoware_ekf_localizer/media/ekf_autoware_res.png and /dev/null differ diff --git a/localization/autoware_ekf_localizer/media/ekf_delay_comp.png b/localization/autoware_ekf_localizer/media/ekf_delay_comp.png deleted file mode 100644 index 54d934caecc92..0000000000000 Binary files a/localization/autoware_ekf_localizer/media/ekf_delay_comp.png and /dev/null differ diff --git a/localization/autoware_ekf_localizer/media/ekf_diagnostics.png b/localization/autoware_ekf_localizer/media/ekf_diagnostics.png deleted file mode 100644 index a1561c3f52707..0000000000000 Binary files a/localization/autoware_ekf_localizer/media/ekf_diagnostics.png and /dev/null differ diff --git a/localization/autoware_ekf_localizer/media/ekf_diagnostics_callback_pose.png b/localization/autoware_ekf_localizer/media/ekf_diagnostics_callback_pose.png deleted file mode 100644 index 9349a225c887b..0000000000000 Binary files a/localization/autoware_ekf_localizer/media/ekf_diagnostics_callback_pose.png and /dev/null differ diff --git a/localization/autoware_ekf_localizer/media/ekf_diagnostics_callback_twist.png b/localization/autoware_ekf_localizer/media/ekf_diagnostics_callback_twist.png deleted file mode 100644 index 4608a90e55220..0000000000000 Binary files a/localization/autoware_ekf_localizer/media/ekf_diagnostics_callback_twist.png and /dev/null differ diff --git a/localization/autoware_ekf_localizer/media/ekf_dynamics.png b/localization/autoware_ekf_localizer/media/ekf_dynamics.png deleted file mode 100644 index 63c81261a718e..0000000000000 Binary files a/localization/autoware_ekf_localizer/media/ekf_dynamics.png and /dev/null differ diff --git a/localization/autoware_ekf_localizer/media/ekf_flowchart.png b/localization/autoware_ekf_localizer/media/ekf_flowchart.png deleted file mode 100644 index 0a80cb06b85f0..0000000000000 Binary files a/localization/autoware_ekf_localizer/media/ekf_flowchart.png and /dev/null differ diff --git a/localization/autoware_ekf_localizer/media/ekf_smooth_update.png b/localization/autoware_ekf_localizer/media/ekf_smooth_update.png deleted file mode 100644 index 7ac26fc604c6f..0000000000000 Binary files a/localization/autoware_ekf_localizer/media/ekf_smooth_update.png and /dev/null differ diff --git a/localization/autoware_ekf_localizer/package.xml b/localization/autoware_ekf_localizer/package.xml deleted file mode 100644 index e0e37f59d1acb..0000000000000 --- a/localization/autoware_ekf_localizer/package.xml +++ /dev/null @@ -1,47 +0,0 @@ - - - - autoware_ekf_localizer - 0.40.0 - The autoware_ekf_localizer package - Takamasa Horibe - Yamato Ando - Takeshi Ishita - Masahiro Sakamoto - Kento Yabuuchi - NGUYEN Viet Anh - Taiki Yamada - Shintaro Sakoda - Ryu Yamamoto - Apache License 2.0 - Takamasa Horibe - - ament_cmake_auto - autoware_cmake - eigen3_cmake_module - - eigen - - autoware_internal_debug_msgs - autoware_kalman_filter - autoware_localization_util - autoware_universe_utils - diagnostic_msgs - fmt - geometry_msgs - nav_msgs - rclcpp - rclcpp_components - std_srvs - tf2 - tf2_ros - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - ros_testing - - - ament_cmake - - diff --git a/localization/autoware_ekf_localizer/schema/ekf_localizer.schema.json b/localization/autoware_ekf_localizer/schema/ekf_localizer.schema.json deleted file mode 100644 index 61fbcc2973aae..0000000000000 --- a/localization/autoware_ekf_localizer/schema/ekf_localizer.schema.json +++ /dev/null @@ -1,52 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "EKF Localizer Configuration", - "type": "object", - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "type": "object", - "properties": { - "node": { - "$ref": "sub/node.sub_schema.json#/definitions/node" - }, - "pose_measurement": { - "$ref": "sub/pose_measurement.sub_schema.json#/definitions/pose_measurement" - }, - "twist_measurement": { - "$ref": "sub/twist_measurement.sub_schema.json#/definitions/twist_measurement" - }, - "process_noise": { - "$ref": "sub/process_noise.sub_schema.json#/definitions/process_noise" - }, - "simple_1d_filter_parameters": { - "$ref": "sub/simple_1d_filter_parameters.sub_schema.json#/definitions/simple_1d_filter_parameters" - }, - "diagnostics": { - "$ref": "sub/diagnostics.sub_schema.json#/definitions/diagnostics" - }, - "misc": { - "$ref": "sub/misc.sub_schema.json#/definitions/misc" - } - }, - "required": [ - "node", - "pose_measurement", - "twist_measurement", - "process_noise", - "simple_1d_filter_parameters", - "diagnostics", - "misc" - ], - "additionalProperties": false - } - }, - "required": ["ros__parameters"], - "additionalProperties": false - } - }, - "required": ["/**"], - "additionalProperties": false -} diff --git a/localization/autoware_ekf_localizer/schema/sub/diagnostics.sub_schema.json b/localization/autoware_ekf_localizer/schema/sub/diagnostics.sub_schema.json deleted file mode 100644 index eda9e7d06f6f4..0000000000000 --- a/localization/autoware_ekf_localizer/schema/sub/diagnostics.sub_schema.json +++ /dev/null @@ -1,63 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "EKF Localizer Configuration for Diagnostics", - "definitions": { - "diagnostics": { - "type": "object", - "properties": { - "pose_no_update_count_threshold_warn": { - "type": "integer", - "description": "The threshold at which a WARN state is triggered due to the Pose Topic update not happening continuously for a certain number of times", - "default": 50 - }, - "pose_no_update_count_threshold_error": { - "type": "integer", - "description": "The threshold at which an ERROR state is triggered due to the Pose Topic update not happening continuously for a certain number of times", - "default": 100 - }, - "twist_no_update_count_threshold_warn": { - "type": "integer", - "description": "The threshold at which a WARN state is triggered due to the Twist Topic update not happening continuously for a certain number of times", - "default": 50 - }, - "twist_no_update_count_threshold_error": { - "type": "integer", - "description": "The threshold at which an ERROR state is triggered due to the Twist Topic update not happening continuously for a certain number of times", - "default": 100 - }, - "ellipse_scale": { - "type": "number", - "description": "The scale factor to apply the error ellipse size", - "default": 3.0 - }, - "error_ellipse_size": { - "type": "number", - "description": "The long axis size of the error ellipse to trigger a ERROR state", - "default": 1.5 - }, - "warn_ellipse_size": { - "type": "number", - "description": "The long axis size of the error ellipse to trigger a WARN state", - "default": 1.2 - }, - "error_ellipse_size_lateral_direction": { - "type": "number", - "description": "The lateral direction size of the error ellipse to trigger a ERROR state", - "default": 0.3 - }, - "warn_ellipse_size_lateral_direction": { - "type": "number", - "description": "The lateral direction size of the error ellipse to trigger a WARN state", - "default": 0.25 - } - }, - "required": [ - "pose_no_update_count_threshold_warn", - "pose_no_update_count_threshold_error", - "twist_no_update_count_threshold_warn", - "twist_no_update_count_threshold_error" - ], - "additionalProperties": false - } - } -} diff --git a/localization/autoware_ekf_localizer/schema/sub/misc.sub_schema.json b/localization/autoware_ekf_localizer/schema/sub/misc.sub_schema.json deleted file mode 100644 index cc36a5bf41ec6..0000000000000 --- a/localization/autoware_ekf_localizer/schema/sub/misc.sub_schema.json +++ /dev/null @@ -1,23 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "EKF Localizer Configuration for MISC", - "definitions": { - "misc": { - "type": "object", - "properties": { - "threshold_observable_velocity_mps": { - "type": "number", - "description": "Minimum value for velocity that will be used for EKF. Mainly used for dead zone in velocity sensor [m/s] (0.0 means disabled)", - "default": 0.0 - }, - "pose_frame_id": { - "type": "string", - "description": "Parent frame_id of EKF output pose", - "default": "map" - } - }, - "required": ["threshold_observable_velocity_mps", "pose_frame_id"], - "additionalProperties": false - } - } -} diff --git a/localization/autoware_ekf_localizer/schema/sub/node.sub_schema.json b/localization/autoware_ekf_localizer/schema/sub/node.sub_schema.json deleted file mode 100644 index cbaf34de6eaa7..0000000000000 --- a/localization/autoware_ekf_localizer/schema/sub/node.sub_schema.json +++ /dev/null @@ -1,44 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "EKF Localizer Configuration for node", - "definitions": { - "node": { - "type": "object", - "properties": { - "show_debug_info": { - "type": "boolean", - "description": "Flag to display debug info", - "default": false - }, - "predict_frequency": { - "type": "number", - "description": "Frequency for filtering and publishing [Hz]", - "default": 50.0 - }, - "tf_rate": { - "type": "number", - "description": "Frequency for tf broadcasting [Hz]", - "default": 50.0 - }, - "extend_state_step": { - "type": "integer", - "description": "Max delay step which can be dealt with in EKF. Large number increases computational cost.", - "default": 50 - }, - "enable_yaw_bias_estimation": { - "type": "boolean", - "description": "Flag to enable yaw bias estimation", - "default": true - } - }, - "required": [ - "show_debug_info", - "predict_frequency", - "tf_rate", - "extend_state_step", - "enable_yaw_bias_estimation" - ], - "additionalProperties": false - } - } -} diff --git a/localization/autoware_ekf_localizer/schema/sub/pose_measurement.sub_schema.json b/localization/autoware_ekf_localizer/schema/sub/pose_measurement.sub_schema.json deleted file mode 100644 index d2bc313d4dc75..0000000000000 --- a/localization/autoware_ekf_localizer/schema/sub/pose_measurement.sub_schema.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "EKF Localizer Configuration for Pose Measurement", - "definitions": { - "pose_measurement": { - "type": "object", - "properties": { - "pose_additional_delay": { - "type": "number", - "description": "Additional delay time for pose measurement [s]", - "default": 0.0 - }, - "pose_measure_uncertainty_time": { - "type": "number", - "description": "Measured time uncertainty used for covariance calculation [s]", - "default": 0.01 - }, - "pose_smoothing_steps": { - "type": "integer", - "description": "A value for smoothing steps", - "default": 5 - }, - "pose_gate_dist": { - "type": "number", - "description": "Limit of Mahalanobis distance used for outliers detection", - "default": 49.5 - } - }, - "required": [ - "pose_additional_delay", - "pose_measure_uncertainty_time", - "pose_smoothing_steps", - "pose_gate_dist" - ], - "additionalProperties": false - } - } -} diff --git a/localization/autoware_ekf_localizer/schema/sub/process_noise.sub_schema.json b/localization/autoware_ekf_localizer/schema/sub/process_noise.sub_schema.json deleted file mode 100644 index 37a8c248edd2c..0000000000000 --- a/localization/autoware_ekf_localizer/schema/sub/process_noise.sub_schema.json +++ /dev/null @@ -1,28 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "EKF Localizer Configuration for Process Noise", - "definitions": { - "process_noise": { - "type": "object", - "properties": { - "proc_stddev_vx_c": { - "type": "number", - "description": "Standard deviation of process noise in time differentiation expression of linear velocity x, noise for d_vx = 0", - "default": 10.0 - }, - "proc_stddev_wz_c": { - "type": "number", - "description": "Standard deviation of process noise in time differentiation expression of angular velocity z, noise for d_wz = 0", - "default": 5.0 - }, - "proc_stddev_yaw_c": { - "type": "number", - "description": "Standard deviation of process noise in time differentiation expression of yaw, noise for d_yaw = omega", - "default": 0.005 - } - }, - "required": ["proc_stddev_yaw_c", "proc_stddev_vx_c", "proc_stddev_wz_c"], - "additionalProperties": false - } - } -} diff --git a/localization/autoware_ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json b/localization/autoware_ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json deleted file mode 100644 index 00679ee92ad24..0000000000000 --- a/localization/autoware_ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json +++ /dev/null @@ -1,28 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "EKF Localizer Configuration of Simple 1D Filter Parameters", - "definitions": { - "simple_1d_filter_parameters": { - "type": "object", - "properties": { - "z_filter_proc_dev": { - "type": "number", - "description": "Simple1DFilter - Z filter process deviation", - "default": 1.0 - }, - "roll_filter_proc_dev": { - "type": "number", - "description": "Simple1DFilter - Roll filter process deviation", - "default": 0.1 - }, - "pitch_filter_proc_dev": { - "type": "number", - "description": "Simple1DFilter - Pitch filter process deviation", - "default": 0.1 - } - }, - "required": ["z_filter_proc_dev", "roll_filter_proc_dev", "pitch_filter_proc_dev"], - "additionalProperties": false - } - } -} diff --git a/localization/autoware_ekf_localizer/schema/sub/twist_measurement.sub_schema.json b/localization/autoware_ekf_localizer/schema/sub/twist_measurement.sub_schema.json deleted file mode 100644 index 7b80133cb38aa..0000000000000 --- a/localization/autoware_ekf_localizer/schema/sub/twist_measurement.sub_schema.json +++ /dev/null @@ -1,28 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "EKF Localizer Configuration for Twist Measurement", - "definitions": { - "twist_measurement": { - "type": "object", - "properties": { - "twist_additional_delay": { - "type": "number", - "description": "Additional delay time for twist [s]", - "default": 0.0 - }, - "twist_smoothing_steps": { - "type": "integer", - "description": "A value for smoothing steps", - "default": 2 - }, - "twist_gate_dist": { - "type": "number", - "description": "Limit of Mahalanobis distance used for outliers detection", - "default": 46.1 - } - }, - "required": ["twist_additional_delay", "twist_smoothing_steps", "twist_gate_dist"], - "additionalProperties": false - } - } -} diff --git a/localization/autoware_ekf_localizer/src/covariance.cpp b/localization/autoware_ekf_localizer/src/covariance.cpp deleted file mode 100644 index 9ed3a83fbf705..0000000000000 --- a/localization/autoware_ekf_localizer/src/covariance.cpp +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/ekf_localizer/covariance.hpp" - -#include "autoware/ekf_localizer/state_index.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" - -namespace autoware::ekf_localizer -{ - -using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - -std::array ekf_covariance_to_pose_message_covariance(const Matrix6d & P) -{ - std::array covariance{}; - covariance.fill(0.); - - covariance[COV_IDX::X_X] = P(IDX::X, IDX::X); - covariance[COV_IDX::X_Y] = P(IDX::X, IDX::Y); - covariance[COV_IDX::X_YAW] = P(IDX::X, IDX::YAW); - covariance[COV_IDX::Y_X] = P(IDX::Y, IDX::X); - covariance[COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); - covariance[COV_IDX::Y_YAW] = P(IDX::Y, IDX::YAW); - covariance[COV_IDX::YAW_X] = P(IDX::YAW, IDX::X); - covariance[COV_IDX::YAW_Y] = P(IDX::YAW, IDX::Y); - covariance[COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); - - return covariance; -} - -std::array ekf_covariance_to_twist_message_covariance(const Matrix6d & P) -{ - std::array covariance{}; - covariance.fill(0.); - - covariance[COV_IDX::X_X] = P(IDX::VX, IDX::VX); - covariance[COV_IDX::X_YAW] = P(IDX::VX, IDX::WZ); - covariance[COV_IDX::YAW_X] = P(IDX::WZ, IDX::VX); - covariance[COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); - - return covariance; -} - -} // namespace autoware::ekf_localizer diff --git a/localization/autoware_ekf_localizer/src/diagnostics.cpp b/localization/autoware_ekf_localizer/src/diagnostics.cpp deleted file mode 100644 index 45468abf72d6c..0000000000000 --- a/localization/autoware_ekf_localizer/src/diagnostics.cpp +++ /dev/null @@ -1,226 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/ekf_localizer/diagnostics.hpp" - -#include - -#include -#include - -namespace autoware::ekf_localizer -{ - -diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_activated) -{ - diagnostic_msgs::msg::DiagnosticStatus stat; - - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "is_activated"; - key_value.value = is_activated ? "True" : "False"; - stat.values.push_back(key_value); - - stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - stat.message = "OK"; - if (!is_activated) { - stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - stat.message = "[WARN]process is not activated"; - } - - return stat; -} - -diagnostic_msgs::msg::DiagnosticStatus check_set_initialpose(const bool is_set_initialpose) -{ - diagnostic_msgs::msg::DiagnosticStatus stat; - - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "is_set_initialpose"; - key_value.value = is_set_initialpose ? "True" : "False"; - stat.values.push_back(key_value); - - stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - stat.message = "OK"; - if (!is_set_initialpose) { - stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - stat.message = "[WARN]initial pose is not set"; - } - - return stat; -} - -diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated( - const std::string & measurement_type, const size_t no_update_count, - const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error) -{ - diagnostic_msgs::msg::DiagnosticStatus stat; - - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = measurement_type + "_no_update_count"; - key_value.value = std::to_string(no_update_count); - stat.values.push_back(key_value); - key_value.key = measurement_type + "_no_update_count_threshold_warn"; - key_value.value = std::to_string(no_update_count_threshold_warn); - stat.values.push_back(key_value); - key_value.key = measurement_type + "_no_update_count_threshold_error"; - key_value.value = std::to_string(no_update_count_threshold_error); - stat.values.push_back(key_value); - - stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - stat.message = "OK"; - if (no_update_count >= no_update_count_threshold_warn) { - stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - stat.message = "[WARN]" + measurement_type + " is not updated"; - } - if (no_update_count >= no_update_count_threshold_error) { - stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - stat.message = "[ERROR]" + measurement_type + " is not updated"; - } - - return stat; -} - -diagnostic_msgs::msg::DiagnosticStatus check_measurement_queue_size( - const std::string & measurement_type, const size_t queue_size) -{ - diagnostic_msgs::msg::DiagnosticStatus stat; - - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = measurement_type + "_queue_size"; - key_value.value = std::to_string(queue_size); - stat.values.push_back(key_value); - - stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - stat.message = "OK"; - - return stat; -} - -diagnostic_msgs::msg::DiagnosticStatus check_measurement_delay_gate( - const std::string & measurement_type, const bool is_passed_delay_gate, const double delay_time, - const double delay_time_threshold) -{ - diagnostic_msgs::msg::DiagnosticStatus stat; - - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = measurement_type + "_is_passed_delay_gate"; - key_value.value = is_passed_delay_gate ? "True" : "False"; - stat.values.push_back(key_value); - key_value.key = measurement_type + "_delay_time"; - key_value.value = std::to_string(delay_time); - stat.values.push_back(key_value); - key_value.key = measurement_type + "_delay_time_threshold"; - key_value.value = std::to_string(delay_time_threshold); - stat.values.push_back(key_value); - - stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - stat.message = "OK"; - if (!is_passed_delay_gate) { - stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - stat.message = "[WARN]" + measurement_type + " topic is delay"; - } - - return stat; -} - -diagnostic_msgs::msg::DiagnosticStatus check_measurement_mahalanobis_gate( - const std::string & measurement_type, const bool is_passed_mahalanobis_gate, - const double mahalanobis_distance, const double mahalanobis_distance_threshold) -{ - diagnostic_msgs::msg::DiagnosticStatus stat; - - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = measurement_type + "_is_passed_mahalanobis_gate"; - key_value.value = is_passed_mahalanobis_gate ? "True" : "False"; - stat.values.push_back(key_value); - key_value.key = measurement_type + "_mahalanobis_distance"; - key_value.value = std::to_string(mahalanobis_distance); - stat.values.push_back(key_value); - key_value.key = measurement_type + "_mahalanobis_distance_threshold"; - key_value.value = std::to_string(mahalanobis_distance_threshold); - stat.values.push_back(key_value); - - stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - stat.message = "OK"; - if (!is_passed_mahalanobis_gate) { - stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - stat.message = "[WARN]mahalanobis distance of " + measurement_type + " topic is large"; - } - - return stat; -} - -diagnostic_msgs::msg::DiagnosticStatus check_covariance_ellipse( - const std::string & name, const double curr_size, const double warn_threshold, - const double error_threshold) -{ - diagnostic_msgs::msg::DiagnosticStatus stat; - - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = name + "_size"; - key_value.value = std::to_string(curr_size); - stat.values.push_back(key_value); - - key_value.key = name + "_warn_threshold"; - key_value.value = std::to_string(warn_threshold); - stat.values.push_back(key_value); - - key_value.key = name + "_error_threshold"; - key_value.value = std::to_string(error_threshold); - stat.values.push_back(key_value); - - stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - stat.message = "OK"; - if (curr_size >= warn_threshold) { - stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - stat.message = "[WARN]" + name + " is large"; - } - if (curr_size >= error_threshold) { - stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - stat.message = "[ERROR]" + name + " is large"; - } - - return stat; -} - -// The highest level within the stat_array will be reflected in the merged_stat. -// When all stat_array entries are 'OK,' the message of merged_stat will be "OK" -diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status( - const std::vector & stat_array) -{ - diagnostic_msgs::msg::DiagnosticStatus merged_stat; - - for (const auto & stat : stat_array) { - if ((stat.level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { - if (!merged_stat.message.empty()) { - merged_stat.message += "; "; - } - merged_stat.message += stat.message; - } - if (stat.level > merged_stat.level) { - merged_stat.level = stat.level; - } - for (const auto & value : stat.values) { - merged_stat.values.push_back(value); - } - } - - if (merged_stat.level == diagnostic_msgs::msg::DiagnosticStatus::OK) { - merged_stat.message = "OK"; - } - - return merged_stat; -} - -} // namespace autoware::ekf_localizer diff --git a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp b/localization/autoware_ekf_localizer/src/ekf_localizer.cpp deleted file mode 100644 index 5638a5416e6ab..0000000000000 --- a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp +++ /dev/null @@ -1,463 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/ekf_localizer/ekf_localizer.hpp" - -#include "autoware/ekf_localizer/diagnostics.hpp" -#include "autoware/ekf_localizer/string.hpp" -#include "autoware/ekf_localizer/warning_message.hpp" -#include "autoware/localization_util/covariance_ellipse.hpp" - -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware::ekf_localizer -{ - -// clang-format off -#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl // NOLINT -#define DEBUG_INFO(...) {if (params_.show_debug_info) {RCLCPP_INFO(__VA_ARGS__);}} // NOLINT -// clang-format on - -using std::placeholders::_1; - -EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) -: rclcpp::Node("ekf_localizer", node_options), - warning_(std::make_shared(this)), - tf2_buffer_(this->get_clock()), - tf2_listener_(tf2_buffer_), - params_(this), - ekf_dt_(params_.ekf_dt), - pose_queue_(params_.pose_smoothing_steps), - twist_queue_(params_.twist_smoothing_steps) -{ - is_activated_ = false; - is_set_initialpose_ = false; - - /* initialize ros system */ - timer_control_ = rclcpp::create_timer( - this, get_clock(), rclcpp::Duration::from_seconds(ekf_dt_), - std::bind(&EKFLocalizer::timer_callback, this)); - - pub_pose_ = create_publisher("ekf_pose", 1); - pub_pose_cov_ = - create_publisher("ekf_pose_with_covariance", 1); - pub_odom_ = create_publisher("ekf_odom", 1); - pub_twist_ = create_publisher("ekf_twist", 1); - pub_twist_cov_ = create_publisher( - "ekf_twist_with_covariance", 1); - pub_yaw_bias_ = - create_publisher("estimated_yaw_bias", 1); - pub_biased_pose_ = create_publisher("ekf_biased_pose", 1); - pub_biased_pose_cov_ = create_publisher( - "ekf_biased_pose_with_covariance", 1); - pub_diag_ = this->create_publisher("/diagnostics", 10); - pub_processing_time_ = create_publisher( - "debug/processing_time_ms", 1); - sub_initialpose_ = create_subscription( - "initialpose", 1, std::bind(&EKFLocalizer::callback_initial_pose, this, _1)); - sub_pose_with_cov_ = create_subscription( - "in_pose_with_covariance", 1, - std::bind(&EKFLocalizer::callback_pose_with_covariance, this, _1)); - sub_twist_with_cov_ = create_subscription( - "in_twist_with_covariance", 1, - std::bind(&EKFLocalizer::callback_twist_with_covariance, this, _1)); - service_trigger_node_ = create_service( - "trigger_node_srv", - std::bind( - &EKFLocalizer::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2), - rclcpp::ServicesQoS().get_rmw_qos_profile()); - - tf_br_ = std::make_shared( - std::shared_ptr(this, [](auto) {})); - - ekf_module_ = std::make_unique(warning_, params_); - logger_configure_ = std::make_unique(this); -} - -/* - * update_predict_frequency - */ -void EKFLocalizer::update_predict_frequency(const rclcpp::Time & current_time) -{ - if (last_predict_time_) { - if (current_time < *last_predict_time_) { - warning_->warn("Detected jump back in time"); - } else { - /* Measure dt */ - ekf_dt_ = (current_time - *last_predict_time_).seconds(); - DEBUG_INFO( - get_logger(), "[EKF] update ekf_dt_ to %f seconds (= %f hz)", ekf_dt_, 1 / ekf_dt_); - - if (ekf_dt_ > 10.0) { - ekf_dt_ = 10.0; - RCLCPP_WARN( - get_logger(), "Large ekf_dt_ detected!! (%f sec) Capped to 10.0 seconds", ekf_dt_); - } else if (ekf_dt_ > static_cast(params_.pose_smoothing_steps) / params_.ekf_rate) { - RCLCPP_WARN( - get_logger(), "EKF period may be too slow to finish pose smoothing!! (%f sec) ", ekf_dt_); - } - - /* Register dt and accumulate time delay */ - ekf_module_->accumulate_delay_time(ekf_dt_); - } - } - last_predict_time_ = std::make_shared(current_time); -} - -/* - * timer_callback - */ -void EKFLocalizer::timer_callback() -{ - stop_watch_timer_cb_.tic(); - - const rclcpp::Time current_time = this->now(); - - if (!is_activated_) { - warning_->warn_throttle( - "The node is not activated. Provide initial pose to pose_initializer", 2000); - publish_diagnostics(geometry_msgs::msg::PoseStamped{}, current_time); - return; - } - - if (!is_set_initialpose_) { - warning_->warn_throttle( - "Initial pose is not set. Provide initial pose to pose_initializer", 2000); - publish_diagnostics(geometry_msgs::msg::PoseStamped{}, current_time); - return; - } - - DEBUG_INFO(get_logger(), "========================= timer called ========================="); - - /* update predict frequency with measured timer rate */ - update_predict_frequency(current_time); - - /* predict model in EKF */ - stop_watch_.tic(); - DEBUG_INFO(get_logger(), "------------------------- start prediction -------------------------"); - ekf_module_->predict_with_delay(ekf_dt_); - DEBUG_INFO(get_logger(), "[EKF] predictKinematicsModel calc time = %f [ms]", stop_watch_.toc()); - DEBUG_INFO(get_logger(), "------------------------- end prediction -------------------------\n"); - - /* pose measurement update */ - pose_diag_info_.queue_size = pose_queue_.size(); - pose_diag_info_.is_passed_delay_gate = true; - pose_diag_info_.delay_time = 0.0; - pose_diag_info_.delay_time_threshold = 0.0; - pose_diag_info_.is_passed_mahalanobis_gate = true; - pose_diag_info_.mahalanobis_distance = 0.0; - - bool pose_is_updated = false; - - if (!pose_queue_.empty()) { - DEBUG_INFO(get_logger(), "------------------------- start Pose -------------------------"); - stop_watch_.tic(); - - // save the initial size because the queue size can change in the loop - const size_t n = pose_queue_.size(); - for (size_t i = 0; i < n; ++i) { - const auto pose = pose_queue_.pop_increment_age(); - bool is_updated = ekf_module_->measurement_update_pose(*pose, current_time, pose_diag_info_); - if (is_updated) { - pose_is_updated = true; - } - } - DEBUG_INFO( - get_logger(), "[EKF] measurement_update_pose calc time = %f [ms]", stop_watch_.toc()); - DEBUG_INFO(get_logger(), "------------------------- end Pose -------------------------\n"); - } - pose_diag_info_.no_update_count = pose_is_updated ? 0 : (pose_diag_info_.no_update_count + 1); - - /* twist measurement update */ - twist_diag_info_.queue_size = twist_queue_.size(); - twist_diag_info_.is_passed_delay_gate = true; - twist_diag_info_.delay_time = 0.0; - twist_diag_info_.delay_time_threshold = 0.0; - twist_diag_info_.is_passed_mahalanobis_gate = true; - twist_diag_info_.mahalanobis_distance = 0.0; - - bool twist_is_updated = false; - - if (!twist_queue_.empty()) { - DEBUG_INFO(get_logger(), "------------------------- start Twist -------------------------"); - stop_watch_.tic(); - - // save the initial size because the queue size can change in the loop - const size_t n = twist_queue_.size(); - for (size_t i = 0; i < n; ++i) { - const auto twist = twist_queue_.pop_increment_age(); - bool is_updated = - ekf_module_->measurement_update_twist(*twist, current_time, twist_diag_info_); - if (is_updated) { - twist_is_updated = true; - } - } - DEBUG_INFO( - get_logger(), "[EKF] measurement_update_twist calc time = %f [ms]", stop_watch_.toc()); - DEBUG_INFO(get_logger(), "------------------------- end Twist -------------------------\n"); - } - twist_diag_info_.no_update_count = twist_is_updated ? 0 : (twist_diag_info_.no_update_count + 1); - - const geometry_msgs::msg::PoseStamped current_ekf_pose = - ekf_module_->get_current_pose(current_time, false); - const geometry_msgs::msg::PoseStamped current_biased_ekf_pose = - ekf_module_->get_current_pose(current_time, true); - const geometry_msgs::msg::TwistStamped current_ekf_twist = - ekf_module_->get_current_twist(current_time); - - /* publish ekf result */ - publish_estimate_result(current_ekf_pose, current_biased_ekf_pose, current_ekf_twist); - publish_diagnostics(current_ekf_pose, current_time); - - /* publish processing time */ - const double elapsed_time = stop_watch_timer_cb_.toc(); - pub_processing_time_->publish( - autoware_internal_debug_msgs::build() - .stamp(current_time) - .data(elapsed_time)); -} - -/* - * get_transform_from_tf - */ -bool EKFLocalizer::get_transform_from_tf( - std::string parent_frame, std::string child_frame, - geometry_msgs::msg::TransformStamped & transform) -{ - parent_frame = erase_leading_slash(parent_frame); - child_frame = erase_leading_slash(child_frame); - - try { - transform = tf2_buffer_.lookupTransform(parent_frame, child_frame, tf2::TimePointZero); - return true; - } catch (tf2::TransformException & ex) { - warning_->warn(ex.what()); - } - return false; -} - -/* - * callback_initial_pose - */ -void EKFLocalizer::callback_initial_pose( - geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) -{ - geometry_msgs::msg::TransformStamped transform; - if (!get_transform_from_tf(params_.pose_frame_id, msg->header.frame_id, transform)) { - RCLCPP_ERROR( - get_logger(), "[EKF] TF transform failed. parent = %s, child = %s", - params_.pose_frame_id.c_str(), msg->header.frame_id.c_str()); - } - ekf_module_->initialize(*msg, transform); - - is_set_initialpose_ = true; -} - -/* - * callback_pose_with_covariance - */ -void EKFLocalizer::callback_pose_with_covariance( - geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) -{ - if (!is_activated_ && !is_set_initialpose_) { - return; - } - - pose_queue_.push(msg); - - publish_callback_return_diagnostics("pose", msg->header.stamp); -} - -/* - * callback_twist_with_covariance - */ -void EKFLocalizer::callback_twist_with_covariance( - geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) -{ - // Ignore twist if velocity is too small. - // Note that this inequality must not include "equal". - if (std::abs(msg->twist.twist.linear.x) < params_.threshold_observable_velocity_mps) { - msg->twist.covariance[0 * 6 + 0] = 10000.0; - } - twist_queue_.push(msg); - - publish_callback_return_diagnostics("twist", msg->header.stamp); -} - -/* - * publish_estimate_result - */ -void EKFLocalizer::publish_estimate_result( - const geometry_msgs::msg::PoseStamped & current_ekf_pose, - const geometry_msgs::msg::PoseStamped & current_biased_ekf_pose, - const geometry_msgs::msg::TwistStamped & current_ekf_twist) -{ - /* publish latest pose */ - pub_pose_->publish(current_ekf_pose); - pub_biased_pose_->publish(current_biased_ekf_pose); - - /* publish latest pose with covariance */ - geometry_msgs::msg::PoseWithCovarianceStamped pose_cov; - pose_cov.header.stamp = current_ekf_pose.header.stamp; - 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(); - pub_pose_cov_->publish(pose_cov); - - geometry_msgs::msg::PoseWithCovarianceStamped biased_pose_cov = pose_cov; - biased_pose_cov.pose.pose = current_biased_ekf_pose.pose; - pub_biased_pose_cov_->publish(biased_pose_cov); - - /* publish latest twist */ - pub_twist_->publish(current_ekf_twist); - - /* publish latest twist with covariance */ - geometry_msgs::msg::TwistWithCovarianceStamped twist_cov; - twist_cov.header.stamp = current_ekf_twist.header.stamp; - twist_cov.header.frame_id = current_ekf_twist.header.frame_id; - twist_cov.twist.twist = current_ekf_twist.twist; - twist_cov.twist.covariance = ekf_module_->get_current_twist_covariance(); - pub_twist_cov_->publish(twist_cov); - - /* publish yaw bias */ - autoware_internal_debug_msgs::msg::Float64Stamped yawb; - yawb.stamp = current_ekf_twist.header.stamp; - yawb.data = ekf_module_->get_yaw_bias(); - pub_yaw_bias_->publish(yawb); - - /* publish latest odometry */ - nav_msgs::msg::Odometry odometry; - odometry.header.stamp = current_ekf_pose.header.stamp; - odometry.header.frame_id = current_ekf_pose.header.frame_id; - odometry.child_frame_id = "base_link"; - odometry.pose = pose_cov.pose; - odometry.twist = twist_cov.twist; - pub_odom_->publish(odometry); - - /* publish tf */ - const geometry_msgs::msg::TransformStamped transform_stamped = - autoware::universe_utils::pose2transform(current_ekf_pose, "base_link"); - tf_br_->sendTransform(transform_stamped); -} - -void EKFLocalizer::publish_diagnostics( - const geometry_msgs::msg::PoseStamped & current_ekf_pose, const rclcpp::Time & current_time) -{ - std::vector diag_status_array; - - diag_status_array.push_back(check_process_activated(is_activated_)); - diag_status_array.push_back(check_set_initialpose(is_set_initialpose_)); - - if (is_activated_ && is_set_initialpose_) { - diag_status_array.push_back(check_measurement_updated( - "pose", pose_diag_info_.no_update_count, params_.pose_no_update_count_threshold_warn, - params_.pose_no_update_count_threshold_error)); - diag_status_array.push_back(check_measurement_queue_size("pose", pose_diag_info_.queue_size)); - diag_status_array.push_back(check_measurement_delay_gate( - "pose", pose_diag_info_.is_passed_delay_gate, pose_diag_info_.delay_time, - pose_diag_info_.delay_time_threshold)); - diag_status_array.push_back(check_measurement_mahalanobis_gate( - "pose", pose_diag_info_.is_passed_mahalanobis_gate, pose_diag_info_.mahalanobis_distance, - params_.pose_gate_dist)); - - diag_status_array.push_back(check_measurement_updated( - "twist", twist_diag_info_.no_update_count, params_.twist_no_update_count_threshold_warn, - params_.twist_no_update_count_threshold_error)); - diag_status_array.push_back(check_measurement_queue_size("twist", twist_diag_info_.queue_size)); - diag_status_array.push_back(check_measurement_delay_gate( - "twist", twist_diag_info_.is_passed_delay_gate, twist_diag_info_.delay_time, - twist_diag_info_.delay_time_threshold)); - diag_status_array.push_back(check_measurement_mahalanobis_gate( - "twist", twist_diag_info_.is_passed_mahalanobis_gate, twist_diag_info_.mahalanobis_distance, - params_.twist_gate_dist)); - - geometry_msgs::msg::PoseWithCovariance pose_cov; - pose_cov.pose = current_ekf_pose.pose; - pose_cov.covariance = ekf_module_->get_current_pose_covariance(); - const autoware::localization_util::Ellipse ellipse = - autoware::localization_util::calculate_xy_ellipse(pose_cov, params_.ellipse_scale); - diag_status_array.push_back(check_covariance_ellipse( - "cov_ellipse_long_axis", ellipse.long_radius, params_.warn_ellipse_size, - params_.error_ellipse_size)); - diag_status_array.push_back(check_covariance_ellipse( - "cov_ellipse_lateral_direction", ellipse.size_lateral_direction, - params_.warn_ellipse_size_lateral_direction, params_.error_ellipse_size_lateral_direction)); - } - - diagnostic_msgs::msg::DiagnosticStatus diag_merged_status; - diag_merged_status = merge_diagnostic_status(diag_status_array); - diag_merged_status.name = "localization: " + std::string(this->get_name()); - diag_merged_status.hardware_id = this->get_name(); - - diagnostic_msgs::msg::DiagnosticArray diag_msg; - diag_msg.header.stamp = current_time; - diag_msg.status.push_back(diag_merged_status); - pub_diag_->publish(diag_msg); -} - -void EKFLocalizer::publish_callback_return_diagnostics( - const std::string & callback_name, const rclcpp::Time & current_time) -{ - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "topic_time_stamp"; - key_value.value = std::to_string(current_time.nanoseconds()); - diagnostic_msgs::msg::DiagnosticStatus diag_status; - diag_status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - diag_status.name = - "localization: " + std::string(this->get_name()) + ": callback_" + callback_name; - diag_status.hardware_id = this->get_name(); - diag_status.message = "OK"; - diag_status.values.push_back(key_value); - diagnostic_msgs::msg::DiagnosticArray diag_msg; - diag_msg.header.stamp = current_time; - diag_msg.status.push_back(diag_status); - pub_diag_->publish(diag_msg); -} - -/** - * @brief trigger node - */ -void EKFLocalizer::service_trigger_node( - const std_srvs::srv::SetBool::Request::SharedPtr req, - std_srvs::srv::SetBool::Response::SharedPtr res) -{ - if (req->data) { - pose_queue_.clear(); - twist_queue_.clear(); - is_activated_ = true; - } else { - is_activated_ = false; - is_set_initialpose_ = false; - } - res->success = true; -} - -} // namespace autoware::ekf_localizer - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(autoware::ekf_localizer::EKFLocalizer) diff --git a/localization/autoware_ekf_localizer/src/ekf_module.cpp b/localization/autoware_ekf_localizer/src/ekf_module.cpp deleted file mode 100644 index 02c9f8ee30e08..0000000000000 --- a/localization/autoware_ekf_localizer/src/ekf_module.cpp +++ /dev/null @@ -1,459 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/ekf_localizer/ekf_module.hpp" - -#include "autoware/ekf_localizer/covariance.hpp" -#include "autoware/ekf_localizer/mahalanobis.hpp" -#include "autoware/ekf_localizer/matrix_types.hpp" -#include "autoware/ekf_localizer/measurement.hpp" -#include "autoware/ekf_localizer/numeric.hpp" -#include "autoware/ekf_localizer/state_transition.hpp" -#include "autoware/ekf_localizer/warning_message.hpp" - -#include -#include - -#include -#include -#include - -#include -#include - -namespace autoware::ekf_localizer -{ - -// clang-format off -#define DEBUG_PRINT_MAT(X) {if (params_.show_debug_info) {std::cout << #X << ": " << X << std::endl;}} // NOLINT -// clang-format on - -EKFModule::EKFModule(std::shared_ptr warning, const HyperParameters & params) -: warning_(std::move(warning)), - dim_x_(6), // x, y, yaw, yaw_bias, vx, wz - accumulated_delay_times_(params.extend_state_step, 1.0E15), - params_(params), - last_angular_velocity_(0.0, 0.0, 0.0) -{ - Eigen::MatrixXd x = Eigen::MatrixXd::Zero(dim_x_, 1); - Eigen::MatrixXd p = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15; // for x & y - p(IDX::YAW, IDX::YAW) = 50.0; // for yaw - if (params_.enable_yaw_bias_estimation) { - p(IDX::YAWB, IDX::YAWB) = 50.0; // for yaw bias - } - p(IDX::VX, IDX::VX) = 1000.0; // for vx - p(IDX::WZ, IDX::WZ) = 50.0; // for wz - - kalman_filter_.init(x, p, static_cast(params_.extend_state_step)); - z_filter_.set_proc_var(params_.z_filter_proc_dev * params_.z_filter_proc_dev); - roll_filter_.set_proc_var(params_.roll_filter_proc_dev * params_.roll_filter_proc_dev); - pitch_filter_.set_proc_var(params_.pitch_filter_proc_dev * params_.pitch_filter_proc_dev); -} - -void EKFModule::initialize( - const PoseWithCovariance & initial_pose, const geometry_msgs::msg::TransformStamped & transform) -{ - Eigen::MatrixXd x(dim_x_, 1); - Eigen::MatrixXd p = Eigen::MatrixXd::Zero(dim_x_, dim_x_); - - x(IDX::X) = initial_pose.pose.pose.position.x + transform.transform.translation.x; - x(IDX::Y) = initial_pose.pose.pose.position.y + transform.transform.translation.y; - x(IDX::YAW) = - tf2::getYaw(initial_pose.pose.pose.orientation) + tf2::getYaw(transform.transform.rotation); - x(IDX::YAWB) = 0.0; - x(IDX::VX) = 0.0; - x(IDX::WZ) = 0.0; - - using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - p(IDX::X, IDX::X) = initial_pose.pose.covariance[COV_IDX::X_X]; - p(IDX::Y, IDX::Y) = initial_pose.pose.covariance[COV_IDX::Y_Y]; - p(IDX::YAW, IDX::YAW) = initial_pose.pose.covariance[COV_IDX::YAW_YAW]; - - if (params_.enable_yaw_bias_estimation) { - p(IDX::YAWB, IDX::YAWB) = 0.0001; - } - p(IDX::VX, IDX::VX) = 0.01; - p(IDX::WZ, IDX::WZ) = 0.01; - - kalman_filter_.init(x, p, static_cast(params_.extend_state_step)); - - const double z = initial_pose.pose.pose.position.z; - - const auto rpy = autoware::universe_utils::getRPY(initial_pose.pose.pose.orientation); - - const double z_var = initial_pose.pose.covariance[COV_IDX::Z_Z]; - const double roll_var = initial_pose.pose.covariance[COV_IDX::ROLL_ROLL]; - const double pitch_var = initial_pose.pose.covariance[COV_IDX::PITCH_PITCH]; - - z_filter_.init(z, z_var); - roll_filter_.init(rpy.x, roll_var); - pitch_filter_.init(rpy.y, pitch_var); -} - -geometry_msgs::msg::PoseStamped EKFModule::get_current_pose( - const rclcpp::Time & current_time, bool get_biased_yaw) const -{ - const double z = z_filter_.get_x(); - const double roll = roll_filter_.get_x(); - const double pitch = pitch_filter_.get_x(); - - const double x = kalman_filter_.getXelement(IDX::X); - const double y = kalman_filter_.getXelement(IDX::Y); - /* - getXelement(IDX::YAW) is surely `biased_yaw`. - Please note how `yaw` and `yaw_bias` are used in the state transition model and - how the observed pose is handled in the measurement pose update. - */ - const double biased_yaw = kalman_filter_.getXelement(IDX::YAW); - const double yaw_bias = kalman_filter_.getXelement(IDX::YAWB); - const double yaw = biased_yaw + yaw_bias; - - Pose current_ekf_pose; - current_ekf_pose.header.frame_id = params_.pose_frame_id; - current_ekf_pose.header.stamp = current_time; - current_ekf_pose.pose.position = autoware::universe_utils::createPoint(x, y, z); - if (get_biased_yaw) { - current_ekf_pose.pose.orientation = - autoware::universe_utils::createQuaternionFromRPY(roll, pitch, biased_yaw); - } else { - current_ekf_pose.pose.orientation = - autoware::universe_utils::createQuaternionFromRPY(roll, pitch, yaw); - } - return current_ekf_pose; -} - -geometry_msgs::msg::TwistStamped EKFModule::get_current_twist( - const rclcpp::Time & current_time) const -{ - const double vx = kalman_filter_.getXelement(IDX::VX); - const double wz = kalman_filter_.getXelement(IDX::WZ); - - Twist current_ekf_twist; - current_ekf_twist.header.frame_id = "base_link"; - current_ekf_twist.header.stamp = current_time; - current_ekf_twist.twist.linear.x = vx; - current_ekf_twist.twist.angular.z = wz; - return current_ekf_twist; -} - -std::array EKFModule::get_current_pose_covariance() const -{ - std::array cov = - ekf_covariance_to_pose_message_covariance(kalman_filter_.getLatestP()); - - using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - cov[COV_IDX::Z_Z] = z_filter_.get_var(); - cov[COV_IDX::ROLL_ROLL] = roll_filter_.get_var(); - cov[COV_IDX::PITCH_PITCH] = pitch_filter_.get_var(); - - return cov; -} - -std::array EKFModule::get_current_twist_covariance() const -{ - return ekf_covariance_to_twist_message_covariance(kalman_filter_.getLatestP()); -} - -double EKFModule::get_yaw_bias() const -{ - return kalman_filter_.getLatestX()(IDX::YAWB); -} - -size_t EKFModule::find_closest_delay_time_index(double target_value) const -{ - // If target_value is too large, return last index + 1 - if (target_value > accumulated_delay_times_.back()) { - return accumulated_delay_times_.size(); - } - - auto lower = std::lower_bound( - accumulated_delay_times_.begin(), accumulated_delay_times_.end(), target_value); - - // If the lower bound is the first element, return its index. - // If the lower bound is beyond the last element, return the last index. - // If else, take the closest element. - if (lower == accumulated_delay_times_.begin()) { - return 0; - } - if (lower == accumulated_delay_times_.end()) { - return accumulated_delay_times_.size() - 1; - } - // Compare the target with the lower bound and the previous element. - auto prev = lower - 1; - bool is_closer_to_prev = (target_value - *prev) < (*lower - target_value); - - // Return the index of the closer element. - return is_closer_to_prev ? std::distance(accumulated_delay_times_.begin(), prev) - : std::distance(accumulated_delay_times_.begin(), lower); -} - -void EKFModule::accumulate_delay_time(const double dt) -{ - // Shift the delay times to the right. - std::copy_backward( - accumulated_delay_times_.begin(), accumulated_delay_times_.end() - 1, - accumulated_delay_times_.end()); - - // Add a new element (=0) and, and add delay time to the previous elements. - accumulated_delay_times_.front() = 0.0; - for (size_t i = 1; i < accumulated_delay_times_.size(); ++i) { - accumulated_delay_times_[i] += dt; - } -} - -void EKFModule::predict_with_delay(const double dt) -{ - const Eigen::MatrixXd x_curr = kalman_filter_.getLatestX(); - const Eigen::MatrixXd p_curr = kalman_filter_.getLatestP(); - - const double proc_cov_vx_d = std::pow(params_.proc_stddev_vx_c * dt, 2.0); - const double proc_cov_wz_d = std::pow(params_.proc_stddev_wz_c * dt, 2.0); - const double proc_cov_yaw_d = std::pow(params_.proc_stddev_yaw_c * dt, 2.0); - - const Vector6d x_next = predict_next_state(x_curr, dt); - const Matrix6d a = create_state_transition_matrix(x_curr, dt); - const Matrix6d q = process_noise_covariance(proc_cov_yaw_d, proc_cov_vx_d, proc_cov_wz_d); - kalman_filter_.predictWithDelay(x_next, a, q); - ekf_dt_ = dt; -} - -bool EKFModule::measurement_update_pose( - const PoseWithCovariance & pose, const rclcpp::Time & t_curr, EKFDiagnosticInfo & pose_diag_info) -{ - if (pose.header.frame_id != params_.pose_frame_id) { - warning_->warn_throttle( - fmt::format( - "pose frame_id is %s, but pose_frame is set as %s. They must be same.", - pose.header.frame_id.c_str(), params_.pose_frame_id.c_str()), - 2000); - } - const Eigen::MatrixXd x_curr = kalman_filter_.getLatestX(); - DEBUG_PRINT_MAT(x_curr.transpose()); - - constexpr int dim_y = 3; // pos_x, pos_y, yaw, depending on Pose output - - /* Calculate delay step */ - double delay_time = (t_curr - pose.header.stamp).seconds() + params_.pose_additional_delay; - if (delay_time < 0.0) { - warning_->warn_throttle(pose_delay_time_warning_message(delay_time), 1000); - } - - delay_time = std::max(delay_time, 0.0); - - const size_t delay_step = find_closest_delay_time_index(delay_time); - - pose_diag_info.delay_time = std::max(delay_time, pose_diag_info.delay_time); - pose_diag_info.delay_time_threshold = accumulated_delay_times_.back(); - if (delay_step >= params_.extend_state_step) { - pose_diag_info.is_passed_delay_gate = false; - warning_->warn_throttle( - pose_delay_step_warning_message( - pose_diag_info.delay_time, pose_diag_info.delay_time_threshold), - 2000); - return false; - } - - /* Since the kalman filter cannot handle the rotation angle directly, - offset the yaw angle so that the difference from the yaw angle that ekf holds internally - is less than 2 pi. */ - double yaw = tf2::getYaw(pose.pose.pose.orientation); - const double ekf_yaw = kalman_filter_.getXelement(delay_step * dim_x_ + IDX::YAW); - const double yaw_error = normalize_yaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi - yaw = yaw_error + ekf_yaw; - - /* Set measurement matrix */ - Eigen::MatrixXd y(dim_y, 1); - y << pose.pose.pose.position.x, pose.pose.pose.position.y, yaw; - - if (has_nan(y) || has_inf(y)) { - warning_->warn( - "[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message."); - return false; - } - - /* Gate */ - const Eigen::Vector3d y_ekf( - kalman_filter_.getXelement(delay_step * dim_x_ + IDX::X), - kalman_filter_.getXelement(delay_step * dim_x_ + IDX::Y), ekf_yaw); - const Eigen::MatrixXd p_curr = kalman_filter_.getLatestP(); - const Eigen::MatrixXd p_y = p_curr.block(0, 0, dim_y, dim_y); - - const double distance = mahalanobis(y_ekf, y, p_y); - pose_diag_info.mahalanobis_distance = std::max(distance, pose_diag_info.mahalanobis_distance); - if (distance > params_.pose_gate_dist) { - pose_diag_info.is_passed_mahalanobis_gate = false; - warning_->warn_throttle(mahalanobis_warning_message(distance, params_.pose_gate_dist), 2000); - warning_->warn_throttle("Ignore the measurement data.", 2000); - return false; - } - - DEBUG_PRINT_MAT(y.transpose()); - DEBUG_PRINT_MAT(y_ekf.transpose()); - DEBUG_PRINT_MAT((y - y_ekf).transpose()); - - const Eigen::Matrix c = pose_measurement_matrix(); - const Eigen::Matrix3d r = - pose_measurement_covariance(pose.pose.covariance, params_.pose_smoothing_steps); - - kalman_filter_.updateWithDelay(y, c, r, static_cast(delay_step)); - - // Update Simple 1D filter with considering change of roll, pitch and height (position z) - // values due to measurement pose delay - auto pose_with_rph_delay_compensation = - compensate_rph_with_delay(pose, last_angular_velocity_, delay_time); - update_simple_1d_filters(pose_with_rph_delay_compensation, params_.pose_smoothing_steps); - - // debug - const Eigen::MatrixXd x_result = kalman_filter_.getLatestX(); - DEBUG_PRINT_MAT(x_result.transpose()); - DEBUG_PRINT_MAT((x_result - x_curr).transpose()); - - return true; -} - -geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_rph_with_delay( - const PoseWithCovariance & pose, tf2::Vector3 last_angular_velocity, const double delay_time) -{ - tf2::Quaternion delta_orientation; - if (last_angular_velocity.length() > 0.0) { - delta_orientation.setRotation( - last_angular_velocity.normalized(), last_angular_velocity.length() * delay_time); - } else { - delta_orientation.setValue(0.0, 0.0, 0.0, 1.0); - } - - tf2::Quaternion prev_orientation = tf2::Quaternion( - pose.pose.pose.orientation.x, pose.pose.pose.orientation.y, pose.pose.pose.orientation.z, - pose.pose.pose.orientation.w); - - tf2::Quaternion curr_orientation; - curr_orientation = prev_orientation * delta_orientation; - curr_orientation.normalize(); - - PoseWithCovariance pose_with_delay; - pose_with_delay = pose; - pose_with_delay.header.stamp = - rclcpp::Time(pose.header.stamp) + rclcpp::Duration::from_seconds(delay_time); - pose_with_delay.pose.pose.orientation.x = curr_orientation.x(); - pose_with_delay.pose.pose.orientation.y = curr_orientation.y(); - pose_with_delay.pose.pose.orientation.z = curr_orientation.z(); - pose_with_delay.pose.pose.orientation.w = curr_orientation.w(); - - const auto rpy = autoware::universe_utils::getRPY(pose_with_delay.pose.pose.orientation); - const double delta_z = kalman_filter_.getXelement(IDX::VX) * delay_time * std::sin(-rpy.y); - pose_with_delay.pose.pose.position.z += delta_z; - - return pose_with_delay; -} - -bool EKFModule::measurement_update_twist( - const TwistWithCovariance & twist, const rclcpp::Time & t_curr, - EKFDiagnosticInfo & twist_diag_info) -{ - if (twist.header.frame_id != "base_link") { - warning_->warn_throttle("twist frame_id must be base_link", 2000); - } - - last_angular_velocity_ = tf2::Vector3(0.0, 0.0, 0.0); - - const Eigen::MatrixXd x_curr = kalman_filter_.getLatestX(); - DEBUG_PRINT_MAT(x_curr.transpose()); - - constexpr int dim_y = 2; // vx, wz - - /* Calculate delay step */ - double delay_time = (t_curr - twist.header.stamp).seconds() + params_.twist_additional_delay; - if (delay_time < 0.0) { - warning_->warn_throttle(twist_delay_time_warning_message(delay_time), 1000); - } - delay_time = std::max(delay_time, 0.0); - - const size_t delay_step = find_closest_delay_time_index(delay_time); - - twist_diag_info.delay_time = std::max(delay_time, twist_diag_info.delay_time); - twist_diag_info.delay_time_threshold = accumulated_delay_times_.back(); - if (delay_step >= params_.extend_state_step) { - twist_diag_info.is_passed_delay_gate = false; - warning_->warn_throttle( - twist_delay_step_warning_message( - twist_diag_info.delay_time, twist_diag_info.delay_time_threshold), - 2000); - return false; - } - - /* Set measurement matrix */ - Eigen::MatrixXd y(dim_y, 1); - y << twist.twist.twist.linear.x, twist.twist.twist.angular.z; - - if (has_nan(y) || has_inf(y)) { - warning_->warn( - "[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message."); - return false; - } - - const Eigen::Vector2d y_ekf( - kalman_filter_.getXelement(delay_step * dim_x_ + IDX::VX), - kalman_filter_.getXelement(delay_step * dim_x_ + IDX::WZ)); - const Eigen::MatrixXd p_curr = kalman_filter_.getLatestP(); - const Eigen::MatrixXd p_y = p_curr.block(4, 4, dim_y, dim_y); - - const double distance = mahalanobis(y_ekf, y, p_y); - twist_diag_info.mahalanobis_distance = std::max(distance, twist_diag_info.mahalanobis_distance); - if (distance > params_.twist_gate_dist) { - twist_diag_info.is_passed_mahalanobis_gate = false; - warning_->warn_throttle(mahalanobis_warning_message(distance, params_.twist_gate_dist), 2000); - warning_->warn_throttle("Ignore the measurement data.", 2000); - return false; - } - - DEBUG_PRINT_MAT(y.transpose()); - DEBUG_PRINT_MAT(y_ekf.transpose()); - DEBUG_PRINT_MAT((y - y_ekf).transpose()); - - const Eigen::Matrix c = twist_measurement_matrix(); - const Eigen::Matrix2d r = - twist_measurement_covariance(twist.twist.covariance, params_.twist_smoothing_steps); - - kalman_filter_.updateWithDelay(y, c, r, static_cast(delay_step)); - - last_angular_velocity_ = tf2::Vector3( - twist.twist.twist.angular.x, twist.twist.twist.angular.y, twist.twist.twist.angular.z); - - // debug - const Eigen::MatrixXd x_result = kalman_filter_.getLatestX(); - DEBUG_PRINT_MAT(x_result.transpose()); - DEBUG_PRINT_MAT((x_result - x_curr).transpose()); - - return true; -} - -void EKFModule::update_simple_1d_filters( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step) -{ - double z = pose.pose.pose.position.z; - - const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation); - - using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - double z_var = pose.pose.covariance[COV_IDX::Z_Z] * static_cast(smoothing_step); - double roll_var = pose.pose.covariance[COV_IDX::ROLL_ROLL] * static_cast(smoothing_step); - double pitch_var = - pose.pose.covariance[COV_IDX::PITCH_PITCH] * static_cast(smoothing_step); - - z_filter_.update(z, z_var, ekf_dt_); - roll_filter_.update(rpy.x, roll_var, ekf_dt_); - pitch_filter_.update(rpy.y, pitch_var, ekf_dt_); -} - -} // namespace autoware::ekf_localizer diff --git a/localization/autoware_ekf_localizer/src/mahalanobis.cpp b/localization/autoware_ekf_localizer/src/mahalanobis.cpp deleted file mode 100644 index f7e56674b16bd..0000000000000 --- a/localization/autoware_ekf_localizer/src/mahalanobis.cpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/ekf_localizer/mahalanobis.hpp" - -namespace autoware::ekf_localizer -{ - -double squared_mahalanobis( - const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C) -{ - const Eigen::VectorXd d = x - y; - return d.dot(C.inverse() * d); -} - -double mahalanobis(const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C) -{ - return std::sqrt(squared_mahalanobis(x, y, C)); -} - -} // namespace autoware::ekf_localizer diff --git a/localization/autoware_ekf_localizer/src/measurement.cpp b/localization/autoware_ekf_localizer/src/measurement.cpp deleted file mode 100644 index 63db7d250f8d5..0000000000000 --- a/localization/autoware_ekf_localizer/src/measurement.cpp +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/ekf_localizer/measurement.hpp" - -#include "autoware/ekf_localizer/state_index.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" - -namespace autoware::ekf_localizer -{ - -Eigen::Matrix pose_measurement_matrix() -{ - Eigen::Matrix c = Eigen::Matrix::Zero(); - c(0, IDX::X) = 1.0; // for pos x - c(1, IDX::Y) = 1.0; // for pos y - c(2, IDX::YAW) = 1.0; // for yaw - return c; -} - -Eigen::Matrix twist_measurement_matrix() -{ - Eigen::Matrix c = Eigen::Matrix::Zero(); - c(0, IDX::VX) = 1.0; // for vx - c(1, IDX::WZ) = 1.0; // for wz - return c; -} - -Eigen::Matrix3d pose_measurement_covariance( - const std::array & covariance, const size_t smoothing_step) -{ - Eigen::Matrix3d r; - using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - r << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_Y), covariance.at(COV_IDX::X_YAW), - covariance.at(COV_IDX::Y_X), covariance.at(COV_IDX::Y_Y), covariance.at(COV_IDX::Y_YAW), - covariance.at(COV_IDX::YAW_X), covariance.at(COV_IDX::YAW_Y), covariance.at(COV_IDX::YAW_YAW); - return r * static_cast(smoothing_step); -} - -Eigen::Matrix2d twist_measurement_covariance( - const std::array & covariance, const size_t smoothing_step) -{ - Eigen::Matrix2d r; - using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - r << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_YAW), covariance.at(COV_IDX::YAW_X), - covariance.at(COV_IDX::YAW_YAW); - return r * static_cast(smoothing_step); -} - -} // namespace autoware::ekf_localizer diff --git a/localization/autoware_ekf_localizer/src/state_transition.cpp b/localization/autoware_ekf_localizer/src/state_transition.cpp deleted file mode 100644 index f6a50b5839ec4..0000000000000 --- a/localization/autoware_ekf_localizer/src/state_transition.cpp +++ /dev/null @@ -1,102 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/ekf_localizer/state_transition.hpp" - -#include "autoware/ekf_localizer/matrix_types.hpp" -#include "autoware/ekf_localizer/state_index.hpp" - -#include - -namespace autoware::ekf_localizer -{ - -double normalize_yaw(const double & yaw) -{ - // FIXME(IshitaTakeshi) I think the computation here can be simplified - // FIXME(IshitaTakeshi) Rename the function. This is not normalization - return std::atan2(std::sin(yaw), std::cos(yaw)); -} - -/* == Nonlinear model == - * - * x_{k+1} = x_k + vx_k * cos(yaw_k + b_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k + b_k) * dt - * yaw_{k+1} = yaw_k + (wz_k) * dt - * b_{k+1} = b_k - * vx_{k+1} = vx_k - * wz_{k+1} = wz_k - * - * (b_k : yaw_bias_k) - */ - -Vector6d predict_next_state(const Vector6d & X_curr, const double dt) -{ - const double x = X_curr(IDX::X); - const double y = X_curr(IDX::Y); - const double yaw = X_curr(IDX::YAW); - const double yaw_bias = X_curr(IDX::YAWB); - const double vx = X_curr(IDX::VX); - const double wz = X_curr(IDX::WZ); - - Vector6d x_next; - x_next(IDX::X) = x + vx * std::cos(yaw + yaw_bias) * dt; // dx = v * cos(yaw) - x_next(IDX::Y) = y + vx * std::sin(yaw + yaw_bias) * dt; // dy = v * sin(yaw) - x_next(IDX::YAW) = normalize_yaw(yaw + wz * dt); // dyaw = omega + omega_bias - x_next(IDX::YAWB) = yaw_bias; - x_next(IDX::VX) = vx; - x_next(IDX::WZ) = wz; - return x_next; -} - -/* == Linearized model == - * - * A = [ 1, 0, -vx*sin(yaw+b)*dt, -vx*sin(yaw+b)*dt, cos(yaw+b)*dt, 0] - * [ 0, 1, vx*cos(yaw+b)*dt, vx*cos(yaw+b)*dt, sin(yaw+b)*dt, 0] - * [ 0, 0, 1, 0, 0, dt] - * [ 0, 0, 0, 1, 0, 0] - * [ 0, 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 0, 1] - */ -Matrix6d create_state_transition_matrix(const Vector6d & X_curr, const double dt) -{ - const double yaw = X_curr(IDX::YAW); - const double yaw_bias = X_curr(IDX::YAWB); - const double vx = X_curr(IDX::VX); - - Matrix6d a = Matrix6d::Identity(); - a(IDX::X, IDX::YAW) = -vx * sin(yaw + yaw_bias) * dt; - a(IDX::X, IDX::YAWB) = -vx * sin(yaw + yaw_bias) * dt; - a(IDX::X, IDX::VX) = cos(yaw + yaw_bias) * dt; - a(IDX::Y, IDX::YAW) = vx * cos(yaw + yaw_bias) * dt; - a(IDX::Y, IDX::YAWB) = vx * cos(yaw + yaw_bias) * dt; - a(IDX::Y, IDX::VX) = sin(yaw + yaw_bias) * dt; - a(IDX::YAW, IDX::WZ) = dt; - return a; -} - -Matrix6d process_noise_covariance( - const double proc_cov_yaw_d, const double proc_cov_vx_d, const double proc_cov_wz_d) -{ - Matrix6d q = Matrix6d::Zero(); - q(IDX::X, IDX::X) = 0.0; - q(IDX::Y, IDX::Y) = 0.0; - q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d; // for yaw - q(IDX::YAWB, IDX::YAWB) = 0.0; - q(IDX::VX, IDX::VX) = proc_cov_vx_d; // for vx - q(IDX::WZ, IDX::WZ) = proc_cov_wz_d; // for wz - return q; -} - -} // namespace autoware::ekf_localizer diff --git a/localization/autoware_ekf_localizer/src/warning_message.cpp b/localization/autoware_ekf_localizer/src/warning_message.cpp deleted file mode 100644 index 0c1e127747365..0000000000000 --- a/localization/autoware_ekf_localizer/src/warning_message.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/ekf_localizer/warning_message.hpp" - -#include - -#include - -namespace autoware::ekf_localizer -{ - -std::string pose_delay_step_warning_message( - const double delay_time, const double delay_time_threshold) -{ - const std::string s = - "Pose delay exceeds the compensation limit, ignored. " - "delay: {:.3f}[s], limit: {:.3f}[s]"; - return fmt::format(s, delay_time, delay_time_threshold); -} - -std::string twist_delay_step_warning_message( - const double delay_time, const double delay_time_threshold) -{ - const std::string s = - "Twist delay exceeds the compensation limit, ignored. " - "delay: {:.3f}[s], limit: {:.3f}[s]"; - return fmt::format(s, delay_time, delay_time_threshold); -} - -std::string pose_delay_time_warning_message(const double delay_time) -{ - const std::string s = "Pose time stamp is inappropriate, set delay to 0[s]. delay = {:.3f}"; - return fmt::format(s, delay_time); -} - -std::string twist_delay_time_warning_message(const double delay_time) -{ - const std::string s = "Twist time stamp is inappropriate, set delay to 0[s]. delay = {:.3f}"; - return fmt::format(s, delay_time); -} - -std::string mahalanobis_warning_message(const double distance, const double max_distance) -{ - const std::string s = "The Mahalanobis distance {:.4f} is over the limit {:.4f}."; - return fmt::format(s, distance, max_distance); -} - -} // namespace autoware::ekf_localizer diff --git a/localization/autoware_ekf_localizer/test/test_aged_object_queue.cpp b/localization/autoware_ekf_localizer/test/test_aged_object_queue.cpp deleted file mode 100644 index dc9ef008335ed..0000000000000 --- a/localization/autoware_ekf_localizer/test/test_aged_object_queue.cpp +++ /dev/null @@ -1,108 +0,0 @@ -// Copyright 2023 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/ekf_localizer/aged_object_queue.hpp" - -#include - -#include - -namespace autoware::ekf_localizer -{ - -TEST(AgedObjectQueue, DiscardsObjectWhenAgeReachesMaximum) -{ - AgedObjectQueue queue(3); - - queue.push("a"); - EXPECT_EQ(queue.size(), 1U); - - queue.pop_increment_age(); // age = 1 - EXPECT_EQ(queue.size(), 1U); - - queue.pop_increment_age(); // age = 2 - EXPECT_EQ(queue.size(), 1U); - - queue.pop_increment_age(); // age = 3 - EXPECT_EQ(queue.size(), 0U); -} - -TEST(AgedObjectQueue, MultipleObjects) -{ - AgedObjectQueue queue(3); - - queue.push("a"); - EXPECT_EQ(queue.size(), 1U); - EXPECT_EQ(queue.pop_increment_age(), std::string{"a"}); // age of a = 1 - EXPECT_EQ(queue.size(), 1U); - EXPECT_EQ(queue.pop_increment_age(), std::string{"a"}); // age of a = 2 - - queue.push("b"); - - EXPECT_EQ(queue.pop_increment_age(), std::string{"a"}); // age of a = 3 - EXPECT_EQ(queue.size(), 1U); - - EXPECT_EQ(queue.pop_increment_age(), std::string{"b"}); // age of b = 1 - EXPECT_EQ(queue.size(), 1U); - - EXPECT_EQ(queue.pop_increment_age(), std::string{"b"}); // age of b = 2 - EXPECT_EQ(queue.size(), 1U); - - EXPECT_EQ(queue.pop_increment_age(), std::string{"b"}); // age of b = 3 - EXPECT_EQ(queue.size(), 0U); -} - -TEST(AgedObjectQueue, Empty) -{ - AgedObjectQueue queue(2); - - EXPECT_TRUE(queue.empty()); - - queue.push("a"); - - EXPECT_FALSE(queue.empty()); - - queue.pop_increment_age(); - queue.pop_increment_age(); - - EXPECT_TRUE(queue.empty()); -} - -TEST(AgedObjectQueue, Clear) -{ - AgedObjectQueue queue(3); - - queue.push("a"); - queue.push("b"); - - EXPECT_EQ(queue.size(), 2U); - - queue.clear(); - - EXPECT_EQ(queue.size(), 0U); -} - -TEST(AgedObjectQueue, Back) -{ - AgedObjectQueue queue(3); - - queue.push("a"); - - EXPECT_EQ(queue.back(), std::string{"a"}); - queue.push("b"); - - EXPECT_EQ(queue.back(), std::string{"b"}); -} - -} // namespace autoware::ekf_localizer diff --git a/localization/autoware_ekf_localizer/test/test_covariance.cpp b/localization/autoware_ekf_localizer/test/test_covariance.cpp deleted file mode 100644 index cab09367d6340..0000000000000 --- a/localization/autoware_ekf_localizer/test/test_covariance.cpp +++ /dev/null @@ -1,84 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/ekf_localizer/covariance.hpp" - -#include - -namespace autoware::ekf_localizer -{ - -TEST(EKFCovarianceToPoseMessageCovariance, SmokeTest) -{ - { - Matrix6d p = Matrix6d::Zero(); - p(0, 0) = 1.; - p(0, 1) = 2.; - p(0, 2) = 3.; - p(1, 0) = 4.; - p(1, 1) = 5.; - p(1, 2) = 6.; - p(2, 0) = 7.; - p(2, 1) = 8.; - p(2, 2) = 9.; - - std::array covariance = ekf_covariance_to_pose_message_covariance(p); - EXPECT_EQ(covariance[0], 1.); - EXPECT_EQ(covariance[1], 2.); - EXPECT_EQ(covariance[5], 3.); - EXPECT_EQ(covariance[6], 4.); - EXPECT_EQ(covariance[7], 5.); - EXPECT_EQ(covariance[11], 6.); - EXPECT_EQ(covariance[30], 7.); - EXPECT_EQ(covariance[31], 8.); - EXPECT_EQ(covariance[35], 9.); - } - - // ensure other elements are zero - { - Matrix6d p = Matrix6d::Zero(); - std::array covariance = ekf_covariance_to_pose_message_covariance(p); - for (double e : covariance) { - EXPECT_EQ(e, 0.); - } - } -} - -TEST(EKFCovarianceToTwistMessageCovariance, SmokeTest) -{ - { - Matrix6d p = Matrix6d::Zero(); - p(4, 4) = 1.; - p(4, 5) = 2.; - p(5, 4) = 3.; - p(5, 5) = 4.; - - std::array covariance = ekf_covariance_to_twist_message_covariance(p); - EXPECT_EQ(covariance[0], 1.); - EXPECT_EQ(covariance[5], 2.); - EXPECT_EQ(covariance[30], 3.); - EXPECT_EQ(covariance[35], 4.); - } - - // ensure other elements are zero - { - Matrix6d p = Matrix6d::Zero(); - std::array covariance = ekf_covariance_to_twist_message_covariance(p); - for (double e : covariance) { - EXPECT_EQ(e, 0.); - } - } -} - -} // namespace autoware::ekf_localizer diff --git a/localization/autoware_ekf_localizer/test/test_diagnostics.cpp b/localization/autoware_ekf_localizer/test/test_diagnostics.cpp deleted file mode 100644 index 5ce39df484e98..0000000000000 --- a/localization/autoware_ekf_localizer/test/test_diagnostics.cpp +++ /dev/null @@ -1,213 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/ekf_localizer/diagnostics.hpp" - -#include - -#include -#include - -namespace autoware::ekf_localizer -{ - -TEST(TestEkfDiagnostics, check_process_activated) -{ - diagnostic_msgs::msg::DiagnosticStatus stat; - - bool is_activated = true; - stat = check_process_activated(is_activated); - EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); - - is_activated = false; - stat = check_process_activated(is_activated); - EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); -} - -TEST(TestEkfDiagnostics, check_set_initialpose) -{ - diagnostic_msgs::msg::DiagnosticStatus stat; - - bool is_set_initialpose = true; - stat = check_set_initialpose(is_set_initialpose); - EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); - - is_set_initialpose = false; - stat = check_set_initialpose(is_set_initialpose); - EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); -} - -TEST(TestEkfDiagnostics, check_measurement_updated) -{ - diagnostic_msgs::msg::DiagnosticStatus stat; - - const std::string measurement_type = "pose"; // not effect for stat.level - const size_t no_update_count_threshold_warn = 50; - const size_t no_update_count_threshold_error = 250; - - size_t no_update_count = 0; - stat = check_measurement_updated( - measurement_type, no_update_count, no_update_count_threshold_warn, - no_update_count_threshold_error); - EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); - - no_update_count = 1; - stat = check_measurement_updated( - measurement_type, no_update_count, no_update_count_threshold_warn, - no_update_count_threshold_error); - EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); - - no_update_count = 49; - stat = check_measurement_updated( - measurement_type, no_update_count, no_update_count_threshold_warn, - no_update_count_threshold_error); - EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); - - no_update_count = 50; - stat = check_measurement_updated( - measurement_type, no_update_count, no_update_count_threshold_warn, - no_update_count_threshold_error); - EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); - - no_update_count = 249; - stat = check_measurement_updated( - measurement_type, no_update_count, no_update_count_threshold_warn, - no_update_count_threshold_error); - EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); - - no_update_count = 250; - stat = check_measurement_updated( - measurement_type, no_update_count, no_update_count_threshold_warn, - no_update_count_threshold_error); - EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); -} - -TEST(TestEkfDiagnostics, check_measurement_queue_size) -{ - diagnostic_msgs::msg::DiagnosticStatus stat; - - const std::string measurement_type = "pose"; // not effect for stat.level - - size_t queue_size = 0; // not effect for stat.level - stat = check_measurement_queue_size(measurement_type, queue_size); - EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); - - queue_size = 1; // not effect for stat.level - stat = check_measurement_queue_size(measurement_type, queue_size); - EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); -} - -TEST(TestEkfDiagnostics, check_measurement_delay_gate) -{ - diagnostic_msgs::msg::DiagnosticStatus stat; - - const std::string measurement_type = "pose"; // not effect for stat.level - const double delay_time = 0.1; // not effect for stat.level - const double delay_time_threshold = 1.0; // not effect for stat.level - - bool is_passed_delay_gate = true; - stat = check_measurement_delay_gate( - measurement_type, is_passed_delay_gate, delay_time, delay_time_threshold); - EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); - - is_passed_delay_gate = false; - stat = check_measurement_delay_gate( - measurement_type, is_passed_delay_gate, delay_time, delay_time_threshold); - EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); -} - -TEST(TestEkfDiagnostics, check_measurement_mahalanobis_gate) -{ - diagnostic_msgs::msg::DiagnosticStatus stat; - - const std::string measurement_type = "pose"; // not effect for stat.level - const double mahalanobis_distance = 0.1; // not effect for stat.level - const double mahalanobis_distance_threshold = 1.0; // not effect for stat.level - - bool is_passed_mahalanobis_gate = true; - stat = check_measurement_mahalanobis_gate( - measurement_type, is_passed_mahalanobis_gate, mahalanobis_distance, - mahalanobis_distance_threshold); - EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); - - is_passed_mahalanobis_gate = false; - stat = check_measurement_mahalanobis_gate( - measurement_type, is_passed_mahalanobis_gate, mahalanobis_distance, - mahalanobis_distance_threshold); - EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); -} - -TEST(TestLocalizationErrorMonitorDiagnostics, merge_diagnostic_status) -{ - diagnostic_msgs::msg::DiagnosticStatus merged_stat; - std::vector stat_array(2); - - stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK; - stat_array.at(0).message = "OK"; - stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; - stat_array.at(1).message = "OK"; - merged_stat = merge_diagnostic_status(stat_array); - EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); - EXPECT_EQ(merged_stat.message, "OK"); - - stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - stat_array.at(0).message = "WARN0"; - stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; - stat_array.at(1).message = "OK"; - merged_stat = merge_diagnostic_status(stat_array); - EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); - EXPECT_EQ(merged_stat.message, "WARN0"); - - stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK; - stat_array.at(0).message = "OK"; - stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - stat_array.at(1).message = "WARN1"; - merged_stat = merge_diagnostic_status(stat_array); - EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); - EXPECT_EQ(merged_stat.message, "WARN1"); - - stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - stat_array.at(0).message = "WARN0"; - stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - stat_array.at(1).message = "WARN1"; - merged_stat = merge_diagnostic_status(stat_array); - EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); - EXPECT_EQ(merged_stat.message, "WARN0; WARN1"); - - stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK; - stat_array.at(0).message = "OK"; - stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - stat_array.at(1).message = "ERROR1"; - merged_stat = merge_diagnostic_status(stat_array); - EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); - EXPECT_EQ(merged_stat.message, "ERROR1"); - - stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - stat_array.at(0).message = "WARN0"; - stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - stat_array.at(1).message = "ERROR1"; - merged_stat = merge_diagnostic_status(stat_array); - EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); - EXPECT_EQ(merged_stat.message, "WARN0; ERROR1"); - - stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - stat_array.at(0).message = "ERROR0"; - stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - stat_array.at(1).message = "ERROR1"; - merged_stat = merge_diagnostic_status(stat_array); - EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); - EXPECT_EQ(merged_stat.message, "ERROR0; ERROR1"); -} - -} // namespace autoware::ekf_localizer diff --git a/localization/autoware_ekf_localizer/test/test_ekf_localizer_launch.py b/localization/autoware_ekf_localizer/test/test_ekf_localizer_launch.py deleted file mode 100644 index 1db0863636b04..0000000000000 --- a/localization/autoware_ekf_localizer/test/test_ekf_localizer_launch.py +++ /dev/null @@ -1,170 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2023 TIER IV, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os -import time -import unittest - -from ament_index_python import get_package_share_directory -from geometry_msgs.msg import PoseWithCovarianceStamped -import launch -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import AnyLaunchDescriptionSource -from launch.logging import get_logger -import launch_testing -from nav_msgs.msg import Odometry -import pytest -import rclpy -from std_srvs.srv import SetBool - -logger = get_logger(__name__) - - -@pytest.mark.launch_test -def generate_test_description(): - test_ekf_localizer_launch_file = os.path.join( - get_package_share_directory("autoware_ekf_localizer"), - "launch", - "ekf_localizer.launch.xml", - ) - ekf_localizer = IncludeLaunchDescription( - AnyLaunchDescriptionSource(test_ekf_localizer_launch_file), - ) - - return launch.LaunchDescription( - [ - ekf_localizer, - # Start tests right away - no need to wait for anything - launch_testing.actions.ReadyToTest(), - ] - ) - - -class TestEKFLocalizer(unittest.TestCase): - @classmethod - def setUpClass(cls): - # Initialize the ROS context for the test node - rclpy.init() - - @classmethod - def tearDownClass(cls): - # Shutdown the ROS context - rclpy.shutdown() - - def setUp(self): - # Create a ROS node for tests - self.test_node = rclpy.create_node("test_node") - self.evaluation_time = 0.2 # 200ms - - def tearDown(self): - self.test_node.destroy_node() - - @staticmethod - def print_message(stat): - logger.debug("===========================") - logger.debug(stat) - - def test_node_link(self): - # Trigger ekf_localizer to activate the node - cli_trigger = self.test_node.create_client(SetBool, "/trigger_node") - while not cli_trigger.wait_for_service(timeout_sec=1.0): - continue - - request = SetBool.Request() - request.data = True - future = cli_trigger.call_async(request) - rclpy.spin_until_future_complete(self.test_node, future) - - if future.result() is not None: - self.test_node.get_logger().info("Result of bool service: %s" % future.result().message) - else: - self.test_node.get_logger().error( - "Exception while calling service: %r" % future.exception() - ) - - # Send initial pose - pub_init_pose = self.test_node.create_publisher( - PoseWithCovarianceStamped, "/initialpose3d", 10 - ) - init_pose = PoseWithCovarianceStamped() - init_pose.header.frame_id = "map" - init_pose.pose.pose.position.x = 0.0 - init_pose.pose.pose.position.y = 0.0 - init_pose.pose.pose.position.z = 0.0 - init_pose.pose.pose.orientation.x = 0.0 - init_pose.pose.pose.orientation.y = 0.0 - init_pose.pose.pose.orientation.z = 0.0 - init_pose.pose.pose.orientation.w = 1.0 - init_pose.pose.covariance = [ - 0.01, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.01, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.01, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.01, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.01, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.01, - ] - pub_init_pose.publish(init_pose) - - # Receive Odometry - msg_buffer = [] - self.test_node.create_subscription( - Odometry, "/ekf_odom", lambda msg: msg_buffer.append(msg), 10 - ) - - # Wait until the node publishes some topic - end_time = time.time() + self.evaluation_time - while time.time() < end_time: - rclpy.spin_once(self.test_node, timeout_sec=0.1) - - # Check if the EKF outputs some Odometry - self.assertTrue(len(msg_buffer) > 0) - - -@launch_testing.post_shutdown_test() -class TestProcessOutput(unittest.TestCase): - def test_exit_code(self, proc_info): - # Check that process exits with code 0: no error - launch_testing.asserts.assertExitCodes(proc_info) diff --git a/localization/autoware_ekf_localizer/test/test_ekf_localizer_mahalanobis.py b/localization/autoware_ekf_localizer/test/test_ekf_localizer_mahalanobis.py deleted file mode 100644 index 29afd4953520e..0000000000000 --- a/localization/autoware_ekf_localizer/test/test_ekf_localizer_mahalanobis.py +++ /dev/null @@ -1,229 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2023 TIER IV, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os -import time -import unittest - -from ament_index_python import get_package_share_directory -from geometry_msgs.msg import PoseWithCovarianceStamped -import launch -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import AnyLaunchDescriptionSource -from launch.logging import get_logger -import launch_testing -from nav_msgs.msg import Odometry -import pytest -import rclpy -from std_srvs.srv import SetBool - -logger = get_logger(__name__) - - -@pytest.mark.launch_test -def generate_test_description(): - test_ekf_localizer_launch_file = os.path.join( - get_package_share_directory("autoware_ekf_localizer"), - "launch", - "ekf_localizer.launch.xml", - ) - ekf_localizer = IncludeLaunchDescription( - AnyLaunchDescriptionSource(test_ekf_localizer_launch_file), - ) - - return launch.LaunchDescription( - [ - ekf_localizer, - # Start tests right away - no need to wait for anything - launch_testing.actions.ReadyToTest(), - ] - ) - - -class TestEKFLocalizer(unittest.TestCase): - @classmethod - def setUpClass(cls): - # Initialize the ROS context for the test node - rclpy.init() - - @classmethod - def tearDownClass(cls): - # Shutdown the ROS context - rclpy.shutdown() - - def setUp(self): - # Create a ROS node for tests - self.test_node = rclpy.create_node("test_node") - self.evaluation_time = 0.2 # 200ms - - def tearDown(self): - self.test_node.destroy_node() - - @staticmethod - def print_message(stat): - logger.debug("===========================") - logger.debug(stat) - - def test_node_link(self): - # Trigger ekf_localizer to activate the node - cli_trigger = self.test_node.create_client(SetBool, "/trigger_node") - while not cli_trigger.wait_for_service(timeout_sec=1.0): - continue - - request = SetBool.Request() - request.data = True - future = cli_trigger.call_async(request) - rclpy.spin_until_future_complete(self.test_node, future) - - if future.result() is not None: - self.test_node.get_logger().info("Result of bool service: %s" % future.result().message) - else: - self.test_node.get_logger().error( - "Exception while calling service: %r" % future.exception() - ) - - # Send initial pose - pub_init_pose = self.test_node.create_publisher( - PoseWithCovarianceStamped, "/initialpose3d", 10 - ) - init_pose = PoseWithCovarianceStamped() - init_pose.header.frame_id = "map" - init_pose.pose.pose.position.x = 0.0 - init_pose.pose.pose.position.y = 0.0 - init_pose.pose.pose.position.z = 0.0 - init_pose.pose.pose.orientation.x = 0.0 - init_pose.pose.pose.orientation.y = 0.0 - init_pose.pose.pose.orientation.z = 0.0 - init_pose.pose.pose.orientation.w = 1.0 - init_pose.pose.covariance = [ - 0.01, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.01, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.01, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.01, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.01, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.01, - ] - pub_init_pose.publish(init_pose) - rclpy.spin_once(self.test_node, timeout_sec=0.1) - - # Send pose that should be ignored by mahalanobis gate in ekf_localizer - pub_pose = self.test_node.create_publisher( - PoseWithCovarianceStamped, "/in_pose_with_covariance", 10 - ) - pose = PoseWithCovarianceStamped() - pose.header.frame_id = "map" - pose.pose.pose.position.x = 1000000.0 - pose.pose.pose.position.y = 1000000.0 - pose.pose.pose.position.z = 10.0 - pose.pose.pose.orientation.x = 0.0 - pose.pose.pose.orientation.y = 0.0 - pose.pose.pose.orientation.z = 0.0 - pose.pose.pose.orientation.w = 1.0 - pose.pose.covariance = [ - 0.01, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.01, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.01, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.01, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.01, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.01, - ] - pub_pose.publish(pose) - - # Receive Odometry - msg_buffer = [] - self.test_node.create_subscription( - Odometry, "/ekf_odom", lambda msg: msg_buffer.append(msg), 10 - ) - - # Wait until the node publishes some topic - end_time = time.time() + self.evaluation_time - while time.time() < end_time: - rclpy.spin_once(self.test_node, timeout_sec=0.1) - - # Check if the EKF outputs some Odometry - self.assertTrue(len(msg_buffer) > 0) - - # Assert msg to be at the origin - self.assertEqual(msg_buffer[-1].pose.pose.position.x, 0.0) - self.assertEqual(msg_buffer[-1].pose.pose.position.y, 0.0) - self.assertEqual(msg_buffer[-1].pose.pose.position.z, 0.0) - - -@launch_testing.post_shutdown_test() -class TestProcessOutput(unittest.TestCase): - def test_exit_code(self, proc_info): - # Check that process exits with code 0: no error - launch_testing.asserts.assertExitCodes(proc_info) diff --git a/localization/autoware_ekf_localizer/test/test_mahalanobis.cpp b/localization/autoware_ekf_localizer/test/test_mahalanobis.cpp deleted file mode 100644 index 3f867470a3c63..0000000000000 --- a/localization/autoware_ekf_localizer/test/test_mahalanobis.cpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/ekf_localizer/mahalanobis.hpp" - -#include - -namespace autoware::ekf_localizer -{ - -constexpr double tolerance = 1e-8; - -TEST(squared_mahalanobis, SmokeTest) -{ - { - Eigen::Vector2d x(0, 1); - Eigen::Vector2d y(3, 2); - Eigen::Matrix2d c; - c << 10, 0, 0, 10; - - EXPECT_NEAR(squared_mahalanobis(x, y, c), 1.0, tolerance); - } - - { - Eigen::Vector2d x(4, 1); - Eigen::Vector2d y(1, 5); - Eigen::Matrix2d c; - c << 5, 0, 0, 5; - - EXPECT_NEAR(squared_mahalanobis(x, y, c), 5.0, tolerance); - } -} - -TEST(mahalanobis, SmokeTest) -{ - { - Eigen::Vector2d x(0, 1); - Eigen::Vector2d y(3, 2); - Eigen::Matrix2d c; - c << 10, 0, 0, 10; - - EXPECT_NEAR(mahalanobis(x, y, c), 1.0, tolerance); - } - - { - Eigen::Vector2d x(4, 1); - Eigen::Vector2d y(1, 5); - Eigen::Matrix2d c; - c << 5, 0, 0, 5; - - EXPECT_NEAR(mahalanobis(x, y, c), std::sqrt(5.0), tolerance); - } -} - -} // namespace autoware::ekf_localizer diff --git a/localization/autoware_ekf_localizer/test/test_measurement.cpp b/localization/autoware_ekf_localizer/test/test_measurement.cpp deleted file mode 100644 index 99dcc1744c33d..0000000000000 --- a/localization/autoware_ekf_localizer/test/test_measurement.cpp +++ /dev/null @@ -1,86 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/ekf_localizer/measurement.hpp" - -#include - -namespace autoware::ekf_localizer -{ - -TEST(Measurement, pose_measurement_matrix) -{ - const Eigen::Matrix m = pose_measurement_matrix(); - Eigen::Matrix expected; - expected << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0; - EXPECT_EQ((m - expected).norm(), 0); -} - -TEST(Measurement, twist_measurement_matrix) -{ - const Eigen::Matrix m = twist_measurement_matrix(); - Eigen::Matrix expected; - expected << 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1; - EXPECT_EQ((m - expected).norm(), 0); -} - -TEST(Measurement, pose_measurement_covariance) -{ - { - const std::array covariance = {1, 2, 0, 0, 0, 3, 4, 5, 0, 0, 0, 6, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 7, 8, 0, 0, 0, 9}; - - const Eigen::Matrix3d m = pose_measurement_covariance(covariance, 2); - - Eigen::Matrix3d expected; - expected << 2, 4, 6, 8, 10, 12, 14, 16, 18; - - EXPECT_EQ((m - expected).norm(), 0.); - } - - { - // Make sure that other elements are not changed - std::array covariance{}; - covariance.fill(0); - const Eigen::Matrix3d m = pose_measurement_covariance(covariance, 2.); - EXPECT_EQ(m.norm(), 0); - } -} - -TEST(Measurement, twist_measurement_covariance) -{ - { - const std::array covariance = {1, 0, 0, 0, 0, 2, 0, 0, 0, 0, 0, 6, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 4}; - - const Eigen::Matrix2d m = twist_measurement_covariance(covariance, 2); - - Eigen::Matrix2d expected; - expected << 2, 4, 6, 8; - - EXPECT_EQ((m - expected).norm(), 0.); - } - - { - // Make sure that other elements are not changed - std::array covariance{}; - covariance.fill(0); - const Eigen::Matrix2d m = twist_measurement_covariance(covariance, 2.); - EXPECT_EQ(m.norm(), 0); - } -} - -} // namespace autoware::ekf_localizer diff --git a/localization/autoware_ekf_localizer/test/test_numeric.cpp b/localization/autoware_ekf_localizer/test/test_numeric.cpp deleted file mode 100644 index 080ce01f31770..0000000000000 --- a/localization/autoware_ekf_localizer/test/test_numeric.cpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/ekf_localizer/numeric.hpp" - -#include - -#include - -#include - -namespace autoware::ekf_localizer -{ - -TEST(Numeric, has_nan) -{ - const Eigen::VectorXd empty(0); - const double inf = std::numeric_limits::infinity(); - const double nan = std::nan(""); - - EXPECT_FALSE(has_nan(empty)); - EXPECT_FALSE(has_nan(Eigen::Vector3d(0., 0., 1.))); - EXPECT_FALSE(has_nan(Eigen::Vector3d(1e16, 0., 1.))); - EXPECT_FALSE(has_nan(Eigen::Vector3d(0., 1., inf))); - - EXPECT_TRUE(has_nan(Eigen::Vector3d(nan, 1., 0.))); -} - -TEST(Numeric, has_inf) -{ - const Eigen::VectorXd empty(0); - const double inf = std::numeric_limits::infinity(); - const double nan = std::nan(""); - - EXPECT_FALSE(has_inf(empty)); - EXPECT_FALSE(has_inf(Eigen::Vector3d(0., 0., 1.))); - EXPECT_FALSE(has_inf(Eigen::Vector3d(1e16, 0., 1.))); - EXPECT_FALSE(has_inf(Eigen::Vector3d(nan, 1., 0.))); - - EXPECT_TRUE(has_inf(Eigen::Vector3d(0., 1., inf))); -} - -} // namespace autoware::ekf_localizer diff --git a/localization/autoware_ekf_localizer/test/test_state_transition.cpp b/localization/autoware_ekf_localizer/test/test_state_transition.cpp deleted file mode 100644 index 667c6b5ef2df8..0000000000000 --- a/localization/autoware_ekf_localizer/test/test_state_transition.cpp +++ /dev/null @@ -1,101 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#define _USE_MATH_DEFINES -#include "autoware/ekf_localizer/state_index.hpp" -#include "autoware/ekf_localizer/state_transition.hpp" - -#include - -#include - -namespace autoware::ekf_localizer -{ - -TEST(StateTransition, normalize_yaw) -{ - const double tolerance = 1e-6; - EXPECT_NEAR(normalize_yaw(M_PI * 4 / 3), -M_PI * 2 / 3, tolerance); - EXPECT_NEAR(normalize_yaw(-M_PI * 4 / 3), M_PI * 2 / 3, tolerance); - EXPECT_NEAR(normalize_yaw(M_PI * 9 / 2), M_PI * 1 / 2, tolerance); - EXPECT_NEAR(normalize_yaw(M_PI * 4), M_PI * 0, tolerance); -} - -TEST(predict_next_state, predict_next_state) -{ - // This function is the definition of state transition so we just check - // if the calculation is done according to the formula - Vector6d x_curr; - x_curr(0) = 2.; - x_curr(1) = 3.; - x_curr(2) = M_PI / 2.; - x_curr(3) = M_PI / 4.; - x_curr(4) = 10.; - x_curr(5) = 2. * M_PI / 3.; - - const double dt = 0.5; - - const Vector6d x_next = predict_next_state(x_curr, dt); - - const double tolerance = 1e-10; - EXPECT_NEAR(x_next(0), 2. + 10. * std::cos(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); - EXPECT_NEAR(x_next(1), 3. + 10. * std::sin(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); - EXPECT_NEAR(x_next(2), normalize_yaw(M_PI / 2. + M_PI / 3.), tolerance); - EXPECT_NEAR(x_next(3), x_curr(3), tolerance); - EXPECT_NEAR(x_next(4), x_curr(4), tolerance); - EXPECT_NEAR(x_next(5), x_curr(5), tolerance); -} - -TEST(create_state_transition_matrix, NumericalApproximation) -{ - // The transition matrix A = df / dx - // We check if df = A * dx approximates f(x + dx) - f(x) - - { - // check around x = 0 - const double dt = 0.1; - const Vector6d dx = 0.1 * Vector6d::Ones(); - const Vector6d x = Vector6d::Zero(); - - const Matrix6d a = create_state_transition_matrix(x, dt); - const Vector6d df = predict_next_state(x + dx, dt) - predict_next_state(x, dt); - - EXPECT_LT((df - a * dx).norm(), 2e-3); - } - - { - // check around random x - const double dt = 0.1; - const Vector6d dx = 0.1 * Vector6d::Ones(); - const Vector6d x = (Vector6d() << 0.1, 0.2, 0.1, 0.4, 0.1, 0.3).finished(); - - const Matrix6d a = create_state_transition_matrix(x, dt); - const Vector6d df = predict_next_state(x + dx, dt) - predict_next_state(x, dt); - - EXPECT_LT((df - a * dx).norm(), 5e-3); - } -} - -TEST(process_noise_covariance, process_noise_covariance) -{ - const Matrix6d q = process_noise_covariance(1., 2., 3.); - EXPECT_EQ(q(2, 2), 1.); // for yaw - EXPECT_EQ(q(4, 4), 2.); // for vx - EXPECT_EQ(q(5, 5), 3.); // for wz - - // Make sure other elements are zero - EXPECT_EQ(process_noise_covariance(0, 0, 0).norm(), 0.); -} - -} // namespace autoware::ekf_localizer diff --git a/localization/autoware_ekf_localizer/test/test_string.cpp b/localization/autoware_ekf_localizer/test/test_string.cpp deleted file mode 100644 index b1fae1f88b74b..0000000000000 --- a/localization/autoware_ekf_localizer/test/test_string.cpp +++ /dev/null @@ -1,31 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/ekf_localizer/string.hpp" - -#include - -namespace autoware::ekf_localizer -{ - -TEST(erase_leading_slash, SmokeTest) -{ - EXPECT_EQ(erase_leading_slash("/topic"), "topic"); - EXPECT_EQ(erase_leading_slash("topic"), "topic"); // do nothing - - EXPECT_EQ(erase_leading_slash(""), ""); - EXPECT_EQ(erase_leading_slash("/"), ""); -} - -} // namespace autoware::ekf_localizer diff --git a/localization/autoware_ekf_localizer/test/test_warning_message.cpp b/localization/autoware_ekf_localizer/test/test_warning_message.cpp deleted file mode 100644 index 791e86c050352..0000000000000 --- a/localization/autoware_ekf_localizer/test/test_warning_message.cpp +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/ekf_localizer/warning_message.hpp" - -#include - -#include - -namespace autoware::ekf_localizer -{ - -TEST(pose_delay_step_warning_message, SmokeTest) -{ - EXPECT_STREQ( - pose_delay_step_warning_message(6.0, 4.0).c_str(), - "Pose delay exceeds the compensation limit, ignored. " - "delay: 6.000[s], limit: 4.000[s]"); -} - -TEST(twist_delay_step_warning_message, SmokeTest) -{ - EXPECT_STREQ( - twist_delay_step_warning_message(10.0, 6.0).c_str(), - "Twist delay exceeds the compensation limit, ignored. " - "delay: 10.000[s], limit: 6.000[s]"); -} - -TEST(pose_delay_time_warning_message, SmokeTest) -{ - EXPECT_STREQ( - pose_delay_time_warning_message(-1.0).c_str(), - "Pose time stamp is inappropriate, set delay to 0[s]. delay = -1.000"); - EXPECT_STREQ( - pose_delay_time_warning_message(-0.4).c_str(), - "Pose time stamp is inappropriate, set delay to 0[s]. delay = -0.400"); -} - -TEST(twist_delay_time_warning_message, SmokeTest) -{ - EXPECT_STREQ( - twist_delay_time_warning_message(-1.0).c_str(), - "Twist time stamp is inappropriate, set delay to 0[s]. delay = -1.000"); - EXPECT_STREQ( - twist_delay_time_warning_message(-0.4).c_str(), - "Twist time stamp is inappropriate, set delay to 0[s]. delay = -0.400"); -} - -TEST(mahalanobis_warning_message, SmokeTest) -{ - EXPECT_STREQ( - mahalanobis_warning_message(1.0, 0.5).c_str(), - "The Mahalanobis distance 1.0000 is over the limit 0.5000."); -} - -} // namespace autoware::ekf_localizer diff --git a/localization/autoware_pose_covariance_modifier/README.md b/localization/autoware_pose_covariance_modifier/README.md index 201fb8bb37103..53cddbfa981c2 100644 --- a/localization/autoware_pose_covariance_modifier/README.md +++ b/localization/autoware_pose_covariance_modifier/README.md @@ -118,7 +118,7 @@ the [pose_twist_estimator.launch.xml](../../launch/tier4_localization_launch/lau ### Without this condition (default) - The output of the [ndt_scan_matcher](../../localization/autoware_ndt_scan_matcher) is directly sent - to [ekf_localizer](../../localization/autoware_ekf_localizer). + to [ekf_localizer](https://github.com/autowarefoundation/autoware.core/tree/main/localization/autoware_ekf_localizer). - It has a preset covariance value. - **topic name:** `/localization/pose_estimator/pose_with_covariance` - The GNSS pose does not enter the ekf_localizer. @@ -130,7 +130,7 @@ the [pose_twist_estimator.launch.xml](../../launch/tier4_localization_launch/lau - **from:** `/localization/pose_estimator/pose_with_covariance`. - **to:** `/localization/pose_estimator/ndt_scan_matcher/pose_with_covariance`. - The `ndt_scan_matcher` output enters the `autoware_pose_covariance_modifier`. -- The output of this package goes to [ekf_localizer](../../localization/autoware_ekf_localizer) with: +- The output of this package goes to [ekf_localizer](https://github.com/autowarefoundation/autoware.core/tree/main/localization/autoware_ekf_localizer) with: - **topic name:** `/localization/pose_estimator/pose_with_covariance`. ## Node