Skip to content

Commit

Permalink
Backport frame id fixes (#446)
Browse files Browse the repository at this point in the history 
Signed-off-by: Ian Chen <[email protected]>
  • Loading branch information
@iche033
iche033 authored Jul 10, 2024
1 parent ad95288 commit d49310b
Show file tree
Hide file tree
Showing 5 changed files with 60 additions and 18 deletions.
6 changes: 6 additions & 0 deletions src/DopplerVelocityLog.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1945,6 +1945,9 @@ namespace gz
if (this->dataPtr->publishingEstimates)
{
auto * headerMessage = bottomModeMessage.mutable_header();
auto frame = headerMessage->add_data();
frame->set_key("frame_id");
frame->add_value(this->FrameId());
this->AddSequence(headerMessage, "doppler_velocity_log");
this->dataPtr->pub.Publish(bottomModeMessage);
}
Expand All @@ -1964,6 +1967,9 @@ namespace gz
if (this->dataPtr->publishingEstimates)
{
auto * headerMessage = waterMassModeMessage.mutable_header();
auto frame = headerMessage->add_data();
frame->set_key("frame_id");
frame->add_value(this->FrameId());
this->AddSequence(headerMessage, "doppler_velocity_log");
this->dataPtr->pub.Publish(waterMassModeMessage);
}
Expand Down
2 changes: 1 addition & 1 deletion src/GpuLidarSensor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -134,7 +134,7 @@ bool GpuLidarSensor::Load(const sdf::Sensor &_sdf)
// by ROS1: https://github.com/ros/common_msgs/pull/77. Ideally, memory
// alignment should be configured. This same problem is in the
// RgbdCameraSensor.
msgs::InitPointCloudPacked(this->dataPtr->pointMsg, this->Name(), true,
msgs::InitPointCloudPacked(this->dataPtr->pointMsg, this->FrameId(), true,
{{"xyz", msgs::PointCloudPacked::Field::FLOAT32},
{"intensity", msgs::PointCloudPacked::Field::FLOAT32},
{"ring", msgs::PointCloudPacked::Field::UINT16}});
Expand Down
10 changes: 5 additions & 5 deletions src/Sensor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,7 +138,7 @@ class gz::sensors::SensorPrivate
public: std::map<std::string, uint64_t> sequences;

/// \brief frame id
public: std::string frame_id;
public: std::string frameId;

/// \brief If sensor is active or not.
public: bool active = true;
Expand Down Expand Up @@ -187,11 +187,11 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf)
{
if (element->HasElement("gz_frame_id"))
{
this->frame_id = element->Get<std::string>("gz_frame_id");
this->frameId = element->Get<std::string>("gz_frame_id");
}
else
{
this->frame_id = this->name;
this->frameId = this->name;
}
}

Expand Down Expand Up @@ -288,13 +288,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;
}

//////////////////////////////////////////////////
Expand Down
23 changes: 23 additions & 0 deletions test/integration/dvl.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@ using DopplerVelocityLog = sensors::DopplerVelocityLog;
struct DVLConfig
{
std::string name = "dvl";
std::string frameId = "dvl_frame";
std::string topic = "/gz/sensors/test/dvl";
double updateRate = 30; // Hz

Expand Down Expand Up @@ -75,6 +76,7 @@ sdf::ElementPtr MakeDVLSdf(const DVLConfig &_config)
<< " <model name='model'>"
<< " <link name='link'>"
<< " <sensor name='" << _config.name << "' type='custom' gz:type='dvl'>"
<< " <gz_frame_id>" << _config.frameId << "</gz_frame_id>"
<< " <always_on>" << _config.alwaysOn << "</always_on>"
<< " <update_rate>" << _config.updateRate << "</update_rate>"
<< " <topic>" << _config.topic << "</topic>"
Expand Down Expand Up @@ -345,6 +347,13 @@ TEST_P(DopplerVelocityLogTest, BottomTrackingWhileStatic)
}
EXPECT_EQ(0, message.status());

// check frame id
EXPECT_TRUE(message.has_header());
EXPECT_LT(1, message.header().data().size());
EXPECT_EQ("frame_id", message.header().data(0).key());
ASSERT_EQ(1, message.header().data(0).value().size());
EXPECT_EQ("dvl_frame", message.header().data(0).value(0));

this->manager.Remove(sensor->Id());
}

Expand Down Expand Up @@ -436,6 +445,13 @@ TEST_P(DopplerVelocityLogTest, WaterMassTrackingWhileStatic)
}
EXPECT_EQ(0, message.status());

// check frame id
EXPECT_TRUE(message.has_header());
EXPECT_LT(1, message.header().data().size());
EXPECT_EQ("frame_id", message.header().data(0).key());
ASSERT_EQ(1, message.header().data(0).value().size());
EXPECT_EQ("dvl_frame", message.header().data(0).value(0));

this->manager.Remove(sensor->Id());
}

Expand Down Expand Up @@ -519,6 +535,13 @@ TEST_P(DopplerVelocityLogTest, BottomTrackingWhileInMotion)
}
EXPECT_EQ(0, message.status());

// check frame id
EXPECT_TRUE(message.has_header());
EXPECT_LT(1, message.header().data().size());
EXPECT_EQ("frame_id", message.header().data(0).key());
ASSERT_EQ(1, message.header().data(0).value().size());
EXPECT_EQ("dvl_frame", message.header().data(0).value(0));

this->manager.Remove(sensor->Id());
}

Expand Down
37 changes: 25 additions & 12 deletions test/integration/gpu_lidar_sensor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,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
Expand All @@ -74,6 +74,7 @@ sdf::ElementPtr GpuLidarToSdf(const std::string &name,
<< " <model name='m1'>"
<< " <link name='link1'>"
<< " <sensor name='" << name << "' type='gpu_lidar'>"
<< " <gz_frame_id>" << frameId << "</gz_frame_id>"
<< " <pose>" << pose << "</pose>"
<< " <topic>" << topic << "</topic>"
<< " <updateRate>"<< updateRate <<"</updateRate>"
Expand Down Expand Up @@ -194,14 +195,15 @@ 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),
gz::math::Quaterniond::Identity);
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 gz-rendering with an empty scene
auto *engine = gz::rendering::engine(_renderEngine);
Expand Down Expand Up @@ -313,14 +315,15 @@ 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),
gz::math::Quaterniond::Identity);
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 =
Expand Down Expand Up @@ -399,7 +402,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);
Expand All @@ -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();

Expand Down Expand Up @@ -462,22 +471,23 @@ 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),
gz::math::Quaterniond::Identity);
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),
gz::math::Quaterniond(GZ_PI/2.0, 0, 0));
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 =
Expand Down Expand Up @@ -618,14 +628,15 @@ 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),
gz::math::Quaterniond::Identity);
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 =
Expand Down Expand Up @@ -745,22 +756,23 @@ 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),
gz::math::Quaterniond::Identity);
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),
gz::math::Quaterniond::Identity);
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 =
Expand Down Expand Up @@ -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);
Expand All @@ -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<gz::sensors::GpuLidarSensor>(lidarSdf);
ASSERT_NE(nullptr, lidar);
Expand All @@ -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<gz::sensors::GpuLidarSensor>(lidarSdf);
ASSERT_NE(nullptr, lidar);
Expand All @@ -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<gz::sensors::GpuLidarSensor>(lidarSdf);
EXPECT_EQ(nullptr, sensor);
Expand Down

0 comments on commit d49310b

Please sign in to comment.