Merge branch 'ign-gazebo3' into scene_broadcaster_paused

gazebosim · Feb 25, 2021 · 0e3e154 · 0e3e154
2 parents e402c7f + 4a4951e
commit 0e3e154
Show file tree

Hide file tree

Showing 6 changed files with 466 additions and 3 deletions.
diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc
@@ -119,6 +119,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;
 
@@ -271,6 +274,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");
 
@@ -465,9 +475,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);
 }
 
 //////////////////////////////////////////////////

diff --git a/src/systems/diff_drive/DiffDrive.hh b/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,

diff --git a/test/integration/diff_drive_system.cc b/test/integration/diff_drive_system.cc
@@ -404,6 +404,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;

diff --git a/test/integration/scene_broadcaster_system.cc b/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;

diff --git a/test/performance/each.cc b/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)
     {