From 8b40b546e050ad1e8a3e08897e8146a4869d6d3e Mon Sep 17 00:00:00 2001
From: jrutgeer <johan.rutgeerts@lancewood.eu>
Date: Wed, 22 Nov 2023 07:36:43 +0100
Subject: [PATCH] Allow removal of model that has joint_position_controller
 plugin. (#2252)

* Temporary fix for gazebosim/gz-sim#2165 until gazebosim/gz-sim#2217 is resolved.

Signed-off-by: Johan Rutgeerts <johan.rutgeerts@lancewood.eu>

* Added comment to clarify the added code.

Signed-off-by: Johan Rutgeerts <johan.rutgeerts@lancewood.eu>

---------

Signed-off-by: Johan Rutgeerts <johan.rutgeerts@lancewood.eu>
---
 .../JointPositionController.cc                    | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc
index 154b1f8ccc..80d36c2f9e 100644
--- a/src/systems/joint_position_controller/JointPositionController.cc
+++ b/src/systems/joint_position_controller/JointPositionController.cc
@@ -320,6 +320,21 @@ void JointPositionController::PreUpdate(
 {
   GZ_PROFILE("JointPositionController::PreUpdate");
 
+  // \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 << "JointPositionController model no longer valid. "
+           << "Disabling plugin." << std::endl;
+    this->dataPtr->model = Model(kNullEntity);
+    return;
+  }
+
   // \TODO(anyone) Support rewind
   if (_info.dt < std::chrono::steady_clock::duration::zero())
   {