Skip to content

Commit 91c139f

Browse files
iche033iche033
committed
Backport frame id fixes (#446)
Signed-off-by: Ian Chen <ichen@openrobotics.org>
1 parent 53cf4ed commit 91c139f

File tree

2 files changed

+31
-18
lines changed

2 files changed

+31
-18
lines changed

‎src/Sensor.cc‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -108,7 +108,7 @@ class gz::sensors::SensorPrivate
108108
public: std::map<std::string, uint64_t> sequences;
109109

110110
/// \brief frame id
111-
public: std::string frame_id;
111+
public: std::string frameId;
112112

113113
/// \brief If sensor is active or not.
114114
public: bool active = true;
@@ -147,7 +147,7 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf)
147147
{
148148
if (element->HasElement("ignition_frame_id"))
149149
{
150-
this->frame_id = element->Get<std::string>("ignition_frame_id");
150+
this->frameId = element->Get<std::string>("ignition_frame_id");
151151
// Warn if both ignition_frame_id and gz_frame_id are specified
152152
if (element->HasElement("gz_frame_id"))
153153
{
@@ -159,11 +159,11 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf)
159159
{
160160
// Also read gz_frame_id to support SDF that's compatible with newer
161161
// versions of Gazebo.
162-
this->frame_id = element->Get<std::string>("gz_frame_id");
162+
this->frameId = element->Get<std::string>("gz_frame_id");
163163
}
164164
else
165165
{
166-
this->frame_id = this->name;
166+
this->frameId = this->name;
167167
}
168168
}
169169

@@ -260,13 +260,13 @@ std::string Sensor::Name() const
260260
//////////////////////////////////////////////////
261261
std::string Sensor::FrameId() const
262262
{
263-
return this->dataPtr->frame_id;
263+
return this->dataPtr->frameId;
264264
}
265265

266266
//////////////////////////////////////////////////
267267
void Sensor::SetFrameId(const std::string &_frameId)
268268
{
269-
this->dataPtr->frame_id = _frameId;
269+
this->dataPtr->frameId = _frameId;
270270
}
271271

272272
//////////////////////////////////////////////////

‎test/integration/gpu_lidar_sensor.cc‎

Lines changed: 25 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -70,7 +70,7 @@ sdf::ElementPtr GpuLidarToSdf(const std::string &name,
7070
const double vertResolution, const double vertMinAngle,
7171
const double vertMaxAngle, const double rangeResolution,
7272
const double rangeMin, const double rangeMax, const bool alwaysOn,
73-
const bool visualize)
73+
const bool visualize, const std::string &frameId)
7474
{
7575
std::ostringstream stream;
7676
stream
@@ -79,6 +79,7 @@ sdf::ElementPtr GpuLidarToSdf(const std::string &name,
7979
<< " <model name='m1'>"
8080
<< " <link name='link1'>"
8181
<< " <sensor name='" << name << "' type='gpu_lidar'>"
82+
<< " <gz_frame_id>" << frameId << "</gz_frame_id>"
8283
<< " <pose>" << pose << "</pose>"
8384
<< " <topic>" << topic << "</topic>"
8485
<< " <updateRate>"<< updateRate <<"</updateRate>"
@@ -186,14 +187,15 @@ void GpuLidarSensorTest::CreateGpuLidar(const std::string &_renderEngine)
186187
const double rangeMax = 10.0;
187188
const bool alwaysOn = 1;
188189
const bool visualize = 1;
190+
const std::string frameId = "TestGpuLidar_frame";
189191

190192
// Create sensor description in SDF
191193
gz::math::Pose3d testPose(gz::math::Vector3d(0, 0, 0.1),
192194
gz::math::Quaterniond::Identity);
193195
sdf::ElementPtr lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic,
194196
horzSamples, horzResolution, horzMinAngle, horzMaxAngle,
195197
vertSamples, vertResolution, vertMinAngle, vertMaxAngle,
196-
rangeResolution, rangeMin, rangeMax, alwaysOn, visualize);
198+
rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId);
197199

198200
// Setup ign-rendering with an empty scene
199201
auto *engine = gz::rendering::engine(_renderEngine);
@@ -305,14 +307,15 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine)
305307
const double rangeMax = 10.0;
306308
const bool alwaysOn = 1;
307309
const bool visualize = 1;
310+
const std::string frameId = "TestGpuLidar_frame";
308311

309312
// Create sensor SDF
310313
gz::math::Pose3d testPose(gz::math::Vector3d(0.0, 0.0, 0.1),
311314
gz::math::Quaterniond::Identity);
312315
sdf::ElementPtr lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic,
313316
horzSamples, horzResolution, horzMinAngle, horzMaxAngle,
314317
vertSamples, vertResolution, vertMinAngle, vertMaxAngle,
315-
rangeResolution, rangeMin, rangeMax, alwaysOn, visualize);
318+
rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId);
316319

317320
// Create and populate scene
318321
gz::rendering::RenderEngine *engine =
@@ -393,7 +396,7 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine)
393396
LASER_TOL);
394397
EXPECT_DOUBLE_EQ(laserMsgs.back().ranges(last), gz::math::INF_D);
395398

396-
EXPECT_EQ(laserMsgs.back().frame(), name);
399+
EXPECT_EQ(laserMsgs.back().frame(), frameId);
397400
EXPECT_NEAR(laserMsgs.back().angle_min(), horzMinAngle, 1e-4);
398401
EXPECT_NEAR(laserMsgs.back().angle_max(), horzMaxAngle, 1e-4);
399402
EXPECT_NEAR(laserMsgs.back().count(), horzSamples, 1e-4);
@@ -425,6 +428,12 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine)
425428
EXPECT_FALSE(pointMsgs.back().is_dense());
426429
EXPECT_EQ(32u * horzSamples * vertSamples, pointMsgs.back().data().size());
427430

431+
EXPECT_TRUE(pointMsgs.back().has_header());
432+
EXPECT_LT(1, pointMsgs.back().header().data().size());
433+
EXPECT_EQ("frame_id", pointMsgs.back().header().data(0).key());
434+
ASSERT_EQ(1, pointMsgs.back().header().data(0).value().size());
435+
EXPECT_EQ(frameId, pointMsgs.back().header().data(0).value(0));
436+
428437
// Clean up rendering ptrs
429438
visualBox1.reset();
430439

@@ -462,22 +471,23 @@ void GpuLidarSensorTest::TestThreeBoxes(const std::string &_renderEngine)
462471
const double rangeMax = 10.0;
463472
const bool alwaysOn = 1;
464473
const bool visualize = 1;
474+
const std::string frameId = "TestGpuLidar_frame";
465475

466476
// Create sensor SDF
467477
gz::math::Pose3d testPose1(gz::math::Vector3d(0, 0, 0.1),
468478
gz::math::Quaterniond::Identity);
469479
sdf::ElementPtr lidarSdf1 = GpuLidarToSdf(name1, testPose1, updateRate,
470480
topic1, horzSamples, horzResolution, horzMinAngle, horzMaxAngle,
471481
vertSamples, vertResolution, vertMinAngle, vertMaxAngle,
472-
rangeResolution, rangeMin, rangeMax, alwaysOn, visualize);
482+
rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId);
473483

474484
// Create a second sensor SDF rotated
475485
gz::math::Pose3d testPose2(gz::math::Vector3d(0, 0, 0.1),
476486
gz::math::Quaterniond(IGN_PI/2.0, 0, 0));
477487
sdf::ElementPtr lidarSdf2 = GpuLidarToSdf(name2, testPose2, updateRate,
478488
topic2, horzSamples, horzResolution, horzMinAngle, horzMaxAngle,
479489
vertSamples, vertResolution, vertMinAngle, vertMaxAngle,
480-
rangeResolution, rangeMin, rangeMax, alwaysOn, visualize);
490+
rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId);
481491

482492
// Create and populate scene
483493
gz::rendering::RenderEngine *engine =
@@ -618,14 +628,15 @@ void GpuLidarSensorTest::VerticalLidar(const std::string &_renderEngine)
618628
const double rangeMax = 10.0;
619629
const bool alwaysOn = 1;
620630
const bool visualize = 1;
631+
const std::string frameId = "TestGpuLidar_frame";
621632

622633
// Create sensor SDF
623634
gz::math::Pose3d testPose(gz::math::Vector3d(0.25, 0.0, 0.5),
624635
gz::math::Quaterniond::Identity);
625636
sdf::ElementPtr lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic,
626637
horzSamples, horzResolution, horzMinAngle, horzMaxAngle,
627638
vertSamples, vertResolution, vertMinAngle, vertMaxAngle,
628-
rangeResolution, rangeMin, rangeMax, alwaysOn, visualize);
639+
rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId);
629640

630641
// Create and populate scene
631642
gz::rendering::RenderEngine *engine =
@@ -745,22 +756,23 @@ void GpuLidarSensorTest::ManualUpdate(const std::string &_renderEngine)
745756
const double rangeMax = 10.0;
746757
const bool alwaysOn = 1;
747758
const bool visualize = 1;
759+
const std::string frameId = "TestGpuLidar_frame";
748760

749761
// Create sensor SDF
750762
gz::math::Pose3d testPose1(gz::math::Vector3d(0, 0, 0.1),
751763
gz::math::Quaterniond::Identity);
752764
sdf::ElementPtr lidarSdf1 = GpuLidarToSdf(name1, testPose1, updateRate,
753765
topic1, horzSamples, horzResolution, horzMinAngle, horzMaxAngle,
754766
vertSamples, vertResolution, vertMinAngle, vertMaxAngle,
755-
rangeResolution, rangeMin, rangeMax, alwaysOn, visualize);
767+
rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId);
756768

757769
// Create a second sensor SDF at an xy offset of 1
758770
gz::math::Pose3d testPose2(gz::math::Vector3d(1, 1, 0.1),
759771
gz::math::Quaterniond::Identity);
760772
sdf::ElementPtr lidarSdf2 = GpuLidarToSdf(name2, testPose2, updateRate,
761773
topic2, horzSamples, horzResolution, horzMinAngle, horzMaxAngle,
762774
vertSamples, vertResolution, vertMinAngle, vertMaxAngle,
763-
rangeResolution, rangeMin, rangeMax, alwaysOn, visualize);
775+
rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId);
764776

765777
// Create and populate scene
766778
gz::rendering::RenderEngine *engine =
@@ -871,6 +883,7 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine)
871883
const bool alwaysOn = 1;
872884
const bool visualize = 1;
873885
auto testPose = gz::math::Pose3d();
886+
const std::string frameId = "TestGpuLidar_frame";
874887

875888
// Scene
876889
auto engine = gz::rendering::engine(_renderEngine);
@@ -892,7 +905,7 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine)
892905
auto lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic,
893906
horzSamples, horzResolution, horzMinAngle, horzMaxAngle,
894907
vertSamples, vertResolution, vertMinAngle, vertMaxAngle,
895-
rangeResolution, rangeMin, rangeMax, alwaysOn, visualize);
908+
rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId);
896909

897910
auto lidar = mgr.CreateSensor<gz::sensors::GpuLidarSensor>(lidarSdf);
898911
ASSERT_NE(nullptr, lidar);
@@ -906,7 +919,7 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine)
906919
auto lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic,
907920
horzSamples, horzResolution, horzMinAngle, horzMaxAngle,
908921
vertSamples, vertResolution, vertMinAngle, vertMaxAngle,
909-
rangeResolution, rangeMin, rangeMax, alwaysOn, visualize);
922+
rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId);
910923

911924
auto lidar = mgr.CreateSensor<gz::sensors::GpuLidarSensor>(lidarSdf);
912925
ASSERT_NE(nullptr, lidar);
@@ -921,7 +934,7 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine)
921934
auto lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic,
922935
horzSamples, horzResolution, horzMinAngle, horzMaxAngle,
923936
vertSamples, vertResolution, vertMinAngle, vertMaxAngle,
924-
rangeResolution, rangeMin, rangeMax, alwaysOn, visualize);
937+
rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId);
925938

926939
auto sensor = mgr.CreateSensor<gz::sensors::GpuLidarSensor>(lidarSdf);
927940
EXPECT_EQ(nullptr, sensor);

0 commit comments

Comments
 (0)