diff --git a/nav2_costmap_2d/src/observation_buffer.cpp b/nav2_costmap_2d/src/observation_buffer.cpp index 8a06823df38..be3578bbd9b 100644 --- a/nav2_costmap_2d/src/observation_buffer.cpp +++ b/nav2_costmap_2d/src/observation_buffer.cpp @@ -153,6 +153,15 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::msg::PointCloud2 & cloud) modifier.resize(point_count); observation_cloud.header.stamp = cloud.header.stamp; observation_cloud.header.frame_id = global_frame_cloud.header.frame_id; + } catch (tf2::ExtrapolationException & ex) { + // if an exception occurs, we need to remove the empty observation from the list + observation_list_.pop_front(); + RCLCPP_ERROR( + logger_, + "TF Extrapolation Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", + sensor_frame_.c_str(), + cloud.header.frame_id.c_str(), ex.what()); + return; } catch (tf2::TransformException & ex) { // if an exception occurs, we need to remove the empty observation from the list observation_list_.pop_front();