Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add support for custom sensors #652

Merged
merged 4 commits into from
Aug 10, 2021
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
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