From 24df0d391b3553fa45c201258b73a869562e5059 Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Wed, 23 Aug 2023 13:08:01 +0900 Subject: [PATCH] fix(probabilistic_occupancy_gridmap): prevent node death by adding lookupTransform exception in util function (#4718) * fix(probabilistic_occupancy_gridmap): prevent node death by adding lookupTransform exception in util function Signed-off-by: Shumpei Wakabayashi * style(pre-commit): autofix --------- Signed-off-by: Shumpei Wakabayashi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../laserscan_based_occupancy_grid_map_node.cpp | 12 ++++++++++-- .../pointcloud_based_occupancy_grid_map_node.cpp | 12 ++++++++++-- 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp index b65dfb97e2f84..366f9323aab23 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp @@ -42,8 +42,16 @@ bool transformPointcloud( const std::string & target_frame, sensor_msgs::msg::PointCloud2 & output) { geometry_msgs::msg::TransformStamped tf_stamped; - tf_stamped = tf2.lookupTransform( - target_frame, input.header.frame_id, input.header.stamp, rclcpp::Duration::from_seconds(0.5)); + // lookup transform + try { + tf_stamped = tf2.lookupTransform( + target_frame, input.header.frame_id, input.header.stamp, rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN( + rclcpp::get_logger("probabilistic_occupancy_grid_map"), "Failed to lookup transform: %s", + ex.what()); + return false; + } // transform pointcloud Eigen::Matrix4f tf_matrix = tf2::transformToEigen(tf_stamped.transform).matrix().cast(); pcl_ros::transformPointCloud(tf_matrix, input, output); diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index cbfcce22d1b61..e2451d126d532 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -42,8 +42,16 @@ bool transformPointcloud( const std::string & target_frame, sensor_msgs::msg::PointCloud2 & output) { geometry_msgs::msg::TransformStamped tf_stamped; - tf_stamped = tf2.lookupTransform( - target_frame, input.header.frame_id, input.header.stamp, rclcpp::Duration::from_seconds(0.5)); + // lookup transform + try { + tf_stamped = tf2.lookupTransform( + target_frame, input.header.frame_id, input.header.stamp, rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN( + rclcpp::get_logger("probabilistic_occupancy_grid_map"), "Failed to lookup transform: %s", + ex.what()); + return false; + } // transform pointcloud Eigen::Matrix4f tf_matrix = tf2::transformToEigen(tf_stamped.transform).matrix().cast(); pcl_ros::transformPointCloud(tf_matrix, input, output);