fix(probabilistic_occupancy_gridmap): prevent node death by adding lo…

…okupTransform exception in util function (autowarefoundation#4718) * fix(probabilistic_occupancy_gridmap): prevent node death by adding lookupTransform exception in util function Signed-off-by: Shumpei Wakabayashi <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: Shumpei Wakabayashi <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
AutomotiveAIChallenge · Aug 23, 2023 · 24df0d3 · 24df0d3
1 parent 05d091d
commit 24df0d3
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 4 deletions.
diff --git a/...id_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp b/...id_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<float>();
   pcl_ros::transformPointCloud(tf_matrix, input, output);

diff --git a/..._map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/..._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<float>();
   pcl_ros::transformPointCloud(tf_matrix, input, output);