Link.hh: add Sensor accessor APIs (#2693)

This adds SensorByName, Sensors, and SensorCount accessors
to the Link class with associated python bindings.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
(cherry picked from commit 7f002d6)

Author: scpeters

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<sim::Entity> 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<sim::Entity> 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.

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,13 +70,20 @@ 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.")
   .def("collision_count", &gz::sim::Link::CollisionCount,
       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 "

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))

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<Entity> Link::Collisions(const EntityComponentManager &_ecm) const
       components::Collision());
 }
 
+//////////////////////////////////////////////////
+std::vector<Entity> Link::Sensors(const EntityComponentManager &_ecm) const
+{
+  return _ecm.EntitiesByComponents(
+      components::ParentEntity(this->dataPtr->id),
+      components::Sensor());
+}
+
 //////////////////////////////////////////////////
 std::vector<Entity> 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
 {

src/Link_TEST.cc

@@ -17,7 +17,12 @@
 
 #include <gtest/gtest.h>
 
+#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"));
+}

test/integration/link.cc

@@ -41,6 +41,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/EntityComponentManager.hh>
@@ -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<components::Sensor>(eSensor, components::Sensor());
+  ecm.CreateComponent<components::ParentEntity>(eSensor,
+      components::ParentEntity(eLink));
+  ecm.CreateComponent<components::Name>(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)
 {