Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Port frame_id fixes from #444 #446

Merged
merged 1 commit into from
Jul 10, 2024
Merged

Port frame_id fixes from #444 #446

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions src/DopplerVelocityLog.cc
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading