Update rmw_publish tracepoint arguments (#325)

Signed-off-by: Christophe Bedard <[email protected]>

email/include/email/publisher.hpp

@@ -24,6 +24,7 @@
 #include "email/log.hpp"
 #include "email/macros.hpp"
 #include "email/pub_sub.hpp"
+#include "email/timestamp.hpp"
 #include "email/visibility_control.hpp"
 
 namespace email
@@ -50,12 +51,14 @@ class Publisher : public PubSubObject
   /**
    * \param message the message
    * \param additional_headers the additional headers to include
+   * \param timestamp the timestamp to associate to this publication
    */
   EMAIL_PUBLIC
   void
   publish(
     const std::string & message,
-    std::optional<EmailHeaders> additional_headers = std::nullopt);
+    std::optional<EmailHeaders> additional_headers = std::nullopt,
+    std::optional<Timestamp> timestamp = std::nullopt);
 
 private:
   EMAIL_DISABLE_COPY(Publisher)

email/include/email/timestamp.hpp

@@ -76,6 +76,10 @@ class Timestamp
   std::optional<Timestamp>
   from_string(const std::string & timestamp);
 
+  EMAIL_PUBLIC
+  bool
+  operator==(const Timestamp & rhs) const;
+
 private:
   int64_t nanoseconds_;
 };

email/src/publisher.cpp

@@ -48,10 +48,13 @@ Publisher::~Publisher()
 }
 
 void
-Publisher::publish(const std::string & message, std::optional<EmailHeaders> additional_headers)
+Publisher::publish(
+  const std::string & message,
+  std::optional<EmailHeaders> additional_headers,
+  std::optional<Timestamp> timestamp)
 {
   logger_->debug("publish");
-  const Timestamp source_timestamp = Timestamp::now();
+  const Timestamp source_timestamp = timestamp.value_or(Timestamp::now());
   // Add GID and source timestamp to headers
   EmailHeaders headers = {
     {MessageInfo::HEADER_PUBLISHER_GID, get_gid().to_string()},

email/src/timestamp.cpp

@@ -59,4 +59,10 @@ Timestamp::from_string(const std::string & timestamp)
   return Timestamp(nanoseconds_opt.value());
 }
 
+bool
+Timestamp::operator==(const Timestamp & rhs) const
+{
+  return nanoseconds() == rhs.nanoseconds();
+}
+
 }  // namespace email

email/test/test_end_to_end.cpp

@@ -103,7 +103,8 @@ TEST_F(TestEndToEnd, intraprocess_pub_sub)
   EXPECT_FALSE(sub2.get_message().has_value());
 
   pub1.publish("some message");
-  pub2.publish("some other message");
+  const auto ts2 = email::Timestamp::now();
+  pub2.publish("some other message", std::nullopt, ts2);
 
   email::WaitSet waitset;
   waitset.add_subscription(&sub1);
@@ -131,6 +132,7 @@ TEST_F(TestEndToEnd, intraprocess_pub_sub)
   EXPECT_EQ(pub1.get_gid().value(), info_1.publisher_gid().value());
   EXPECT_EQ(pub2.get_gid().value(), info_2.publisher_gid().value());
   EXPECT_LT(info_1.source_timestamp().nanoseconds(), info_2.source_timestamp().nanoseconds());
+  EXPECT_EQ(info_2.source_timestamp(), ts2);
   EXPECT_LT(info_1.source_timestamp().nanoseconds(), info_1.received_timestamp().nanoseconds());
   EXPECT_LT(info_2.source_timestamp().nanoseconds(), info_2.received_timestamp().nanoseconds());
 

rmw_email_cpp/src/rmw_publish.cpp

@@ -15,6 +15,7 @@
 #include <string>
 
 #include "email/publisher.hpp"
+#include "email/timestamp.hpp"
 #include "rmw/error_handling.h"
 #include "rmw/impl/cpp/macros.hpp"
 #include "rmw/rmw.h"
@@ -54,9 +55,14 @@ extern "C" rmw_ret_t rmw_publish(
   // Convert to YAML string and publish
   const std::string msg_yaml = rmw_email_cpp::msg_to_yaml(rmw_email_pub, ros_message);
   assert(!msg_yaml.empty());
-  TRACEPOINT(rmw_publish, ros_message);
+  const email::Timestamp source_timestamp = email::Timestamp::now();
+  TRACEPOINT(
+    rmw_publish,
+    static_cast<const void *>(publisher),
+    ros_message,
+    source_timestamp.nanoseconds());
   RMW_EMAIL_CPP_TRACEPOINT(rmw_publish_yaml, ros_message, static_cast<const void *>(&msg_yaml));
-  email_pub->publish(msg_yaml);
+  email_pub->publish(msg_yaml, std::nullopt, source_timestamp);
   return RMW_RET_OK;
 }