diff --git a/include/ignition/sensors/Sensor.hh b/include/ignition/sensors/Sensor.hh index 12cb076d..c58913c5 100644 --- a/include/ignition/sensors/Sensor.hh +++ b/include/ignition/sensors/Sensor.hh @@ -142,6 +142,14 @@ namespace ignition /// \return Name of sensor. public: std::string Name() 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 public: std::string Topic() const; diff --git a/src/AirPressureSensor.cc b/src/AirPressureSensor.cc index 98fde943..d83eb005 100644 --- a/src/AirPressureSensor.cc +++ b/src/AirPressureSensor.cc @@ -159,7 +159,7 @@ bool AirPressureSensor::Update( *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); 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 a3b5cd8b..4c6a643d 100644 --- a/src/AltimeterSensor.cc +++ b/src/AltimeterSensor.cc @@ -146,7 +146,7 @@ bool AltimeterSensor::Update(const std::chrono::steady_clock::duration &_now) *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); 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 9bca4f80..a41040fd 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -400,7 +400,7 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now) *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); 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 d912aafd..c4f447de 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -545,7 +545,7 @@ bool DepthCameraSensor::Update( *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); 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 a88ed2be..4cd330e3 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -233,7 +233,7 @@ bool ImuSensor::Update(const std::chrono::steady_clock::duration &_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()); if (this->dataPtr->orientationEnabled) { diff --git a/src/Lidar.cc b/src/Lidar.cc index 1b6e61e4..d4be40d8 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -232,8 +232,11 @@ bool Lidar::PublishLidarScan(const std::chrono::steady_clock::duration &_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. + // the ros_ign plugin is using the laserscan.proto 'frame' field frame->add_value(this->Name()); - this->dataPtr->laserMsg.set_frame(this->Name()); + 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 4aa13451..efea8974 100644 --- a/src/LogicalCameraSensor.cc +++ b/src/LogicalCameraSensor.cc @@ -163,7 +163,7 @@ bool LogicalCameraSensor::Update( 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 6836255e..9fb927bc 100644 --- a/src/MagnetometerSensor.cc +++ b/src/MagnetometerSensor.cc @@ -168,7 +168,7 @@ bool MagnetometerSensor::Update( *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); 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 8b24b3ee..ba0eaec0 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -477,7 +477,7 @@ bool RgbdCameraSensor::Update(const std::chrono::steady_clock::duration &_now) *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); 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); @@ -589,7 +589,7 @@ bool RgbdCameraSensor::Update(const std::chrono::steady_clock::duration &_now) *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); 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 d7230a28..10295057 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -106,6 +106,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; @@ -136,6 +139,19 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf) return false; } + sdf::ElementPtr element = _sdf.Element(); + if (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); @@ -226,6 +242,18 @@ std::string Sensor::Name() const return this->dataPtr->name; } +////////////////////////////////////////////////// +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 98ff7d7e..9d38b59f 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 038577b8..8720407e 100644 --- a/src/ThermalCameraSensor.cc +++ b/src/ThermalCameraSensor.cc @@ -457,7 +457,7 @@ bool ThermalCameraSensor::Update( *stamp = msgs::Convert(_now); 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); diff --git a/test/integration/camera.cc b/test/integration/camera.cc index c948437d..6bad4b1c 100644 --- a/test/integration/camera.cc +++ b/test/integration/camera.cc @@ -101,6 +101,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/sdf/camera_sensor_builtin.sdf b/test/sdf/camera_sensor_builtin.sdf index 8e1c3ab8..532ed003 100644 --- a/test/sdf/camera_sensor_builtin.sdf +++ b/test/sdf/camera_sensor_builtin.sdf @@ -4,6 +4,7 @@ 10 + base_camera /test/integration/CameraPlugin_imagesWithBuiltinSDF 1.05