Skip to content

Commit 7f002d6

Browse files
scpetersscpeters
authored
Link.hh: add Sensor accessor APIs (gazebosim#2693)
This adds SensorByName, Sensors, and SensorCount accessors to the Link class with associated python bindings. Signed-off-by: Steve Peters <scpeters@openrobotics.org>
1 parent aa76c3e commit 7f002d6

File tree

6 files changed

+150
-0
lines changed

6 files changed

+150
-0
lines changed

‎include/gz/sim/Link.hh

+20
Original file line numberDiff line numberDiff line change
@@ -136,6 +136,14 @@ namespace gz
136136
public: sim::Entity CollisionByName(const EntityComponentManager &_ecm,
137137
const std::string &_name) const;
138138

139+
/// \brief Get the ID of a sensor entity which is an immediate child of
140+
/// this link.
141+
/// \param[in] _ecm Entity-component manager.
142+
/// \param[in] _name Sensor name.
143+
/// \return Sensor entity.
144+
public: sim::Entity SensorByName(const EntityComponentManager &_ecm,
145+
const std::string &_name) const;
146+
139147
/// \brief Get the ID of a visual entity which is an immediate child of
140148
/// this link.
141149
/// \param[in] _ecm Entity-component manager.
@@ -150,6 +158,12 @@ namespace gz
150158
public: std::vector<sim::Entity> Collisions(
151159
const EntityComponentManager &_ecm) const;
152160

161+
/// \brief Get all sensors which are immediate children of this link.
162+
/// \param[in] _ecm Entity-component manager.
163+
/// \return All sensors in this link.
164+
public: std::vector<sim::Entity> Sensors(
165+
const EntityComponentManager &_ecm) const;
166+
153167
/// \brief Get all visuals which are immediate children of this link.
154168
/// \param[in] _ecm Entity-component manager.
155169
/// \return All visuals in this link.
@@ -162,6 +176,12 @@ namespace gz
162176
/// \return Number of collisions in this link.
163177
public: uint64_t CollisionCount(const EntityComponentManager &_ecm) const;
164178

179+
/// \brief Get the number of sensors which are immediate children of this
180+
/// link.
181+
/// \param[in] _ecm Entity-component manager.
182+
/// \return Number of sensors in this link.
183+
public: uint64_t SensorCount(const EntityComponentManager &_ecm) const;
184+
165185
/// \brief Get the number of visuals which are immediate children of this
166186
/// link.
167187
/// \param[in] _ecm Entity-component manager.

‎python/src/gz/sim/Link.cc

+12
Original file line numberDiff line numberDiff line change
@@ -57,6 +57,11 @@ void defineSimLink(py::object module)
5757
py::arg("name"),
5858
"Get the ID of a collision entity which is an immediate child of "
5959
"this link.")
60+
.def("sensor_by_name", &gz::sim::Link::SensorByName,
61+
py::arg("ecm"),
62+
py::arg("name"),
63+
"Get the ID of a sensor entity which is an immediate child of "
64+
"this link.")
6065
.def("visual_by_name", &gz::sim::Link::VisualByName,
6166
py::arg("ecm"),
6267
py::arg("name"),
@@ -65,13 +70,20 @@ void defineSimLink(py::object module)
6570
.def("collisions", &gz::sim::Link::Collisions,
6671
py::arg("ecm"),
6772
"Get all collisions which are immediate children of this link.")
73+
.def("sensors", &gz::sim::Link::Sensors,
74+
py::arg("ecm"),
75+
"Get all sensors which are immediate children of this link.")
6876
.def("visuals", &gz::sim::Link::Visuals,
6977
py::arg("ecm"),
7078
"Get all visuals which are immediate children of this link.")
7179
.def("collision_count", &gz::sim::Link::CollisionCount,
7280
py::arg("ecm"),
7381
"Get the number of collisions which are immediate children of "
7482
"this link.")
83+
.def("sensor_count", &gz::sim::Link::SensorCount,
84+
py::arg("ecm"),
85+
"Get the number of sensors which are immediate children of this "
86+
"link.")
7587
.def("visual_count", &gz::sim::Link::VisualCount,
7688
py::arg("ecm"),
7789
"Get the number of visuals which are immediate children of this "

‎python/test/link_TEST.py

+3
Original file line numberDiff line numberDiff line change
@@ -56,6 +56,9 @@ def on_pre_udpate_cb(_info, _ecm):
5656
# Collisions Test
5757
self.assertNotEqual(K_NULL_ENTITY, link.collision_by_name(_ecm, 'collision_test'))
5858
self.assertEqual(1, link.collision_count(_ecm))
59+
# Sensors Test
60+
self.assertNotEqual(K_NULL_ENTITY, link.sensor_by_name(_ecm, 'my_sensor'))
61+
self.assertEqual(1, link.sensor_count(_ecm))
5962
# Visuals Test
6063
self.assertNotEqual(K_NULL_ENTITY, link.visual_by_name(_ecm, 'visual_test'))
6164
self.assertEqual(1, link.visual_count(_ecm))

‎src/Link.cc

+25
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,7 @@
3737
#include "gz/sim/components/Name.hh"
3838
#include "gz/sim/components/ParentEntity.hh"
3939
#include "gz/sim/components/Pose.hh"
40+
#include "gz/sim/components/Sensor.hh"
4041
#include "gz/sim/components/Visual.hh"
4142
#include "gz/sim/components/WindMode.hh"
4243
#include "gz/sim/Util.hh"
@@ -126,6 +127,16 @@ Entity Link::CollisionByName(const EntityComponentManager &_ecm,
126127
components::Collision());
127128
}
128129

130+
//////////////////////////////////////////////////
131+
Entity Link::SensorByName(const EntityComponentManager &_ecm,
132+
const std::string &_name) const
133+
{
134+
return _ecm.EntityByComponents(
135+
components::ParentEntity(this->dataPtr->id),
136+
components::Name(_name),
137+
components::Sensor());
138+
}
139+
129140
//////////////////////////////////////////////////
130141
Entity Link::VisualByName(const EntityComponentManager &_ecm,
131142
const std::string &_name) const
@@ -144,6 +155,14 @@ std::vector<Entity> Link::Collisions(const EntityComponentManager &_ecm) const
144155
components::Collision());
145156
}
146157

158+
//////////////////////////////////////////////////
159+
std::vector<Entity> Link::Sensors(const EntityComponentManager &_ecm) const
160+
{
161+
return _ecm.EntitiesByComponents(
162+
components::ParentEntity(this->dataPtr->id),
163+
components::Sensor());
164+
}
165+
147166
//////////////////////////////////////////////////
148167
std::vector<Entity> Link::Visuals(const EntityComponentManager &_ecm) const
149168
{
@@ -158,6 +177,12 @@ uint64_t Link::CollisionCount(const EntityComponentManager &_ecm) const
158177
return this->Collisions(_ecm).size();
159178
}
160179

180+
//////////////////////////////////////////////////
181+
uint64_t Link::SensorCount(const EntityComponentManager &_ecm) const
182+
{
183+
return this->Sensors(_ecm).size();
184+
}
185+
161186
//////////////////////////////////////////////////
162187
uint64_t Link::VisualCount(const EntityComponentManager &_ecm) const
163188
{

‎src/Link_TEST.cc

+65
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,12 @@
1717

1818
#include <gtest/gtest.h>
1919

20+
#include "gz/sim/EntityComponentManager.hh"
2021
#include "gz/sim/Link.hh"
22+
#include "gz/sim/components/Link.hh"
23+
#include "gz/sim/components/Name.hh"
24+
#include "gz/sim/components/ParentEntity.hh"
25+
#include "gz/sim/components/Sensor.hh"
2126

2227
/////////////////////////////////////////////////
2328
TEST(LinkTest, Constructor)
@@ -74,3 +79,63 @@ TEST(LinkTest, MoveAssignmentOperator)
7479
linkMoved = std::move(link);
7580
EXPECT_EQ(id, linkMoved.Entity());
7681
}
82+
83+
/////////////////////////////////////////////////
84+
TEST(LinkTest, Sensors)
85+
{
86+
// linkA
87+
// - sensorAA
88+
// - sensorAB
89+
//
90+
// linkC
91+
92+
gz::sim::EntityComponentManager ecm;
93+
94+
// Link A
95+
auto linkAEntity = ecm.CreateEntity();
96+
ecm.CreateComponent(linkAEntity, gz::sim::components::Link());
97+
ecm.CreateComponent(linkAEntity,
98+
gz::sim::components::Name("linkA_name"));
99+
100+
// Sensor AA - Child of Link A
101+
auto sensorAAEntity = ecm.CreateEntity();
102+
ecm.CreateComponent(sensorAAEntity, gz::sim::components::Sensor());
103+
ecm.CreateComponent(sensorAAEntity,
104+
gz::sim::components::Name("sensorAA_name"));
105+
ecm.CreateComponent(sensorAAEntity,
106+
gz::sim::components::ParentEntity(linkAEntity));
107+
108+
// Sensor AB - Child of Link A
109+
auto sensorABEntity = ecm.CreateEntity();
110+
ecm.CreateComponent(sensorABEntity, gz::sim::components::Sensor());
111+
ecm.CreateComponent(sensorABEntity,
112+
gz::sim::components::Name("sensorAB_name"));
113+
ecm.CreateComponent(sensorABEntity,
114+
gz::sim::components::ParentEntity(linkAEntity));
115+
116+
// Link C
117+
auto linkCEntity = ecm.CreateEntity();
118+
ecm.CreateComponent(linkCEntity, gz::sim::components::Link());
119+
ecm.CreateComponent(linkCEntity,
120+
gz::sim::components::Name("linkC_name"));
121+
122+
std::size_t foundSensors = 0;
123+
124+
gz::sim::Link linkA(linkAEntity);
125+
auto sensors = linkA.Sensors(ecm);
126+
EXPECT_EQ(2u, sensors.size());
127+
for (const auto &sensor : sensors)
128+
{
129+
if (sensor == sensorAAEntity || sensor == sensorABEntity)
130+
foundSensors++;
131+
}
132+
EXPECT_EQ(foundSensors, sensors.size());
133+
134+
EXPECT_EQ(sensorAAEntity, linkA.SensorByName(ecm, "sensorAA_name"));
135+
EXPECT_EQ(sensorABEntity, linkA.SensorByName(ecm, "sensorAB_name"));
136+
EXPECT_EQ(gz::sim::kNullEntity, linkA.SensorByName(ecm, "invalid"));
137+
138+
gz::sim::Link linkC(linkCEntity);
139+
EXPECT_EQ(0u, linkC.Sensors(ecm).size());
140+
EXPECT_EQ(gz::sim::kNullEntity, linkC.SensorByName(ecm, "invalid"));
141+
}

‎test/integration/link.cc

+25
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,7 @@
4141
#include <gz/sim/components/Name.hh>
4242
#include <gz/sim/components/ParentEntity.hh>
4343
#include <gz/sim/components/Pose.hh>
44+
#include <gz/sim/components/Sensor.hh>
4445
#include <gz/sim/components/Visual.hh>
4546

4647
#include <gz/sim/EntityComponentManager.hh>
@@ -170,6 +171,30 @@ TEST_F(LinkIntegrationTest, VisualByName)
170171
EXPECT_EQ(1u, link.VisualCount(ecm));
171172
}
172173

174+
//////////////////////////////////////////////////
175+
TEST_F(LinkIntegrationTest, SensorByName)
176+
{
177+
EntityComponentManager ecm;
178+
179+
// Link
180+
auto eLink = ecm.CreateEntity();
181+
Link link(eLink);
182+
EXPECT_EQ(eLink, link.Entity());
183+
EXPECT_EQ(0u, link.SensorCount(ecm));
184+
185+
// Sensor
186+
auto eSensor = ecm.CreateEntity();
187+
ecm.CreateComponent<components::Sensor>(eSensor, components::Sensor());
188+
ecm.CreateComponent<components::ParentEntity>(eSensor,
189+
components::ParentEntity(eLink));
190+
ecm.CreateComponent<components::Name>(eSensor,
191+
components::Name("sensor_name"));
192+
193+
// Check link
194+
EXPECT_EQ(eSensor, link.SensorByName(ecm, "sensor_name"));
195+
EXPECT_EQ(1u, link.SensorCount(ecm));
196+
}
197+
173198
//////////////////////////////////////////////////
174199
TEST_F(LinkIntegrationTest, CollisionByName)
175200
{

0 commit comments

Comments
 (0)