diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt new file mode 100644 index 0000000000000..b50565589cc1b --- /dev/null +++ b/localization/ekf_localizer/CMakeLists.txt @@ -0,0 +1,49 @@ +cmake_minimum_required(VERSION 3.5) +project(ekf_localizer) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(Eigen3 REQUIRED) + +ament_auto_add_executable(ekf_localizer + src/ekf_localizer_node.cpp + src/ekf_localizer.cpp +) +ament_target_dependencies(ekf_localizer kalman_filter) + +# if(BUILD_TESTING) +# find_package(ament_cmake_gtest REQUIRED) +# ament_add_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() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md new file mode 100644 index 0000000000000..e729845a4ba7c --- /dev/null +++ b/localization/ekf_localizer/README.md @@ -0,0 +1,192 @@ +# 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 robot such as autonomous driving system. + +## Flowchart + +The overall flowchart of the 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 delay. This is important especially for high speed moving robot, such as autonomous driving vehicle. (see 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 following figure). + +

+ +

+ +

+ +

+ +## Launch + +The `ekf_localizer` starts with the default parameters with the following command. + +```sh +roslaunch ekf_localizer ekf_localizer.launch +``` + +The parameters and input topic names can be set in the `ekf_localizer.launch` file. + +## Node + +### Subscribed Topics + +- measured_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped) + + Input pose source with measurement covariance matrix. + +- measured_twist_with_covariance (geometry_msgs/PoseWithCovarianceStamped) + + Input twist source with measurement covariance matrix. + +- initialpose (geometry_msgs/PoseWithCovarianceStamped) + + Initial pose for EKF. The estimated pose is initialized with zeros at start. It is initialized with this message whenever published. + +### Published Topics + +- ekf_odom (nav_msgs/Odometry) + + Estimated odometry. + +- ekf_pose (geometry_msgs/PoseStamped) + + Estimated pose. + +- ekf_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped) + + Estimated pose with covariance. + +- ekf_pose_with_covariance (geometry_msgs/PoseStamped) + + Estimated pose without yawbias effect. + +- ekf_pose_with_covariance_without_yawbias (geometry_msgs/PoseWithCovarianceStamped) + + Estimated pose with covariance without yawbias effect. + +- ekf_twist (geometry_msgs/TwistStamped) + + Estimated twist. + +- ekf_twist_with_covariance (geometry_msgs/TwistWithCovarianceStamped) + + Estimated twist with covariance. + +### 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 constant interval (`predict_frequency [Hz]`). The prediction equation is described at the end of this page. + +### Measurement Update + +Before 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 + +| Name | Type | Description | Default value | +| :------------------------- | :----- | :---------------------------------------------------------------------------------------- | :------------ | +| show_debug_info | bool | Flag to display debug info | false | +| predict_frequency | double | Frequency for filtering and publishing [Hz] | 50.0 | +| tf_rate | double | Frequency for tf broadcasting [Hz] | 10.0 | +| extend_state_step | int | Max delay step which can be dealt with in EKF. Large number increases computational cost. | 50 | +| enable_yaw_bias_estimation | bool | Flag to enable yaw bias estimation | true | + +### For pose measurement + +| Name | Type | Description | Default value | +| :---------------------------- | :----- | :---------------------------------------------------------------- | :------------ | +| pose_additional_delay | double | Additional delay time for pose measurement [s] | 0.0 | +| pose_measure_uncertainty_time | double | Measured time uncertainty used for covariance calculation [s] | 0.01 | +| pose_rate | double | Approximated input pose rate used for covariance calculation [Hz] | 10.0 | +| pose_gate_dist | double | Limit of Mahalanobis distance used for outliers detection | 10000.0 | + +### For twist measurement + +| Name | Type | Description | Default value | +| :--------------------- | :----- | :----------------------------------------------------------------- | :------------ | +| twist_additional_delay | double | Additional delay time for twist [s] | 0.0 | +| twist_rate | double | Approximated input twist rate used for covariance calculation [Hz] | 10.0 | +| twist_gate_dist | double | Limit of Mahalanobis distance used for outliers detection | 10000.0 | + +### For process noise + +| Name | Type | Description | Default value | +| :--------------------- | :----- | :--------------------------------------------------------------------------------------------------------------- | :------------ | +| proc_stddev_vx_c | double | Standard deviation of process noise in time differentiation expression of linear velocity x, noise for d_vx = 0 | 2.0 | +| proc_stddev_wz_c | double | Standard deviation of process noise in time differentiation expression of angular velocity z, noise for d_wz = 0 | 0.2 | +| proc_stddev_yaw_c | double | Standard deviation of process noise in time differentiation expression of yaw, noise for d_yaw = omega | 0.005 | +| proc_stddev_yaw_bias_c | double | Standard deviation of process noise in time differentiation expression of yaw_bias, noise for d_yaw_bias = 0 | 0.001 | + +note: process noise for position x & y are calculated automatically from nonlinear dynamics. + +## How to turn 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 appropriate time due to timer synchronization problem, use `twist_additional_delay` and `pose_additional_delay` to correct the time. +- Check the relation between measurement pose and twist is appropriate (whether the derivative of pose has 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. Set sensor parameters + +Set sensor-rate and standard-deviation from the basic information of the sensor. The `pose_measure_uncertainty_time` is for uncertainty of the header timestamp data. + +- `pose_measure_uncertainty_time` +- `pose_rate` +- `twist_rate` + +### 2. Set 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. 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 estimate 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. + +## Kalman Filter Model + +### kinematics model in update function + + + +where `b_k` is the yaw-bias. + +### time delay model + +The measurement time delay is handled by an augmented states [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 + +

+ +

+ +## reference + +[1] Anderson, B. D. O., & Moore, J. B. (1979). Optimal filtering. Englewood Cliffs, NJ: Prentice-Hall. diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp new file mode 100644 index 0000000000000..ca1de186855bb --- /dev/null +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -0,0 +1,234 @@ +// 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 EKF_LOCALIZER__EKF_LOCALIZER_HPP_ +#define EKF_LOCALIZER__EKF_LOCALIZER_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +class EKFLocalizer : public rclcpp::Node +{ +public: + EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & options); + +private: + //!< @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 debug info publisher + rclcpp::Publisher::SharedPtr pub_debug_; + //!< @brief debug measurement pose publisher + rclcpp::Publisher::SharedPtr pub_measured_pose_; + //!< @brief ekf estimated yaw bias publisher + rclcpp::Publisher::SharedPtr pub_yaw_bias_; + //!< @brief ekf estimated yaw bias publisher + rclcpp::Publisher::SharedPtr pub_pose_no_yawbias_; + //!< @brief ekf estimated yaw bias publisher + rclcpp::Publisher::SharedPtr + pub_pose_cov_no_yawbias_; + //!< @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 timer to send transform + rclcpp::TimerBase::SharedPtr timer_tf_; + //!< @brief tf broadcaster + std::shared_ptr tf_br_; + //!< @brief extended kalman filter instance. + TimeDelayKalmanFilter ekf_; + + /* parameters */ + bool show_debug_info_; + double ekf_rate_; //!< @brief EKF predict rate + double ekf_dt_; //!< @brief = 1 / ekf_rate_ + double tf_rate_; //!< @brief tf publish rate + bool enable_yaw_bias_estimation_; //!< @brief for LiDAR mount error. + //!< if true,publish /estimate_yaw_bias + std::string pose_frame_id_; + + int dim_x_; //!< @brief dimension of EKF state + int extend_state_step_; //!< @brief for time delay compensation + int dim_x_ex_; //!< @brief dimension of extended EKF state (dim_x_ * extended_state_step) + + /* Pose */ + double pose_additional_delay_; //!< @brief compensated pose delay time = + //!< (pose.header.stamp - now) + additional_delay [s] + double pose_measure_uncertainty_time_; //!< @brief added for measurement covariance + double pose_rate_; //!< @brief pose rate [s], used for covariance calculation + //!< @brief the mahalanobis distance threshold to ignore pose measurement + double pose_gate_dist_; + + /* twist */ + double twist_additional_delay_; //!< @brief compensated delay = (twist.header.stamp - now) + //!< + additional_delay [s] + double twist_rate_; //!< @brief rate [s], used for covariance calculation + //!< @brief measurement is ignored if the mahalanobis distance is larger than this value. + double twist_gate_dist_; + + /* process noise variance for discrete model */ + double proc_cov_yaw_d_; //!< @brief discrete yaw process noise + double proc_cov_yaw_bias_d_; //!< @brief discrete yaw bias process noise + double proc_cov_vx_d_; //!< @brief discrete process noise in d_vx=0 + double proc_cov_wz_d_; //!< @brief discrete process noise in d_wz=0 + + enum IDX { + X = 0, + Y = 1, + YAW = 2, + YAWB = 3, + VX = 4, + WZ = 5, + }; + + /* for model prediction */ + geometry_msgs::msg::TwistStamped::SharedPtr + current_twist_ptr_; //!< @brief current measured twist + geometry_msgs::msg::PoseStamped::SharedPtr current_pose_ptr_; //!< @brief current measured pose + geometry_msgs::msg::PoseStamped current_ekf_pose_; //!< @brief current estimated pose + geometry_msgs::msg::PoseStamped + current_ekf_pose_no_yawbias_; //!< @brief current estimated pose w/o yaw bias + geometry_msgs::msg::TwistStamped current_ekf_twist_; //!< @brief current estimated twist + std::array current_pose_covariance_; + std::array current_twist_covariance_; + + /** + * @brief computes update & prediction of EKF for each ekf_dt_[s] time + */ + void timerCallback(); + + /** + * @brief publish tf for tf_rate [Hz] + */ + void timerTFCallback(); + + /** + * @brief set poseWithCovariance measurement + */ + void callbackPoseWithCovariance(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + + /** + * @brief set twistWithCovariance measurement + */ + void callbackTwistWithCovariance(geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + + /** + * @brief set initial_pose to current EKF pose + */ + void callbackInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + + /** + * @brief initialization of EKF + */ + void initEKF(); + + /** + * @brief compute EKF prediction + */ + void predictKinematicsModel(); + + /** + * @brief compute EKF update with pose measurement + * @param pose measurement value + */ + void measurementUpdatePose(const geometry_msgs::msg::PoseStamped & pose); + + /** + * @brief compute EKF update with pose measurement + * @param twist measurement value + */ + void measurementUpdateTwist(const geometry_msgs::msg::TwistStamped & twist); + + /** + * @brief check whether a measurement value falls within the mahalanobis distance threshold + * @param dist_max mahalanobis distance threshold + * @param estimated current estimated state + * @param measured measured state + * @param estimated_cov current estimation covariance + * @return whether it falls within the mahalanobis distance threshold + */ + bool mahalanobisGate( + const double & dist_max, const Eigen::MatrixXd & estimated, const Eigen::MatrixXd & measured, + const Eigen::MatrixXd & estimated_cov) const; + + /** + * @brief get transform from frame_id + */ + bool getTransformFromTF( + std::string parent_frame, std::string child_frame, + geometry_msgs::msg::TransformStamped & transform); + + /** + * @brief normalize yaw angle + * @param yaw yaw angle + * @return normalized yaw + */ + double normalizeYaw(const double & yaw) const; + + /** + * @brief set current EKF estimation result to current_ekf_pose_ & current_ekf_twist_ + */ + void setCurrentResult(); + + /** + * @brief publish current EKF estimation result + */ + void publishEstimateResult(); + + /** + * @brief for debug + */ + void showCurrentX(); + + autoware_utils::StopWatch stop_watch_; + + friend class EKFLocalizerTestSuite; // for test code +}; +#endif // EKF_LOCALIZER__EKF_LOCALIZER_HPP_ diff --git a/localization/ekf_localizer/launch/ekf_localizer.launch.xml b/localization/ekf_localizer/launch/ekf_localizer.launch.xml new file mode 100644 index 0000000000000..a48118bb20f6f --- /dev/null +++ b/localization/ekf_localizer/launch/ekf_localizer.launch.xml @@ -0,0 +1,80 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/ekf_localizer/media/delay_model_eq.png b/localization/ekf_localizer/media/delay_model_eq.png new file mode 100644 index 0000000000000..41777d756b974 Binary files /dev/null and b/localization/ekf_localizer/media/delay_model_eq.png differ diff --git a/localization/ekf_localizer/media/ekf_autoware_res.png b/localization/ekf_localizer/media/ekf_autoware_res.png new file mode 100644 index 0000000000000..c771e3620b161 Binary files /dev/null and b/localization/ekf_localizer/media/ekf_autoware_res.png differ diff --git a/localization/ekf_localizer/media/ekf_delay_comp.png b/localization/ekf_localizer/media/ekf_delay_comp.png new file mode 100644 index 0000000000000..54d934caecc92 Binary files /dev/null and b/localization/ekf_localizer/media/ekf_delay_comp.png differ diff --git a/localization/ekf_localizer/media/ekf_dynamics.png b/localization/ekf_localizer/media/ekf_dynamics.png new file mode 100644 index 0000000000000..63c81261a718e Binary files /dev/null and b/localization/ekf_localizer/media/ekf_dynamics.png differ diff --git a/localization/ekf_localizer/media/ekf_flowchart.png b/localization/ekf_localizer/media/ekf_flowchart.png new file mode 100644 index 0000000000000..0a80cb06b85f0 Binary files /dev/null and b/localization/ekf_localizer/media/ekf_flowchart.png differ diff --git a/localization/ekf_localizer/media/ekf_smooth_update.png b/localization/ekf_localizer/media/ekf_smooth_update.png new file mode 100644 index 0000000000000..7ac26fc604c6f Binary files /dev/null and b/localization/ekf_localizer/media/ekf_smooth_update.png differ diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml new file mode 100644 index 0000000000000..de9a8ae3ac48f --- /dev/null +++ b/localization/ekf_localizer/package.xml @@ -0,0 +1,33 @@ + + + + ekf_localizer + 0.1.0 + The ekf_localizer package + horibe + Apache License 2.0 + + ament_cmake_auto + eigen3_cmake_module + eigen + + autoware_debug_msgs + autoware_utils + geometry_msgs + kalman_filter + nav_msgs + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_ros + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp new file mode 100644 index 0000000000000..4aff52cf9bcd9 --- /dev/null +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -0,0 +1,718 @@ +// 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 "ekf_localizer/ekf_localizer.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +// clang-format off +#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl +#define DEBUG_INFO(...) {if (show_debug_info_) {RCLCPP_INFO(__VA_ARGS__);}} +#define DEBUG_PRINT_MAT(X) {if (show_debug_info_) {std::cout << #X << ": " << X << std::endl;}} + +// clang-format on + +using std::placeholders::_1; + +EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & node_options) +: rclcpp::Node(node_name, node_options), dim_x_(6 /* x, y, yaw, yaw_bias, vx, wz */) +{ + show_debug_info_ = declare_parameter("show_debug_info", false); + ekf_rate_ = declare_parameter("predict_frequency", 50.0); + ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1); + tf_rate_ = declare_parameter("tf_rate", 10.0); + enable_yaw_bias_estimation_ = declare_parameter("enable_yaw_bias_estimation", true); + extend_state_step_ = declare_parameter("extend_state_step", 50); + pose_frame_id_ = declare_parameter("pose_frame_id", std::string("map")); + + /* pose measurement */ + pose_additional_delay_ = declare_parameter("pose_additional_delay", 0.0); + pose_measure_uncertainty_time_ = declare_parameter("pose_measure_uncertainty_time", 0.01); + pose_rate_ = declare_parameter("pose_rate", 10.0); // used for covariance calculation + pose_gate_dist_ = declare_parameter("pose_gate_dist", 10000.0); // Mahalanobis limit + + /* twist measurement */ + twist_additional_delay_ = declare_parameter("twist_additional_delay", 0.0); + twist_rate_ = declare_parameter("twist_rate", 10.0); // used for covariance calculation + twist_gate_dist_ = declare_parameter("twist_gate_dist", 10000.0); // Mahalanobis limit + + /* process noise */ + double proc_stddev_yaw_c, proc_stddev_yaw_bias_c, proc_stddev_vx_c, proc_stddev_wz_c; + proc_stddev_yaw_c = declare_parameter("proc_stddev_yaw_c", 0.005); + proc_stddev_yaw_bias_c = declare_parameter("proc_stddev_yaw_bias_c", 0.001); + proc_stddev_vx_c = declare_parameter("proc_stddev_vx_c", 5.0); + proc_stddev_wz_c = declare_parameter("proc_stddev_wz_c", 1.0); + if (!enable_yaw_bias_estimation_) { + proc_stddev_yaw_bias_c = 0.0; + } + + /* convert to continuous to discrete */ + proc_cov_vx_d_ = std::pow(proc_stddev_vx_c * ekf_dt_, 2.0); + proc_cov_wz_d_ = std::pow(proc_stddev_wz_c * ekf_dt_, 2.0); + proc_cov_yaw_d_ = std::pow(proc_stddev_yaw_c * ekf_dt_, 2.0); + proc_cov_yaw_bias_d_ = std::pow(proc_stddev_yaw_bias_c * ekf_dt_, 2.0); + + /* initialize ros system */ + auto timer_control_callback = std::bind(&EKFLocalizer::timerCallback, this); + auto period_control = + std::chrono::duration_cast(std::chrono::duration(ekf_dt_)); + timer_control_ = std::make_shared>( + get_clock(), period_control, std::move(timer_control_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_control_, nullptr); + + auto timer_tf_callback = std::bind(&EKFLocalizer::timerTFCallback, this); + auto period_tf = std::chrono::duration_cast( + std::chrono::duration(1.0 / tf_rate_)); + timer_tf_ = std::make_shared>( + get_clock(), period_tf, std::move(timer_tf_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_tf_, nullptr); + + 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_pose_no_yawbias_ = + create_publisher("ekf_pose_without_yawbias", 1); + pub_pose_cov_no_yawbias_ = create_publisher( + "ekf_pose_with_covariance_without_yawbias", 1); + sub_initialpose_ = create_subscription( + "initialpose", 1, std::bind(&EKFLocalizer::callbackInitialPose, this, _1)); + sub_pose_with_cov_ = create_subscription( + "in_pose_with_covariance", 1, std::bind(&EKFLocalizer::callbackPoseWithCovariance, this, _1)); + sub_twist_with_cov_ = create_subscription( + "in_twist_with_covariance", 1, std::bind(&EKFLocalizer::callbackTwistWithCovariance, this, _1)); + + dim_x_ex_ = dim_x_ * extend_state_step_; + + tf_br_ = std::make_shared( + std::shared_ptr(this, [](auto) {})); + + initEKF(); + + /* debug */ + pub_debug_ = create_publisher("debug", 1); + pub_measured_pose_ = create_publisher("debug/measured_pose", 1); +} + +/* + * timerCallback + */ +void EKFLocalizer::timerCallback() +{ + DEBUG_INFO(get_logger(), "========================= timer called ========================="); + + /* predict model in EKF */ + stop_watch_.tic(); + DEBUG_INFO(get_logger(), "------------------------- start prediction -------------------------"); + predictKinematicsModel(); + DEBUG_INFO(get_logger(), "[EKF] predictKinematicsModel calc time = %f [ms]", stop_watch_.toc()); + DEBUG_INFO(get_logger(), "------------------------- end prediction -------------------------\n"); + + /* pose measurement update */ + if (current_pose_ptr_ != nullptr) { + DEBUG_INFO(get_logger(), "------------------------- start Pose -------------------------"); + stop_watch_.tic(); + measurementUpdatePose(*current_pose_ptr_); + DEBUG_INFO(get_logger(), "[EKF] measurementUpdatePose calc time = %f [ms]", stop_watch_.toc()); + DEBUG_INFO(get_logger(), "------------------------- end Pose -------------------------\n"); + } + + /* twist measurement update */ + if (current_twist_ptr_ != nullptr) { + DEBUG_INFO(get_logger(), "------------------------- start twist -------------------------"); + stop_watch_.tic(); + measurementUpdateTwist(*current_twist_ptr_); + DEBUG_INFO(get_logger(), "[EKF] measurementUpdateTwist calc time = %f [ms]", stop_watch_.toc()); + DEBUG_INFO(get_logger(), "------------------------- end twist -------------------------\n"); + } + + /* set current pose, twist */ + setCurrentResult(); + + /* publish ekf result */ + publishEstimateResult(); +} + +void EKFLocalizer::showCurrentX() +{ + if (show_debug_info_) { + Eigen::MatrixXd X(dim_x_, 1); + ekf_.getLatestX(X); + DEBUG_PRINT_MAT(X.transpose()); + } +} + +/* + * setCurrentResult + */ +void EKFLocalizer::setCurrentResult() +{ + current_ekf_pose_.header.frame_id = pose_frame_id_; + current_ekf_pose_.header.stamp = this->now(); + current_ekf_pose_.pose.position.x = ekf_.getXelement(IDX::X); + current_ekf_pose_.pose.position.y = ekf_.getXelement(IDX::Y); + + tf2::Quaternion q_tf; + double roll = 0.0, pitch = 0.0; + if (current_pose_ptr_ != nullptr) { + current_ekf_pose_.pose.position.z = current_pose_ptr_->pose.position.z; + tf2::fromMsg(current_pose_ptr_->pose.orientation, q_tf); /* use Pose pitch and roll */ + double yaw_tmp; + tf2::Matrix3x3(q_tf).getRPY(roll, pitch, yaw_tmp); + } + double yaw = ekf_.getXelement(IDX::YAW) + ekf_.getXelement(IDX::YAWB); + current_ekf_pose_.pose.orientation = autoware_utils::createQuaternionFromRPY(roll, pitch, yaw); + + current_ekf_pose_no_yawbias_ = current_ekf_pose_; + current_ekf_pose_no_yawbias_.pose.orientation = + autoware_utils::createQuaternionFromRPY(roll, pitch, ekf_.getXelement(IDX::YAW)); + + current_ekf_twist_.header.frame_id = "base_link"; + current_ekf_twist_.header.stamp = this->now(); + current_ekf_twist_.twist.linear.x = ekf_.getXelement(IDX::VX); + current_ekf_twist_.twist.angular.z = ekf_.getXelement(IDX::WZ); +} + +/* + * timerTFCallback + */ +void EKFLocalizer::timerTFCallback() +{ + if (current_ekf_pose_.header.frame_id == "") { + return; + } + + geometry_msgs::msg::TransformStamped transformStamped; + transformStamped.header.stamp = this->now(); + transformStamped.header.frame_id = current_ekf_pose_.header.frame_id; + transformStamped.child_frame_id = "base_link"; + transformStamped.transform.translation.x = current_ekf_pose_.pose.position.x; + transformStamped.transform.translation.y = current_ekf_pose_.pose.position.y; + transformStamped.transform.translation.z = current_ekf_pose_.pose.position.z; + + transformStamped.transform.rotation.x = current_ekf_pose_.pose.orientation.x; + transformStamped.transform.rotation.y = current_ekf_pose_.pose.orientation.y; + transformStamped.transform.rotation.z = current_ekf_pose_.pose.orientation.z; + transformStamped.transform.rotation.w = current_ekf_pose_.pose.orientation.w; + + tf_br_->sendTransform(transformStamped); +} + +/* + * getTransformFromTF + */ +bool EKFLocalizer::getTransformFromTF( + std::string parent_frame, std::string child_frame, + geometry_msgs::msg::TransformStamped & transform) +{ + tf2::BufferCore tf_buffer; + tf2_ros::TransformListener tf_listener(tf_buffer); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + if (parent_frame.front() == '/') { + parent_frame.erase(0, 1); + } + if (child_frame.front() == '/') { + child_frame.erase(0, 1); + } + + for (int i = 0; i < 50; ++i) { + try { + transform = tf_buffer.lookupTransform(parent_frame, child_frame, tf2::TimePointZero); + return true; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(get_logger(), "%s", ex.what()); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + } + return false; +} + +/* + * callbackInitialPose + */ +void EKFLocalizer::callbackInitialPose( + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr initialpose) +{ + geometry_msgs::msg::TransformStamped transform; + if (!getTransformFromTF(pose_frame_id_, initialpose->header.frame_id, transform)) { + RCLCPP_ERROR( + get_logger(), "[EKF] TF transform failed. parent = %s, child = %s", pose_frame_id_.c_str(), + initialpose->header.frame_id.c_str()); + } + + Eigen::MatrixXd X(dim_x_, 1); + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x_, dim_x_); + + // TODO(mitsudome-r) need mutex + + X(IDX::X) = initialpose->pose.pose.position.x + transform.transform.translation.x; + X(IDX::Y) = initialpose->pose.pose.position.y + transform.transform.translation.y; + current_ekf_pose_.pose.position.z = + initialpose->pose.pose.position.z + transform.transform.translation.z; + X(IDX::YAW) = + tf2::getYaw(initialpose->pose.pose.orientation) + tf2::getYaw(transform.transform.rotation); + X(IDX::YAWB) = 0.0; + X(IDX::VX) = 0.0; + X(IDX::WZ) = 0.0; + + P(IDX::X, IDX::X) = initialpose->pose.covariance[0]; + P(IDX::Y, IDX::Y) = initialpose->pose.covariance[6 + 1]; + P(IDX::YAW, IDX::YAW) = initialpose->pose.covariance[6 * 5 + 5]; + P(IDX::YAWB, IDX::YAWB) = 0.0001; + P(IDX::VX, IDX::VX) = 0.01; + P(IDX::WZ, IDX::WZ) = 0.01; + + ekf_.init(X, P, extend_state_step_); + + current_pose_ptr_ = nullptr; +} + +/* + * callbackPoseWithCovariance + */ +void EKFLocalizer::callbackPoseWithCovariance( + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) +{ + geometry_msgs::msg::PoseStamped pose; + pose.header = msg->header; + pose.pose = msg->pose.pose; + current_pose_ptr_ = std::make_shared(pose); + current_pose_covariance_ = msg->pose.covariance; +} + +/* + * callbackTwistWithCovariance + */ +void EKFLocalizer::callbackTwistWithCovariance( + geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) +{ + geometry_msgs::msg::TwistStamped twist; + twist.header = msg->header; + twist.twist = msg->twist.twist; + current_twist_ptr_ = std::make_shared(twist); + current_twist_covariance_ = msg->twist.covariance; +} + +/* + * initEKF + */ +void EKFLocalizer::initEKF() +{ + 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 + P(IDX::YAWB, IDX::YAWB) = proc_cov_yaw_bias_d_; // for yaw bias + P(IDX::VX, IDX::VX) = 1000.0; // for vx + P(IDX::WZ, IDX::WZ) = 50.0; // for wz + + ekf_.init(X, P, extend_state_step_); +} + +/* + * predictKinematicsModel + */ +void EKFLocalizer::predictKinematicsModel() +{ + /* == 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} = vz_k + * wz_{k+1} = wz_k + * + * (b_k : yaw_bias_k) + */ + + /* == 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] + */ + + Eigen::MatrixXd X_curr(dim_x_, 1); // current state + Eigen::MatrixXd X_next(dim_x_, 1); // predicted state + ekf_.getLatestX(X_curr); + DEBUG_PRINT_MAT(X_curr.transpose()); + + Eigen::MatrixXd P_curr; + ekf_.getLatestP(P_curr); + + 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); + const double dt = ekf_dt_; + + /* Update for latest state */ + X_next(IDX::X) = X_curr(IDX::X) + vx * cos(yaw + yaw_bias) * dt; // dx = v * cos(yaw) + X_next(IDX::Y) = X_curr(IDX::Y) + vx * sin(yaw + yaw_bias) * dt; // dy = v * sin(yaw) + X_next(IDX::YAW) = X_curr(IDX::YAW) + (wz)*dt; // dyaw = omega + omega_bias + X_next(IDX::YAWB) = yaw_bias; + X_next(IDX::VX) = vx; + X_next(IDX::WZ) = wz; + + X_next(IDX::YAW) = std::atan2(std::sin(X_next(IDX::YAW)), std::cos(X_next(IDX::YAW))); + + /* Set A matrix for latest state */ + Eigen::MatrixXd A = Eigen::MatrixXd::Identity(dim_x_, dim_x_); + 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; + + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(dim_x_, dim_x_); + + 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) = proc_cov_yaw_bias_d_; // for yaw bias + Q(IDX::VX, IDX::VX) = proc_cov_vx_d_; // for vx + Q(IDX::WZ, IDX::WZ) = proc_cov_wz_d_; // for wz + + ekf_.predictWithDelay(X_next, A, Q); + + // debug + Eigen::MatrixXd X_result(dim_x_, 1); + ekf_.getLatestX(X_result); + DEBUG_PRINT_MAT(X_result.transpose()); + DEBUG_PRINT_MAT((X_result - X_curr).transpose()); +} + +/* + * measurementUpdatePose + */ +void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseStamped & pose) +{ + if (pose.header.frame_id != pose_frame_id_) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(2000).count(), + "pose frame_id is %s, but pose_frame is set as %s. They must be same.", + pose.header.frame_id.c_str(), pose_frame_id_.c_str()); + } + Eigen::MatrixXd X_curr(dim_x_, 1); // current state + ekf_.getLatestX(X_curr); + DEBUG_PRINT_MAT(X_curr.transpose()); + + constexpr int dim_y = 3; // pos_x, pos_y, yaw, depending on Pose output + const rclcpp::Time t_curr = this->now(); + + /* Calculate delay step */ + double delay_time = (t_curr - pose.header.stamp).seconds() + pose_additional_delay_; + if (delay_time < 0.0) { + delay_time = 0.0; + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "Pose time stamp is inappropriate, set delay to 0[s]. delay = %f", delay_time); + } + int delay_step = std::roundf(delay_time / ekf_dt_); + if (delay_step > extend_state_step_ - 1) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "Pose delay exceeds the compensation limit, ignored. delay: %f[s], limit = " + "extend_state_step * ekf_dt : %f [s]", + delay_time, extend_state_step_ * ekf_dt_); + return; + } + DEBUG_INFO(get_logger(), "delay_time: %f [s]", delay_time); + + /* Set yaw */ + double yaw = tf2::getYaw(pose.pose.orientation); + const double ekf_yaw = ekf_.getXelement(delay_step * dim_x_ + IDX::YAW); + const double yaw_error = normalizeYaw(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.position.x, pose.pose.position.y, yaw; + + if (isnan(y.array()).any() || isinf(y.array()).any()) { + RCLCPP_WARN( + get_logger(), + "[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message."); + return; + } + + /* Gate */ + Eigen::MatrixXd y_ekf(dim_y, 1); + y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::X), + ekf_.getXelement(delay_step * dim_x_ + IDX::Y), ekf_yaw; + Eigen::MatrixXd P_curr, P_y; + ekf_.getLatestP(P_curr); + P_y = P_curr.block(0, 0, dim_y, dim_y); + if (!mahalanobisGate(pose_gate_dist_, y_ekf, y, P_y)) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(2000).count(), + "[EKF] Pose measurement update, mahalanobis distance is over limit. ignore " + "measurement data."); + return; + } + + DEBUG_PRINT_MAT(y.transpose()); + DEBUG_PRINT_MAT(y_ekf.transpose()); + DEBUG_PRINT_MAT((y - y_ekf).transpose()); + + /* Set measurement matrix */ + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_); + 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 + + /* Set measurement noise covariance */ + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); + R(0, 0) = current_pose_covariance_.at(0); // x - x + R(0, 1) = current_pose_covariance_.at(1); // x - y + R(0, 2) = current_pose_covariance_.at(5); // x - yaw + R(1, 0) = current_pose_covariance_.at(6); // y - x + R(1, 1) = current_pose_covariance_.at(7); // y - y + R(1, 2) = current_pose_covariance_.at(11); // y - yaw + R(2, 0) = current_pose_covariance_.at(30); // yaw - x + R(2, 1) = current_pose_covariance_.at(31); // yaw - y + R(2, 2) = current_pose_covariance_.at(35); // yaw - yaw + + /* In order to avoid a large change at the time of updating, + * measurement update is performed by dividing at every step. */ + R *= (ekf_rate_ / pose_rate_); + + ekf_.updateWithDelay(y, C, R, delay_step); + + // debug + Eigen::MatrixXd X_result(dim_x_, 1); + ekf_.getLatestX(X_result); + DEBUG_PRINT_MAT(X_result.transpose()); + DEBUG_PRINT_MAT((X_result - X_curr).transpose()); +} + +/* + * measurementUpdateTwist + */ +void EKFLocalizer::measurementUpdateTwist(const geometry_msgs::msg::TwistStamped & twist) +{ + if (twist.header.frame_id != "base_link") { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(2000).count(), + "twist frame_id must be base_link"); + } + + Eigen::MatrixXd X_curr(dim_x_, 1); // current state + ekf_.getLatestX(X_curr); + DEBUG_PRINT_MAT(X_curr.transpose()); + + constexpr int dim_y = 2; // vx, wz + const rclcpp::Time t_curr = this->now(); + + /* Calculate delay step */ + double delay_time = (t_curr - twist.header.stamp).seconds() + twist_additional_delay_; + if (delay_time < 0.0) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "Twist time stamp is inappropriate (delay = %f [s]), set delay to 0[s].", delay_time); + delay_time = 0.0; + } + int delay_step = std::roundf(delay_time / ekf_dt_); + if (delay_step > extend_state_step_ - 1) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "Twist delay exceeds the compensation limit, ignored. delay: %f[s], limit = " + "extend_state_step * ekf_dt : %f [s]", + delay_time, extend_state_step_ * ekf_dt_); + return; + } + DEBUG_INFO(get_logger(), "delay_time: %f [s]", delay_time); + + /* Set measurement matrix */ + Eigen::MatrixXd y(dim_y, 1); + y << twist.twist.linear.x, twist.twist.angular.z; + + if (isnan(y.array()).any() || isinf(y.array()).any()) { + RCLCPP_WARN( + get_logger(), + "[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message."); + return; + } + + /* Gate */ + Eigen::MatrixXd y_ekf(dim_y, 1); + y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::VX), + ekf_.getXelement(delay_step * dim_x_ + IDX::WZ); + Eigen::MatrixXd P_curr, P_y; + ekf_.getLatestP(P_curr); + P_y = P_curr.block(4, 4, dim_y, dim_y); + if (!mahalanobisGate(twist_gate_dist_, y_ekf, y, P_y)) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(2000).count(), + "[EKF] Twist measurement update, mahalanobis distance is over limit. ignore " + "measurement data."); + return; + } + + DEBUG_PRINT_MAT(y.transpose()); + DEBUG_PRINT_MAT(y_ekf.transpose()); + DEBUG_PRINT_MAT((y - y_ekf).transpose()); + + /* Set measurement matrix */ + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_); + C(0, IDX::VX) = 1.0; // for vx + C(1, IDX::WZ) = 1.0; // for wz + + /* Set measurement noise covariance */ + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); + R(0, 0) = current_twist_covariance_.at(0); // vx - vx + R(0, 1) = current_twist_covariance_.at(5); // vx - wz + R(1, 0) = current_twist_covariance_.at(30); // wz - vx + R(1, 1) = current_twist_covariance_.at(35); // wz - wz + + /* In order to avoid a large change by update, measurement update is performed + * by dividing at every step. measurement update is performed by dividing at every step. */ + R *= (ekf_rate_ / twist_rate_); + + ekf_.updateWithDelay(y, C, R, delay_step); + + // debug + Eigen::MatrixXd X_result(dim_x_, 1); + ekf_.getLatestX(X_result); + DEBUG_PRINT_MAT(X_result.transpose()); + DEBUG_PRINT_MAT((X_result - X_curr).transpose()); +} + +/* + * mahalanobisGate + */ +bool EKFLocalizer::mahalanobisGate( + const double & dist_max, const Eigen::MatrixXd & x, const Eigen::MatrixXd & obj_x, + const Eigen::MatrixXd & cov) const +{ + Eigen::MatrixXd mahalanobis_squared = (x - obj_x).transpose() * cov.inverse() * (x - obj_x); + DEBUG_INFO( + get_logger(), "measurement update: mahalanobis = %f, gate limit = %f", + std::sqrt(mahalanobis_squared(0)), dist_max); + if (mahalanobis_squared(0) > dist_max * dist_max) { + return false; + } + + return true; +} + +/* + * publishEstimateResult + */ +void EKFLocalizer::publishEstimateResult() +{ + rclcpp::Time current_time = this->now(); + Eigen::MatrixXd X(dim_x_, 1); + Eigen::MatrixXd P(dim_x_, dim_x_); + ekf_.getLatestX(X); + ekf_.getLatestP(P); + + /* publish latest pose */ + pub_pose_->publish(current_ekf_pose_); + pub_pose_no_yawbias_->publish(current_ekf_pose_no_yawbias_); + + /* publish latest pose with covariance */ + geometry_msgs::msg::PoseWithCovarianceStamped pose_cov; + pose_cov.header.stamp = current_time; + pose_cov.header.frame_id = current_ekf_pose_.header.frame_id; + pose_cov.pose.pose = current_ekf_pose_.pose; + pose_cov.pose.covariance[0] = P(IDX::X, IDX::X); + pose_cov.pose.covariance[1] = P(IDX::X, IDX::Y); + pose_cov.pose.covariance[5] = P(IDX::X, IDX::YAW); + pose_cov.pose.covariance[6] = P(IDX::Y, IDX::X); + pose_cov.pose.covariance[7] = P(IDX::Y, IDX::Y); + pose_cov.pose.covariance[11] = P(IDX::Y, IDX::YAW); + pose_cov.pose.covariance[30] = P(IDX::YAW, IDX::X); + pose_cov.pose.covariance[31] = P(IDX::YAW, IDX::Y); + pose_cov.pose.covariance[35] = P(IDX::YAW, IDX::YAW); + pub_pose_cov_->publish(pose_cov); + + geometry_msgs::msg::PoseWithCovarianceStamped pose_cov_no_yawbias = pose_cov; + pose_cov_no_yawbias.pose.pose = current_ekf_pose_no_yawbias_.pose; + pub_pose_cov_no_yawbias_->publish(pose_cov_no_yawbias); + + /* 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_time; + twist_cov.header.frame_id = current_ekf_twist_.header.frame_id; + twist_cov.twist.twist = current_ekf_twist_.twist; + twist_cov.twist.covariance[0] = P(IDX::VX, IDX::VX); + twist_cov.twist.covariance[5] = P(IDX::VX, IDX::WZ); + twist_cov.twist.covariance[30] = P(IDX::WZ, IDX::VX); + twist_cov.twist.covariance[35] = P(IDX::WZ, IDX::WZ); + pub_twist_cov_->publish(twist_cov); + + /* publish yaw bias */ + autoware_debug_msgs::msg::Float64Stamped yawb; + yawb.stamp = current_time; + yawb.data = X(IDX::YAWB); + pub_yaw_bias_->publish(yawb); + + /* publish latest odometry */ + nav_msgs::msg::Odometry odometry; + odometry.header.stamp = current_time; + 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); + + /* debug measured pose */ + if (current_pose_ptr_ != nullptr) { + geometry_msgs::msg::PoseStamped p; + p = *current_pose_ptr_; + p.header.stamp = current_time; + pub_measured_pose_->publish(p); + } + + /* debug publish */ + double pose_yaw = 0.0; + if (current_pose_ptr_ != nullptr) { + pose_yaw = tf2::getYaw(current_pose_ptr_->pose.orientation); + } + + autoware_debug_msgs::msg::Float64MultiArrayStamped msg; + msg.stamp = current_time; + msg.data.push_back(autoware_utils::rad2deg(X(IDX::YAW))); // [0] ekf yaw angle + msg.data.push_back(autoware_utils::rad2deg(pose_yaw)); // [1] measurement yaw angle + msg.data.push_back(autoware_utils::rad2deg(X(IDX::YAWB))); // [2] yaw bias + pub_debug_->publish(msg); +} + +double EKFLocalizer::normalizeYaw(const double & yaw) const +{ + return std::atan2(std::sin(yaw), std::cos(yaw)); +} diff --git a/localization/ekf_localizer/src/ekf_localizer_node.cpp b/localization/ekf_localizer/src/ekf_localizer_node.cpp new file mode 100644 index 0000000000000..0f75f19c5d50b --- /dev/null +++ b/localization/ekf_localizer/src/ekf_localizer_node.cpp @@ -0,0 +1,28 @@ +// 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 "ekf_localizer/ekf_localizer.hpp" + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + auto node = std::make_shared("ekf_localizer", node_options); + + rclcpp::spin(node); + + return 0; +} diff --git a/localization/ekf_localizer/test/src/test_ekf_localizer.cpp b/localization/ekf_localizer/test/src/test_ekf_localizer.cpp new file mode 100644 index 0000000000000..1d4260859bb22 --- /dev/null +++ b/localization/ekf_localizer/test/src/test_ekf_localizer.cpp @@ -0,0 +1,308 @@ +// 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 "ekf_localizer/ekf_localizer.hpp" + +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +using std::placeholders::_1; + +class EKFLocalizerTestSuite : public ::testing::Test +{ +protected: + void SetUp() { rclcpp::init(0, nullptr); } + void TearDown() { (void)rclcpp::shutdown(); } +}; // sanity_check + +class TestEKFLocalizerNode : public EKFLocalizer +{ +public: + TestEKFLocalizerNode(const std::string & node_name, const rclcpp::NodeOptions & node_options) + : EKFLocalizer(node_name, node_options) + { + sub_twist = this->create_subscription( + "/ekf_twist", 1, std::bind(&TestEKFLocalizerNode::testCallbackTwist, this, _1)); + sub_pose = this->create_subscription( + "/ekf_pose", 1, std::bind(&TestEKFLocalizerNode::testCallbackPose, this, _1)); + + auto test_timer_callback = std::bind(&TestEKFLocalizerNode::testTimerCallback, this); + auto period = std::chrono::milliseconds(100); + test_timer_ = std::make_shared>( + this->get_clock(), period, std::move(test_timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(test_timer_, nullptr); + } + ~TestEKFLocalizerNode() {} + + std::string frame_id_a_ = "world"; + std::string frame_id_b_ = "base_link"; + + rclcpp::Subscription::SharedPtr sub_twist; + rclcpp::Subscription::SharedPtr sub_pose; + + rclcpp::TimerBase::SharedPtr test_timer_; + + geometry_msgs::msg::PoseStamped::SharedPtr test_current_pose_ptr_; + geometry_msgs::msg::TwistStamped::SharedPtr test_current_twist_ptr_; + + void testTimerCallback() + { + /* !!! this should be defined before sendTransform() !!! */ + static std::shared_ptr br = + std::make_shared(shared_from_this()); + geometry_msgs::msg::TransformStamped sent; + + rclcpp::Time current_time = this->now(); + + sent.header.stamp = current_time; + sent.header.frame_id = frame_id_a_; + sent.child_frame_id = frame_id_b_; + sent.transform.translation.x = -7.11; + sent.transform.translation.y = 0.0; + sent.transform.translation.z = 0.0; + tf2::Quaternion q; + q.setRPY(0, 0, 0.5); + sent.transform.rotation.x = q.x(); + sent.transform.rotation.y = q.y(); + sent.transform.rotation.z = q.z(); + sent.transform.rotation.w = q.w(); + + br->sendTransform(sent); + } + + void testCallbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose) + { + test_current_pose_ptr_ = std::make_shared(*pose); + } + + void testCallbackTwist(geometry_msgs::msg::TwistStamped::SharedPtr twist) + { + test_current_twist_ptr_ = std::make_shared(*twist); + } + + void resetCurrentPoseAndTwist() + { + test_current_pose_ptr_ = nullptr; + test_current_twist_ptr_ = nullptr; + } +}; + +TEST_F(EKFLocalizerTestSuite, measurementUpdatePose) +{ + rclcpp::NodeOptions node_options; + auto ekf = std::make_shared("EKFLocalizerTestSuite", node_options); + + auto pub_pose = ekf->create_publisher("/in_pose", 1); + + geometry_msgs::msg::PoseStamped in_pose; + in_pose.header.frame_id = "world"; + in_pose.pose.position.x = 1.0; + in_pose.pose.position.y = 2.0; + in_pose.pose.position.z = 3.0; + in_pose.pose.orientation.x = 0.0; + in_pose.pose.orientation.y = 0.0; + in_pose.pose.orientation.z = 0.0; + in_pose.pose.orientation.w = 1.0; + + /* test for valid value */ + const double pos_x = 12.3; + in_pose.pose.position.x = pos_x; // for valid value + + for (int i = 0; i < 20; ++i) { + in_pose.header.stamp = ekf->now(); + pub_pose->publish(in_pose); + rclcpp::spin_some(ekf); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + ASSERT_NE(ekf->test_current_pose_ptr_, nullptr); + ASSERT_NE(ekf->test_current_twist_ptr_, nullptr); + + double ekf_x = ekf->test_current_pose_ptr_->pose.position.x; + bool is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); + ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; + + ASSERT_TRUE(std::fabs(ekf_x - pos_x) < 0.1) + << "ekf pos x: " << ekf_x << " should be close to " << pos_x; + + /* test for invalid value */ + in_pose.pose.position.x = NAN; // check for invalid values + for (int i = 0; i < 10; ++i) { + in_pose.header.stamp = ekf->now(); + pub_pose->publish(in_pose); + rclcpp::spin_some(ekf); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); + ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; + + ekf->resetCurrentPoseAndTwist(); +} + +TEST_F(EKFLocalizerTestSuite, measurementUpdateTwist) +{ + rclcpp::NodeOptions node_options; + auto ekf = std::make_shared("EKFLocalizerTestSuite", node_options); + + auto pub_twist = ekf->create_publisher("/in_twist", 1); + geometry_msgs::msg::TwistStamped in_twist; + in_twist.header.frame_id = "base_link"; + + /* test for valid value */ + const double vx = 12.3; + in_twist.twist.linear.x = vx; // for valid value + for (int i = 0; i < 20; ++i) { + in_twist.header.stamp = ekf->now(); + pub_twist->publish(in_twist); + rclcpp::spin_some(ekf); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + ASSERT_FALSE(ekf->test_current_pose_ptr_ == nullptr); + ASSERT_FALSE(ekf->test_current_twist_ptr_ == nullptr); + + double ekf_vx = ekf->test_current_twist_ptr_->twist.linear.x; + bool is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); + ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; + ASSERT_TRUE(std::fabs(ekf_vx - vx) < 0.1) + << "ekf vel x: " << ekf_vx << ", should be close to " << vx; + + /* test for invalid value */ + in_twist.twist.linear.x = NAN; // check for invalid values + for (int i = 0; i < 10; ++i) { + in_twist.header.stamp = ekf->now(); + pub_twist->publish(in_twist); + rclcpp::spin_some(ekf); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + ekf_vx = ekf->test_current_twist_ptr_->twist.linear.x; + is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); + ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; + + ekf->resetCurrentPoseAndTwist(); +} + +TEST_F(EKFLocalizerTestSuite, measurementUpdatePoseWithCovariance) +{ + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("use_pose_with_covariance", true); + rclcpp::sleep_for(std::chrono::milliseconds(200)); + auto ekf = std::make_shared("EKFLocalizerTestSuite", node_options); + + auto pub_pose = ekf->create_publisher( + "/in_pose_with_covariance", 1); + geometry_msgs::msg::PoseWithCovarianceStamped in_pose; + in_pose.header.frame_id = "world"; + in_pose.pose.pose.position.x = 1.0; + in_pose.pose.pose.position.y = 2.0; + in_pose.pose.pose.position.z = 3.0; + in_pose.pose.pose.orientation.x = 0.0; + in_pose.pose.pose.orientation.y = 0.0; + in_pose.pose.pose.orientation.z = 0.0; + in_pose.pose.pose.orientation.w = 1.0; + for (int i = 0; i < 36; ++i) { + in_pose.pose.covariance[i] = 0.1; + } + + /* test for valid value */ + const double pos_x = 99.3; + in_pose.pose.pose.position.x = pos_x; // for valid value + + for (int i = 0; i < 20; ++i) { + in_pose.header.stamp = ekf->now(); + pub_pose->publish(in_pose); + rclcpp::spin_some(ekf); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + ASSERT_FALSE(ekf->test_current_pose_ptr_ == nullptr); + ASSERT_FALSE(ekf->test_current_twist_ptr_ == nullptr); + + double ekf_x = ekf->test_current_pose_ptr_->pose.position.x; + bool is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); + ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; + ASSERT_TRUE(std::fabs(ekf_x - pos_x) < 0.1) + << "ekf pos x: " << ekf_x << " should be close to " << pos_x; + + /* test for invalid value */ + in_pose.pose.pose.position.x = NAN; // check for invalid values + for (int i = 0; i < 10; ++i) { + in_pose.header.stamp = ekf->now(); + pub_pose->publish(in_pose); + rclcpp::spin_some(ekf); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); + ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; + + ekf->resetCurrentPoseAndTwist(); +} + +TEST_F(EKFLocalizerTestSuite, measurementUpdateTwistWithCovariance) +{ + rclcpp::NodeOptions node_options; + auto ekf = std::make_shared("EKFLocalizerTestSuite", node_options); + + auto pub_twist = ekf->create_publisher( + "/in_twist_with_covariance", 1); + geometry_msgs::msg::TwistWithCovarianceStamped in_twist; + in_twist.header.frame_id = "base_link"; + + /* test for valid value */ + const double vx = 12.3; + in_twist.twist.twist.linear.x = vx; // for valid value + for (int i = 0; i < 36; ++i) { + in_twist.twist.covariance[i] = 0.1; + } + for (int i = 0; i < 10; ++i) { + in_twist.header.stamp = ekf->now(); + pub_twist->publish(in_twist); + rclcpp::spin_some(ekf); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + ASSERT_FALSE(ekf->test_current_pose_ptr_ == nullptr); + ASSERT_FALSE(ekf->test_current_twist_ptr_ == nullptr); + + double ekf_vx = ekf->test_current_twist_ptr_->twist.linear.x; + bool is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); + ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; + ASSERT_TRUE((ekf_vx - vx) < 0.1) << "vel x should be close to " << vx; + + /* test for invalid value */ + in_twist.twist.twist.linear.x = NAN; // check for invalid values + for (int i = 0; i < 10; ++i) { + in_twist.header.stamp = ekf->now(); + pub_twist->publish(in_twist); + rclcpp::spin_some(ekf); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + ekf_vx = ekf->test_current_twist_ptr_->twist.linear.x; + is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); + ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; +} diff --git a/localization/ekf_localizer/test/test_ekf_localizer.test b/localization/ekf_localizer/test/test_ekf_localizer.test new file mode 100644 index 0000000000000..4eecfa6e094b7 --- /dev/null +++ b/localization/ekf_localizer/test/test_ekf_localizer.test @@ -0,0 +1,5 @@ + + + + + diff --git a/localization/stop_filter/CMakeLists.txt b/localization/stop_filter/CMakeLists.txt new file mode 100644 index 0000000000000..6eb7a5085b6f6 --- /dev/null +++ b/localization/stop_filter/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.5) +project(stop_filter) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + # add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wno-unused-parameter) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_executable(stop_filter + src/stop_filter_node.cpp + src/stop_filter.cpp +) +ament_target_dependencies(stop_filter) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/localization/stop_filter/README.md b/localization/stop_filter/README.md new file mode 100644 index 0000000000000..a2c3bf1dc0acf --- /dev/null +++ b/localization/stop_filter/README.md @@ -0,0 +1,31 @@ +# stop_filter + +## Purpose + +When this function did not exist, each node used a different criterion to determine whether the vehicle is stopping or not, resulting that some nodes were in operation of stopping the vehicle and some nodes continued running in the drive mode. +This node aims to: + +- apply a uniform stopping decision criterion to several nodes. +- suppress the control noise by overwriting the velocity and angular velocity with zero. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------ | ------------------------- | --------------------- | +| `input/odom` | `nav_msgs::msg::Odometry` | localization odometry | + +### Output + +| Name | Type | Description | +| ----------------- | --------------------------------------- | -------------------------------------------------------- | +| `output/odom` | `nav_msgs::msg::Odometry` | odometry with suppressed longitudinal and yaw twist | +| `debug/stop_flag` | `autoware_debug_msgs::msg::BoolStamped` | flag to represent whether the vehicle is stopping or not | + +## Parameters + +| Name | Type | Description | +| -------------- | ------ | --------------------------------------------------------------------------------------------- | +| `vx_threshold` | double | longitudinal velocity threshold to determine if the vehicle is stopping [m/s] (default: 0.01) | +| `wz_threshold` | double | yaw velocity threshold to determine if the vehicle is stopping [rad/s] (default: 0.01) | diff --git a/localization/stop_filter/include/stop_filter/stop_filter.hpp b/localization/stop_filter/include/stop_filter/stop_filter.hpp new file mode 100644 index 0000000000000..d97eae03fc98a --- /dev/null +++ b/localization/stop_filter/include/stop_filter/stop_filter.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 TierIV +// +// 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 STOP_FILTER__STOP_FILTER_HPP_ +#define STOP_FILTER__STOP_FILTER_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +class StopFilter : public rclcpp::Node +{ +public: + StopFilter(const std::string & node_name, const rclcpp::NodeOptions & options); + +private: + rclcpp::Publisher::SharedPtr pub_odom_; //!< @brief odom publisher + rclcpp::Publisher::SharedPtr + pub_stop_flag_; //!< @brief stop flag publisher + rclcpp::Subscription::SharedPtr + sub_odom_; //!< @brief measurement odometry subscriber + + double vx_threshold_; //!< @brief vx threshold + double wz_threshold_; //!< @brief wz threshold + + /** + * @brief set odometry measurement + */ + void callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr msg); +}; +#endif // STOP_FILTER__STOP_FILTER_HPP_ diff --git a/localization/stop_filter/launch/stop_filter.launch.xml b/localization/stop_filter/launch/stop_filter.launch.xml new file mode 100644 index 0000000000000..36a66a2c143c0 --- /dev/null +++ b/localization/stop_filter/launch/stop_filter.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/localization/stop_filter/package.xml b/localization/stop_filter/package.xml new file mode 100644 index 0000000000000..598264f68c8fb --- /dev/null +++ b/localization/stop_filter/package.xml @@ -0,0 +1,23 @@ + + + + stop_filter + 0.0.0 + The stop filter package + Koji Minoda + Apache License 2.0 + + ament_cmake_auto + + autoware_debug_msgs + geometry_msgs + nav_msgs + rclcpp + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/localization/stop_filter/src/stop_filter.cpp b/localization/stop_filter/src/stop_filter.cpp new file mode 100644 index 0000000000000..59bae091400ca --- /dev/null +++ b/localization/stop_filter/src/stop_filter.cpp @@ -0,0 +1,65 @@ +// Copyright 2021 TierIV +// +// 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 "stop_filter/stop_filter.hpp" + +#include + +#include +#include +#include +#include +#include + +// clang-format off +#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl +#define DEBUG_INFO(...) {if (show_debug_info_) {RCLCPP_INFO(__VA_ARGS__);}} +#define DEBUG_PRINT_MAT(X) {if (show_debug_info_) {std::cout << #X << ": " << X << std::endl;}} + +// clang-format on +using std::placeholders::_1; + +StopFilter::StopFilter(const std::string & node_name, const rclcpp::NodeOptions & node_options) +: rclcpp::Node(node_name, node_options) +{ + vx_threshold_ = declare_parameter("vx_threshold", 0.01); + wz_threshold_ = declare_parameter("wz_threshold", 0.01); + + sub_odom_ = create_subscription( + "input/odom", 1, std::bind(&StopFilter::callbackOdometry, this, _1)); + + pub_odom_ = create_publisher("output/odom", 1); + pub_stop_flag_ = create_publisher("debug/stop_flag", 1); +} + +void StopFilter::callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + autoware_debug_msgs::msg::BoolStamped stop_flag_msg; + stop_flag_msg.stamp = msg->header.stamp; + stop_flag_msg.data = false; + + nav_msgs::msg::Odometry odom_msg; + odom_msg = *msg; + + if ( + std::fabs(msg->twist.twist.linear.x) < vx_threshold_ && + std::fabs(msg->twist.twist.angular.z) < wz_threshold_) { + odom_msg.twist.twist.linear.x = 0.0; + odom_msg.twist.twist.angular.z = 0.0; + stop_flag_msg.data = true; + } + + pub_stop_flag_->publish(stop_flag_msg); + pub_odom_->publish(odom_msg); +} diff --git a/localization/stop_filter/src/stop_filter_node.cpp b/localization/stop_filter/src/stop_filter_node.cpp new file mode 100644 index 0000000000000..9748214828de2 --- /dev/null +++ b/localization/stop_filter/src/stop_filter_node.cpp @@ -0,0 +1,28 @@ +// Copyright 2021 TierIV +// +// 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 "stop_filter/stop_filter.hpp" + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + auto node = std::make_shared("stop_filter", node_options); + + rclcpp::spin(node); + + return 0; +}