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("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("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"( + + + + )" + + _sensorParam + R"( + + + + + )"; + 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, "custom_frame_id"); + EXPECT_EQ("custom_frame_id", testSensor.FrameId()); + } + { + TestSensor testSensor; + loadSensorWithSdfParam(testSensor, + "custom_frame_id"); + EXPECT_EQ("custom_frame_id", testSensor.FrameId()); + } + { + TestSensor testSensor; + loadSensorWithSdfParam(testSensor, R"( + custom_frame_id + other_custom_frame_id)"); + EXPECT_EQ("custom_frame_id", testSensor.FrameId()); + } +} ////////////////////////////////////////////////// int main(int argc, char **argv)