add frame in dvl

Signed-off-by: Ian Chen <[email protected]>
gazebosim · Jul 3, 2024 · e6568bd · e6568bd
1 parent 615f3e7
commit e6568bd
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 0 deletions.
diff --git a/src/DopplerVelocityLog.cc b/src/DopplerVelocityLog.cc
@@ -1946,6 +1946,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);
         }
@@ -1965,6 +1968,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);
         }

diff --git a/test/integration/dvl.cc b/test/integration/dvl.cc
@@ -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
 
@@ -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'>"
+    << "    <frame_id>" << _config.frameId << "</frame_id>"
     << "    <always_on>" << _config.alwaysOn << "</always_on>"
     << "    <update_rate>" << _config.updateRate << "</update_rate>"
     << "    <topic>" << _config.topic << "</topic>"
@@ -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());
 }
 
@@ -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());
 }
 
@@ -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());
 }