From 858c394b656017d9b7ff897083f7a98780cd8f05 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 18 Feb 2022 16:11:36 +0100 Subject: [PATCH 1/6] Added frame_id parameter to lidar sensor Signed-off-by: ahcorde --- src/Lidar.cc | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/src/Lidar.cc b/src/Lidar.cc index b34e649b..3b6094bf 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -45,6 +45,9 @@ class ignition::sensors::LidarPrivate /// \brief Sdf sensor. public: sdf::Lidar sdfLidar; + + /// \brief frame id + public: std::string frame_id; }; ////////////////////////////////////////////////// @@ -166,6 +169,17 @@ bool Lidar::Load(const sdf::Sensor &_sdf) } } + sdf::ElementPtr element = _sdf.Element(); + // Get the background color, if specified. + if (element->HasElement("frame_id")) + { + this->dataPtr->frame_id = element->Get("frame_id"); + } + else + { + this->dataPtr->frame_id = this->Name(); + } + this->initialized = true; return true; } @@ -232,7 +246,7 @@ bool Lidar::PublishLidarScan(const ignition::common::Time &_now) auto frame = this->dataPtr->laserMsg.mutable_header()->add_data(); frame->set_key("frame_id"); frame->add_value(this->Name()); - this->dataPtr->laserMsg.set_frame(this->Name()); + this->dataPtr->laserMsg.set_frame(this->dataPtr->frame_id); // Store the latest laser scans into laserMsg msgs::Set(this->dataPtr->laserMsg.mutable_world_pose(), From 9a56643f76ec701f1df54e4da177b035b95c674f Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 23 Feb 2022 20:01:20 +0100 Subject: [PATCH 2/6] Removed copy/paste trace Signed-off-by: ahcorde --- src/Lidar.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/Lidar.cc b/src/Lidar.cc index 3b6094bf..69216a60 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -170,7 +170,6 @@ bool Lidar::Load(const sdf::Sensor &_sdf) } sdf::ElementPtr element = _sdf.Element(); - // Get the background color, if specified. if (element->HasElement("frame_id")) { this->dataPtr->frame_id = element->Get("frame_id"); From ea11709f601e10febee800db2aa3327bbe9c70ec Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 28 Feb 2022 13:32:29 +0100 Subject: [PATCH 3/6] Added ignition_frame_id tag to conform ros format for header field frame_id Signed-off-by: ahcorde --- include/ignition/sensors/Sensor.hh | 4 ++++ src/AirPressureSensor.cc | 2 +- src/AltimeterSensor.cc | 2 +- src/CameraSensor.cc | 4 ++-- src/DepthCameraSensor.cc | 2 +- src/ImuSensor.cc | 2 +- src/Lidar.cc | 17 +++-------------- src/LogicalCameraSensor.cc | 2 +- src/MagnetometerSensor.cc | 2 +- src/RgbdCameraSensor.cc | 4 ++-- src/Sensor.cc | 18 ++++++++++++++++++ src/ThermalCameraSensor.cc | 2 +- 12 files changed, 36 insertions(+), 25 deletions(-) diff --git a/include/ignition/sensors/Sensor.hh b/include/ignition/sensors/Sensor.hh index fa8fe4ab..d177e967 100644 --- a/include/ignition/sensors/Sensor.hh +++ b/include/ignition/sensors/Sensor.hh @@ -138,6 +138,10 @@ namespace ignition /// \return Name of sensor. public: std::string Name() const; + /// \brief FrameID. + /// \return FrameID of sensor. + public: std::string FrameID() const; + /// \brief Get topic where sensor data is published. /// \return Topic sensor publishes data to public: std::string Topic() const; diff --git a/src/AirPressureSensor.cc b/src/AirPressureSensor.cc index f56362d3..7880343b 100644 --- a/src/AirPressureSensor.cc +++ b/src/AirPressureSensor.cc @@ -159,7 +159,7 @@ bool AirPressureSensor::Update(const ignition::common::Time &_now) msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->Name()); + frame->add_value(this->FrameID()); // This block of code comes from RotorS: // https://github.com/ethz-asl/rotors_simulator/blob/master/rotors_gazebo_plugins/src/gazebo_pressure_plugin.cpp diff --git a/src/AltimeterSensor.cc b/src/AltimeterSensor.cc index 0689c83c..651ba540 100644 --- a/src/AltimeterSensor.cc +++ b/src/AltimeterSensor.cc @@ -147,7 +147,7 @@ bool AltimeterSensor::Update(const ignition::common::Time &_now) msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->Name()); + frame->add_value(this->FrameID()); // Apply altimeter vertical position noise if (this->dataPtr->noises.find(ALTIMETER_VERTICAL_POSITION_NOISE_METERS) != diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index 80968b0e..28dd27f2 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -400,7 +400,7 @@ bool CameraSensor::Update(const ignition::common::Time &_now) msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->Name()); + frame->add_value(this->FrameID()); msg.set_data(data, this->dataPtr->camera->ImageMemorySize()); } @@ -606,7 +606,7 @@ void CameraSensor::PopulateInfo(const sdf::Camera *_cameraSdf) // can populate it with arbitrary frames. auto infoFrame = this->dataPtr->infoMsg.mutable_header()->add_data(); infoFrame->set_key("frame_id"); - infoFrame->add_value(this->Name()); + infoFrame->add_value(this->FrameID()); this->dataPtr->infoMsg.set_width(width); this->dataPtr->infoMsg.set_height(height); diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc index 06dadb8e..01b70a41 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -543,7 +543,7 @@ bool DepthCameraSensor::Update(const ignition::common::Time &_now) msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->Name()); + frame->add_value(this->FrameID()); std::lock_guard lock(this->dataPtr->mutex); msg.set_data(this->dataPtr->depthBuffer, diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index 8427f717..44c60d95 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -216,7 +216,7 @@ bool ImuSensor::Update(const ignition::common::Time &_now) msg.set_entity_name(this->Name()); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->Name()); + frame->add_value(this->FrameID()); msgs::Set(msg.mutable_orientation(), this->dataPtr->orientation); msgs::Set(msg.mutable_angular_velocity(), this->dataPtr->angularVel); diff --git a/src/Lidar.cc b/src/Lidar.cc index 69216a60..b8727c85 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -45,9 +45,6 @@ class ignition::sensors::LidarPrivate /// \brief Sdf sensor. public: sdf::Lidar sdfLidar; - - /// \brief frame id - public: std::string frame_id; }; ////////////////////////////////////////////////// @@ -169,16 +166,6 @@ bool Lidar::Load(const sdf::Sensor &_sdf) } } - sdf::ElementPtr element = _sdf.Element(); - if (element->HasElement("frame_id")) - { - this->dataPtr->frame_id = element->Get("frame_id"); - } - else - { - this->dataPtr->frame_id = this->Name(); - } - this->initialized = true; return true; } @@ -244,8 +231,10 @@ bool Lidar::PublishLidarScan(const ignition::common::Time &_now) this->dataPtr->laserMsg.mutable_header()->clear_data(); auto frame = this->dataPtr->laserMsg.mutable_header()->add_data(); frame->set_key("frame_id"); + // keeping here the sensor name instead of frame_id because the visualizeLidar + // plugin relies on this value to get the position of the lidar frame->add_value(this->Name()); - this->dataPtr->laserMsg.set_frame(this->dataPtr->frame_id); + this->dataPtr->laserMsg.set_frame(this->FrameID()); // Store the latest laser scans into laserMsg msgs::Set(this->dataPtr->laserMsg.mutable_world_pose(), diff --git a/src/LogicalCameraSensor.cc b/src/LogicalCameraSensor.cc index ad647f91..d28cd463 100644 --- a/src/LogicalCameraSensor.cc +++ b/src/LogicalCameraSensor.cc @@ -166,7 +166,7 @@ bool LogicalCameraSensor::Update(const ignition::common::Time &_now) this->dataPtr->msg.mutable_header()->clear_data(); auto frame = this->dataPtr->msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->Name()); + frame->add_value(this->FrameID()); // publish this->AddSequence(this->dataPtr->msg.mutable_header()); diff --git a/src/MagnetometerSensor.cc b/src/MagnetometerSensor.cc index 5bc375b9..af47bf8f 100644 --- a/src/MagnetometerSensor.cc +++ b/src/MagnetometerSensor.cc @@ -168,7 +168,7 @@ bool MagnetometerSensor::Update(const ignition::common::Time &_now) msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->Name()); + frame->add_value(this->FrameID()); // Apply magnetometer noise after converting to body frame if (this->dataPtr->noises.find(MAGNETOMETER_X_NOISE_TESLA) != diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index 3a04045f..4b2d7bde 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -468,7 +468,7 @@ bool RgbdCameraSensor::Update(const ignition::common::Time &_now) msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->Name()); + frame->add_value(this->FrameID()); std::lock_guard lock(this->dataPtr->mutex); @@ -583,7 +583,7 @@ bool RgbdCameraSensor::Update(const ignition::common::Time &_now) msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->Name()); + frame->add_value(this->FrameID()); msg.set_data(data, rendering::PixelUtil::MemorySize(rendering::PF_R8G8B8, width, height)); diff --git a/src/Sensor.cc b/src/Sensor.cc index 14df12fb..2e92984c 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -91,6 +91,9 @@ class ignition::sensors::SensorPrivate /// A map is used so that a single sensor can have multiple sensor /// streams each with a sequence counter. public: std::map sequences; + + /// \brief frame id + public: std::string frame_id; }; SensorId SensorPrivate::idCounter = 0; @@ -121,6 +124,16 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf) return false; } + sdf::ElementPtr element = _sdf.Element(); + if (element->HasElement("ignition:frame_id")) + { + this->frame_id = element->Get("ignition:frame_id"); + } + else + { + this->frame_id = this->name; + } + // Try resolving the pose first, and only use the raw pose if that fails auto semPose = _sdf.SemanticPose(); sdf::Errors errors = semPose.Resolve(this->pose); @@ -192,6 +205,11 @@ std::string Sensor::Name() const return this->dataPtr->name; } +////////////////////////////////////////////////// +std::string Sensor::FrameID() const +{ + return this->dataPtr->frame_id; +} ////////////////////////////////////////////////// std::string Sensor::Topic() const { diff --git a/src/ThermalCameraSensor.cc b/src/ThermalCameraSensor.cc index a25da365..f4de6e41 100644 --- a/src/ThermalCameraSensor.cc +++ b/src/ThermalCameraSensor.cc @@ -414,7 +414,7 @@ bool ThermalCameraSensor::Update(const ignition::common::Time &_now) stamp->set_nsec(_now.nsec); auto frame = this->dataPtr->thermalMsg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->Name()); + frame->add_value(this->FrameID()); std::lock_guard lock(this->dataPtr->mutex); this->dataPtr->thermalMsg.set_data(this->dataPtr->thermalBuffer, From e861a71674106ae50a3dfb213cf769a71775a0ae Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 28 Feb 2022 13:33:04 +0100 Subject: [PATCH 4/6] update comment Signed-off-by: ahcorde --- src/Lidar.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/Lidar.cc b/src/Lidar.cc index b8727c85..a9baedba 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -232,7 +232,8 @@ bool Lidar::PublishLidarScan(const ignition::common::Time &_now) auto frame = this->dataPtr->laserMsg.mutable_header()->add_data(); frame->set_key("frame_id"); // keeping here the sensor name instead of frame_id because the visualizeLidar - // plugin relies on this value to get the position of the lidar + // plugin relies on this value to get the position of the lidar. + // the ros_ign plugin is using the laserscan.proto 'frame' field frame->add_value(this->Name()); this->dataPtr->laserMsg.set_frame(this->FrameID()); From d5c188bc64f70ac85abca87fcefed801ad1c8d3c Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 28 Feb 2022 13:52:45 +0100 Subject: [PATCH 5/6] Fixed test Signed-off-by: ahcorde --- src/Sensor.cc | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/Sensor.cc b/src/Sensor.cc index 2e92984c..a432c8cd 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -125,13 +125,16 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf) } sdf::ElementPtr element = _sdf.Element(); - if (element->HasElement("ignition:frame_id")) + if (element) { - this->frame_id = element->Get("ignition:frame_id"); - } - else - { - this->frame_id = this->name; + if (element->HasElement("ignition:frame_id")) + { + this->frame_id = element->Get("ignition:frame_id"); + } + else + { + this->frame_id = this->name; + } } // Try resolving the pose first, and only use the raw pose if that fails From b2ad054c2e95b67eac01f7e7341f8df871425872 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 1 Mar 2022 10:45:05 +0100 Subject: [PATCH 6/6] change method name and added tests Signed-off-by: ahcorde --- include/ignition/sensors/Sensor.hh | 10 +++++++--- src/AirPressureSensor.cc | 2 +- src/AltimeterSensor.cc | 2 +- src/CameraSensor.cc | 4 ++-- src/DepthCameraSensor.cc | 2 +- src/ImuSensor.cc | 2 +- src/Lidar.cc | 2 +- src/LogicalCameraSensor.cc | 2 +- src/MagnetometerSensor.cc | 2 +- src/RgbdCameraSensor.cc | 4 ++-- src/Sensor.cc | 13 ++++++++++--- src/Sensor_TEST.cc | 4 ++++ src/ThermalCameraSensor.cc | 2 +- test/integration/camera_plugin.cc | 2 ++ test/integration/camera_sensor_builtin.sdf | 1 + 15 files changed, 36 insertions(+), 18 deletions(-) diff --git a/include/ignition/sensors/Sensor.hh b/include/ignition/sensors/Sensor.hh index d177e967..c4d4be70 100644 --- a/include/ignition/sensors/Sensor.hh +++ b/include/ignition/sensors/Sensor.hh @@ -138,9 +138,13 @@ namespace ignition /// \return Name of sensor. public: std::string Name() const; - /// \brief FrameID. - /// \return FrameID of sensor. - public: std::string FrameID() const; + /// \brief FrameId. + /// \return FrameId of sensor. + public: std::string FrameId() const; + + /// \brief Set Frame ID of the sensor + /// \param[in] _frameId Frame ID of the sensor + public: void SetFrameId(const std::string &_frameId); /// \brief Get topic where sensor data is published. /// \return Topic sensor publishes data to diff --git a/src/AirPressureSensor.cc b/src/AirPressureSensor.cc index 7880343b..baafc46f 100644 --- a/src/AirPressureSensor.cc +++ b/src/AirPressureSensor.cc @@ -159,7 +159,7 @@ bool AirPressureSensor::Update(const ignition::common::Time &_now) msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->FrameID()); + frame->add_value(this->FrameId()); // This block of code comes from RotorS: // https://github.com/ethz-asl/rotors_simulator/blob/master/rotors_gazebo_plugins/src/gazebo_pressure_plugin.cpp diff --git a/src/AltimeterSensor.cc b/src/AltimeterSensor.cc index 651ba540..2297263c 100644 --- a/src/AltimeterSensor.cc +++ b/src/AltimeterSensor.cc @@ -147,7 +147,7 @@ bool AltimeterSensor::Update(const ignition::common::Time &_now) msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->FrameID()); + frame->add_value(this->FrameId()); // Apply altimeter vertical position noise if (this->dataPtr->noises.find(ALTIMETER_VERTICAL_POSITION_NOISE_METERS) != diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index 28dd27f2..779e8f56 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -400,7 +400,7 @@ bool CameraSensor::Update(const ignition::common::Time &_now) msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->FrameID()); + frame->add_value(this->FrameId()); msg.set_data(data, this->dataPtr->camera->ImageMemorySize()); } @@ -606,7 +606,7 @@ void CameraSensor::PopulateInfo(const sdf::Camera *_cameraSdf) // can populate it with arbitrary frames. auto infoFrame = this->dataPtr->infoMsg.mutable_header()->add_data(); infoFrame->set_key("frame_id"); - infoFrame->add_value(this->FrameID()); + infoFrame->add_value(this->FrameId()); this->dataPtr->infoMsg.set_width(width); this->dataPtr->infoMsg.set_height(height); diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc index 01b70a41..33caedb5 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -543,7 +543,7 @@ bool DepthCameraSensor::Update(const ignition::common::Time &_now) msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->FrameID()); + frame->add_value(this->FrameId()); std::lock_guard lock(this->dataPtr->mutex); msg.set_data(this->dataPtr->depthBuffer, diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index 44c60d95..f2d066d3 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -216,7 +216,7 @@ bool ImuSensor::Update(const ignition::common::Time &_now) msg.set_entity_name(this->Name()); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->FrameID()); + frame->add_value(this->FrameId()); msgs::Set(msg.mutable_orientation(), this->dataPtr->orientation); msgs::Set(msg.mutable_angular_velocity(), this->dataPtr->angularVel); diff --git a/src/Lidar.cc b/src/Lidar.cc index a9baedba..32a2204a 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -235,7 +235,7 @@ bool Lidar::PublishLidarScan(const ignition::common::Time &_now) // plugin relies on this value to get the position of the lidar. // the ros_ign plugin is using the laserscan.proto 'frame' field frame->add_value(this->Name()); - this->dataPtr->laserMsg.set_frame(this->FrameID()); + this->dataPtr->laserMsg.set_frame(this->FrameId()); // Store the latest laser scans into laserMsg msgs::Set(this->dataPtr->laserMsg.mutable_world_pose(), diff --git a/src/LogicalCameraSensor.cc b/src/LogicalCameraSensor.cc index d28cd463..7fa1af39 100644 --- a/src/LogicalCameraSensor.cc +++ b/src/LogicalCameraSensor.cc @@ -166,7 +166,7 @@ bool LogicalCameraSensor::Update(const ignition::common::Time &_now) this->dataPtr->msg.mutable_header()->clear_data(); auto frame = this->dataPtr->msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->FrameID()); + frame->add_value(this->FrameId()); // publish this->AddSequence(this->dataPtr->msg.mutable_header()); diff --git a/src/MagnetometerSensor.cc b/src/MagnetometerSensor.cc index af47bf8f..b736df75 100644 --- a/src/MagnetometerSensor.cc +++ b/src/MagnetometerSensor.cc @@ -168,7 +168,7 @@ bool MagnetometerSensor::Update(const ignition::common::Time &_now) msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->FrameID()); + frame->add_value(this->FrameId()); // Apply magnetometer noise after converting to body frame if (this->dataPtr->noises.find(MAGNETOMETER_X_NOISE_TESLA) != diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index 4b2d7bde..2afd1bc5 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -468,7 +468,7 @@ bool RgbdCameraSensor::Update(const ignition::common::Time &_now) msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->FrameID()); + frame->add_value(this->FrameId()); std::lock_guard lock(this->dataPtr->mutex); @@ -583,7 +583,7 @@ bool RgbdCameraSensor::Update(const ignition::common::Time &_now) msg.mutable_header()->mutable_stamp()->set_nsec(_now.nsec); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->FrameID()); + frame->add_value(this->FrameId()); msg.set_data(data, rendering::PixelUtil::MemorySize(rendering::PF_R8G8B8, width, height)); diff --git a/src/Sensor.cc b/src/Sensor.cc index a432c8cd..0561cf1c 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -127,9 +127,9 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf) sdf::ElementPtr element = _sdf.Element(); if (element) { - if (element->HasElement("ignition:frame_id")) + if (element->HasElement("ignition_frame_id")) { - this->frame_id = element->Get("ignition:frame_id"); + this->frame_id = element->Get("ignition_frame_id"); } else { @@ -209,10 +209,17 @@ std::string Sensor::Name() const } ////////////////////////////////////////////////// -std::string Sensor::FrameID() const +std::string Sensor::FrameId() const { return this->dataPtr->frame_id; } + +////////////////////////////////////////////////// +void Sensor::SetFrameId(const std::string &_frameId) +{ + this->dataPtr->frame_id = _frameId; +} + ////////////////////////////////////////////////// std::string Sensor::Topic() const { diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index d9157ec8..edeabdbd 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -88,6 +88,10 @@ TEST(Sensor_TEST, Sensor) EXPECT_EQ(1u, sensor.Id()); EXPECT_EQ(nullptr, sensor.SDF()); + + EXPECT_EQ(sensor.Name(), sensor.FrameId()); + sensor.SetFrameId("frame_id_12"); + EXPECT_EQ(std::string("frame_id_12"), sensor.FrameId()); } ////////////////////////////////////////////////// diff --git a/src/ThermalCameraSensor.cc b/src/ThermalCameraSensor.cc index f4de6e41..b4931a6b 100644 --- a/src/ThermalCameraSensor.cc +++ b/src/ThermalCameraSensor.cc @@ -414,7 +414,7 @@ bool ThermalCameraSensor::Update(const ignition::common::Time &_now) stamp->set_nsec(_now.nsec); auto frame = this->dataPtr->thermalMsg.mutable_header()->add_data(); frame->set_key("frame_id"); - frame->add_value(this->FrameID()); + frame->add_value(this->FrameId()); std::lock_guard lock(this->dataPtr->mutex); this->dataPtr->thermalMsg.set_data(this->dataPtr->thermalBuffer, diff --git a/test/integration/camera_plugin.cc b/test/integration/camera_plugin.cc index 39e2440b..8b8320a5 100644 --- a/test/integration/camera_plugin.cc +++ b/test/integration/camera_plugin.cc @@ -95,6 +95,8 @@ void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) EXPECT_EQ(256u, sensor->ImageWidth()); EXPECT_EQ(257u, sensor->ImageHeight()); + EXPECT_EQ(std::string("base_camera"), sensor->FrameId()); + std::string topic = "/test/integration/CameraPlugin_imagesWithBuiltinSDF"; WaitForMessageTestHelper helper(topic); diff --git a/test/integration/camera_sensor_builtin.sdf b/test/integration/camera_sensor_builtin.sdf index 8e1c3ab8..532ed003 100644 --- a/test/integration/camera_sensor_builtin.sdf +++ b/test/integration/camera_sensor_builtin.sdf @@ -4,6 +4,7 @@ 10 + base_camera /test/integration/CameraPlugin_imagesWithBuiltinSDF 1.05