Odometry publisher: also publish Pose_V (TF) (#1534)

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

examples/worlds/multicopter_velocity_control.sdf

@@ -16,6 +16,10 @@ You can use the velocity controller and command linear velocity and yaw angular
 
     ign topic -e -t "/model/x3/odometry"
 
+  Listen to poses:
+
+    ign topic -e -t "/model/x3/pose"
+
 
   Send commands to the hexacopter to go straight up:
 

src/systems/diff_drive/DiffDrive.hh

@@ -63,7 +63,7 @@ namespace systems
   /// `/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,
+  /// 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

src/systems/odometry_publisher/OdometryPublisher.cc

@@ -74,12 +74,15 @@ class ignition::gazebo::systems::OdometryPublisherPrivate
   /// \brief Last sim time odom was published.
   public: std::chrono::steady_clock::duration lastOdomPubTime{0};
 
-  /// \brief Diff drive odometry message publisher.
+  /// \brief Odometry message publisher.
   public: transport::Node::Publisher odomPub;
 
-  /// \brief Diff drive odometry with covariance message publisher.
+  /// \brief Odometry with covariance message publisher.
   public: transport::Node::Publisher odomCovPub;
 
+  /// \brief Pose vector (TF) message publisher.
+  public: transport::Node::Publisher tfPub;
+
   /// \brief Rolling mean accumulators for the linear velocity
   public: std::tuple<math::RollingMean, math::RollingMean, math::RollingMean>
     linearMean;
@@ -144,7 +147,7 @@ void OdometryPublisher::Configure(const Entity &_entity,
   this->dataPtr->odomFrame = this->dataPtr->model.Name(_ecm) + "/" + "odom";
   if (!_sdf->HasElement("odom_frame"))
   {
-    ignwarn << "OdometryPublisher system plugin missing <odom_frame>, "
+    igndbg << "OdometryPublisher system plugin missing <odom_frame>, "
       << "defaults to \"" << this->dataPtr->odomFrame << "\"" << std::endl;
   }
   else
@@ -174,7 +177,7 @@ void OdometryPublisher::Configure(const Entity &_entity,
     + "/" + "base_footprint";
   if (!_sdf->HasElement("robot_base_frame"))
   {
-    ignwarn << "OdometryPublisher system plugin missing <robot_base_frame>, "
+    igndbg << "OdometryPublisher system plugin missing <robot_base_frame>, "
       << "defaults to \"" << this->dataPtr->robotBaseFrame << "\"" << std::endl;
   }
   else
@@ -229,6 +232,8 @@ void OdometryPublisher::Configure(const Entity &_entity,
   {
     this->dataPtr->odomPub = this->dataPtr->node.Advertise<msgs::Odometry>(
         odomTopicValid);
+    ignmsg << "OdometryPublisher publishing odometry on [" << odomTopicValid
+           << "]" << std::endl;
   }
 
   std::string odomCovTopicValid {
@@ -242,6 +247,26 @@ void OdometryPublisher::Configure(const Entity &_entity,
   {
     this->dataPtr->odomCovPub = this->dataPtr->node.Advertise<
         msgs::OdometryWithCovariance>(odomCovTopicValid);
+    ignmsg << "OdometryPublisher publishing odometry with covariance on ["
+           << odomCovTopicValid << "]" << std::endl;
+  }
+
+  std::string tfTopic{"/model/" + this->dataPtr->model.Name(_ecm) +
+    "/pose"};
+  if (_sdf->HasElement("tf_topic"))
+    tfTopic = _sdf->Get<std::string>("tf_topic");
+  std::string tfTopicValid {transport::TopicUtils::AsValidTopic(tfTopic)};
+  if (tfTopicValid.empty())
+  {
+    ignerr << "Failed to generate valid TF topic from [" << tfTopic << "]"
+           << std::endl;
+  }
+  else
+  {
+    this->dataPtr->tfPub = this->dataPtr->node.Advertise<msgs::Pose_V>(
+        tfTopicValid);
+    ignmsg << "OdometryPublisher publishing Pose_V (TF) on ["
+           << tfTopicValid << "]" << std::endl;
   }
 }
 
@@ -399,17 +424,19 @@ void OdometryPublisherPrivate::UpdateOdometry(
     ignition::math::Rand::DblNormal(0, this->gaussianNoise));
 
   // Set the time stamp in the header.
-  msg.mutable_header()->mutable_stamp()->CopyFrom(
-      convert<msgs::Time>(_info.simTime));
+  msgs::Header header;
+  header.mutable_stamp()->CopyFrom(convert<msgs::Time>(_info.simTime));
 
   // Set the frame ids.
-  auto frame = msg.mutable_header()->add_data();
+  auto frame = header.add_data();
   frame->set_key("frame_id");
   frame->add_value(odomFrame);
-  auto childFrame = msg.mutable_header()->add_data();
+  auto childFrame = header.add_data();
   childFrame->set_key("child_frame_id");
   childFrame->add_value(robotBaseFrame);
 
+  msg.mutable_header()->CopyFrom(header);
+
   this->lastUpdatePose = pose;
   this->lastUpdateTime = std::chrono::steady_clock::time_point(_info.simTime);
 
@@ -430,16 +457,7 @@ void OdometryPublisherPrivate::UpdateOdometry(
   msgs::OdometryWithCovariance msgCovariance;
 
   // Set the time stamp in the header.
-  msgCovariance.mutable_header()->mutable_stamp()->CopyFrom(
-      convert<msgs::Time>(_info.simTime));
-
-  // Set the frame ids.
-  frame = msgCovariance.mutable_header()->add_data();
-  frame->set_key("frame_id");
-  frame->add_value(odomFrame);
-  childFrame = msg.mutable_header()->add_data();
-  childFrame->set_key("child_frame_id");
-  childFrame->add_value(robotBaseFrame);
+  msgCovariance.mutable_header()->CopyFrom(header);
 
   // Copy position from odometry msg.
   msgCovariance.mutable_pose_with_covariance()->
@@ -488,6 +506,16 @@ void OdometryPublisherPrivate::UpdateOdometry(
   {
     this->odomCovPub.Publish(msgCovariance);
   }
+
+  if (this->tfPub.Valid())
+  {
+    msgs::Pose_V tfMsg;
+    auto tfMsgPose = tfMsg.add_pose();
+    tfMsgPose->CopyFrom(msg.pose());
+    tfMsgPose->mutable_header()->CopyFrom(header);
+
+    this->tfPub.Publish(tfMsg);
+  }
 }
 
 IGNITION_ADD_PLUGIN(OdometryPublisher,

src/systems/odometry_publisher/OdometryPublisher.hh

@@ -57,6 +57,10 @@ namespace systems
   /// odometry with covariance messages. This element is optional, and the
   /// default value is `/model/{name_of_model}/odometry_with_covariance`.
   ///
+  /// `<tf_topic>`: Custom topic on which this system will publish the
+  /// transform from `odom_frame` to `robot_base_frame`. This element is
+  /// optional, and the default value is `/model/{name_of_model}/pose`.
+  ///
   /// `<dimensions>`: Number of dimensions to represent odometry. Only 2 and 3
   /// dimensional spaces are supported. This element is optional, and the
   /// default value is 2.

test/integration/odometry_publisher.cc

@@ -208,8 +208,16 @@ class OdometryPublisherTest
 
   /// \param[in] _sdfFile SDF file to load.
   /// \param[in] _odomTopic Odometry topic.
+  /// \param[in] _tfTopic TF / Pose_V topic.
+  /// \param[in] _frameId Name of the world-fixed coordinate frame
+  /// for the odometry message.
+  /// \param[in] _childFrameId Name of the coordinate frame rigidly
+  /// attached to the mobile robot base.
   protected: void TestMovement3d(const std::string &_sdfFile,
-                                 const std::string &_odomTopic)
+                                 const std::string &_odomTopic,
+                                 const std::string &_tfTopic,
+                                 const std::string &_frameId,
+                                 const std::string &_childFrameId)
   {
     // Start server
     ServerConfig serverConfig;
@@ -253,6 +261,7 @@ class OdometryPublisherTest
     std::vector<math::Pose3d> odomPoses;
     std::vector<math::Vector3d> odomLinVels;
     std::vector<math::Vector3d> odomAngVels;
+    std::vector<math::Pose3d> tfPoses;
     // Create function to store data from odometry messages
     std::function<void(const msgs::Odometry &)> odomCb =
       [&](const msgs::Odometry &_msg)
@@ -271,10 +280,30 @@ class OdometryPublisherTest
         odomLinVels.push_back(msgs::Convert(_msg.twist().linear()));
         odomAngVels.push_back(msgs::Convert(_msg.twist().angular()));
       };
+    // Create function to store data from Pose_V messages
+    std::function<void(const msgs::Pose_V &)> tfCb =
+      [&](const msgs::Pose_V &_msg)
+      {
+        ASSERT_EQ(_msg.pose_size(), 1);
+        EXPECT_TRUE(_msg.pose(0).has_header());
+        EXPECT_TRUE(_msg.pose(0).has_position());
+        EXPECT_TRUE(_msg.pose(0).has_orientation());
+
+        ASSERT_EQ(_msg.pose(0).header().data_size(), 2);
+
+        EXPECT_EQ(_msg.pose(0).header().data(0).key(), "frame_id");
+        EXPECT_EQ(_msg.pose(0).header().data(0).value().Get(0), _frameId);
+
+        EXPECT_EQ(_msg.pose(0).header().data(1).key(), "child_frame_id");
+        EXPECT_EQ(_msg.pose(0).header().data(1).value().Get(0), _childFrameId);
+
+        tfPoses.push_back(msgs::Convert(_msg.pose(0)));
+      };
     // Create node for publishing twist messages
     transport::Node node;
     auto cmdVel = node.Advertise<msgs::Twist>("/X3/gazebo/command/twist");
     node.Subscribe(_odomTopic, odomCb);
+    node.Subscribe(_tfTopic, tfCb);
 
     test::Relay velocityRamp;
     math::Vector3d linVelCmd(0.5, 0.3, 1.5);
@@ -299,17 +328,19 @@ class OdometryPublisherTest
 
     int sleep = 0;
     int maxSleep = 30;
-    for (; odomPoses.size() < 150 && sleep < maxSleep; ++sleep)
+    for (; (odomPoses.size() < 150 || tfPoses.size() < 150) &&
+        sleep < maxSleep; ++sleep)
     {
       std::this_thread::sleep_for(std::chrono::milliseconds(100));
     }
-    ASSERT_NE(maxSleep, sleep);
+    EXPECT_NE(maxSleep, sleep);
 
     // Odom for 3s
     ASSERT_FALSE(odomPoses.empty());
     EXPECT_EQ(150u, odomPoses.size());
     EXPECT_EQ(150u, odomLinVels.size());
     EXPECT_EQ(150u, odomAngVels.size());
+    EXPECT_EQ(150u, tfPoses.size());
 
     // Check accuracy of poses published in the odometry message
     auto finalModelFramePose = odomPoses.back();
@@ -333,6 +364,14 @@ class OdometryPublisherTest
     EXPECT_NEAR(odomAngVels.back().X(), 0.0, 1e-1);
     EXPECT_NEAR(odomAngVels.back().Y(), 0.0, 1e-1);
     EXPECT_NEAR(odomAngVels.back().Z(), angVelCmd[2], 1e-1);
+
+    // Check TF
+    EXPECT_NEAR(poses.back().Pos().X(), tfPoses.back().Pos().X(), 1e-2);
+    EXPECT_NEAR(poses.back().Pos().Y(), tfPoses.back().Pos().Y(), 1e-2);
+    EXPECT_NEAR(poses.back().Pos().Z(), tfPoses.back().Pos().Z(), 1e-2);
+    EXPECT_NEAR(poses.back().Rot().X(), tfPoses.back().Rot().X(), 1e-2);
+    EXPECT_NEAR(poses.back().Rot().Y(), tfPoses.back().Rot().Y(), 1e-2);
+    EXPECT_NEAR(poses.back().Rot().Z(), tfPoses.back().Rot().Z(), 1e-2);
   }
 
   /// \param[in] _sdfFile SDF file to load.
@@ -572,7 +611,7 @@ TEST_P(OdometryPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(Movement3d))
   TestMovement3d(
       ignition::common::joinPaths(PROJECT_SOURCE_PATH,
       "test", "worlds", "odometry_publisher_3d.sdf"),
-      "/model/X3/odometry");
+      "/model/X3/odometry", "/model/X3/pose", "X3/odom", "X3/base_footprint");
 }
 
 /////////////////////////////////////////////////