👩‍🌾 3 ➡️ 4

src/systems/diff_drive/DiffDrive.cc

@@ -121,6 +121,9 @@ class ignition::gazebo::systems::DiffDrivePrivate
   /// \brief Diff drive odometry message publisher.
   public: transport::Node::Publisher odomPub;
 
+  /// \brief Diff drive tf message publisher.
+  public: transport::Node::Publisher tfPub;
+
   /// \brief Linear velocity limiter.
   public: std::unique_ptr<SpeedLimiter> limiterLin;
 
@@ -283,6 +286,13 @@ void DiffDrive::Configure(const Entity &_entity,
   this->dataPtr->odomPub = this->dataPtr->node.Advertise<msgs::Odometry>(
       odomTopic);
 
+  std::string tfTopic{"/model/" + this->dataPtr->model.Name(_ecm) +
+    "/tf"};
+  if (_sdf->HasElement("tf_topic"))
+    tfTopic = _sdf->Get<std::string>("tf_topic");
+  this->dataPtr->tfPub = this->dataPtr->node.Advertise<msgs::Pose_V>(
+      tfTopic);
+
   if (_sdf->HasElement("frame_id"))
     this->dataPtr->sdfFrameId = _sdf->Get<std::string>("frame_id");
 
@@ -506,9 +516,17 @@ void DiffDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info,
     childFrame->add_value(this->sdfChildFrameId);
   }
 
+  // Construct the Pose_V/tf message and publish it.
+  msgs::Pose_V tfMsg;
+  ignition::msgs::Pose *tfMsgPose = nullptr;
+  tfMsgPose = tfMsg.add_pose();
+  tfMsgPose->mutable_header()->CopyFrom(*msg.mutable_header());
+  tfMsgPose->mutable_position()->CopyFrom(msg.mutable_pose()->position());
+  tfMsgPose->mutable_orientation()->CopyFrom(msg.mutable_pose()->orientation());
 
-  // Publish the message
+  // Publish the messages
   this->odomPub.Publish(msg);
+  this->tfPub.Publish(tfMsg);
 }
 
 //////////////////////////////////////////////////

src/systems/diff_drive/DiffDrive.hh

@@ -61,6 +61,22 @@ namespace systems
   /// `<odom_topic>`: Custom topic on which this system will publish odometry
   /// messages. This element if optional, and the default value is
   /// `/model/{name_of_model}/odometry`.
+  ///
+  /// `<tf_topic>`: Custom topic on which this system will publish the
+  /// transform from `frame_id` to `child_frame_id`. This element if optional,
+  ///  and the default value is `/model/{name_of_model}/tf`.
+  ///
+  /// `<frame_id>`: Custom `frame_id` field that this system will use as the
+  /// origin of the odometry transform in both the `<tf_topic>`
+  /// `ignition.msgs.Pose_V` message and the `<odom_topic>`
+  /// `ignition.msgs.Odometry` message. This element if optional, and the
+  /// default value is `{name_of_model}/odom`.
+  ///
+  /// `<child_frame_id>`: Custom `child_frame_id` that this system will use as
+  /// the target of the odometry trasnform in both the `<tf_topic>`
+  /// `ignition.msgs.Pose_V` message and the `<odom_topic>`
+  /// `ignition.msgs.Odometry` message. This element if optional,
+  ///  and the default value is `{name_of_model}/{name_of_link}`.
   class IGNITION_GAZEBO_VISIBLE DiffDrive
       : public System,
         public ISystemConfigure,

test/integration/diff_drive_system.cc

@@ -405,6 +405,190 @@ TEST_P(DiffDriveTest, OdomCustomFrameId)
   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, Pose_VFrameId)
+{
+  // 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::Pose_V &)> pose_VCb =
+    [&odomPosesCount](const msgs::Pose_V &_msg)
+    {
+      ASSERT_TRUE(_msg.pose(0).has_header());
+      ASSERT_TRUE(_msg.pose(0).header().has_stamp());
+
+      ASSERT_GT(_msg.pose(0).header().data_size(), 1);
+
+      EXPECT_STREQ(_msg.pose(0).header().data(0).key().c_str(),
+                   "frame_id");
+      EXPECT_STREQ(_msg.pose(0).header().data(0).value().Get(0).c_str(),
+                   "vehicle/odom");
+
+      EXPECT_STREQ(_msg.pose(0).header().data(1).key().c_str(),
+                   "child_frame_id");
+      EXPECT_STREQ(_msg.pose(0).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/tf", pose_VCb);
+
+  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, Pose_VCustomFrameId)
+{
+  // 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::Pose_V &)> Pose_VCb =
+    [&odomPosesCount](const msgs::Pose_V &_msg)
+    {
+      ASSERT_TRUE(_msg.pose(0).has_header());
+      ASSERT_TRUE(_msg.pose(0).header().has_stamp());
+
+      ASSERT_GT(_msg.pose(0).header().data_size(), 1);
+
+      EXPECT_STREQ(_msg.pose(0).header().data(0).key().c_str(),
+                   "frame_id");
+      EXPECT_STREQ(_msg.pose(0).header().data(0).value().Get(0).c_str(),
+                   "odom");
+
+      EXPECT_STREQ(_msg.pose(0).header().data(1).key().c_str(),
+                   "child_frame_id");
+      EXPECT_STREQ(_msg.pose(0).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/tf", Pose_VCb);
+
+  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, Pose_VCustomTfTopic)
+{
+  // Start server
+  ServerConfig serverConfig;
+  serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
+      "/test/worlds/diff_drive_custom_tf_topic.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::Pose_V &)> pose_VCb =
+    [&odomPosesCount](const msgs::Pose_V &_msg)
+    {
+      ASSERT_TRUE(_msg.pose(0).has_header());
+      ASSERT_TRUE(_msg.pose(0).header().has_stamp());
+
+      ASSERT_GT(_msg.pose(0).header().data_size(), 1);
+
+      EXPECT_STREQ(_msg.pose(0).header().data(0).key().c_str(), "frame_id");
+      EXPECT_STREQ(
+            _msg.pose(0).header().data(0).value().Get(0).c_str(),
+            "vehicle/odom");
+
+      EXPECT_STREQ(
+            _msg.pose(0).header().data(1).key().c_str(), "child_frame_id");
+      EXPECT_STREQ(
+            _msg.pose(0).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("/tf_foo", pose_VCb);
+
+  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;

test/integration/scene_broadcaster_system.cc

@@ -438,6 +438,7 @@ TEST_P(SceneBroadcasterTest, State)
   while (!received && sleep++ < maxSleep)
     IGN_SLEEP_MS(100);
   EXPECT_TRUE(received);
+  EXPECT_TRUE(node.Unsubscribe("/world/default/state"));
 
   // test async state request
   received = false;

test/integration/velocity_control_system.cc

@@ -96,43 +96,33 @@ class VelocityControlTest : public ::testing::TestWithParam<int>
     // and max velocity (0.5 m/s).
     // See <max_velocity< and <max_aceleration> parameters
     // in "/test/worlds/velocity_control.sdf".
-    test::Relay velocityRamp;
     const double desiredLinVel = 10.5;
     const double desiredAngVel = 0.2;
-    velocityRamp.OnPreUpdate(
-        [&](const gazebo::UpdateInfo &/*_info*/,
-            const gazebo::EntityComponentManager &)
-        {
-          msgs::Set(msg.mutable_linear(),
-                    math::Vector3d(desiredLinVel, 0, 0));
-          msgs::Set(msg.mutable_angular(),
-                    math::Vector3d(0.0, 0, desiredAngVel));
-          pub.Publish(msg);
-        });
-
-    server.AddSystem(velocityRamp.systemPtr);
+    msgs::Set(msg.mutable_linear(),
+              math::Vector3d(desiredLinVel, 0, 0));
+    msgs::Set(msg.mutable_angular(),
+              math::Vector3d(0.0, 0, desiredAngVel));
+    pub.Publish(msg);
+
+    // Give some time for message to be received
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
 
     server.Run(true, 3000, false);
 
     // Poses for 4s
     ASSERT_EQ(4000u, poses.size());
 
-    int sleep = 0;
-    int maxSleep = 30;
-
-    ASSERT_NE(maxSleep, sleep);
-
     // verify that the vehicle is moving in +x and rotating towards +y
     for (unsigned int i = 1001; i < poses.size(); ++i)
     {
-      EXPECT_GT(poses[i].Pos().X(), poses[i-1].Pos().X());
-      EXPECT_GT(poses[i].Pos().Y(), poses[i-1].Pos().Y());
+      EXPECT_GT(poses[i].Pos().X(), poses[i-1].Pos().X()) << i;
+      EXPECT_GT(poses[i].Pos().Y(), poses[i-1].Pos().Y()) << i;
       EXPECT_NEAR(poses[i].Pos().Z(), poses[i-1].Pos().Z(), 1e-5);
       EXPECT_NEAR(poses[i].Rot().Euler().X(),
-          poses[i-1].Rot().Euler().X(), 1e-5);
+          poses[i-1].Rot().Euler().X(), 1e-5) << i;
       EXPECT_NEAR(poses[i].Rot().Euler().Y(),
-          poses[i-1].Rot().Euler().Y(), 1e-5);
-      EXPECT_GT(poses[i].Rot().Euler().Z(), poses[i-1].Rot().Euler().Z());
+          poses[i-1].Rot().Euler().Y(), 1e-5) << i;
+      EXPECT_GT(poses[i].Rot().Euler().Z(), poses[i-1].Rot().Euler().Z()) << i;
     }
   }
 };

test/performance/each.cc

@@ -54,10 +54,10 @@ TEST(EntityComponentManagerPerfrormance, Each)
   // Initial allocation of resources can throw off calculations.
   warmstart();
 
-  for (int matchingEntityCount = 1; matchingEntityCount < maxEntityCount;
+  for (int matchingEntityCount = 10; matchingEntityCount < maxEntityCount;
        matchingEntityCount += step)
   {
-    for (int nonmatchingEntityCount = 1;
+    for (int nonmatchingEntityCount = 10;
          nonmatchingEntityCount < maxEntityCount;
          nonmatchingEntityCount += step)
     {