Add API to use message_info instead unserialized message (#170)

Signed-off-by: CursedRock17 <mtglucas1@gmail.com>
Signed-off-by: Michael Orlov <michael.orlov@apex.ai>
Co-authored-by: Michael Orlov <michael.orlov@apex.ai>

CMakeLists.txt

@@ -32,6 +32,7 @@ find_package(ament_cmake_ros REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(rcl REQUIRED)
 find_package(rcpputils REQUIRED)
+find_package(rmw REQUIRED)
 find_package(statistics_msgs REQUIRED)
 
 add_library(${PROJECT_NAME}
@@ -49,6 +50,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC
 ament_target_dependencies(${PROJECT_NAME}
   "builtin_interfaces"
   "rcl"
+  "rmw"
   "rcpputils"
   "statistics_msgs"
 )
@@ -67,7 +69,7 @@ ament_export_libraries(${PROJECT_NAME})
 # Export modern CMake targets
 ament_export_targets(${PROJECT_NAME})
 
-ament_export_dependencies("builtin_interfaces" "rcl" "rcpputils" "rosidl_default_runtime" "statistics_msgs")
+ament_export_dependencies("builtin_interfaces" "rcl" "rcpputils" "rmw" "rosidl_default_runtime" "statistics_msgs")
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -91,12 +93,12 @@ if(BUILD_TESTING)
   ament_add_gtest(test_received_message_period
     test/topic_statistics_collector/test_received_message_period.cpp)
   target_link_libraries(test_received_message_period ${PROJECT_NAME})
-  ament_target_dependencies(test_received_message_period "rcl" "rcpputils")
+  ament_target_dependencies(test_received_message_period "rcl" "rmw" "rcpputils")
 
   ament_add_gtest(test_received_message_age
     test/topic_statistics_collector/test_received_message_age.cpp)
   target_link_libraries(test_received_message_age ${PROJECT_NAME})
-  ament_target_dependencies(test_received_message_age "rcl" "rcpputils")
+  ament_target_dependencies(test_received_message_age "rcl" "rmw" "rcpputils")
 
   rosidl_generate_interfaces(libstatistics_collector_test_msgs
     "test/msg/DummyMessage.msg"

include/libstatistics_collector/topic_statistics_collector/received_message_age.hpp

@@ -28,6 +28,8 @@
 #include "builtin_interfaces/msg/time.hpp"
 #include "rcl/time.h"
 
+#include "rmw/types.h"
+
 namespace libstatistics_collector
 {
 namespace topic_statistics_collector
@@ -85,14 +87,32 @@ struct TimeStamp<M, typename std::enable_if<HasHeader<M>::value>::type>
 };
 
 /**
- * Class used to measure the received messsage, tparam T, age from a ROS2 subscriber.
+ * Primary specialization class template until deprecated templated class is phased out
+ * @warning Don't use templated version of the ReceivedMessageAgeCollector, use
+ * libstatistics_collector::ReceivedMessageAgeCollector alias with rmw_message_info_t
+ * parameter in the OnMessageReceived callback
+ */
+template<typename T = rmw_message_info_t, typename Enable = void>
+class ReceivedMessageAgeCollector : public TopicStatisticsCollector<T>
+{};
+
+/**
+ * Class used to measure the received message, tparam T, age from a ROS2 subscriber.
  *
  * @tparam T the message type to receive from the subscriber / listener
 */
 template<typename T>
-class ReceivedMessageAgeCollector : public TopicStatisticsCollector<T>
+class
+  [[deprecated("Don't use templated version of the ReceivedMessageAgeCollector, use"
+  "libstatistics_collector::ReceivedMessageAgeCollector alias instead")]]
+  ReceivedMessageAgeCollector<T, std::enable_if_t<!std::is_same<T, rmw_message_info_t>::value>>
+  : public TopicStatisticsCollector<T>
 {
 public:
+  /**
+   * Construct a ReceivedMessageAgeCollector object.
+   *
+   */
   ReceivedMessageAgeCollector() = default;
 
   virtual ~ReceivedMessageAgeCollector() = default;
@@ -131,7 +151,7 @@ class ReceivedMessageAgeCollector : public TopicStatisticsCollector<T>
   }
 
   /**
-   * Return messge age metric unit
+   * Return message age metric unit
    *
    * @return a string representing messager age metric unit
    */
@@ -152,7 +172,76 @@ class ReceivedMessageAgeCollector : public TopicStatisticsCollector<T>
   }
 };
 
+/**
+ * Class used to measure the received message age from a ROS2 subscriber.
+*/
+template<>
+class ReceivedMessageAgeCollector<
+    rmw_message_info_t,
+    std::enable_if_t<std::is_same<rmw_message_info_t, rmw_message_info_t>::value>>
+  : public TopicStatisticsCollector<>
+{
+public:
+  ReceivedMessageAgeCollector() = default;
+
+  ~ReceivedMessageAgeCollector() override = default;
+
+  /**
+  * Handle a new incoming message. Calculate message age if timestamps in message info are valid.
+  *
+  * @param message_info the message information of the received message.
+  * @param now_nanoseconds time the message was received in nanoseconds
+  */
+  void OnMessageReceived(
+    const rmw_message_info_t & message_info,
+    const rcl_time_point_value_t now_nanoseconds) override
+  {
+    // only compare if non-zero
+    if (message_info.source_timestamp && now_nanoseconds) {
+      const std::chrono::nanoseconds age_nanos{now_nanoseconds -
+        message_info.source_timestamp};
+      const auto age_millis = std::chrono::duration<double, std::milli>(age_nanos);
+
+      collector::Collector::AcceptData(static_cast<double>(age_millis.count()));
+    }  // else no valid time to compute age
+  }
+
+  /**
+   * Return message age metric name
+   *
+   * @return a string representing message age metric name
+   */
+  std::string GetMetricName() const override
+  {
+    return topic_statistics_constants::kMsgAgeStatName;
+  }
+
+  /**
+   * Return message age metric unit
+   *
+   * @return a string representing message age metric unit
+   */
+  std::string GetMetricUnit() const override
+  {
+    return topic_statistics_constants::kMillisecondUnitName;
+  }
+
+protected:
+  bool SetupStart() override
+  {
+    return true;
+  }
+
+  bool SetupStop() override
+  {
+    return true;
+  }
+};
+
 }  // namespace topic_statistics_collector
+
+using ReceivedMessageAgeCollector = topic_statistics_collector::ReceivedMessageAgeCollector<>;
+
 }  // namespace libstatistics_collector
 
 #endif  // LIBSTATISTICS_COLLECTOR__TOPIC_STATISTICS_COLLECTOR__RECEIVED_MESSAGE_AGE_HPP_

include/libstatistics_collector/topic_statistics_collector/received_message_period.hpp

@@ -24,6 +24,8 @@
 
 #include "rcl/time.h"
 
+#include "rmw/types.h"
+
 namespace libstatistics_collector
 {
 namespace topic_statistics_collector
@@ -32,13 +34,27 @@ namespace topic_statistics_collector
 constexpr const int64_t kUninitializedTime{0};
 
 /**
- * Class used to measure the received messsage, tparam T, period from a ROS2 subscriber. This class
+ * Primary specialization class template until deprecated templated class is phased out
+ * @warning Don't use templated version of the ReceivedMessagePeriodCollector, use
+ * libstatistics_collector::ReceivedMessagePeriodCollector alias with rmw_message_info_t
+ * parameter in the OnMessageReceived callback
+ */
+template<typename T = rmw_message_info_t, typename Enable = void>
+class ReceivedMessagePeriodCollector : public TopicStatisticsCollector<T>
+{};
+
+/**
+ * Class used to measure the received message, tparam T, period from a ROS2 subscriber. This class
  * is thread safe and acquires a mutex when the member OnMessageReceived is executed.
  *
  * @tparam T the message type to receive from the subscriber / listener
 */
 template<typename T>
-class ReceivedMessagePeriodCollector : public TopicStatisticsCollector<T>
+class
+  [[deprecated("Don't use templated version of the ReceivedMessagePeriodCollector, use"
+  "libstatistics_collector::ReceivedMessagePeriodCollector alias instead")]]
+  ReceivedMessagePeriodCollector<T, std::enable_if_t<!std::is_same<T, rmw_message_info_t>::value>>
+  : public TopicStatisticsCollector<T>
 {
 public:
   /**
@@ -129,7 +145,105 @@ class ReceivedMessagePeriodCollector : public TopicStatisticsCollector<T>
   mutable std::mutex mutex_;
 };
 
+template<>
+class ReceivedMessagePeriodCollector<
+    rmw_message_info_t,
+    std::enable_if_t<std::is_same<rmw_message_info_t, rmw_message_info_t>::value>>
+  : public TopicStatisticsCollector<>
+{
+public:
+  /**
+   * Construct a ReceivedMessagePeriodCollector object.
+   *
+   */
+  ReceivedMessagePeriodCollector()
+  {
+    ResetTimeLastMessageReceived();
+  }
+
+  ~ReceivedMessagePeriodCollector() override = default;
+
+  /**
+   * Handle a message received and measure its received period. This member is thread safe and acquires
+   * a lock to prevent race conditions when setting the time_last_message_received_ member.
+   *
+   * @param message_info the message information of the received message
+   * @param now_nanoseconds time the message was received in nanoseconds
+   */
+  void OnMessageReceived(
+    const rmw_message_info_t & message_info,
+    const rcl_time_point_value_t now_nanoseconds)
+  override RCPPUTILS_TSA_REQUIRES(mutex_)
+  {
+    std::unique_lock<std::mutex> ulock{mutex_};
+
+    if (time_last_message_received_ == kUninitializedTime) {
+      time_last_message_received_ = now_nanoseconds;
+    } else {
+      const std::chrono::nanoseconds nanos{now_nanoseconds - time_last_message_received_};
+      const auto period = std::chrono::duration<double, std::milli>(nanos);
+      time_last_message_received_ = now_nanoseconds;
+      collector::Collector::AcceptData(static_cast<double>(period.count()));
+    }
+  }
+
+  /**
+   * Return message period metric name
+   *
+   * @return a string representing message period metric name
+   */
+  std::string GetMetricName() const override
+  {
+    return topic_statistics_constants::kMsgPeriodStatName;
+  }
+
+  /**
+   * Return message period metric unit
+   *
+   * @return a string representing message period metric unit
+   */
+  std::string GetMetricUnit() const override
+  {
+    return topic_statistics_constants::kMillisecondUnitName;
+  }
+
+protected:
+  /**
+   * Reset the time_last_message_received_ member.
+   * @return true
+   */
+  bool SetupStart() override
+  {
+    ResetTimeLastMessageReceived();
+    return true;
+  }
+
+  bool SetupStop() override
+  {
+    return true;
+  }
+
+private:
+  /**
+   * Resets time_last_message_received_ to the expected uninitialized_time_.
+   */
+  void ResetTimeLastMessageReceived()
+  {
+    time_last_message_received_ = kUninitializedTime;
+  }
+
+  /**
+   * Default uninitialized time.
+   */
+  rcl_time_point_value_t time_last_message_received_ RCPPUTILS_TSA_GUARDED_BY(mutex_) =
+    kUninitializedTime;
+  mutable std::mutex mutex_;
+};
+
 }  // namespace topic_statistics_collector
+
+using ReceivedMessagePeriodCollector = topic_statistics_collector::ReceivedMessagePeriodCollector<>;
+
 }  // namespace libstatistics_collector
 
 #endif  // LIBSTATISTICS_COLLECTOR__TOPIC_STATISTICS_COLLECTOR__RECEIVED_MESSAGE_PERIOD_HPP_

include/libstatistics_collector/topic_statistics_collector/topic_statistics_collector.hpp

@@ -21,20 +21,36 @@
 
 #include "rcl/time.h"
 
+#include "rmw/types.h"
+
 #include "libstatistics_collector/collector/collector.hpp"
 
 namespace libstatistics_collector
 {
 namespace topic_statistics_collector
 {
 
+/**
+ * Primary specialization class template until deprecated templated class is phased out
+ * @warning Don't use templated version of the TopicStatisticsCollector, use
+ * libstatistics_collector::TopicStatisticsCollector alias with rmw_message_info_t parameter in
+ * the OnMessageReceived callback
+ */
+template<typename T = rmw_message_info_t, typename Enable = void>
+class TopicStatisticsCollector : public collector::Collector
+{};
+
 /**
  * Interface to collect and perform measurements for ROS2 topic statistics.
  *
  * @tparam T the ROS2 message type to collect
  */
 template<typename T>
-class TopicStatisticsCollector : public collector::Collector
+class
+  [[deprecated("Don't use templated version of the TopicStatisticsCollector, use"
+  "libstatistics_collector::TopicStatisticsCollector alias instead")]]
+  TopicStatisticsCollector<T, std::enable_if_t<!std::is_same<T, rmw_message_info_t>::value>>
+  : public collector::Collector
 {
 public:
   TopicStatisticsCollector() = default;
@@ -54,7 +70,37 @@ class TopicStatisticsCollector : public collector::Collector
     const rcl_time_point_value_t now_nanoseconds) = 0;
 };
 
+/**
+ * Interface to collect and perform measurements for ROS2 topic statistics.
+ */
+template<>
+class TopicStatisticsCollector<rmw_message_info_t,
+    std::enable_if_t<std::is_same<rmw_message_info_t, rmw_message_info_t>::value>>
+  : public collector::Collector
+{
+public:
+  TopicStatisticsCollector() = default;
+
+  virtual ~TopicStatisticsCollector() = default;
+
+  /**
+   * Handle receiving a single message of type rmw_message_info_t.
+   *
+   * @param received_message rmw_message_info_t the ROS2 message type to collect
+   * @param now_nanoseconds nanoseconds the time the message was received.
+   * Any metrics using this time assumes the following:
+   * 1). the time provided is strictly monotonic
+   * 2). the time provided uses the same source as time obtained from the message header.
+   */
+  virtual void OnMessageReceived(
+    const rmw_message_info_t & received_message,
+    const rcl_time_point_value_t now_nanoseconds) = 0;
+};
+
 }  // namespace topic_statistics_collector
+
+using TopicStatisticsCollector = topic_statistics_collector::TopicStatisticsCollector<>;
+
 }  // namespace libstatistics_collector
 
 #endif  // LIBSTATISTICS_COLLECTOR__TOPIC_STATISTICS_COLLECTOR__TOPIC_STATISTICS_COLLECTOR_HPP_

package.xml

@@ -12,6 +12,7 @@
   <depend>builtin_interfaces</depend>
   <depend>rcl</depend>
   <depend>rcpputils</depend>
+  <depend>rmw</depend>
   <depend>statistics_msgs</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>