From d9d2fc7712275b2c4b2b9b745b7eaa9a94f4111f Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 10 Jul 2024 09:24:11 -0700 Subject: [PATCH] Backport frame id fixes (#446) Signed-off-by: Ian Chen --- src/Lidar.cc | 2 -- src/Sensor.cc | 12 ++++----- test/integration/gpu_lidar_sensor.cc | 37 +++++++++++++++++++--------- 3 files changed, 31 insertions(+), 20 deletions(-) diff --git a/src/Lidar.cc b/src/Lidar.cc index bbf31deb..f14ab617 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -250,8 +250,6 @@ 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->FrameId()); this->dataPtr->laserMsg.set_frame(this->FrameId()); diff --git a/src/Sensor.cc b/src/Sensor.cc index bc25f7ec..f9a2d5db 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -108,7 +108,7 @@ class gz::sensors::SensorPrivate public: std::map sequences; /// \brief frame id - public: std::string frame_id; + public: std::string frameId; /// \brief If sensor is active or not. public: bool active = true; @@ -147,7 +147,7 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf) { if (element->HasElement("ignition_frame_id")) { - this->frame_id = element->Get("ignition_frame_id"); + this->frameId = element->Get("ignition_frame_id"); // Warn if both ignition_frame_id and gz_frame_id are specified if (element->HasElement("gz_frame_id")) { @@ -159,11 +159,11 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf) { // Also read gz_frame_id to support SDF that's compatible with newer // versions of Gazebo. - this->frame_id = element->Get("gz_frame_id"); + this->frameId = element->Get("gz_frame_id"); } else { - this->frame_id = this->name; + this->frameId = this->name; } } @@ -260,13 +260,13 @@ std::string Sensor::Name() const ////////////////////////////////////////////////// std::string Sensor::FrameId() const { - return this->dataPtr->frame_id; + return this->dataPtr->frameId; } ////////////////////////////////////////////////// void Sensor::SetFrameId(const std::string &_frameId) { - this->dataPtr->frame_id = _frameId; + this->dataPtr->frameId = _frameId; } ////////////////////////////////////////////////// diff --git a/test/integration/gpu_lidar_sensor.cc b/test/integration/gpu_lidar_sensor.cc index f26afb6c..769a20e8 100644 --- a/test/integration/gpu_lidar_sensor.cc +++ b/test/integration/gpu_lidar_sensor.cc @@ -70,7 +70,7 @@ sdf::ElementPtr GpuLidarToSdf(const std::string &name, const double vertResolution, const double vertMinAngle, const double vertMaxAngle, const double rangeResolution, const double rangeMin, const double rangeMax, const bool alwaysOn, - const bool visualize) + const bool visualize, const std::string &frameId) { std::ostringstream stream; stream @@ -79,6 +79,7 @@ sdf::ElementPtr GpuLidarToSdf(const std::string &name, << " " << " " << " " + << " " << frameId << "" << " " << pose << "" << " " << topic << "" << " "<< updateRate <<"" @@ -186,6 +187,7 @@ void GpuLidarSensorTest::CreateGpuLidar(const std::string &_renderEngine) const double rangeMax = 10.0; const bool alwaysOn = 1; const bool visualize = 1; + const std::string frameId = "TestGpuLidar_frame"; // Create sensor description in SDF gz::math::Pose3d testPose(gz::math::Vector3d(0, 0, 0.1), @@ -193,7 +195,7 @@ void GpuLidarSensorTest::CreateGpuLidar(const std::string &_renderEngine) sdf::ElementPtr lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, - rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId); // Setup ign-rendering with an empty scene auto *engine = gz::rendering::engine(_renderEngine); @@ -305,6 +307,7 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) const double rangeMax = 10.0; const bool alwaysOn = 1; const bool visualize = 1; + const std::string frameId = "TestGpuLidar_frame"; // Create sensor SDF gz::math::Pose3d testPose(gz::math::Vector3d(0.0, 0.0, 0.1), @@ -312,7 +315,7 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) sdf::ElementPtr lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, - rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId); // Create and populate scene gz::rendering::RenderEngine *engine = @@ -393,7 +396,7 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) LASER_TOL); EXPECT_DOUBLE_EQ(laserMsgs.back().ranges(last), gz::math::INF_D); - EXPECT_EQ(laserMsgs.back().frame(), name); + EXPECT_EQ(laserMsgs.back().frame(), frameId); EXPECT_NEAR(laserMsgs.back().angle_min(), horzMinAngle, 1e-4); EXPECT_NEAR(laserMsgs.back().angle_max(), horzMaxAngle, 1e-4); EXPECT_NEAR(laserMsgs.back().count(), horzSamples, 1e-4); @@ -425,6 +428,12 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) EXPECT_FALSE(pointMsgs.back().is_dense()); EXPECT_EQ(32u * horzSamples * vertSamples, pointMsgs.back().data().size()); + EXPECT_TRUE(pointMsgs.back().has_header()); + EXPECT_LT(1, pointMsgs.back().header().data().size()); + EXPECT_EQ("frame_id", pointMsgs.back().header().data(0).key()); + ASSERT_EQ(1, pointMsgs.back().header().data(0).value().size()); + EXPECT_EQ(frameId, pointMsgs.back().header().data(0).value(0)); + // Clean up rendering ptrs visualBox1.reset(); @@ -462,6 +471,7 @@ void GpuLidarSensorTest::TestThreeBoxes(const std::string &_renderEngine) const double rangeMax = 10.0; const bool alwaysOn = 1; const bool visualize = 1; + const std::string frameId = "TestGpuLidar_frame"; // Create sensor SDF gz::math::Pose3d testPose1(gz::math::Vector3d(0, 0, 0.1), @@ -469,7 +479,7 @@ void GpuLidarSensorTest::TestThreeBoxes(const std::string &_renderEngine) sdf::ElementPtr lidarSdf1 = GpuLidarToSdf(name1, testPose1, updateRate, topic1, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, - rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId); // Create a second sensor SDF rotated gz::math::Pose3d testPose2(gz::math::Vector3d(0, 0, 0.1), @@ -477,7 +487,7 @@ void GpuLidarSensorTest::TestThreeBoxes(const std::string &_renderEngine) sdf::ElementPtr lidarSdf2 = GpuLidarToSdf(name2, testPose2, updateRate, topic2, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, - rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId); // Create and populate scene gz::rendering::RenderEngine *engine = @@ -618,6 +628,7 @@ void GpuLidarSensorTest::VerticalLidar(const std::string &_renderEngine) const double rangeMax = 10.0; const bool alwaysOn = 1; const bool visualize = 1; + const std::string frameId = "TestGpuLidar_frame"; // Create sensor SDF gz::math::Pose3d testPose(gz::math::Vector3d(0.25, 0.0, 0.5), @@ -625,7 +636,7 @@ void GpuLidarSensorTest::VerticalLidar(const std::string &_renderEngine) sdf::ElementPtr lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, - rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId); // Create and populate scene gz::rendering::RenderEngine *engine = @@ -745,6 +756,7 @@ void GpuLidarSensorTest::ManualUpdate(const std::string &_renderEngine) const double rangeMax = 10.0; const bool alwaysOn = 1; const bool visualize = 1; + const std::string frameId = "TestGpuLidar_frame"; // Create sensor SDF gz::math::Pose3d testPose1(gz::math::Vector3d(0, 0, 0.1), @@ -752,7 +764,7 @@ void GpuLidarSensorTest::ManualUpdate(const std::string &_renderEngine) sdf::ElementPtr lidarSdf1 = GpuLidarToSdf(name1, testPose1, updateRate, topic1, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, - rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId); // Create a second sensor SDF at an xy offset of 1 gz::math::Pose3d testPose2(gz::math::Vector3d(1, 1, 0.1), @@ -760,7 +772,7 @@ void GpuLidarSensorTest::ManualUpdate(const std::string &_renderEngine) sdf::ElementPtr lidarSdf2 = GpuLidarToSdf(name2, testPose2, updateRate, topic2, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, - rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId); // Create and populate scene gz::rendering::RenderEngine *engine = @@ -871,6 +883,7 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine) const bool alwaysOn = 1; const bool visualize = 1; auto testPose = gz::math::Pose3d(); + const std::string frameId = "TestGpuLidar_frame"; // Scene auto engine = gz::rendering::engine(_renderEngine); @@ -892,7 +905,7 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine) auto lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, - rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId); auto lidar = mgr.CreateSensor(lidarSdf); ASSERT_NE(nullptr, lidar); @@ -906,7 +919,7 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine) auto lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, - rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId); auto lidar = mgr.CreateSensor(lidarSdf); ASSERT_NE(nullptr, lidar); @@ -921,7 +934,7 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine) auto lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, - rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize, frameId); auto sensor = mgr.CreateSensor(lidarSdf); EXPECT_EQ(nullptr, sensor);