Add support for custom sensors (#652)

Signed-off-by: Louise Poubel <louise@openrobotics.org> Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
gazebosim · Aug 10, 2021 · afdf3a5 · afdf3a5
1 parent 22a968e
commit afdf3a5
Show file tree

Hide file tree

Showing 6 changed files with 35 additions and 6 deletions.
diff --git 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
@@ -12,6 +12,7 @@
                   altimeter,
                   camera,
                   contact,
+                  custom,
                   depth_camera, depth,
                   force_torque,
                   gps,

diff --git a/src/Sensor.cc b/src/Sensor.cc
@@ -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
@@ -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
@@ -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 =
   {
@@ -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
@@ -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<int>());
+
   // 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
@@ -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">
@@ -308,7 +314,6 @@
         </lidar>
       </sensor>
 
-
       <sensor name="rfid_sensor" type="rfid">
         <pose>4 5 6 0 0 0</pose>
       </sensor>