Skip to content

Commit

Permalink
Add support for custom sensors (#652)
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 and ahcorde authored Aug 10, 2021
1 parent 22a968e commit afdf3a5
Show file tree
Hide file tree
Showing 6 changed files with 35 additions and 6 deletions.
5 changes: 4 additions & 1 deletion include/sdf/Sensor.hh
Original file line number Diff line number Diff line change
Expand Up @@ -112,7 +112,10 @@ namespace sdf
RGBD_CAMERA = 19,

/// \brief A thermal camera sensor
THERMAL_CAMERA = 20
THERMAL_CAMERA = 20,

/// \brief A custom sensor
CUSTOM = 21
};

/// \brief Information about an SDF sensor.
Expand Down
1 change: 1 addition & 0 deletions sdf/1.9/sensor.sdf
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
altimeter,
camera,
contact,
custom,
depth_camera, depth,
force_torque,
gps,
Expand Down
7 changes: 6 additions & 1 deletion src/Sensor.cc
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,8 @@ const std::vector<std::string> sensorTypeStrs =
"wireless_transmitter",
"air_pressure",
"rgbd_camera",
"thermal_camera"
"thermal_camera",
"custom"
};

class sdf::Sensor::Implementation
Expand Down Expand Up @@ -245,6 +246,10 @@ Errors Sensor::Load(ElementPtr _sdf)
{
this->dataPtr->type = SensorType::CONTACT;
}
else if (type == "custom")
{
this->dataPtr->type = SensorType::CUSTOM;
}
else if (type == "depth" || type == "depth_camera")
{
this->dataPtr->type = SensorType::DEPTH_CAMERA;
Expand Down
6 changes: 4 additions & 2 deletions src/Sensor_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -259,7 +259,8 @@ TEST(DOMSensor, Type)
sdf::SensorType::SONAR,
sdf::SensorType::WIRELESS_RECEIVER,
sdf::SensorType::WIRELESS_TRANSMITTER,
sdf::SensorType::THERMAL_CAMERA
sdf::SensorType::THERMAL_CAMERA,
sdf::SensorType::CUSTOM
};
std::vector<std::string> typeStrs =
{
Expand All @@ -281,7 +282,8 @@ TEST(DOMSensor, Type)
"sonar",
"wireless_receiver",
"wireless_transmitter",
"thermal_camera"
"thermal_camera",
"custom"
};

for (size_t i = 0; i < types.size(); ++i)
Expand Down
15 changes: 14 additions & 1 deletion test/integration/link_dom.cc
Original file line number Diff line number Diff line change
Expand Up @@ -242,7 +242,7 @@ TEST(DOMLink, Sensors)
const sdf::Link *link = model->LinkByIndex(0);
ASSERT_NE(nullptr, link);
EXPECT_EQ("link", link->Name());
EXPECT_EQ(22u, link->SensorCount());
EXPECT_EQ(23u, link->SensorCount());

// Get the altimeter sensor
const sdf::Sensor *altimeterSensor = link->SensorByIndex(0);
Expand Down Expand Up @@ -314,6 +314,19 @@ TEST(DOMLink, Sensors)
EXPECT_TRUE(contactSensor->SemanticPose().Resolve(pose).empty());
EXPECT_EQ(ignition::math::Pose3d(4, 5, 3, 0, 0, 0), pose);

// Get the custom sensor
const sdf::Sensor *customSensor = link->SensorByName("custom_sensor");
ASSERT_NE(nullptr, customSensor);
EXPECT_EQ("custom_sensor", customSensor->Name());
EXPECT_EQ(sdf::SensorType::CUSTOM, customSensor->Type());
ASSERT_NE(nullptr, customSensor->Element());
auto myNamespaceElem = customSensor->Element()->GetElement(
"my_namespace:custom_sensor");
ASSERT_NE(nullptr, myNamespaceElem);
auto someParamElem = myNamespaceElem->GetElement("some_param");
ASSERT_NE(nullptr, someParamElem);
EXPECT_EQ(123, someParamElem->Get<int>());

// Get the depth sensor
const sdf::Sensor *depthSensor = link->SensorByName("depth_sensor");
ASSERT_NE(nullptr, depthSensor);
Expand Down
7 changes: 6 additions & 1 deletion test/sdf/sensors.sdf
Original file line number Diff line number Diff line change
Expand Up @@ -80,6 +80,12 @@
<pose relative_to="__model__">4 5 6 0 0 0</pose>
</sensor>

<sensor name="custom_sensor" type="custom">
<my_namespace:custom_sensor>
<some_param>123</some_param>
</my_namespace:custom_sensor>
</sensor>

<sensor name="depth_sensor" type="depth">
<pose>7 8 9 0 0 0</pose>
<camera name="my_depth_camera">
Expand Down Expand Up @@ -308,7 +314,6 @@
</lidar>
</sensor>


<sensor name="rfid_sensor" type="rfid">
<pose>4 5 6 0 0 0</pose>
</sensor>
Expand Down

0 comments on commit afdf3a5

Please sign in to comment.