diff --git a/include/ignition/sensors/Sensor.hh b/include/ignition/sensors/Sensor.hh index fa8fe4ab..c4d4be70 100644 --- a/include/ignition/sensors/Sensor.hh +++ b/include/ignition/sensors/Sensor.hh @@ -138,6 +138,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 f56362d3..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->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..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->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..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->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..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->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..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->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 b34e649b..32a2204a 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -231,8 +231,11 @@ 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. + // 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 ad647f91..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->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..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->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..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->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..0561cf1c 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,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); @@ -192,6 +208,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 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 a25da365..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->Name()); + 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