add: rmw_take filter

Signed-off-by: yamasaki <114902604+ymski@users.noreply.github.com>

CARET_trace/include/caret_trace/tracing_controller.hpp

@@ -45,6 +45,13 @@ class TracingController
   void add_subscription_handle(
     const void * node_handle, const void * subscription_handle, std::string topic_name);
 
+  /// @brief Register topic name for rcl_subscription_init hook.
+  /// @param node_handle Address of the node handle.
+  /// @param rmw_subscription_handle Address of the rmw_subscription handle.
+  /// @param topic_name topic name.
+  void add_rmw_subscription_handle(
+    const void * node_handle, const void * rmw_subscription_handle, std::string topic_name);
+
   /// @brief Register binding information for rclcpp_subscription_init tracepoint hook.
   /// @param subscription_handle Address of the subscription handle.
   /// @param subscription Address of subscription instance.
@@ -93,6 +100,11 @@ class TracingController
   /// @return True if the subscription is enabled, false otherwise.
   bool is_allowed_subscription_handle(const void * subscription_handle);
 
+  /// @brief Check if trace point is a enabled subscription
+  /// @param rmw_subscription_handle Address of the rmw_subscription handle.
+  /// @return True if the rmw_subscription is enabled, false otherwise.
+  bool is_allowed_rmw_subscription_handle(const void * rmw_subscription_handle);
+
 private:
   void debug(std::string message) const;
   void info(std::string message) const;
@@ -116,6 +128,12 @@ class TracingController
   std::unordered_map<const void *, const void *> subscription_to_subscription_handles_;
   std::unordered_map<const void *, const void *> callback_to_subscriptions_;
 
+  std::unordered_map<const void *, const void *> rmw_subscription_handle_to_node_handles_;
+  std::unordered_map<const void *, std::string> rmw_subscription_handle_to_topic_names_;
+  std::unordered_map<const void *, const void *> rmw_subscription_handle_to_subscription_handle;
+  std::unordered_map<const void *, const void *> callback_to_rmw_subscriptions_;
+  std::unordered_map<const void *, bool> allowed_rmw_subscription_handles_;
+
   std::unordered_map<const void *, std::string> node_handle_to_node_names_;
   std::unordered_map<const void *, const void *> callback_to_timer_handles_;
   std::unordered_map<const void *, const void *> timer_handle_to_node_handles_;

CARET_trace/src/ros_trace_points.cpp

@@ -247,6 +247,7 @@ void ros_trace_rcl_subscription_init(
   auto now = clock.now();
 
   controller.add_subscription_handle(node_handle, subscription_handle, topic_name);
+  controller.add_rmw_subscription_handle(node_handle, rmw_subscription_handle, topic_name);
 
   if (!data_container.is_assigned_rcl_subscription_init()) {
     data_container.assign_rcl_subscription_init(record);
@@ -915,9 +916,12 @@ void ros_trace_rmw_take(
 )
 {
   static auto & context = Singleton<Context>::get_instance();
+  static auto & controller = context.get_controller();
   static void * orig_func = dlsym(RTLD_NEXT, __func__);
   using functionT = void (*)(const void *, const void *, int64_t, const bool);
-  if (context.is_recording_allowed()) {
+  if (controller.is_allowed_rmw_subscription_handle(rmw_subscription_handle) &&
+    context.is_recording_allowed())
+  {
     ((functionT) orig_func)(rmw_subscription_handle, message,
       source_timestamp, taken);
 

CARET_trace/src/tracing_controller.cpp

@@ -268,6 +268,63 @@ bool TracingController::is_allowed_subscription_handle(const void * subscription
   return true;
 }
 
+bool TracingController::is_allowed_rmw_subscription_handle(const void * rmw_subscription_handle)
+{
+  std::unordered_map<const void *, bool>::iterator is_allowed_it;
+  {
+    std::shared_lock<std::shared_timed_mutex> lock(mutex_);
+    is_allowed_it = allowed_rmw_subscription_handles_.find(rmw_subscription_handle);
+    if (is_allowed_it != allowed_rmw_subscription_handles_.end()) {
+      return is_allowed_it->second;
+    }
+  }
+
+  {
+    std::lock_guard<std::shared_timed_mutex> lock(mutex_);
+    auto node_handle = rmw_subscription_handle_to_node_handles_[rmw_subscription_handle];
+    auto node_name = node_handle_to_node_names_[node_handle];
+    auto topic_name = rmw_subscription_handle_to_topic_names_[rmw_subscription_handle];
+
+    if (select_enabled_) {
+      auto is_selected_topic = partial_match(selected_topic_names_, topic_name);
+      auto is_selected_node = partial_match(selected_node_names_, node_name);
+
+      if (selected_topic_names_.size() > 0 && is_selected_topic) {
+        allowed_rmw_subscription_handles_.insert(std::make_pair(rmw_subscription_handle, true));
+        return true;
+      }
+      if (selected_node_names_.size() > 0 && is_selected_node) {
+        allowed_rmw_subscription_handles_.insert(std::make_pair(rmw_subscription_handle, true));
+        return true;
+      }
+      if (selected_node_names_.size() == 0 && topic_name == "") {  // allow timer callback
+        allowed_rmw_subscription_handles_.insert(std::make_pair(rmw_subscription_handle, true));
+        return true;
+      }
+
+      allowed_rmw_subscription_handles_.insert(std::make_pair(rmw_subscription_handle, false));
+      return false;
+    }
+    if (ignore_enabled_) {
+      auto is_ignored_node = partial_match(ignored_node_names_, node_name);
+      auto is_ignored_topic = partial_match(ignored_topic_names_, topic_name);
+
+      if (ignored_node_names_.size() > 0 && is_ignored_node) {
+        allowed_rmw_subscription_handles_.insert(std::make_pair(rmw_subscription_handle, false));
+        return false;
+      }
+      if (ignored_topic_names_.size() > 0 && is_ignored_topic) {
+        allowed_rmw_subscription_handles_.insert(std::make_pair(rmw_subscription_handle, false));
+        return false;
+      }
+      allowed_rmw_subscription_handles_.insert(std::make_pair(rmw_subscription_handle, true));
+      return true;
+    }
+    allowed_rmw_subscription_handles_.insert(std::make_pair(rmw_subscription_handle, true));
+    return true;
+  }
+}
+
 bool TracingController::is_allowed_publisher_handle(const void * publisher_handle)
 {
   std::unordered_map<const void *, bool>::iterator is_allowed_it;
@@ -407,6 +464,14 @@ void TracingController::add_subscription_handle(
   subscription_handle_to_topic_names_.insert(std::make_pair(subscription_handle, topic_name));
 }
 
+void TracingController::add_rmw_subscription_handle(
+  const void * node_handle, const void * rmw_subscription_handle, std::string topic_name)
+{
+  std::lock_guard<std::shared_timed_mutex> lock(mutex_);
+  rmw_subscription_handle_to_node_handles_.insert(std::make_pair(rmw_subscription_handle, node_handle));
+  rmw_subscription_handle_to_topic_names_.insert(std::make_pair(rmw_subscription_handle, topic_name));
+}
+
 void TracingController::add_subscription(
   const void * subscription_handle, const void * subscription)
 {