Add Tf publishing to AckermannSteering system (#1576)

Signed-off-by: Andrew Ealovega <[email protected]>
Signed-off-by: Louise Poubel <[email protected]>

Co-authored-by: Alejandro Hernández Cordero <[email protected]>
Co-authored-by: Louise Poubel <[email protected]>

.gitignore

@@ -12,3 +12,6 @@ build_*
 
 .ycm_extra_conf.py
 *.orig
+
+# clangd index
+.cache

src/systems/ackermann_steering/AckermannSteering.cc

@@ -143,6 +143,9 @@ class ignition::gazebo::systems::AckermannSteeringPrivate
   /// \brief Ackermann steering odometry message publisher.
   public: transport::Node::Publisher odomPub;
 
+  /// \brief Ackermann tf message publisher.
+  public: transport::Node::Publisher tfPub;
+
   /// \brief Odometry X value
   public: double odomX{0.0};
 
@@ -343,6 +346,24 @@ void AckermannSteering::Configure(const Entity &_entity,
   this->dataPtr->odomPub = this->dataPtr->node.Advertise<msgs::Odometry>(
       odomTopic);
 
+  std::vector<std::string> tfTopics;
+  if (_sdf->HasElement("tf_topic"))
+  {
+    tfTopics.push_back(_sdf->Get<std::string>("tf_topic"));
+  }
+  tfTopics.push_back("/model/" + this->dataPtr->model.Name(_ecm) +
+    "/tf");
+  auto tfTopic = validTopic(tfTopics);
+  if (tfTopic.empty())
+  {
+    ignerr << "AckermannSteering plugin invalid tf topic name "
+           << "Failed to initialize." << std::endl;
+    return;
+  }
+
+  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");
 
@@ -667,8 +688,16 @@ void AckermannSteeringPrivate::UpdateOdometry(
     childFrame->add_value(this->sdfChildFrameId);
   }
 
+  // Construct the Pose_V/tf message and publish it.
+  msgs::Pose_V tfMsg;
+  ignition::msgs::Pose *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
   this->odomPub.Publish(msg);
+  this->tfPub.Publish(tfMsg);
 }
 
 //////////////////////////////////////////////////

src/systems/ackermann_steering/AckermannSteering.hh

@@ -75,12 +75,12 @@ namespace systems
   /// `<odom_publish_frequency>`: Odometry publication frequency. This
   /// element is optional, and the default value is 50Hz.
   ///
-  /// '<min_velocity>': Minimum velocity [m/s], usually <= 0.
-  /// '<max_velocity>': Maximum velocity [m/s], usually >= 0.
-  /// '<min_acceleration>': Minimum acceleration [m/s^2], usually <= 0.
-  /// '<max_acceleration>': Maximum acceleration [m/s^2], usually >= 0.
-  /// '<min_jerk Minimum>': jerk [m/s^3], usually <= 0.
-  /// '<max_jerk Maximum>': jerk [m/s^3], usually >= 0.
+  /// `<min_velocity>`: Minimum velocity [m/s], usually <= 0.
+  /// `<max_velocity>`: Maximum velocity [m/s], usually >= 0.
+  /// `<min_acceleration>`: Minimum acceleration [m/s^2], usually <= 0.
+  /// `<max_acceleration>`: Maximum acceleration [m/s^2], usually >= 0.
+  /// `<min_jerk Minimum>`: jerk [m/s^3], usually <= 0.
+  /// `<max_jerk Maximum>`: jerk [m/s^3], usually >= 0.
   ///
   /// `<topic>`: Custom topic that this system will subscribe to in order to
   /// receive command velocity messages. This element if optional, and the
@@ -90,6 +90,22 @@ namespace systems
   /// 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 is 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 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}/{name_of_link}`.
+  ///
   /// A robot with rear drive and front steering would have one each
   /// of left_joint, right_joint, left_steering_joint and
   /// right_steering_joint

src/systems/diff_drive/DiffDrive.hh

@@ -73,7 +73,7 @@ namespace systems
   /// 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>`
+  /// the target 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}/{name_of_link}`.

test/integration/ackermann_steering_system.cc

@@ -15,7 +15,10 @@
  *
 */
 
+#include <cstdint>
 #include <gtest/gtest.h>
+#include <ignition/msgs/pose.pb.h>
+
 #include <ignition/common/Console.hh>
 #include <ignition/common/Util.hh>
 #include <ignition/math/Pose3.hh>
@@ -309,6 +312,126 @@ TEST_P(AckermannSteeringTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(SkidPublishCmd))
   EXPECT_LT(poses[0].Rot().Z(), poses[3999].Rot().Z());
 }
 
+/////////////////////////////////////////////////
+TEST_P(AckermannSteeringTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(TfPublishes))
+{
+  // Start server
+  ServerConfig serverConfig;
+  serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+      "test", "worlds", "ackermann_steering_slow_odom.sdf"));
+
+  Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  server.SetUpdatePeriod(0ns);
+
+  // Create a system that records the vehicle poses
+  test::Relay testSystem;
+
+  std::vector<math::Pose3d> poses;
+  testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &,
+    const gazebo::EntityComponentManager &_ecm)
+    {
+      auto id = _ecm.EntityByComponents(
+        components::Model(),
+        components::Name("vehicle"));
+      EXPECT_NE(kNullEntity, id);
+
+      auto poseComp = _ecm.Component<components::Pose>(id);
+      ASSERT_NE(nullptr, poseComp);
+
+      poses.push_back(poseComp->Data());
+    });
+  server.AddSystem(testSystem.systemPtr);
+
+  // Run server and check that vehicle didn't move
+  server.Run(true, 1000, false);
+
+  EXPECT_EQ(1000u, poses.size());
+
+  for (const auto &pose : poses)
+  {
+    EXPECT_EQ(poses[0], pose);
+  }
+
+  // Publish command and check that vehicle moved
+  double period{1.0};
+  double lastMsgTime{1.0};
+  std::vector<math::Pose3d> odomPoses;
+  std::function<void(const msgs::Odometry &)> odomCb =
+    [&](const msgs::Odometry &_msg)
+    {
+      ASSERT_TRUE(_msg.has_header());
+      ASSERT_TRUE(_msg.header().has_stamp());
+
+      double msgTime =
+          static_cast<double>(_msg.header().stamp().sec()) +
+          static_cast<double>(_msg.header().stamp().nsec()) * 1e-9;
+
+      EXPECT_DOUBLE_EQ(msgTime, lastMsgTime + period);
+      lastMsgTime = msgTime;
+
+      odomPoses.push_back(msgs::Convert(_msg.pose()));
+    };
+
+  // Capture Tf data to compare to odom
+  double periodTf{1.0};
+  double lastMsgTimeTf{1.0};
+  std::vector<math::Pose3d> tfPoses;
+  std::function<void(const msgs::Pose_V &)> tfCb =
+    [&](const msgs::Pose_V &_msg)
+    {
+      ASSERT_TRUE(_msg.pose().Get(0).has_header());
+
+      double msgTime =
+          static_cast<double>(_msg.pose().Get(0).header().stamp().sec()) +
+          static_cast<double>(_msg.pose().Get(0).header().stamp().nsec()) *
+            1e-9;
+
+      EXPECT_DOUBLE_EQ(msgTime, lastMsgTimeTf + periodTf);
+      lastMsgTimeTf = msgTime;
+
+      // Use position pose to match odom (index 0)
+      tfPoses.push_back(msgs::Convert(_msg.pose().Get(0)));
+    };
+
+  transport::Node node;
+  auto pub = node.Advertise<msgs::Twist>("/model/vehicle/cmd_vel");
+  node.Subscribe("/model/vehicle/odometry", odomCb);
+  node.Subscribe("/model/vehicle/tf", tfCb);
+
+  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, 3000, false);
+
+  // Poses for 4s
+  EXPECT_EQ(4000u, poses.size());
+
+  int sleep = 0;
+  int maxSleep = 30;
+  for (; odomPoses.size() < 3 && sleep < maxSleep; ++sleep)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+  EXPECT_NE(maxSleep, sleep);
+
+  // There should have been the same amount of odom and tf
+  ASSERT_FALSE(odomPoses.empty());
+  ASSERT_FALSE(tfPoses.empty());
+  ASSERT_EQ(odomPoses.size(), tfPoses.size());
+
+  // Ensure all data is equal between the two topics
+  for (uint64_t i = 0; i < odomPoses.size(); i++)
+  {
+    ASSERT_EQ(odomPoses[i], tfPoses[i]);
+  }
+}
+
 /////////////////////////////////////////////////
 TEST_P(AckermannSteeringTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(OdomFrameId))
 {