From 89a9a54b649ec6fd17f0eb337a444f982910f4b9 Mon Sep 17 00:00:00 2001 From: Anton Bogdanov Date: Fri, 1 Mar 2024 22:18:35 +0300 Subject: [PATCH 1/4] add entity validation to OdometryPublisher system Signed-off-by: Anton Bogdanov --- 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); } From 83c05c7b7906c67ec89276776ad5523ee377a3c9 Mon Sep 17 00:00:00 2001 From: Anton Bogdanov Date: Sat, 2 Mar 2024 00:00:06 +0300 Subject: [PATCH 2/4] copy solution from JointPositionController Signed-off-by: Anton Bogdanov --- .../odometry_publisher/OdometryPublisher.cc | 32 ++++++++++++++++--- 1 file changed, 28 insertions(+), 4 deletions(-) diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index fd4bab9a57..4408ed8ecd 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -278,7 +278,18 @@ void OdometryPublisher::PreUpdate(const gz::sim::UpdateInfo &_info, { GZ_PROFILE("OdometryPublisher::PreUpdate"); - if (!this->dataPtr->model.Valid(_ecm)) { + // \TODO(anyone) This is a temporary fix for + // gazebosim/gz-sim#2165 until gazebosim/gz-sim#2217 is resolved. + if (kNullEntity == this->dataPtr->model.Entity()) + { + return; + } + + if (!this->dataPtr->model.Valid(_ecm)) + { + gzwarn << "OdometryPublisher model no longer valid. " + << "Disabling plugin." << std::endl; + this->dataPtr->model = Model(kNullEntity); return; } @@ -305,14 +316,27 @@ void OdometryPublisher::PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) { GZ_PROFILE("OdometryPublisher::PostUpdate"); - // Nothing left to do if paused. - if (_info.paused) + gzwarn << "HElllo Anton" << std::endl; + + // \TODO(anyone) This is a temporary fix for + // gazebosim/gz-sim#2165 until gazebosim/gz-sim#2217 is resolved. + if (kNullEntity == this->dataPtr->model.Entity()) + { return; + } - if (!this->dataPtr->model.Valid(_ecm)) { + if (!this->dataPtr->model.Valid(_ecm)) + { + gzwarn << "OdometryPublisher model no longer valid. " + << "Disabling plugin." << std::endl; + this->dataPtr->model = Model(kNullEntity); return; } + // Nothing left to do if paused. + if (_info.paused) + return; + this->dataPtr->UpdateOdometry(_info, _ecm); } From 9d535de56f09fc1ac6b3099bcf315ca3b9b10f5e Mon Sep 17 00:00:00 2001 From: Anton Bogdanov Date: Sat, 2 Mar 2024 00:02:14 +0300 Subject: [PATCH 3/4] delete invalid Signed-off-by: Anton Bogdanov --- src/systems/odometry_publisher/OdometryPublisher.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index 4408ed8ecd..aceb898d19 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -316,7 +316,6 @@ void OdometryPublisher::PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) { GZ_PROFILE("OdometryPublisher::PostUpdate"); - gzwarn << "HElllo Anton" << std::endl; // \TODO(anyone) This is a temporary fix for // gazebosim/gz-sim#2165 until gazebosim/gz-sim#2217 is resolved. From 50a9b4e87ec1d730cfbffd2400ef87dd727a729f Mon Sep 17 00:00:00 2001 From: Anton Bogdanov Date: Sat, 2 Mar 2024 00:20:11 +0300 Subject: [PATCH 4/4] one check enough Signed-off-by: Anton Bogdanov --- src/systems/odometry_publisher/OdometryPublisher.cc | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index aceb898d19..8473328320 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -324,13 +324,6 @@ void OdometryPublisher::PostUpdate(const UpdateInfo &_info, return; } - if (!this->dataPtr->model.Valid(_ecm)) - { - gzwarn << "OdometryPublisher model no longer valid. " - << "Disabling plugin." << std::endl; - this->dataPtr->model = Model(kNullEntity); - return; - } // Nothing left to do if paused. if (_info.paused)