diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index 748b6ffe5..1fa136872 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -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. diff --git a/sdf/1.9/sensor.sdf b/sdf/1.9/sensor.sdf index 4f506ff7c..080aeb430 100644 --- a/sdf/1.9/sensor.sdf +++ b/sdf/1.9/sensor.sdf @@ -12,6 +12,7 @@ altimeter, camera, contact, + custom, depth_camera, depth, force_torque, gps, diff --git a/src/Sensor.cc b/src/Sensor.cc index fa849adfe..bc6651351 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -59,7 +59,8 @@ const std::vector sensorTypeStrs = "wireless_transmitter", "air_pressure", "rgbd_camera", - "thermal_camera" + "thermal_camera", + "custom" }; class sdf::Sensor::Implementation @@ -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; diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index ea170a9a9..f7bf9313b 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -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 typeStrs = { @@ -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) diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc index 16cd02ce3..8aad28c8b 100644 --- a/test/integration/link_dom.cc +++ b/test/integration/link_dom.cc @@ -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); @@ -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()); + // Get the depth sensor const sdf::Sensor *depthSensor = link->SensorByName("depth_sensor"); ASSERT_NE(nullptr, depthSensor); diff --git a/test/sdf/sensors.sdf b/test/sdf/sensors.sdf index b65f07d0d..c09bb9f50 100644 --- a/test/sdf/sensors.sdf +++ b/test/sdf/sensors.sdf @@ -80,6 +80,12 @@ 4 5 6 0 0 0 + + + 123 + + + 7 8 9 0 0 0 @@ -308,7 +314,6 @@ - 4 5 6 0 0 0