feat: support rmw_take tracepoint (#113)

* remove dispatch_subscription_callback tracepoint Signed-off-by: yamasaki <[email protected]> * feat: support rmw_take Signed-off-by: yamasaki <[email protected]> * add: rmw_take filter Signed-off-by: yamasaki <[email protected]> * remove unused variables Signed-off-by: yamasaki <[email protected]> * change the method of not outputting `dispatch_callback_subscription` from deleting functions to adding filters Signed-off-by: yamasaki <[email protected]> * fix code stype Signed-off-by: yamasaki <[email protected]> * fix: compile error Signed-off-by: yamasaki <[email protected]> * Update CARET_trace/src/tracing_controller.cpp remove an unnecessary condition from rmw_subscription filter Co-authored-by: isp-uetsuki <[email protected]> --------- Signed-off-by: yamasaki <[email protected]> Co-authored-by: isp-uetsuki <[email protected]>
tier4 · Jun 7, 2023 · 00895e3 · 00895e3
1 parent 0d8fb9b
commit 00895e3
Show file tree

Hide file tree

Showing 3 changed files with 109 additions and 29 deletions.
diff --git a/CARET_trace/include/caret_trace/tracing_controller.hpp b/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,10 @@ 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 *, 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_;

diff --git a/CARET_trace/src/ros_trace_points.cpp b/CARET_trace/src/ros_trace_points.cpp
@@ -120,7 +120,6 @@ namespace ORIG_FUNC
 static void * DEFINE_ORIG_FUNC(ros_trace_callback_end);
 static void * DEFINE_ORIG_FUNC(ros_trace_callback_start);
 static void * DEFINE_ORIG_FUNC(ros_trace_dispatch_intra_process_subscription_callback);
-static void * DEFINE_ORIG_FUNC(ros_trace_dispatch_subscription_callback);
 static void * DEFINE_ORIG_FUNC(ros_trace_message_construct);
 static void * DEFINE_ORIG_FUNC(ros_trace_rcl_lifecycle_state_machine_init);
 static void * DEFINE_ORIG_FUNC(ros_trace_rcl_lifecycle_transition);
@@ -248,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);
@@ -459,24 +459,18 @@ void ros_trace_dispatch_subscription_callback(
   const uint64_t source_timestamp,
   const uint64_t message_timestamp)
 {
-  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 *, const uint64_t, const uint64_t);
-  if (controller.is_allowed_callback(callback) &&
-    context.is_recording_allowed())
-  {
-    ((functionT) orig_func)(message, callback, source_timestamp, message_timestamp);
+  (void) message;
+  (void) callback;
+  (void) source_timestamp;
+  (void) message_timestamp;
 
-#ifdef DEBUG_OUTPUT
-    std::cerr << "dispatch_subscription_callback," <<
-      message << "," <<
-      callback << "," <<
-      source_timestamp << "," <<
-      message_timestamp << std::endl;
-#endif
-  }
+// #ifdef DEBUG_OUTPUT
+//     std::cerr << "dispatch_subscription_callback," <<
+//       message << "," <<
+//       callback << "," <<
+//       source_timestamp << "," <<
+//       message_timestamp << std::endl;
+// #endif
 }
 
 void ros_trace_dispatch_intra_process_subscription_callback(
@@ -941,17 +935,24 @@ void ros_trace_rmw_take(
   const bool taken
 )
 {
-  (void) rmw_subscription_handle;
-  (void) message;
-  (void) source_timestamp;
-  (void) taken;
-// #ifdef DEBUG_OUTPUT
-//   std::cerr << "rmw_take," <<
-//     rmw_subscription_handle << "," <<
-//     message << "," <<
-//     source_timestamp << "," <<
-//     taken << "," << std::endl;
-// #endif
+  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 (controller.is_allowed_rmw_subscription_handle(rmw_subscription_handle) &&
+    context.is_recording_allowed())
+  {
+    ((functionT) orig_func)(rmw_subscription_handle, message,
+      source_timestamp, taken);
+
+#ifdef DEBUG_OUTPUT
+  std::cerr << "rmw_take," <<
+    rmw_subscription_handle << "," <<
+    message << "," <<
+    source_timestamp << "," <<
+    taken << "," << std::endl;
+#endif
+  }
 }
 
 void ros_trace_rmw_publish(

diff --git a/CARET_trace/src/tracing_controller.cpp b/CARET_trace/src/tracing_controller.cpp
@@ -268,6 +268,59 @@ 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;
+      }
+
+      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 +460,16 @@ 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)
 {