From 4585958ea9f4368ebf70605301e875d33760ccf9 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 19 Jul 2022 11:14:28 -0700 Subject: [PATCH] Helper function to get an entity from an entity message (#1595) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel Co-authored-by: Alejandro Hernández Cordero --- CMakeLists.txt | 2 +- include/ignition/gazebo/Util.hh | 22 ++ src/Util.cc | 80 ++++++++ src/Util_TEST.cc | 238 ++++++++++++++++++++++ src/systems/user_commands/UserCommands.cc | 38 +--- 5 files changed, 343 insertions(+), 37 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4e2f9e21cc..b4d4db3684 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,7 +62,7 @@ set(IGN_TRANSPORT_VER ${ignition-transport8_VERSION_MAJOR}) #-------------------------------------- # Find ignition-msgs -ign_find_package(ignition-msgs5 REQUIRED VERSION 5.3) +ign_find_package(ignition-msgs5 REQUIRED VERSION 5.10) set(IGN_MSGS_VER ${ignition-msgs5_VERSION_MAJOR}) #-------------------------------------- diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index d4e9274ea7..4221f8c5d5 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -17,6 +17,8 @@ #ifndef IGNITION_GAZEBO_UTIL_HH_ #define IGNITION_GAZEBO_UTIL_HH_ +#include + #include #include #include @@ -218,6 +220,26 @@ namespace ignition return changed; } + /// \brief Helper function to get an entity from an entity message. + /// The returned entity is not guaranteed to exist. + /// + /// The message is used as follows: + /// + /// if id not null + /// use id + /// else if name not null and type not null + /// use name + type + /// else + /// error + /// end + /// + /// \param[in] _ecm Entity component manager + /// \param[in] _msg Entity message + /// \return Entity ID, or kNullEntity if a matching entity couldn't be + /// found. + Entity IGNITION_GAZEBO_VISIBLE entityFromMsg( + const EntityComponentManager &_ecm, const msgs::Entity &_msg); + /// \brief Environment variable holding resource paths. const std::string kResourcePathEnv{"IGN_GAZEBO_RESOURCE_PATH"}; diff --git a/src/Util.cc b/src/Util.cc index ee779f83d5..2acccedc42 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -527,6 +527,86 @@ std::string validTopic(const std::vector &_topics) } return std::string(); } + +////////////////////////////////////////////////// +Entity entityFromMsg(const EntityComponentManager &_ecm, + const msgs::Entity &_msg) +{ + if (_msg.id() != kNullEntity) + { + return _msg.id(); + } + + // If there's no ID, check name + type + if (_msg.type() == msgs::Entity::NONE) + { + return kNullEntity; + } + + auto entities = entitiesFromScopedName(_msg.name(), _ecm); + if (entities.empty()) + { + return kNullEntity; + } + + for (const auto &entity : entities) + { + if (_msg.type() == msgs::Entity::LIGHT && + _ecm.Component(entity)) + { + return entity; + } + + if (_msg.type() == msgs::Entity::MODEL && + _ecm.Component(entity)) + { + return entity; + } + + if (_msg.type() == msgs::Entity::LINK && + _ecm.Component(entity)) + { + return entity; + } + + if (_msg.type() == msgs::Entity::VISUAL && + _ecm.Component(entity)) + { + return entity; + } + + if (_msg.type() == msgs::Entity::COLLISION && + _ecm.Component(entity)) + { + return entity; + } + + if (_msg.type() == msgs::Entity::SENSOR && + _ecm.Component(entity)) + { + return entity; + } + + if (_msg.type() == msgs::Entity::JOINT && + _ecm.Component(entity)) + { + return entity; + } + + if (_msg.type() == msgs::Entity::ACTOR && + _ecm.Component(entity)) + { + return entity; + } + + if (_msg.type() == msgs::Entity::WORLD && + _ecm.Component(entity)) + { + return entity; + } + } + return kNullEntity; +} } } } diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index f4b5054711..55194f3a28 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -616,3 +616,241 @@ TEST_F(UtilTest, EnableComponent) EXPECT_FALSE(enableComponent(ecm, entity1, false)); EXPECT_EQ(nullptr, ecm.Component(entity1)); } + +///////////////////////////////////////////////// +TEST_F(UtilTest, EntityFromMsg) +{ + EntityComponentManager ecm; + + // world + // - lightA (light) + // - modelB + // - linkB (link) + // - lightB (light) + // - sensorB (banana) + // - modelC + // - linkC (link) + // - collisionC + // - visualC (banana) + // - jointC + // - modelCC + // - linkCC (link) + // - actorD + + // World + auto worldEntity = ecm.CreateEntity(); + EXPECT_EQ(kNullEntity, gazebo::worldEntity(ecm)); + EXPECT_EQ(kNullEntity, gazebo::worldEntity(worldEntity, ecm)); + ecm.CreateComponent(worldEntity, components::World()); + ecm.CreateComponent(worldEntity, components::Name("world")); + + // Light A + auto lightAEntity = ecm.CreateEntity(); + ecm.CreateComponent(lightAEntity, components::Light(sdf::Light())); + ecm.CreateComponent(lightAEntity, components::Name("light")); + ecm.CreateComponent(lightAEntity, components::ParentEntity(worldEntity)); + + // Model B + auto modelBEntity = ecm.CreateEntity(); + ecm.CreateComponent(modelBEntity, components::Model()); + ecm.CreateComponent(modelBEntity, components::Name("modelB")); + ecm.CreateComponent(modelBEntity, components::ParentEntity(worldEntity)); + + // Link B + auto linkBEntity = ecm.CreateEntity(); + ecm.CreateComponent(linkBEntity, components::Link()); + ecm.CreateComponent(linkBEntity, components::Name("link")); + ecm.CreateComponent(linkBEntity, components::ParentEntity(modelBEntity)); + + // Light B + auto lightBEntity = ecm.CreateEntity(); + ecm.CreateComponent(lightBEntity, components::Light(sdf::Light())); + ecm.CreateComponent(lightBEntity, components::Name("light")); + ecm.CreateComponent(lightBEntity, components::ParentEntity(linkBEntity)); + + // Sensor B + auto sensorBEntity = ecm.CreateEntity(); + ecm.CreateComponent(sensorBEntity, components::Sensor()); + ecm.CreateComponent(sensorBEntity, components::Name("banana")); + ecm.CreateComponent(sensorBEntity, components::ParentEntity(linkBEntity)); + + // Model C + auto modelCEntity = ecm.CreateEntity(); + ecm.CreateComponent(modelCEntity, components::Model()); + ecm.CreateComponent(modelCEntity, components::Name("modelC")); + ecm.CreateComponent(modelCEntity, components::ParentEntity(worldEntity)); + + // Link C + auto linkCEntity = ecm.CreateEntity(); + ecm.CreateComponent(linkCEntity, components::Link()); + ecm.CreateComponent(linkCEntity, components::Name("link")); + ecm.CreateComponent(linkCEntity, components::ParentEntity(modelCEntity)); + + // Collision C + auto collisionCEntity = ecm.CreateEntity(); + ecm.CreateComponent(collisionCEntity, components::Collision()); + ecm.CreateComponent(collisionCEntity, components::Name("collisionC")); + ecm.CreateComponent(collisionCEntity, components::ParentEntity(linkCEntity)); + + // Visual C + auto visualCEntity = ecm.CreateEntity(); + ecm.CreateComponent(visualCEntity, components::Visual()); + ecm.CreateComponent(visualCEntity, components::Name("banana")); + ecm.CreateComponent(visualCEntity, components::ParentEntity(linkCEntity)); + + // Link C + auto jointCEntity = ecm.CreateEntity(); + ecm.CreateComponent(jointCEntity, components::Joint()); + ecm.CreateComponent(jointCEntity, components::Name("jointC")); + ecm.CreateComponent(jointCEntity, components::ParentEntity(modelCEntity)); + + // Model CC + auto modelCCEntity = ecm.CreateEntity(); + ecm.CreateComponent(modelCCEntity, components::Model()); + ecm.CreateComponent(modelCCEntity, components::Name("modelCC")); + ecm.CreateComponent(modelCCEntity, components::ParentEntity(modelCEntity)); + + // Link CC + auto linkCCEntity = ecm.CreateEntity(); + ecm.CreateComponent(linkCCEntity, components::Link()); + ecm.CreateComponent(linkCCEntity, components::Name("link")); + ecm.CreateComponent(linkCCEntity, components::ParentEntity(modelCCEntity)); + + // Actor D + auto actorDEntity = ecm.CreateEntity(); + ecm.CreateComponent(actorDEntity, components::Actor(sdf::Actor())); + ecm.CreateComponent(actorDEntity, components::Name("actorD")); + ecm.CreateComponent(actorDEntity, components::ParentEntity(worldEntity)); + + // Check entities + auto createMsg = [&ecm](Entity _id, const std::string &_name = "", + msgs::Entity::Type _type = msgs::Entity::NONE) -> msgs::Entity + { + msgs::Entity msg; + + if (_id != kNullEntity) + msg.set_id(_id); + + if (!_name.empty()) + msg.set_name(_name); + + if (_type != msgs::Entity::NONE) + msg.set_type(_type); + + return msg; + }; + + // Only ID + EXPECT_EQ(worldEntity, entityFromMsg(ecm, createMsg(worldEntity))); + EXPECT_EQ(lightAEntity, entityFromMsg(ecm, createMsg(lightAEntity))); + EXPECT_EQ(modelBEntity, entityFromMsg(ecm, createMsg(modelBEntity))); + EXPECT_EQ(linkBEntity, entityFromMsg(ecm, createMsg(linkBEntity))); + EXPECT_EQ(lightBEntity, entityFromMsg(ecm, createMsg(lightBEntity))); + EXPECT_EQ(sensorBEntity, entityFromMsg(ecm, createMsg(sensorBEntity))); + EXPECT_EQ(modelCEntity, entityFromMsg(ecm, createMsg(modelCEntity))); + EXPECT_EQ(linkCEntity, entityFromMsg(ecm, createMsg(linkCEntity))); + EXPECT_EQ(collisionCEntity, entityFromMsg(ecm, createMsg(collisionCEntity))); + EXPECT_EQ(visualCEntity, entityFromMsg(ecm, createMsg(visualCEntity))); + EXPECT_EQ(jointCEntity, entityFromMsg(ecm, createMsg(jointCEntity))); + EXPECT_EQ(modelCCEntity, entityFromMsg(ecm, createMsg(modelCCEntity))); + EXPECT_EQ(linkCCEntity, entityFromMsg(ecm, createMsg(linkCCEntity))); + EXPECT_EQ(actorDEntity, entityFromMsg(ecm, createMsg(actorDEntity))); + + // Name and type, with different scopes + EXPECT_EQ(worldEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world", msgs::Entity::WORLD))); + + // There's more than one "light", so we need to scope it + EXPECT_EQ(lightAEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::light", msgs::Entity::LIGHT))); + + EXPECT_EQ(modelBEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelB", msgs::Entity::MODEL))); + EXPECT_EQ(modelBEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelB", msgs::Entity::MODEL))); + + // There's more than one "link", so we need to scope it + EXPECT_EQ(linkBEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelB::link", msgs::Entity::LINK))); + EXPECT_EQ(linkBEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelB::link", msgs::Entity::LINK))); + + // There's more than one "light", so we need to scope it + EXPECT_EQ(lightBEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "link::light", msgs::Entity::LIGHT))); + EXPECT_EQ(lightBEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelB::link::light", msgs::Entity::LIGHT))); + EXPECT_EQ(lightBEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelB::link::light", msgs::Entity::LIGHT))); + + // There's more than one "link::banana", so we need to scope it + EXPECT_EQ(sensorBEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelB::link::banana", msgs::Entity::SENSOR))); + EXPECT_EQ(sensorBEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelB::link::banana", msgs::Entity::SENSOR))); + + EXPECT_EQ(modelCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelC", msgs::Entity::MODEL))); + EXPECT_EQ(modelCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelC", msgs::Entity::MODEL))); + + // There's more than one "link", so we need to scope it + EXPECT_EQ(linkCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelC::link", msgs::Entity::LINK))); + EXPECT_EQ(linkCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelC::link", msgs::Entity::LINK))); + + EXPECT_EQ(collisionCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "collisionC", msgs::Entity::COLLISION))); + EXPECT_EQ(collisionCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "link::collisionC", msgs::Entity::COLLISION))); + EXPECT_EQ(collisionCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelC::link::collisionC", msgs::Entity::COLLISION))); + EXPECT_EQ(collisionCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelC::link::collisionC", msgs::Entity::COLLISION))); + + // There's more than one "banana", so we need to scope it + EXPECT_EQ(visualCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "link::banana", msgs::Entity::VISUAL))); + EXPECT_EQ(visualCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelC::link::banana", msgs::Entity::VISUAL))); + EXPECT_EQ(visualCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelC::link::banana", msgs::Entity::VISUAL))); + + EXPECT_EQ(jointCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "jointC", msgs::Entity::JOINT))); + EXPECT_EQ(jointCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelC::jointC", msgs::Entity::JOINT))); + EXPECT_EQ(jointCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelC::jointC", msgs::Entity::JOINT))); + + EXPECT_EQ(modelCCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelCC", msgs::Entity::MODEL))); + EXPECT_EQ(modelCCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelC::modelCC", msgs::Entity::MODEL))); + EXPECT_EQ(modelCCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelC::modelCC", msgs::Entity::MODEL))); + + // There's more than one "link", so we need to scope it + EXPECT_EQ(linkCCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "modelC::modelCC::link", msgs::Entity::LINK))); + EXPECT_EQ(linkCCEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::modelC::modelCC::link", msgs::Entity::LINK))); + + EXPECT_EQ(actorDEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "actorD", msgs::Entity::ACTOR))); + EXPECT_EQ(actorDEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "world::actorD", msgs::Entity::ACTOR))); + + // Inexistent entity + EXPECT_EQ(1000u, entityFromMsg(ecm, createMsg(1000))); + + // Not found + EXPECT_EQ(kNullEntity, entityFromMsg(ecm, createMsg(kNullEntity))); + EXPECT_EQ(kNullEntity, entityFromMsg(ecm, createMsg(kNullEntity, + "blueberry"))); + EXPECT_EQ(kNullEntity, entityFromMsg(ecm, createMsg(kNullEntity, "banana", + msgs::Entity::COLLISION))); + EXPECT_EQ(kNullEntity, entityFromMsg(ecm, createMsg(kNullEntity, "peach", + msgs::Entity::WORLD))); +} diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 65d72ec60f..4920c5cb07 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -50,6 +50,7 @@ #include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/SdfEntityCreator.hh" +#include "ignition/gazebo/Util.hh" using namespace ignition; using namespace gazebo; @@ -669,42 +670,7 @@ bool RemoveCommand::Execute() return false; } - Entity entity{kNullEntity}; - if (removeMsg->id() != kNullEntity) - { - entity = removeMsg->id(); - } - else if (!removeMsg->name().empty() && - removeMsg->type() != msgs::Entity::NONE) - { - if (removeMsg->type() == msgs::Entity::MODEL) - { - entity = this->iface->ecm->EntityByComponents(components::Model(), - components::Name(removeMsg->name())); - } - else if (removeMsg->type() == msgs::Entity::LIGHT) - { - entity = this->iface->ecm->EntityByComponents( - components::Name(removeMsg->name())); - - auto lightComp = this->iface->ecm->Component(entity); - if (nullptr == lightComp) - entity = kNullEntity; - } - else - { - ignerr << "Deleting entities of type [" << removeMsg->type() - << "] is not supported." << std::endl; - return false; - } - } - else - { - ignerr << "Remove command missing either entity's ID or name + type" - << std::endl; - return false; - } - + auto entity = entityFromMsg(*this->iface->ecm, *removeMsg); if (entity == kNullEntity) { ignerr << "Entity named [" << removeMsg->name() << "] of type ["