From 3f577d5579c0c1b1c494e195940313795c8b577c Mon Sep 17 00:00:00 2001 From: Addisu Taddese Date: Tue, 28 Jul 2020 10:25:23 -0500 Subject: [PATCH] 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 --- .../performer_detector/PerformerDetector.cc | 9 +++ test/integration/performer_detector.cc | 81 ++++++++++++++++++- test/worlds/performer_detector.sdf | 10 +++ 3 files changed, 98 insertions(+), 2 deletions(-) diff --git a/src/systems/performer_detector/PerformerDetector.cc b/src/systems/performer_detector/PerformerDetector.cc index 7d45f24ee2..9aa1dc3a45 100644 --- 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 index d445614201..58f7be3af4 100644 --- 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 StartServer(const std::string &_filePath) + protected: std::unique_ptr 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(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(vehicle); + EXPECT_NEAR(-100.0, pose->Data().Pos().X(), 1e-3); + ASSERT_TRUE(nullptr == _ecm.Component(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(vehicle); + EXPECT_NEAR(5, pose->Data().Pos().X(), 1e-3); + } + }); + + server->AddSystem(testSystem.systemPtr); + + transport::Node node; + auto cmdVelPub = node.Advertise("/model/vehicle_blue/cmd_vel"); + + auto detectorCb = std::function( + [this](const auto &_msg) + { + std::lock_guard 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 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 index a9bbc52818..0312aa4033 100644 --- a/test/worlds/performer_detector.sdf +++ b/test/worlds/performer_detector.sdf @@ -373,6 +373,16 @@ + + 5 3 2.5 0 0 0 + + + 30 30 10 + + + 5 + detector2 +