diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh index 18037896a7..24a85d0463 100644 --- a/include/gz/sim/Link.hh +++ b/include/gz/sim/Link.hh @@ -136,6 +136,14 @@ namespace gz public: sim::Entity CollisionByName(const EntityComponentManager &_ecm, const std::string &_name) const; + /// \brief Get the ID of a sensor entity which is an immediate child of + /// this link. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _name Sensor name. + /// \return Sensor entity. + public: sim::Entity SensorByName(const EntityComponentManager &_ecm, + const std::string &_name) const; + /// \brief Get the ID of a visual entity which is an immediate child of /// this link. /// \param[in] _ecm Entity-component manager. @@ -150,6 +158,12 @@ namespace gz public: std::vector Collisions( const EntityComponentManager &_ecm) const; + /// \brief Get all sensors which are immediate children of this link. + /// \param[in] _ecm Entity-component manager. + /// \return All sensors in this link. + public: std::vector Sensors( + const EntityComponentManager &_ecm) const; + /// \brief Get all visuals which are immediate children of this link. /// \param[in] _ecm Entity-component manager. /// \return All visuals in this link. @@ -162,6 +176,12 @@ namespace gz /// \return Number of collisions in this link. public: uint64_t CollisionCount(const EntityComponentManager &_ecm) const; + /// \brief Get the number of sensors which are immediate children of this + /// link. + /// \param[in] _ecm Entity-component manager. + /// \return Number of sensors in this link. + public: uint64_t SensorCount(const EntityComponentManager &_ecm) const; + /// \brief Get the number of visuals which are immediate children of this /// link. /// \param[in] _ecm Entity-component manager. diff --git a/python/src/gz/sim/Link.cc b/python/src/gz/sim/Link.cc index 1c8bb8381d..f6c425fcb4 100644 --- a/python/src/gz/sim/Link.cc +++ b/python/src/gz/sim/Link.cc @@ -57,6 +57,11 @@ void defineSimLink(py::object module) py::arg("name"), "Get the ID of a collision entity which is an immediate child of " "this link.") + .def("sensor_by_name", &gz::sim::Link::SensorByName, + py::arg("ecm"), + py::arg("name"), + "Get the ID of a sensor entity which is an immediate child of " + "this link.") .def("visual_by_name", &gz::sim::Link::VisualByName, py::arg("ecm"), py::arg("name"), @@ -65,6 +70,9 @@ void defineSimLink(py::object module) .def("collisions", &gz::sim::Link::Collisions, py::arg("ecm"), "Get all collisions which are immediate children of this link.") + .def("sensors", &gz::sim::Link::Sensors, + py::arg("ecm"), + "Get all sensors which are immediate children of this link.") .def("visuals", &gz::sim::Link::Visuals, py::arg("ecm"), "Get all visuals which are immediate children of this link.") @@ -72,6 +80,10 @@ void defineSimLink(py::object module) py::arg("ecm"), "Get the number of collisions which are immediate children of " "this link.") + .def("sensor_count", &gz::sim::Link::SensorCount, + py::arg("ecm"), + "Get the number of sensors which are immediate children of this " + "link.") .def("visual_count", &gz::sim::Link::VisualCount, py::arg("ecm"), "Get the number of visuals which are immediate children of this " diff --git a/python/test/link_TEST.py b/python/test/link_TEST.py index 4d6be57631..d28c48d9e6 100755 --- a/python/test/link_TEST.py +++ b/python/test/link_TEST.py @@ -56,6 +56,9 @@ def on_pre_udpate_cb(_info, _ecm): # Collisions Test self.assertNotEqual(K_NULL_ENTITY, link.collision_by_name(_ecm, 'collision_test')) self.assertEqual(1, link.collision_count(_ecm)) + # Sensors Test + self.assertNotEqual(K_NULL_ENTITY, link.sensor_by_name(_ecm, 'my_sensor')) + self.assertEqual(1, link.sensor_count(_ecm)) # Visuals Test self.assertNotEqual(K_NULL_ENTITY, link.visual_by_name(_ecm, 'visual_test')) self.assertEqual(1, link.visual_count(_ecm)) diff --git a/src/Link.cc b/src/Link.cc index 348923a44a..715e541766 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -37,6 +37,7 @@ #include "gz/sim/components/Name.hh" #include "gz/sim/components/ParentEntity.hh" #include "gz/sim/components/Pose.hh" +#include "gz/sim/components/Sensor.hh" #include "gz/sim/components/Visual.hh" #include "gz/sim/components/WindMode.hh" #include "gz/sim/Util.hh" @@ -126,6 +127,16 @@ Entity Link::CollisionByName(const EntityComponentManager &_ecm, components::Collision()); } +////////////////////////////////////////////////// +Entity Link::SensorByName(const EntityComponentManager &_ecm, + const std::string &_name) const +{ + return _ecm.EntityByComponents( + components::ParentEntity(this->dataPtr->id), + components::Name(_name), + components::Sensor()); +} + ////////////////////////////////////////////////// Entity Link::VisualByName(const EntityComponentManager &_ecm, const std::string &_name) const @@ -144,6 +155,14 @@ std::vector Link::Collisions(const EntityComponentManager &_ecm) const components::Collision()); } +////////////////////////////////////////////////// +std::vector Link::Sensors(const EntityComponentManager &_ecm) const +{ + return _ecm.EntitiesByComponents( + components::ParentEntity(this->dataPtr->id), + components::Sensor()); +} + ////////////////////////////////////////////////// std::vector Link::Visuals(const EntityComponentManager &_ecm) const { @@ -158,6 +177,12 @@ uint64_t Link::CollisionCount(const EntityComponentManager &_ecm) const return this->Collisions(_ecm).size(); } +////////////////////////////////////////////////// +uint64_t Link::SensorCount(const EntityComponentManager &_ecm) const +{ + return this->Sensors(_ecm).size(); +} + ////////////////////////////////////////////////// uint64_t Link::VisualCount(const EntityComponentManager &_ecm) const { diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index f5da1459f0..b2653ca2d9 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -17,7 +17,12 @@ #include +#include "gz/sim/EntityComponentManager.hh" #include "gz/sim/Link.hh" +#include "gz/sim/components/Link.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Sensor.hh" ///////////////////////////////////////////////// TEST(LinkTest, Constructor) @@ -74,3 +79,63 @@ TEST(LinkTest, MoveAssignmentOperator) linkMoved = std::move(link); EXPECT_EQ(id, linkMoved.Entity()); } + +///////////////////////////////////////////////// +TEST(LinkTest, Sensors) +{ + // linkA + // - sensorAA + // - sensorAB + // + // linkC + + gz::sim::EntityComponentManager ecm; + + // Link A + auto linkAEntity = ecm.CreateEntity(); + ecm.CreateComponent(linkAEntity, gz::sim::components::Link()); + ecm.CreateComponent(linkAEntity, + gz::sim::components::Name("linkA_name")); + + // Sensor AA - Child of Link A + auto sensorAAEntity = ecm.CreateEntity(); + ecm.CreateComponent(sensorAAEntity, gz::sim::components::Sensor()); + ecm.CreateComponent(sensorAAEntity, + gz::sim::components::Name("sensorAA_name")); + ecm.CreateComponent(sensorAAEntity, + gz::sim::components::ParentEntity(linkAEntity)); + + // Sensor AB - Child of Link A + auto sensorABEntity = ecm.CreateEntity(); + ecm.CreateComponent(sensorABEntity, gz::sim::components::Sensor()); + ecm.CreateComponent(sensorABEntity, + gz::sim::components::Name("sensorAB_name")); + ecm.CreateComponent(sensorABEntity, + gz::sim::components::ParentEntity(linkAEntity)); + + // Link C + auto linkCEntity = ecm.CreateEntity(); + ecm.CreateComponent(linkCEntity, gz::sim::components::Link()); + ecm.CreateComponent(linkCEntity, + gz::sim::components::Name("linkC_name")); + + std::size_t foundSensors = 0; + + gz::sim::Link linkA(linkAEntity); + auto sensors = linkA.Sensors(ecm); + EXPECT_EQ(2u, sensors.size()); + for (const auto &sensor : sensors) + { + if (sensor == sensorAAEntity || sensor == sensorABEntity) + foundSensors++; + } + EXPECT_EQ(foundSensors, sensors.size()); + + EXPECT_EQ(sensorAAEntity, linkA.SensorByName(ecm, "sensorAA_name")); + EXPECT_EQ(sensorABEntity, linkA.SensorByName(ecm, "sensorAB_name")); + EXPECT_EQ(gz::sim::kNullEntity, linkA.SensorByName(ecm, "invalid")); + + gz::sim::Link linkC(linkCEntity); + EXPECT_EQ(0u, linkC.Sensors(ecm).size()); + EXPECT_EQ(gz::sim::kNullEntity, linkC.SensorByName(ecm, "invalid")); +} diff --git a/test/integration/link.cc b/test/integration/link.cc index 9e140a9499..c706c1135a 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -41,6 +41,7 @@ #include #include #include +#include #include #include @@ -170,6 +171,30 @@ TEST_F(LinkIntegrationTest, VisualByName) EXPECT_EQ(1u, link.VisualCount(ecm)); } +////////////////////////////////////////////////// +TEST_F(LinkIntegrationTest, SensorByName) +{ + EntityComponentManager ecm; + + // Link + auto eLink = ecm.CreateEntity(); + Link link(eLink); + EXPECT_EQ(eLink, link.Entity()); + EXPECT_EQ(0u, link.SensorCount(ecm)); + + // Sensor + auto eSensor = ecm.CreateEntity(); + ecm.CreateComponent(eSensor, components::Sensor()); + ecm.CreateComponent(eSensor, + components::ParentEntity(eLink)); + ecm.CreateComponent(eSensor, + components::Name("sensor_name")); + + // Check link + EXPECT_EQ(eSensor, link.SensorByName(ecm, "sensor_name")); + EXPECT_EQ(1u, link.SensorCount(ecm)); +} + ////////////////////////////////////////////////// TEST_F(LinkIntegrationTest, CollisionByName) {