From 9d9ec7c4ea17df9e3231a0568d2579e73828d932 Mon Sep 17 00:00:00 2001
From: "Addisu Z. Taddese" <addisu@openrobotics.org>
Date: Thu, 11 Jan 2024 21:44:55 -0600
Subject: [PATCH] Allow specifying gz_frame_id as an alternative to
 ignition_frame_id

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 <addisu@openrobotics.org>
---
 src/Sensor.cc      | 12 ++++++++++
 src/Sensor_TEST.cc | 55 ++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 67 insertions(+)

diff --git a/src/Sensor.cc b/src/Sensor.cc
index ad884105..bc25f7ec 100644
--- 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
index 79eb9cde..9bc4d5b6 100644
--- 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)