add frame_id and child_frame_id attribute support for DiffDrive (#361)

Add configuration of the odom frame_id and child_frame_id fields from sdf attributes <frame_id> and <child_frame_id> Signed-off-by: Guillaume <[email protected]> Signed-off-by: Guillaume Doisy <[email protected]> Signed-off-by: Jose Luis Rivero <[email protected]>
gazebosim · Jan 4, 2021 · 3a2a66c · 3a2a66c
1 parent fc79eb1
commit 3a2a66c
Show file tree

Hide file tree

Showing 3 changed files with 384 additions and 3 deletions.
diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc
@@ -136,6 +136,12 @@ class ignition::gazebo::systems::DiffDrivePrivate
 
   /// \brief A mutex to protect the target velocity command.
   public: std::mutex mutex;
+
+  /// \brief frame_id from sdf.
+  public: std::string sdfFrameId;
+
+  /// \brief child_frame_id from sdf.
+  public: std::string sdfChildFrameId;
 };
 
 //////////////////////////////////////////////////
@@ -265,6 +271,12 @@ void DiffDrive::Configure(const Entity &_entity,
   this->dataPtr->odomPub = this->dataPtr->node.Advertise<msgs::Odometry>(
       odomTopic);
 
+  if (_sdf->HasElement("frame_id"))
+    this->dataPtr->sdfFrameId = _sdf->Get<std::string>("frame_id");
+
+  if (_sdf->HasElement("child_frame_id"))
+    this->dataPtr->sdfChildFrameId = _sdf->Get<std::string>("child_frame_id");
+
   ignmsg << "DiffDrive subscribing to twist messages on [" << topic << "]"
          << std::endl;
 }
@@ -427,16 +439,33 @@ void DiffDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info,
   // Set the frame id.
   auto frame = msg.mutable_header()->add_data();
   frame->set_key("frame_id");
-  frame->add_value(this->model.Name(_ecm) + "/odom");
+  if (this->sdfFrameId.empty())
+  {
+    frame->add_value(this->model.Name(_ecm) + "/odom");
+  }
+  else
+  {
+    frame->add_value(this->sdfFrameId);
+  }
 
   std::optional<std::string> linkName = this->canonicalLink.Name(_ecm);
-  if (linkName)
+  if (this->sdfChildFrameId.empty())
+  {
+    if (linkName)
+    {
+      auto childFrame = msg.mutable_header()->add_data();
+      childFrame->set_key("child_frame_id");
+      childFrame->add_value(this->model.Name(_ecm) + "/" + *linkName);
+    }
+  }
+  else
   {
     auto childFrame = msg.mutable_header()->add_data();
     childFrame->set_key("child_frame_id");
-    childFrame->add_value(this->model.Name(_ecm) + "/" + *linkName);
+    childFrame->add_value(this->sdfChildFrameId);
   }
 
+
   // Publish the message
   this->odomPub.Publish(msg);
 }

diff --git a/test/integration/diff_drive_system.cc b/test/integration/diff_drive_system.cc
@@ -310,6 +310,113 @@ TEST_P(DiffDriveTest, SkidPublishCmd)
   EXPECT_LT(poses[0].Rot().Z(), poses[3999].Rot().Z());
 }
 
+/////////////////////////////////////////////////
+TEST_P(DiffDriveTest, OdomFrameId)
+{
+  // Start server
+  ServerConfig serverConfig;
+  serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
+      "/test/worlds/diff_drive.sdf");
+
+  Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  server.SetUpdatePeriod(0ns);
+
+  unsigned int odomPosesCount = 0;
+  std::function<void(const msgs::Odometry &)> odomCb =
+    [&odomPosesCount](const msgs::Odometry &_msg)
+    {
+      ASSERT_TRUE(_msg.has_header());
+      ASSERT_TRUE(_msg.header().has_stamp());
+
+      ASSERT_GT(_msg.header().data_size(), 1);
+
+      EXPECT_STREQ(_msg.header().data(0).key().c_str(), "frame_id");
+      EXPECT_STREQ(
+            _msg.header().data(0).value().Get(0).c_str(), "vehicle/odom");
+
+      EXPECT_STREQ(_msg.header().data(1).key().c_str(), "child_frame_id");
+      EXPECT_STREQ(
+            _msg.header().data(1).value().Get(0).c_str(), "vehicle/chassis");
+
+      odomPosesCount++;
+    };
+
+  transport::Node node;
+  auto pub = node.Advertise<msgs::Twist>("/model/vehicle/cmd_vel");
+  node.Subscribe("/model/vehicle/odometry", odomCb);
+
+  msgs::Twist msg;
+  msgs::Set(msg.mutable_linear(), math::Vector3d(0.5, 0, 0));
+  msgs::Set(msg.mutable_angular(), math::Vector3d(0.0, 0, 0.2));
+
+  pub.Publish(msg);
+
+  server.Run(true, 100, false);
+
+  int sleep = 0;
+  int maxSleep = 30;
+  for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+  ASSERT_NE(maxSleep, sleep);
+
+  EXPECT_EQ(5u, odomPosesCount);
+}
+
+/////////////////////////////////////////////////
+TEST_P(DiffDriveTest, OdomCustomFrameId)
+{
+  // Start server
+  ServerConfig serverConfig;
+  serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
+      "/test/worlds/diff_drive_custom_frame_id.sdf");
+
+  Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  server.SetUpdatePeriod(0ns);
+
+  unsigned int odomPosesCount = 0;
+  std::function<void(const msgs::Odometry &)> odomCb =
+    [&odomPosesCount](const msgs::Odometry &_msg)
+    {
+      ASSERT_TRUE(_msg.has_header());
+      ASSERT_TRUE(_msg.header().has_stamp());
+
+      ASSERT_GT(_msg.header().data_size(), 1);
+
+      EXPECT_STREQ(_msg.header().data(0).key().c_str(), "frame_id");
+      EXPECT_STREQ(_msg.header().data(0).value().Get(0).c_str(), "odom");
+
+      EXPECT_STREQ(_msg.header().data(1).key().c_str(), "child_frame_id");
+      EXPECT_STREQ(
+            _msg.header().data(1).value().Get(0).c_str(), "base_footprint");
+
+      odomPosesCount++;
+    };
+
+  transport::Node node;
+  auto pub = node.Advertise<msgs::Twist>("/model/vehicle/cmd_vel");
+  node.Subscribe("/model/vehicle/odometry", odomCb);
+
+  server.Run(true, 100, false);
+
+  int sleep = 0;
+  int maxSleep = 30;
+  for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+  ASSERT_NE(maxSleep, sleep);
+
+  EXPECT_EQ(5u, odomPosesCount);
+}
+
 // Run multiple times
 INSTANTIATE_TEST_CASE_P(ServerRepeat, DiffDriveTest,
     ::testing::Range(1, 2));