fix transform

Signed-off-by: scepter914 <scepter914@gmail.com>
autowarefoundation · Nov 24, 2022 · bb7104f · bb7104f
1 parent a0f29e1
commit bb7104f
Showing 1 changed file with 9 additions and 3 deletions.
diff --git a/...ud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp b/...ud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp
@@ -128,18 +128,24 @@ rcl_interfaces::msg::SetParametersResult RadarStaticPointcloudFilterNode::onSetP
 void RadarStaticPointcloudFilterNode::onData(
   const RadarScan::ConstSharedPtr radar_msg, const Odometry::ConstSharedPtr odom_msg)
 {
-  geometry_msgs::msg::TransformStamped::ConstSharedPtr transform_ =
-    transform_listener_->getTransform(
+  geometry_msgs::msg::TransformStamped::ConstSharedPtr transform;
+
+  try {
+    transform = transform_listener_->getTransform(
       odom_msg->header.frame_id, radar_msg->header.frame_id, odom_msg->header.stamp,
       rclcpp::Duration::from_seconds(0.01));
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_INFO(this->get_logger(), "Could not transform");
+    return;
+  }
 
   RadarScan static_radar_{};
   RadarScan dynamic_radar_{};
   static_radar_.header = radar_msg->header;
   dynamic_radar_.header = radar_msg->header;
 
   for (const auto & radar_return : radar_msg->returns) {
-    if (isStaticPointcloud(radar_return, odom_msg, transform_)) {
+    if (isStaticPointcloud(radar_return, odom_msg, transform)) {
       static_radar_.returns.emplace_back(radar_return);
     } else {
       dynamic_radar_.returns.emplace_back(radar_return);