From 344efb0f48f7c9b8c6db07f9de952c601e36cbe5 Mon Sep 17 00:00:00 2001
From: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
Date: Tue, 11 Jun 2024 17:45:00 +0900
Subject: [PATCH] Fixed lookup error before subscribe /tf_static
Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
---
.../src/deviation_estimator_main.cpp | 13 +++++++++----
1 file changed, 9 insertions(+), 4 deletions(-)
diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator_main.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator_main.cpp
index 6346e8e0..f53a3c4e 100644
--- a/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator_main.cpp
+++ b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator_main.cpp
@@ -113,11 +113,16 @@ int main(int argc, char ** argv)
sensor_msgs::msg::Imu::SharedPtr imu_msg = std::make_shared<sensor_msgs::msg::Imu>();
serialization_imu.deserialize_message(&msg, imu_msg.get());
imu_frame_id = imu_msg->header.frame_id;
- const geometry_msgs::msg::TransformStamped transform =
- tf_buffer.lookupTransform("base_link", imu_msg->header.frame_id, tf2::TimePointZero);
geometry_msgs::msg::Vector3Stamped vec_stamped_transformed;
- vec_stamped_transformed.header = imu_msg->header;
- tf2::doTransform(imu_msg->angular_velocity, vec_stamped_transformed.vector, transform);
+ try {
+ const geometry_msgs::msg::TransformStamped transform =
+ tf_buffer.lookupTransform("base_link", imu_msg->header.frame_id, tf2::TimePointZero);
+ vec_stamped_transformed.header = imu_msg->header;
+ tf2::doTransform(imu_msg->angular_velocity, vec_stamped_transformed.vector, transform);
+ } catch (const tf2::TransformException & ex) {
+ std::cerr << "Transform exception: " << ex.what() << std::endl;
+ continue;
+ }
const rclcpp::Time curr_stamp = imu_msg->header.stamp;
first_stamp = std::min(first_stamp, curr_stamp);
const double diff_sec = (curr_stamp - first_stamp).seconds();