Deactivate PerformerDetector if its parent model gets removed (#260)

* Failing/Crashing test showing the current behavior of PerformerDetector and levels * Deactivate PerformerDetector if its parent model gets removed Signed-off-by: Addisu Z. Taddese <[email protected]>
gazebosim · Jul 28, 2020 · 3f577d5 · 3f577d5
1 parent eae408f
commit 3f577d5
Show file tree

Hide file tree

Showing 3 changed files with 98 additions and 2 deletions.
diff --git a/src/systems/performer_detector/PerformerDetector.cc b/src/systems/performer_detector/PerformerDetector.cc
@@ -29,6 +29,7 @@
 #include "ignition/gazebo/Model.hh"
 #include "ignition/gazebo/Util.hh"
 #include "ignition/gazebo/components/Geometry.hh"
+#include "ignition/gazebo/components/Model.hh"
 #include "ignition/gazebo/components/Name.hh"
 #include "ignition/gazebo/components/ParentEntity.hh"
 #include "ignition/gazebo/components/Performer.hh"
@@ -134,6 +135,14 @@ void PerformerDetector::PostUpdate(
 {
   IGN_PROFILE("PerformerDetector::PostUpdate");
 
+  if (this->initialized && !this->model.Valid(_ecm))
+  {
+    // Deactivate this performer if the parent model has been removed, for
+    // example, by the level manager
+    this->initialized = false;
+    return;
+  }
+
   if (_info.paused)
     return;
 

diff --git a/test/integration/performer_detector.cc b/test/integration/performer_detector.cc
@@ -22,9 +22,13 @@
 
 #include "ignition/gazebo/Server.hh"
 #include "ignition/gazebo/SystemLoader.hh"
+#include "ignition/gazebo/components/Model.hh"
+#include "ignition/gazebo/components/Name.hh"
+#include "ignition/gazebo/components/Pose.hh"
+#include "ignition/gazebo/components/PoseCmd.hh"
 #include "ignition/gazebo/test_config.hh"
 
-#include "plugins/MockSystem.hh"
+#include "helpers/Relay.hh"
 
 using namespace ignition;
 using namespace gazebo;
@@ -40,11 +44,13 @@ class PerformerDetectorTest : public ::testing::Test
            (std::string(PROJECT_BINARY_PATH) + "/lib").c_str(), 1);
   }
 
-  protected: std::unique_ptr<Server> StartServer(const std::string &_filePath)
+  protected: std::unique_ptr<Server> StartServer(const std::string &_filePath,
+                                      bool _useLevels = false)
   {
     ServerConfig serverConfig;
     const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + _filePath;
     serverConfig.SetSdfFile(sdfFile);
+    serverConfig.SetUseLevels(_useLevels);
 
     auto server = std::make_unique<Server>(serverConfig);
     EXPECT_FALSE(server->Running());
@@ -177,3 +183,74 @@ TEST_F(PerformerDetectorTest, MovingPerformer)
   EXPECT_NEAR(2.5, this->poseMsgs[3].position().x(), 1e-2);
   EXPECT_NEAR(-1, this->poseMsgs[3].position().y(), 1e-2);
 }
+
+/////////////////////////////////////////////////
+// Test that Performer detector handles the case where the associated model is
+// removed, for example, by the level manager
+TEST_F(PerformerDetectorTest, HandlesRemovedParentModel)
+{
+  auto server = this->StartServer("/test/worlds/performer_detector.sdf", true);
+
+  test::Relay testSystem;
+  testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &_info,
+                             gazebo::EntityComponentManager &_ecm)
+  {
+    Entity vehicle = _ecm.EntityByComponents(
+        components::Model(), components::Name("vehicle_blue"));
+    ASSERT_FALSE(kNullEntity == vehicle);
+
+    if (_info.iterations == 2)
+    {
+      // Move vehicle out of level1
+      _ecm.CreateComponent(vehicle,
+          components::WorldPoseCmd(math::Pose3d({-100, 0, 0}, {})));
+    }
+    else if (_info.iterations == 4)
+    {
+      auto pose = _ecm.Component<components::Pose>(vehicle);
+      EXPECT_NEAR(-100.0, pose->Data().Pos().X(), 1e-3);
+      ASSERT_TRUE(nullptr == _ecm.Component<components::WorldPoseCmd>(vehicle));
+
+      // Move vehicle back into level1 and in the detectors' region
+      _ecm.CreateComponent(vehicle,
+          components::WorldPoseCmd(math::Pose3d({5, 2, 0.325}, {})));
+    }
+    else if (_info.iterations == 5)
+    {
+      auto pose = _ecm.Component<components::Pose>(vehicle);
+      EXPECT_NEAR(5, pose->Data().Pos().X(), 1e-3);
+    }
+  });
+
+  server->AddSystem(testSystem.systemPtr);
+
+  transport::Node node;
+  auto cmdVelPub = node.Advertise<msgs::Twist>("/model/vehicle_blue/cmd_vel");
+
+  auto detectorCb = std::function<void(const msgs::Pose &)>(
+      [this](const auto &_msg)
+      {
+        std::lock_guard<std::mutex> lock(this->poseMsgsMutex);
+        this->poseMsgs.push_back(_msg);
+      });
+
+  node.Subscribe("/performer_detector", detectorCb);
+
+  server->Run(true, 10, false);
+
+  // Wait for messages to arrive in poseMsgs or a timeout is reached
+  const auto timeOut = 5s;
+  auto tInit = std::chrono::steady_clock::now();
+  auto tNow = tInit;
+  while (tNow - tInit < timeOut)
+  {
+    std::this_thread::sleep_for(100ms);
+
+    std::lock_guard<std::mutex> lock(this->poseMsgsMutex);
+    if (this->poseMsgs.size() >= 1)
+      break;
+
+    tNow = std::chrono::steady_clock::now();
+  }
+  EXPECT_EQ(2u, this->poseMsgs.size());
+}
diff --git a/test/worlds/performer_detector.sdf b/test/worlds/performer_detector.sdf
@@ -373,6 +373,16 @@
         </box>
       </geometry>
     </performer>
+    <level name="level1">
+      <pose>5 3 2.5 0 0 0</pose>
+      <geometry>
+        <box>
+          <size>30 30 10</size>
+        </box>
+      </geometry>
+      <buffer>5</buffer>
+      <ref>detector2</ref>
+    </level>
   </plugin>
 
 </world>