diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index 87cc510c5fe14..02f14a86cd7df 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -83,6 +83,7 @@ The parameters are set in `launch/ekf_localizer.launch` . | 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 | +| publish_tf | bool | Whether to publish tf | true | | 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 | diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index 1db45db411b0c..9de7f56f5c006 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml @@ -4,6 +4,7 @@ enable_yaw_bias_estimation: true predict_frequency: 50.0 tf_rate: 50.0 + publish_tf: true extend_state_step: 50 # for Pose measurement diff --git a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp index e0e34f382e3dc..5139f900a339e 100644 --- a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp @@ -28,6 +28,7 @@ class HyperParameters ekf_rate(node->declare_parameter("predict_frequency", 50.0)), ekf_dt(1.0 / std::max(ekf_rate, 0.1)), tf_rate_(node->declare_parameter("tf_rate", 10.0)), + publish_tf_(node->declare_parameter("publish_tf", true)), enable_yaw_bias_estimation(node->declare_parameter("enable_yaw_bias_estimation", true)), extend_state_step(node->declare_parameter("extend_state_step", 50)), pose_frame_id(node->declare_parameter("pose_frame_id", std::string("map"))), @@ -60,6 +61,7 @@ class HyperParameters const double ekf_rate; const double ekf_dt; const double tf_rate_; + const bool publish_tf_; const bool enable_yaw_bias_estimation; const int extend_state_step; const std::string pose_frame_id; diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 0ab88202e0c0d..b3ee3665cf45e 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -60,9 +60,11 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti this, get_clock(), rclcpp::Duration::from_seconds(ekf_dt_), std::bind(&EKFLocalizer::timerCallback, this)); - timer_tf_ = rclcpp::create_timer( - this, get_clock(), rclcpp::Rate(params_.tf_rate_).period(), - std::bind(&EKFLocalizer::timerTFCallback, this)); + if (params_.publish_tf_) { + timer_tf_ = rclcpp::create_timer( + this, get_clock(), rclcpp::Rate(params_.tf_rate_).period(), + std::bind(&EKFLocalizer::timerTFCallback, this)); + } pub_pose_ = create_publisher("ekf_pose", 1); pub_pose_cov_ =