Skip to content

Commit

Permalink
Helper function to get an entity from an entity message (#1595)
Browse files Browse the repository at this point in the history 
Signed-off-by: Louise Poubel <[email protected]>

Co-authored-by: Alejandro Hernández Cordero <[email protected]>
  • Loading branch information
@chapulina @ahcorde
chapulina and ahcorde authored Jul 19, 2022
1 parent c565398 commit 4585958
Show file tree
Hide file tree
Showing 5 changed files with 343 additions and 37 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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})

#--------------------------------------
Expand Down
22 changes: 22 additions & 0 deletions include/ignition/gazebo/Util.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,8 @@
#ifndef IGNITION_GAZEBO_UTIL_HH_
#define IGNITION_GAZEBO_UTIL_HH_

#include <ignition/msgs/entity.pb.h>

#include <string>
#include <unordered_set>
#include <vector>
Expand Down Expand Up @@ -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"};

Expand Down
80 changes: 80 additions & 0 deletions src/Util.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -527,6 +527,86 @@ std::string validTopic(const std::vector<std::string> &_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<components::Light>(entity))
{
return entity;
}

if (_msg.type() == msgs::Entity::MODEL &&
_ecm.Component<components::Model>(entity))
{
return entity;
}

if (_msg.type() == msgs::Entity::LINK &&
_ecm.Component<components::Link>(entity))
{
return entity;
}

if (_msg.type() == msgs::Entity::VISUAL &&
_ecm.Component<components::Visual>(entity))
{
return entity;
}

if (_msg.type() == msgs::Entity::COLLISION &&
_ecm.Component<components::Collision>(entity))
{
return entity;
}

if (_msg.type() == msgs::Entity::SENSOR &&
_ecm.Component<components::Sensor>(entity))
{
return entity;
}

if (_msg.type() == msgs::Entity::JOINT &&
_ecm.Component<components::Joint>(entity))
{
return entity;
}

if (_msg.type() == msgs::Entity::ACTOR &&
_ecm.Component<components::Actor>(entity))
{
return entity;
}

if (_msg.type() == msgs::Entity::WORLD &&
_ecm.Component<components::World>(entity))
{
return entity;
}
}
return kNullEntity;
}
}
}
}
Expand Down
238 changes: 238 additions & 0 deletions src/Util_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -616,3 +616,241 @@ TEST_F(UtilTest, EnableComponent)
EXPECT_FALSE(enableComponent<components::Name>(ecm, entity1, false));
EXPECT_EQ(nullptr, ecm.Component<components::Name>(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)));
}
Loading

0 comments on commit 4585958

Please sign in to comment.