Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(autoware_ekf_localizer): remove timer_tf_ #9244

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -99,8 +99,6 @@ class EKFLocalizer : public rclcpp::Node
//!< @brief trigger_node service
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr service_trigger_node_;

//!< @brief timer to send transform
rclcpp::TimerBase::SharedPtr timer_tf_;
//!< @brief tf broadcaster
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_br_;
//!< @brief tf buffer
Expand Down Expand Up @@ -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
*/
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,6 @@ class HyperParameters
ekf_rate(node->declare_parameter<double>("node.predict_frequency")),
ekf_dt(1.0 / std::max(ekf_rate, 0.1)),
tf_rate_(node->declare_parameter<double>("node.tf_rate")),
publish_tf_(node->declare_parameter<bool>("node.publish_tf")),
enable_yaw_bias_estimation(node->declare_parameter<bool>("node.enable_yaw_bias_estimation")),
extend_state_step(node->declare_parameter<int>("node.extend_state_step")),
pose_frame_id(node->declare_parameter<std::string>("misc.pose_frame_id")),
Expand Down Expand Up @@ -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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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.",
Expand All @@ -40,7 +35,6 @@
"show_debug_info",
"predict_frequency",
"tf_rate",
"publish_tf",
"extend_state_step",
"enable_yaw_bias_estimation"
],
Expand Down
33 changes: 5 additions & 28 deletions localization/autoware_ekf_localizer/src/ekf_localizer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<geometry_msgs::msg::PoseStamped>("ekf_pose", 1);
pub_pose_cov_ =
create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("ekf_pose_with_covariance", 1);
Expand Down Expand Up @@ -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
*/
Expand Down Expand Up @@ -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(
Expand Down
Loading