add entity validation to OdometryPublisher system

Signed-off-by: Anton Bogdanov <[email protected]>
gazebosim · Mar 1, 2024 · 89a9a54 · 89a9a54
1 parent 633ce72
commit 89a9a54
Showing 1 changed file with 8 additions and 0 deletions.
diff --git 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);
 }