From 89a9a54b649ec6fd17f0eb337a444f982910f4b9 Mon Sep 17 00:00:00 2001
From: Anton Bogdanov <bogdan0v.am@yandex.ru>
Date: Fri, 1 Mar 2024 22:18:35 +0300
Subject: [PATCH] add entity validation to OdometryPublisher system

Signed-off-by: Anton Bogdanov <bogdan0v.am@yandex.ru>
---
 src/systems/odometry_publisher/OdometryPublisher.cc | 8 ++++++++
 1 file changed, 8 insertions(+)

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);
 }