feat(ekf_localizer): tf publisher as an option (autowarefoundation#6004)

Signed-off-by: amadeuszsz <amadeusz.szymko@put.poznan.pl>
Interplai · May 26, 2024 · 333e978 · 333e978
1 parent 9b94405
commit 333e978
Show file tree

Hide file tree

Showing 4 changed files with 9 additions and 3 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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<geometry_msgs::msg::PoseStamped>("ekf_pose", 1);
   pub_pose_cov_ =