From 6315bfa3c072709d4d40a1fc6dc5675963d4b804 Mon Sep 17 00:00:00 2001
From: miyakoshi <92290821+miyakoshi-dev@users.noreply.github.com>
Date: Thu, 20 Jul 2023 12:04:24 +0900
Subject: [PATCH] feat: ignore rcl_timer_init() from ros2 launch command (#129)

* Ignore rcl_timer_init() from ros2 launch command

* ci(pre-commit): autofix

* Change sprintf to snprintf

* ci(pre-commit): autofix

* internal review results reflected

* ci(pre-commit): autofix

* change using-directives to using-declarations

* ci(pre-commit): autofix

* Internal review results reflected

* ci(pre-commit): autofix

* Change check method to "python3" only

* confirmed by exclusion in python3

* reflecting review comments

---------

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
---
 CARET_trace/src/ros_trace_points.cpp | 76 ++++++++++++++++++++++++++++
 1 file changed, 76 insertions(+)

diff --git a/CARET_trace/src/ros_trace_points.cpp b/CARET_trace/src/ros_trace_points.cpp
index 984abe6d..4e52866f 100644
--- a/CARET_trace/src/ros_trace_points.cpp
+++ b/CARET_trace/src/ros_trace_points.cpp
@@ -32,6 +32,7 @@
 #include <time.h>
 
 #include <cassert>
+#include <fstream>
 #include <iomanip>
 #include <iostream>
 #include <memory>
@@ -51,6 +52,75 @@
 std::unique_ptr<std::thread> trace_node_thread;
 thread_local bool trace_filter_is_rcl_publish_recorded;
 
+using std::string;
+using std::vector;
+static bool ignore_rcl_timer_init = false;
+
+static vector<string> string_split(string & str, char delim)
+{
+  using std::stringstream;
+  vector<string> elems;
+  stringstream ss(str);
+  string item;
+  while (getline(ss, item, delim)) {
+    if (!item.empty()) {
+      elems.push_back(item);
+    }
+  }
+  return elems;
+}
+
+static bool is_python3_command()
+{
+  using std::ifstream;
+  using std::ios;
+  using std::istreambuf_iterator;
+  using std::to_string;
+  pid_t pid = getpid();
+  string fileName = "/proc/" + to_string(pid) + "/cmdline";
+
+  ifstream ifs(fileName, ios::in | ios::binary);
+  if (!ifs) {
+    return false;
+  }
+  istreambuf_iterator<char> it_ifs_begin(ifs);
+  istreambuf_iterator<char> it_ifs_end{};
+  vector<char> input_data(it_ifs_begin, it_ifs_end);
+  if (!ifs) {
+    ifs.close();
+    return false;
+  }
+  ifs.close();
+
+  vector<string> cmd_line;
+  auto itr_begin = input_data.begin();
+  auto itr_find = input_data.begin();
+  for (;;) {
+    itr_find = find(itr_begin, input_data.end(), 0x00);
+    if (itr_find == input_data.end()) {
+      break;
+    }
+    string output_string(itr_begin, itr_find);
+    cmd_line.push_back(output_string);
+    itr_begin = itr_find + 1;
+  }
+
+  // Check if the right side of the last '/' in cmdline[0] matches 'python3'.
+  //   pattern 1: /usr/bin/python3 /opt/ros/humble/bin/ros2 launch ...
+  //   pattern 2: python3 /opt/ros/humble/lib/rosbridge_server/rosbridge_websocket ...
+  if (cmd_line.size() < 1) {
+    return false;
+  }
+  if (cmd_line[0].find("python3") == string::npos) {
+    return false;
+  }
+  auto subStr = string_split(cmd_line[0], '/');
+  if (subStr[subStr.size() - 1].compare("python3") == 0) {
+    return true;
+  }
+  return false;
+}
+
 void run_caret_trace_node()
 {
   // When the python implementation node is executed,
@@ -85,6 +155,8 @@ void run_caret_trace_node()
   auto trace_node = std::make_shared<TraceNode>(node_name_base, option, lttng, data_container);
   RCLCPP_INFO(trace_node->get_logger(), "%s started", trace_node->get_fully_qualified_name());
 
+  ignore_rcl_timer_init = is_python3_command();
+
   context.assign_node(trace_node);
   auto exec = rclcpp::executors::SingleThreadedExecutor();
   exec.add_node(trace_node);
@@ -553,6 +625,10 @@ void ros_trace_rcl_timer_init(
   static auto & data_container = context.get_data_container();
   // TODO(hsgwa): Add filtering of timer initialization using node_handle
 
+  if (ignore_rcl_timer_init == true) {
+    return;
+  }
+
   static auto record = [](const void * timer_handle, int64_t period, int64_t init_time) {
     tracepoint(TRACEPOINT_PROVIDER, rcl_timer_init, timer_handle, period, init_time);
   };