Allow specifying gz_frame_id as an alternative to ignition_frame_id (#…

…409) This is to help migration to newer versions of Gazebo and allows users to replace ignition_frame_id in their SDF files while still in Fortress. Signed-off-by: Addisu Z. Taddese <[email protected]>
gazebosim · Jan 12, 2024 · ff5bbb1 · ff5bbb1
1 parent 0ddddae
commit ff5bbb1
Show file tree

Hide file tree

Showing 2 changed files with 67 additions and 0 deletions.
diff --git a/src/Sensor.cc b/src/Sensor.cc
@@ -148,6 +148,18 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf)
     if (element->HasElement("ignition_frame_id"))
     {
       this->frame_id = element->Get<std::string>("ignition_frame_id");
+      // Warn if both ignition_frame_id and gz_frame_id are specified
+      if (element->HasElement("gz_frame_id"))
+      {
+        ignwarn << "Found both `ignition_frame_id` and `gz_frame_id` in sensor"
+                << this->name << ". Only `ignition_frame_id` will be used\n";
+      }
+    }
+    else if (element->HasElement("gz_frame_id"))
+    {
+      // Also read gz_frame_id to support SDF that's compatible with newer
+      // versions of Gazebo.
+      this->frame_id = element->Get<std::string>("gz_frame_id");
     }
     else
     {

diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc
@@ -350,6 +350,61 @@ TEST(Sensor_TEST, SetRateZeroService)
   EXPECT_FLOAT_EQ(20.0, sensor.UpdateRate());
 }
 
+//////////////////////////////////////////////////
+TEST(Sensor_TEST, FrameIdFromSdf)
+{
+  auto loadSensorWithSdfParam =
+      [](TestSensor &_testSensor, const std::string &_sensorParam)
+  {
+    const std::string sensorSdf = R"(
+    <sdf version="1.9">
+      <model name="m1">
+        <link name="link1">
+          <sensor name="test" type="imu">)" +
+                                  _sensorParam + R"(
+          </sensor>
+        </link>
+      </model>
+    </sdf>
+    )";
+    sdf::Root root;
+    sdf::Errors errors = root.LoadSdfString(sensorSdf);
+    ASSERT_TRUE(errors.empty()) << errors;
+    auto *model = root.Model();
+    ASSERT_NE(model, nullptr);
+    auto *link = model->LinkByIndex(0);
+    ASSERT_NE(link, nullptr);
+    auto *sensor = link->SensorByIndex(0);
+    ASSERT_NE(sensor, nullptr);
+
+    _testSensor.Load(*sensor);
+  };
+
+  {
+    TestSensor testSensor;
+    loadSensorWithSdfParam(testSensor, "");
+    EXPECT_EQ("test", testSensor.FrameId());
+  }
+  {
+    TestSensor testSensor;
+    loadSensorWithSdfParam(
+        testSensor, "<ignition_frame_id>custom_frame_id</ignition_frame_id>");
+    EXPECT_EQ("custom_frame_id", testSensor.FrameId());
+  }
+  {
+    TestSensor testSensor;
+    loadSensorWithSdfParam(testSensor,
+                           "<gz_frame_id>custom_frame_id</gz_frame_id>");
+    EXPECT_EQ("custom_frame_id", testSensor.FrameId());
+  }
+  {
+    TestSensor testSensor;
+    loadSensorWithSdfParam(testSensor, R"(
+      <ignition_frame_id>custom_frame_id</ignition_frame_id>
+      <gz_frame_id>other_custom_frame_id</gz_frame_id>)");
+    EXPECT_EQ("custom_frame_id", testSensor.FrameId());
+  }
+}
 
 //////////////////////////////////////////////////
 int main(int argc, char **argv)