diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index 1195e7398f..fd4bab9a57 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -278,6 +278,10 @@ void OdometryPublisher::PreUpdate(const gz::sim::UpdateInfo &_info, { GZ_PROFILE("OdometryPublisher::PreUpdate"); + if (!this->dataPtr->model.Valid(_ecm)) { + return; + } + // \TODO(anyone) Support rewind if (_info.dt < std::chrono::steady_clock::duration::zero()) { @@ -305,6 +309,10 @@ void OdometryPublisher::PostUpdate(const UpdateInfo &_info, if (_info.paused) return; + if (!this->dataPtr->model.Valid(_ecm)) { + return; + } + this->dataPtr->UpdateOdometry(_info, _ecm); }