From 5bce9d8b2ae5925010a7b7daa443da71954caa49 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Wed, 6 Nov 2024 14:11:16 +0900 Subject: [PATCH] Removed timer_tf_ Signed-off-by: Shintaro Sakoda --- .../config/ekf_localizer.param.yaml | 1 - .../autoware/ekf_localizer/ekf_localizer.hpp | 7 ---- .../ekf_localizer/hyper_parameters.hpp | 2 -- .../schema/sub/node.sub_schema.json | 6 ---- .../src/ekf_localizer.cpp | 33 +++---------------- 5 files changed, 5 insertions(+), 44 deletions(-) diff --git a/localization/autoware_ekf_localizer/config/ekf_localizer.param.yaml b/localization/autoware_ekf_localizer/config/ekf_localizer.param.yaml index 13319521e35ab..e00d5e1c10ae2 100644 --- a/localization/autoware_ekf_localizer/config/ekf_localizer.param.yaml +++ b/localization/autoware_ekf_localizer/config/ekf_localizer.param.yaml @@ -5,7 +5,6 @@ enable_yaw_bias_estimation: true predict_frequency: 50.0 tf_rate: 50.0 - publish_tf: true extend_state_step: 50 pose_measurement: 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 index f56aa8cf78c53..d61faa39a93a0 100644 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp +++ b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp @@ -99,8 +99,6 @@ class EKFLocalizer : public rclcpp::Node //!< @brief trigger_node service rclcpp::Service::SharedPtr service_trigger_node_; - //!< @brief timer to send transform - rclcpp::TimerBase::SharedPtr timer_tf_; //!< @brief tf broadcaster std::shared_ptr tf_br_; //!< @brief tf buffer @@ -131,11 +129,6 @@ class EKFLocalizer : public rclcpp::Node */ void timer_callback(); - /** - * @brief publish tf for tf_rate [Hz] - */ - void timer_tf_callback(); - /** * @brief set pose with covariance measurement */ 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 index 44db5b2dc7ce4..02958199cf2a1 100644 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/hyper_parameters.hpp +++ b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/hyper_parameters.hpp @@ -31,7 +31,6 @@ class HyperParameters 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")), - publish_tf_(node->declare_parameter("node.publish_tf")), 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")), @@ -76,7 +75,6 @@ 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 size_t extend_state_step; const std::string pose_frame_id; diff --git a/localization/autoware_ekf_localizer/schema/sub/node.sub_schema.json b/localization/autoware_ekf_localizer/schema/sub/node.sub_schema.json index 92e083b92d3e7..cbaf34de6eaa7 100644 --- a/localization/autoware_ekf_localizer/schema/sub/node.sub_schema.json +++ b/localization/autoware_ekf_localizer/schema/sub/node.sub_schema.json @@ -20,11 +20,6 @@ "description": "Frequency for tf broadcasting [Hz]", "default": 50.0 }, - "publish_tf": { - "type": "boolean", - "description": "Whether to publish tf", - "default": true - }, "extend_state_step": { "type": "integer", "description": "Max delay step which can be dealt with in EKF. Large number increases computational cost.", @@ -40,7 +35,6 @@ "show_debug_info", "predict_frequency", "tf_rate", - "publish_tf", "extend_state_step", "enable_yaw_bias_estimation" ], diff --git a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp b/localization/autoware_ekf_localizer/src/ekf_localizer.cpp index 1b747c750977c..70ae4c04436ec 100644 --- a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp +++ b/localization/autoware_ekf_localizer/src/ekf_localizer.cpp @@ -61,12 +61,6 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) this, get_clock(), rclcpp::Duration::from_seconds(ekf_dt_), std::bind(&EKFLocalizer::timer_callback, this)); - if (params_.publish_tf_) { - timer_tf_ = rclcpp::create_timer( - this, get_clock(), rclcpp::Rate(params_.tf_rate_).period(), - std::bind(&EKFLocalizer::timer_tf_callback, this)); - } - pub_pose_ = create_publisher("ekf_pose", 1); pub_pose_cov_ = create_publisher("ekf_pose_with_covariance", 1); @@ -227,28 +221,6 @@ void EKFLocalizer::timer_callback() publish_diagnostics(current_ekf_pose, current_time); } -/* - * timer_tf_callback - */ -void EKFLocalizer::timer_tf_callback() -{ - if (!is_activated_) { - return; - } - - if (params_.pose_frame_id.empty()) { - return; - } - - const rclcpp::Time current_time = this->now(); - - geometry_msgs::msg::TransformStamped transform_stamped; - transform_stamped = autoware::universe_utils::pose2transform( - ekf_module_->get_current_pose(current_time, false), "base_link"); - transform_stamped.header.stamp = current_time; - tf_br_->sendTransform(transform_stamped); -} - /* * get_transform_from_tf */ @@ -363,6 +335,11 @@ void EKFLocalizer::publish_estimate_result( 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(