Skip to content

Commit 135d876

Browse files
ahcordeahcorde
authored
Merge pull request #202 from ignitionrobotics/chapulina/3_to_5
3 ➡️ 5
2 parents fe65a92 + aae1f7c commit 135d876

15 files changed

+58
-12
lines changed

‎include/ignition/sensors/Sensor.hh

+8
Original file line numberDiff line numberDiff line change
@@ -180,6 +180,14 @@ namespace ignition
180180
/// \return Name of sensor.
181181
public: std::string Name() const;
182182

183+
/// \brief FrameId.
184+
/// \return FrameId of sensor.
185+
public: std::string FrameId() const;
186+
187+
/// \brief Set Frame ID of the sensor
188+
/// \param[in] _frameId Frame ID of the sensor
189+
public: void SetFrameId(const std::string &_frameId);
190+
183191
/// \brief Get topic where sensor data is published.
184192
/// \return Topic sensor publishes data to
185193
public: std::string Topic() const;

‎src/AirPressureSensor.cc

+1-1
Original file line numberDiff line numberDiff line change
@@ -166,7 +166,7 @@ bool AirPressureSensor::Update(
166166
*msg.mutable_header()->mutable_stamp() = msgs::Convert(_now);
167167
auto frame = msg.mutable_header()->add_data();
168168
frame->set_key("frame_id");
169-
frame->add_value(this->Name());
169+
frame->add_value(this->FrameId());
170170

171171
// This block of code comes from RotorS:
172172
// https://github.com/ethz-asl/rotors_simulator/blob/master/rotors_gazebo_plugins/src/gazebo_pressure_plugin.cpp

‎src/AltimeterSensor.cc

+1-1
Original file line numberDiff line numberDiff line change
@@ -153,7 +153,7 @@ bool AltimeterSensor::Update(const std::chrono::steady_clock::duration &_now)
153153
*msg.mutable_header()->mutable_stamp() = msgs::Convert(_now);
154154
auto frame = msg.mutable_header()->add_data();
155155
frame->set_key("frame_id");
156-
frame->add_value(this->Name());
156+
frame->add_value(this->FrameId());
157157

158158
// Apply altimeter vertical position noise
159159
if (this->dataPtr->noises.find(ALTIMETER_VERTICAL_POSITION_NOISE_METERS) !=

‎src/CameraSensor.cc

+2-2
Original file line numberDiff line numberDiff line change
@@ -407,7 +407,7 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now)
407407
*msg.mutable_header()->mutable_stamp() = msgs::Convert(_now);
408408
auto frame = msg.mutable_header()->add_data();
409409
frame->set_key("frame_id");
410-
frame->add_value(this->Name());
410+
frame->add_value(this->FrameId());
411411
msg.set_data(data, this->dataPtr->camera->ImageMemorySize());
412412
}
413413

@@ -620,7 +620,7 @@ void CameraSensor::PopulateInfo(const sdf::Camera *_cameraSdf)
620620
// can populate it with arbitrary frames.
621621
auto infoFrame = this->dataPtr->infoMsg.mutable_header()->add_data();
622622
infoFrame->set_key("frame_id");
623-
infoFrame->add_value(this->Name());
623+
infoFrame->add_value(this->FrameId());
624624

625625
this->dataPtr->infoMsg.set_width(width);
626626
this->dataPtr->infoMsg.set_height(height);

‎src/DepthCameraSensor.cc

+1-1
Original file line numberDiff line numberDiff line change
@@ -551,7 +551,7 @@ bool DepthCameraSensor::Update(
551551
*msg.mutable_header()->mutable_stamp() = msgs::Convert(_now);
552552
auto frame = msg.mutable_header()->add_data();
553553
frame->set_key("frame_id");
554-
frame->add_value(this->Name());
554+
frame->add_value(this->FrameId());
555555

556556
std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
557557
msg.set_data(this->dataPtr->depthBuffer,

‎src/ImuSensor.cc

+1-1
Original file line numberDiff line numberDiff line change
@@ -222,7 +222,7 @@ bool ImuSensor::Update(const std::chrono::steady_clock::duration &_now)
222222
msg.set_entity_name(this->Name());
223223
auto frame = msg.mutable_header()->add_data();
224224
frame->set_key("frame_id");
225-
frame->add_value(this->Name());
225+
frame->add_value(this->FrameId());
226226

227227
if (this->dataPtr->orientationEnabled)
228228
{

‎src/Lidar.cc

+4-1
Original file line numberDiff line numberDiff line change
@@ -245,8 +245,11 @@ bool Lidar::PublishLidarScan(const std::chrono::steady_clock::duration &_now)
245245
this->dataPtr->laserMsg.mutable_header()->clear_data();
246246
auto frame = this->dataPtr->laserMsg.mutable_header()->add_data();
247247
frame->set_key("frame_id");
248+
// keeping here the sensor name instead of frame_id because the visualizeLidar
249+
// plugin relies on this value to get the position of the lidar.
250+
// the ros_ign plugin is using the laserscan.proto 'frame' field
248251
frame->add_value(this->Name());
249-
this->dataPtr->laserMsg.set_frame(this->Name());
252+
this->dataPtr->laserMsg.set_frame(this->FrameId());
250253

251254
// Store the latest laser scans into laserMsg
252255
msgs::Set(this->dataPtr->laserMsg.mutable_world_pose(),

‎src/LogicalCameraSensor.cc

+1-1
Original file line numberDiff line numberDiff line change
@@ -172,7 +172,7 @@ bool LogicalCameraSensor::Update(
172172
this->dataPtr->msg.mutable_header()->clear_data();
173173
auto frame = this->dataPtr->msg.mutable_header()->add_data();
174174
frame->set_key("frame_id");
175-
frame->add_value(this->Name());
175+
frame->add_value(this->FrameId());
176176

177177
// publish
178178
this->AddSequence(this->dataPtr->msg.mutable_header());

‎src/MagnetometerSensor.cc

+1-1
Original file line numberDiff line numberDiff line change
@@ -175,7 +175,7 @@ bool MagnetometerSensor::Update(
175175
*msg.mutable_header()->mutable_stamp() = msgs::Convert(_now);
176176
auto frame = msg.mutable_header()->add_data();
177177
frame->set_key("frame_id");
178-
frame->add_value(this->Name());
178+
frame->add_value(this->FrameId());
179179

180180
// Apply magnetometer noise after converting to body frame
181181
if (this->dataPtr->noises.find(MAGNETOMETER_X_NOISE_TESLA) !=

‎src/RgbdCameraSensor.cc

+2-2
Original file line numberDiff line numberDiff line change
@@ -475,7 +475,7 @@ bool RgbdCameraSensor::Update(const std::chrono::steady_clock::duration &_now)
475475
*msg.mutable_header()->mutable_stamp() = msgs::Convert(_now);
476476
auto frame = msg.mutable_header()->add_data();
477477
frame->set_key("frame_id");
478-
frame->add_value(this->Name());
478+
frame->add_value(this->FrameId());
479479

480480
std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
481481

@@ -587,7 +587,7 @@ bool RgbdCameraSensor::Update(const std::chrono::steady_clock::duration &_now)
587587
*msg.mutable_header()->mutable_stamp() = msgs::Convert(_now);
588588
auto frame = msg.mutable_header()->add_data();
589589
frame->set_key("frame_id");
590-
frame->add_value(this->Name());
590+
frame->add_value(this->FrameId());
591591
msg.set_data(data, rendering::PixelUtil::MemorySize(rendering::PF_R8G8B8,
592592
width, height));
593593

‎src/Sensor.cc

+28
Original file line numberDiff line numberDiff line change
@@ -106,6 +106,9 @@ class ignition::sensors::SensorPrivate
106106
/// A map is used so that a single sensor can have multiple sensor
107107
/// streams each with a sequence counter.
108108
public: std::map<std::string, uint64_t> sequences;
109+
110+
/// \brief frame id
111+
public: std::string frame_id;
109112
};
110113

111114
SensorId SensorPrivate::idCounter = 0;
@@ -136,6 +139,19 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf)
136139
return false;
137140
}
138141

142+
sdf::ElementPtr element = _sdf.Element();
143+
if (element)
144+
{
145+
if (element->HasElement("ignition_frame_id"))
146+
{
147+
this->frame_id = element->Get<std::string>("ignition_frame_id");
148+
}
149+
else
150+
{
151+
this->frame_id = this->name;
152+
}
153+
}
154+
139155
// Try resolving the pose first, and only use the raw pose if that fails
140156
auto semPose = _sdf.SemanticPose();
141157
sdf::Errors errors = semPose.Resolve(this->pose);
@@ -226,6 +242,18 @@ std::string Sensor::Name() const
226242
return this->dataPtr->name;
227243
}
228244

245+
//////////////////////////////////////////////////
246+
std::string Sensor::FrameId() const
247+
{
248+
return this->dataPtr->frame_id;
249+
}
250+
251+
//////////////////////////////////////////////////
252+
void Sensor::SetFrameId(const std::string &_frameId)
253+
{
254+
this->dataPtr->frame_id = _frameId;
255+
}
256+
229257
//////////////////////////////////////////////////
230258
std::string Sensor::Topic() const
231259
{

‎src/Sensor_TEST.cc

+4
Original file line numberDiff line numberDiff line change
@@ -93,6 +93,10 @@ TEST(Sensor_TEST, Sensor)
9393
EXPECT_EQ(1u, sensor.Id());
9494

9595
EXPECT_EQ(nullptr, sensor.SDF());
96+
97+
EXPECT_EQ(sensor.Name(), sensor.FrameId());
98+
sensor.SetFrameId("frame_id_12");
99+
EXPECT_EQ(std::string("frame_id_12"), sensor.FrameId());
96100
}
97101

98102
//////////////////////////////////////////////////

‎src/ThermalCameraSensor.cc

+1-1
Original file line numberDiff line numberDiff line change
@@ -463,7 +463,7 @@ bool ThermalCameraSensor::Update(
463463
*stamp = msgs::Convert(_now);
464464
auto frame = this->dataPtr->thermalMsg.mutable_header()->add_data();
465465
frame->set_key("frame_id");
466-
frame->add_value(this->Name());
466+
frame->add_value(this->FrameId());
467467

468468
std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
469469

‎test/integration/camera_plugin.cc

+2
Original file line numberDiff line numberDiff line change
@@ -95,6 +95,8 @@ void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine)
9595
EXPECT_EQ(256u, sensor->ImageWidth());
9696
EXPECT_EQ(257u, sensor->ImageHeight());
9797

98+
EXPECT_EQ(std::string("base_camera"), sensor->FrameId());
99+
98100
std::string topic = "/test/integration/CameraPlugin_imagesWithBuiltinSDF";
99101
WaitForMessageTestHelper<ignition::msgs::Image> helper(topic);
100102

‎test/integration/camera_sensor_builtin.sdf

+1
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
<link name="link1">
55
<sensor name="camera1" type="camera">
66
<update_rate>10</update_rate>
7+
<ignition_frame_id>base_camera</ignition_frame_id>
78
<topic>/test/integration/CameraPlugin_imagesWithBuiltinSDF</topic>
89
<camera>
910
<horizontal_fov>1.05</horizontal_fov>

0 commit comments

Comments
 (0)