diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp index 741d333142c9d..f2cd6203b3685 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp @@ -25,8 +25,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/test/test_pointcloud_based_method.py b/perception/probabilistic_occupancy_grid_map/test/test_pointcloud_based_method.py index 6b0150dc3fa7b..093811d4cc213 100644 --- a/perception/probabilistic_occupancy_grid_map/test/test_pointcloud_based_method.py +++ b/perception/probabilistic_occupancy_grid_map/test/test_pointcloud_based_method.py @@ -238,6 +238,29 @@ def test_null_input(self, proc_info): self.assertIn("occupancy_grid_map_node", nodes) self.assertEqual(len(self.msg_buffer), 1) + def test_null_input2(self, proc_info): + """Test null input. + + input: null pointcloud without even frame_id + output: null ogm + """ + # wait for the node to be ready + time.sleep(3) + input_points = [] + pub_raw, pub_obstacle, sub = self.create_pub_sub() + # publish input pointcloud + pt_msg = get_pointcloud_msg(input_points) + pt_msg.header.frame_id = "" + pub_raw.publish(pt_msg) + pub_obstacle.publish(pt_msg) + # try to subscribe output pointcloud once + rclpy.spin_once(self.node, timeout_sec=3.0) + + # check if process is successfully terminated + nodes = self.node.get_node_names() + self.assertIn("occupancy_grid_map_node", nodes) + self.assertEqual(len(self.msg_buffer), 0) + @launch_testing.post_shutdown_test() class TestProcessOutput(unittest.TestCase):