Link: warn if ForceTorque sensor is attached

A ForceTorque sensor is only supported by Joints. Signed-off-by: Steve Peters <[email protected]>
gazebosim · Jan 30, 2024 · 3953e7f · 3953e7f
1 parent b2ae616
commit 3953e7f
Show file tree

Hide file tree

Showing 4 changed files with 98 additions and 19 deletions.
diff --git a/src/Link.cc b/src/Link.cc
@@ -157,6 +157,21 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config)
   Errors sensorLoadErrors = loadUniqueRepeated<Sensor>(_sdf, "sensor",
       this->dataPtr->sensors);
   errors.insert(errors.end(), sensorLoadErrors.begin(), sensorLoadErrors.end());
+  // Check that no ForceTorque sensors are attached.
+  for (const auto &sensor : this->dataPtr->sensors)
+  {
+    if (sensor.Type() == SensorType::FORCE_TORQUE)
+    {
+      Error err(
+        ErrorCode::WARNING,
+        "A force_torque sensor is attached to the link named " +
+        this->dataPtr->name + ", but this sensor type is not supported by "
+        "links."
+      );
+      enforceConfigurablePolicyCondition(
+        _config.WarningsPolicy(), err, errors);
+    }
+  }
 
   // Load all the particle emitters.
   Errors emitterLoadErrors = loadUniqueRepeated<ParticleEmitter>(_sdf,

diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc
@@ -265,13 +265,37 @@ TEST(DOMLink, InertialInvalid)
 }
 
 //////////////////////////////////////////////////
-TEST(DOMLink, Sensors)
+TEST(DOMLink, LinkInvalidSensor)
 {
-  const std::string testFile = sdf::testing::TestFile("sdf", "sensors.sdf");
+  const std::string testFile =
+    sdf::testing::TestFile("sdf", "link_invalid_sensor.sdf");
 
-  // Load the SDF file
+  // Load the SDF file with warnings treated as errors
+  sdf::ParserConfig parserConfig;
+  parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR);
+  //
   sdf::Root root;
-  auto errors = root.Load(testFile);
+  auto errors = root.Load(testFile, parserConfig);
+  for (auto e : errors)
+    std::cout << e << std::endl;
+  ASSERT_FALSE(errors.empty());
+  EXPECT_EQ(1u, errors.size());
+  EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::WARNING);
+  EXPECT_EQ(errors[0].Message(), "A force_torque sensor is attached to the "
+    "link named link, but this sensor type is not supported by links.");
+}
+
+//////////////////////////////////////////////////
+TEST(DOMLink, LinkSensors)
+{
+  const std::string testFile = sdf::testing::TestFile("sdf", "link_sensors.sdf");
+
+  // Load the SDF file with warnings treated as errors
+  sdf::ParserConfig parserConfig;
+  parserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR);
+  //
+  sdf::Root root;
+  auto errors = root.Load(testFile, parserConfig);
   for (auto e : errors)
     std::cout << e << std::endl;
   EXPECT_TRUE(errors.empty());
@@ -285,7 +309,7 @@ TEST(DOMLink, Sensors)
   const sdf::Link *link = model->LinkByIndex(0);
   ASSERT_NE(nullptr, link);
   EXPECT_EQ("link", link->Name());
-  EXPECT_EQ(27u, link->SensorCount());
+  EXPECT_EQ(26u, link->SensorCount());
 
   // Get the altimeter sensor
   const sdf::Sensor *altimeterSensor = link->SensorByIndex(0);
@@ -449,16 +473,6 @@ TEST(DOMLink, Sensors)
   EXPECT_TRUE(boundingboxCamSensor->HasBoundingBoxType());
   EXPECT_EQ("2d", boundingboxCamSensor->BoundingBoxType());
 
-  // Get the force_torque sensor
-  const sdf::Sensor *forceTorqueSensor =
-    link->SensorByName("force_torque_sensor");
-  ASSERT_NE(nullptr, forceTorqueSensor);
-  EXPECT_EQ("force_torque_sensor", forceTorqueSensor->Name());
-  EXPECT_EQ(sdf::SensorType::FORCE_TORQUE, forceTorqueSensor->Type());
-  EXPECT_EQ(gz::math::Pose3d(10, 11, 12, 0, 0, 0),
-      forceTorqueSensor->RawPose());
-  EXPECT_FALSE(forceTorqueSensor->EnableMetrics());
-
   // Get the navsat sensor
   const sdf::Sensor *navSatSensor = link->SensorByName("navsat_sensor");
   ASSERT_NE(nullptr, navSatSensor);

diff --git a/test/sdf/link_invalid_sensor.sdf b/test/sdf/link_invalid_sensor.sdf
@@ -0,0 +1,54 @@
+<?xml version="1.0" ?>
+<sdf version="1.7">
+  <model name="model">
+    <link name="link">
+      <sensor name="force_torque_sensor" type="force_torque">
+        <pose>10 11 12 0 0 0</pose>
+        <force_torque>
+          <frame>parent</frame>
+          <measure_direction>parent_to_child</measure_direction>
+          <force>
+            <x>
+              <noise type="gaussian">
+                <mean>0</mean>
+                <stddev>0.1</stddev>
+              </noise>
+            </x>
+            <y>
+              <noise type="gaussian">
+                <mean>1</mean>
+                <stddev>1.1</stddev>
+              </noise>
+            </y>
+            <z>
+              <noise type="gaussian">
+                <mean>2</mean>
+                <stddev>2.1</stddev>
+              </noise>
+            </z>
+          </force>
+          <torque>
+            <x>
+              <noise type="gaussian">
+                <mean>3</mean>
+                <stddev>3.1</stddev>
+              </noise>
+            </x>
+            <y>
+              <noise type="gaussian">
+                <mean>4</mean>
+                <stddev>4.1</stddev>
+              </noise>
+            </y>
+            <z>
+              <noise type="gaussian">
+                <mean>5</mean>
+                <stddev>5.1</stddev>
+              </noise>
+            </z>
+          </torque>
+        </force_torque>
+      </sensor>
+    </link>
+  </model>
+</sdf>
diff --git a/test/sdf/sensors.sdf → test/sdf/link_sensors.sdf b/test/sdf/sensors.sdf → test/sdf/link_sensors.sdf
@@ -157,10 +157,6 @@
 
       </sensor>
 
-      <sensor name="force_torque_sensor" type="force_torque">
-        <pose>10 11 12 0 0 0</pose>
-      </sensor>
-
       <sensor name="navsat_sensor" type="gps">
         <pose>13 14 15 0 0 0</pose>
         <gps>