{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import os\n",
    "import sys\n",
    "import babeltrace\n",
    "import numpy as np\n",
    "import pandas as pd\n",
    "from matplotlib import pyplot as plt\n",
    "\n",
    "from IPython.display import display, clear_output\n",
    "\n",
    "sys.path.append(\"/home/adlink/ros2-workspace/install/tracetools_read/lib/python3.8/site-packages/\")\n",
    "sys.path.append(\"/home/adlink/ros2-workspace/src/tracetools_analysis/tracetools_analysis/\")\n",
    "from tracetools_read.trace import *\n",
    "from tracetools_analysis.loading import load_file\n",
    "from tracetools_analysis.processor.ros2 import Ros2Handler\n",
    "from tracetools_analysis.utils.ros2 import Ros2DataModelUtil"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "found converted file: /home/adlink/.ros/tracing/autoware-trace/converted\n"
     ]
    }
   ],
   "source": [
    "path = os.path.expanduser(\"~/.ros/tracing/autoware-trace/\")\n",
    "file = load_file(path)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      " [100%] [Ros2Handler]\n"
     ]
    }
   ],
   "source": [
    "handler = Ros2Handler.process(file)\n",
    "util = Ros2DataModelUtil(handler)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "====================ROS 2 DATA MODEL===================\n",
      "██ Contexts: ██\n",
      "                          timestamp   pid version\n",
      "context_handle                                   \n",
      "578889920       1652795531158728529  9807   2.3.0\n",
      "██ Nodes: ██\n",
      "                       timestamp   tid  rmw_handle             name namespace\n",
      "node_handle                                                                  \n",
      "578605264    1652795531205345494  9807   578658512  launch_ros_9807         /\n",
      "██ Publishers (rmw): ██\n",
      "Empty DataFrame\n",
      "Columns: []\n",
      "Index: []\n",
      "██ Publishers (rcl): ██\n",
      "                            timestamp  node_handle  rmw_handle topic_name  depth\n",
      "publisher_handle                                                                \n",
      "281474154018016   1652795531205338094    578605264   580193376    /rosout   1000\n",
      "██ Subscriptions (rmw): ██\n",
      "Empty DataFrame\n",
      "Columns: []\n",
      "Index: []\n",
      "██ Subscriptions (rcl): ██\n",
      "                               timestamp      node_handle       rmw_handle         topic_name  depth\n",
      "subscription_handle                                                                                 \n",
      "187651141000448      1652795532311067501  187651140946384  187651141210960  /parameter_events   1000\n",
      "██ Subscription objects: ██\n",
      "                           timestamp  subscription_handle\n",
      "subscription                                             \n",
      "187651141131344  1652795532311098621      187651141000448\n",
      "██ Services: ██\n",
      "                               timestamp  node_handle  rmw_handle                          service_name\n",
      "service_handle                                                                                         \n",
      "1652795531227060891  1652795531227060891    578605264   578509024  /launch_ros_9807/describe_parameters\n",
      "██ Clients: ██\n",
      "                           timestamp  node_handle  rmw_handle                                       service_name\n",
      "client_handle                                                                                                   \n",
      "281473303560864  1652795531234252437    578605264   580853584  /system/system_monitor/system_monitor/system_m...\n",
      "██ Timers: ██\n",
      "                           timestamp      period   tid\n",
      "timer_handle                                          \n",
      "281472359468464  1652795531230470704  1000000000  9832\n",
      "██ Timer-node links: ██\n",
      "                           timestamp      node_handle\n",
      "timer_handle                                         \n",
      "187650864529200  1652795532337464355  187650863948976\n",
      "██ Callback objects: ██\n",
      "                           timestamp  callback_object\n",
      "reference                                            \n",
      "187651141131344  1652795532311101741  187651141131568\n",
      "██ Callback symbols: ██\n",
      "                           timestamp                                             symbol\n",
      "callback_object                                                                        \n",
      "187651141131568  1652795532311126901  std::_Bind<void (rclcpp::TimeSource::*(rclcpp:...\n",
      "██ Callback instances: ██\n",
      "   callback_object                     timestamp        duration  intra_process\n",
      "0  187650864342544 2022-05-17 13:52:12.337689915 00:00:00.000010          False\n",
      "██ Publish instances (rclcpp): ██\n",
      "             timestamp          message\n",
      "0  1652795532321318177  281474880694112\n",
      "██ Publish instances (rcl): ██\n",
      "   publisher_handle            timestamp    message\n",
      "0         580257728  1652795531226289729  580728240\n",
      "██ Publish instances (rmw): ██\n",
      "Empty DataFrame\n",
      "Columns: []\n",
      "Index: []\n",
      "██ Take instances (rmw): ██\n",
      "Empty DataFrame\n",
      "Columns: []\n",
      "Index: []\n",
      "██ Take instances (rcl): ██\n",
      "Empty DataFrame\n",
      "Columns: []\n",
      "Index: []\n",
      "██ Take instances (rclcpp): ██\n",
      "Empty DataFrame\n",
      "Columns: []\n",
      "Index: []\n",
      "██ Lifecycle state machines: ██\n",
      "Empty DataFrame\n",
      "Columns: []\n",
      "Index: []\n",
      "██ Lifecycle transitions: ██\n",
      "Empty DataFrame\n",
      "Columns: []\n",
      "Index: []\n",
      "==================================================\n"
     ]
    }
   ],
   "source": [
    "n=1\n",
    "self = handler.data\n",
    "print('====================ROS 2 DATA MODEL===================')\n",
    "print('██ Contexts: ██')\n",
    "print(self.contexts[:n].to_string())\n",
    "print('██ Nodes: ██')\n",
    "print(self.nodes[:n].to_string())\n",
    "print('██ Publishers (rmw): ██')\n",
    "print(self.rmw_publishers[:n].to_string())\n",
    "print('██ Publishers (rcl): ██')\n",
    "print(self.rcl_publishers[:n].to_string())\n",
    "print('██ Subscriptions (rmw): ██')\n",
    "print(self.rmw_subscriptions[:n].to_string())\n",
    "print('██ Subscriptions (rcl): ██')\n",
    "print(self.rcl_subscriptions[:n].to_string())\n",
    "print('██ Subscription objects: ██')\n",
    "print(self.subscription_objects[:n].to_string())\n",
    "print('██ Services: ██')\n",
    "print(self.services[:n].to_string())\n",
    "print('██ Clients: ██')\n",
    "print(self.clients[:n].to_string())\n",
    "print('██ Timers: ██')\n",
    "print(self.timers[:n].to_string())\n",
    "print('██ Timer-node links: ██')\n",
    "print(self.timer_node_links[:n].to_string())\n",
    "print('██ Callback objects: ██')\n",
    "print(self.callback_objects[:n].to_string())\n",
    "print('██ Callback symbols: ██')\n",
    "print(self.callback_symbols[:n].to_string())\n",
    "print('██ Callback instances: ██')\n",
    "print(self.callback_instances[:n].to_string())\n",
    "print('██ Publish instances (rclcpp): ██')\n",
    "print(self.rclcpp_publish_instances[:n].to_string())\n",
    "print('██ Publish instances (rcl): ██')\n",
    "print(self.rcl_publish_instances[:n].to_string())\n",
    "print('██ Publish instances (rmw): ██')\n",
    "print(self.rmw_publish_instances[:n].to_string())\n",
    "print('██ Take instances (rmw): ██')\n",
    "print(self.rmw_take_instances[:n].to_string())\n",
    "print('██ Take instances (rcl): ██')\n",
    "print(self.rcl_take_instances[:n].to_string())\n",
    "print('██ Take instances (rclcpp): ██')\n",
    "print(self.rclcpp_take_instances[:n].to_string())\n",
    "print('██ Lifecycle state machines: ██')\n",
    "print(self.lifecycle_state_machines[:n].to_string())\n",
    "print('██ Lifecycle transitions: ██')\n",
    "print(self.lifecycle_transitions[:n].to_string())\n",
    "print('==================================================')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Data Structures"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 84,
   "metadata": {},
   "outputs": [],
   "source": [
    "from dataclasses import dataclass\n",
    "from typing import List, Dict\n",
    "\n",
    "def str_to_cls(classname):\n",
    "    return getattr(sys.modules[__name__], classname)\n",
    "\n",
    "def row_to_type(row, type, has_idx):\n",
    "    return type(id=row.name, **row) if has_idx else type(**row)\n",
    "\n",
    "def df_to_type_list(df, type):\n",
    "    if isinstance(type, str):\n",
    "        type = str_to_cls(type)\n",
    "        \n",
    "    has_idx = not isinstance(df.index, pd.RangeIndex)\n",
    "    return [row_to_type(row, type, has_idx) for _, row in df.iterrows()]\n",
    "\n",
    "def by_index(df, index, type):\n",
    "    return df_to_type_list(df.loc[index], type)\n",
    "\n",
    "def by_column(df, column_name, column_val, type):\n",
    "    return df_to_type_list(df[df[column_name] == column_val], type)\n",
    "\n",
    "def list_to_dict(ls, key='id'):\n",
    "    return {getattr(item, key): item for item in ls}\n",
    "\n",
    "#################################\n",
    "# Predefined (from ROS2DataModel)\n",
    "#################################\n",
    "\n",
    "@dataclass\n",
    "class Node:\n",
    "    id: int\n",
    "    timestamp: int\n",
    "    tid: int\n",
    "    rmw_handle: int\n",
    "    name: str\n",
    "    namespace: str\n",
    "\n",
    "    @property\n",
    "    def path(self) -> str:\n",
    "        return os.path.join(self.namespace, self.name)\n",
    "\n",
    "    @property\n",
    "    def publishers(self) -> List['Publisher']:\n",
    "        return list(filter(lambda pub: pub.node_handle == self.id, publishers.values()))\n",
    "\n",
    "    @property\n",
    "    def subscriptions(self) -> List['Subscription']:\n",
    "        return list(filter(lambda sub: sub.node_handle == self.id, subscriptions.values()))\n",
    "    \n",
    "    @property\n",
    "    def timers(self) -> List['Timer']:\n",
    "        links = [link.id for link in timer_node_links.values() if link.node_handle == self.id]\n",
    "        return list(filter(lambda timer: timer.id in links, timers.values()))\n",
    "\n",
    "@dataclass\n",
    "class Publisher:\n",
    "    id: int\n",
    "    timestamp: int\n",
    "    node_handle: int\n",
    "    rmw_handle: int\n",
    "    topic_name: str\n",
    "    depth: int\n",
    "\n",
    "    @property\n",
    "    def node(self) -> 'Node':\n",
    "        return nodes[self.node_handle]\n",
    "\n",
    "    @property\n",
    "    def subscriptions(self) -> List['Subscription']:\n",
    "        return list(filter(lambda sub: sub.topic_name == self.topic_name, subscriptions.values()))\n",
    "\n",
    "    @property\n",
    "    def instances(self) -> List['PublishInstance']:\n",
    "        return list(filter(lambda inst: inst.publisher_handle == self.id, publish_instances))\n",
    "    \n",
    "    @property\n",
    "    def topic(self) -> 'Topic':\n",
    "        return topics[self.topic_name]\n",
    "\n",
    "\n",
    "@dataclass\n",
    "class Subscription:\n",
    "    id: int\n",
    "    timestamp: int\n",
    "    node_handle: int\n",
    "    rmw_handle: int\n",
    "    topic_name: str\n",
    "    depth: int\n",
    "\n",
    "    @property\n",
    "    def node(self) -> 'Node':\n",
    "        return nodes[self.node_handle]\n",
    "\n",
    "    @property\n",
    "    def publishers(self) -> List['Publisher']:\n",
    "        return list(filter(lambda pub: pub.topic_name == self.topic_name, publishers.values()))\n",
    "    \n",
    "    @property\n",
    "    def subscription_object(self) -> 'SubscriptionObject':\n",
    "        sub_objs = list(filter(lambda sub_obj: sub_obj.subscription_handle == self.id, subscription_objects.values()))\n",
    "        assert len(sub_objs) <= 1\n",
    "        return sub_objs[0] if sub_objs else None\n",
    "\n",
    "    @property\n",
    "    def topic(self) -> 'Topic':\n",
    "        return topics[self.topic_name]\n",
    "    \n",
    "@dataclass\n",
    "class Timer:\n",
    "    id: int\n",
    "    timestamp: int\n",
    "    period: int\n",
    "    tid: int\n",
    "\n",
    "    @property\n",
    "    def nodes(self) -> List['Node']:\n",
    "        links = [link.node_handle for link in timer_node_links.values() if link.id == self.id]\n",
    "        return list(filter(lambda node: node.id in links, nodes.values()))\n",
    "    \n",
    "    @property\n",
    "    def callback_object(self) -> 'CallbackObject':\n",
    "        return callback_objects[self.id]\n",
    "    \n",
    "    @property\n",
    "    def info(self):\n",
    "        return util.get_timer_handle_info(self.id)\n",
    "\n",
    "@dataclass\n",
    "class TimerNodeLink:\n",
    "    id: int\n",
    "    timestamp: int\n",
    "    node_handle: int\n",
    "\n",
    "@dataclass\n",
    "class SubscriptionObject:\n",
    "    id: int             # subscription\n",
    "    timestamp: int\n",
    "    subscription_handle: int\n",
    "\n",
    "    @property\n",
    "    def subscription(self) -> 'Subscription':\n",
    "        return subscriptions[self.subscription_handle]\n",
    "\n",
    "    @property\n",
    "    def callback_object(self) -> 'CallbackObject':\n",
    "        return callback_objects[self.id]\n",
    "\n",
    "    @property\n",
    "    def info(self):\n",
    "        return util.get_subscription_reference_info(self.id)\n",
    "\n",
    "@dataclass\n",
    "class CallbackObject:\n",
    "    id: int             # (reference) = subscription_object.id | timer.id | ....\n",
    "    timestamp: int\n",
    "    callback_object: int\n",
    "\n",
    "    @property\n",
    "    def callback_instances(self) -> List['CallbackInstance']:\n",
    "        return list(filter(lambda inst: inst.callback_object == self.callback_object, callback_instances))\n",
    "\n",
    "    @property\n",
    "    def owner(self):\n",
    "        if self.id in timers:\n",
    "            return timers[self.id]\n",
    "        if self.id in publishers:\n",
    "            return publishers[self.id]\n",
    "        if self.id in subscription_objects:\n",
    "            return subscription_objects[self.id]\n",
    "        if self.id in handler.data.services.index:\n",
    "            return 'Service'\n",
    "        if self.id in handler.data.clients.index:\n",
    "            return 'Client'\n",
    "        return None\n",
    "\n",
    "    @property\n",
    "    def owner_info(self):\n",
    "        info = util.get_callback_owner_info(self.callback_object)\n",
    "        if info is None:\n",
    "            return None, None\n",
    "        \n",
    "        type_name, dict_str = info.split(\" -- \")\n",
    "        kv_strs = dict_str.split(\", \")\n",
    "        info_dict = {k: v for k, v in map(lambda kv_str: kv_str.split(\": \", maxsplit=1), kv_strs)}\n",
    "        return type_name, info_dict\n",
    "\n",
    "@dataclass\n",
    "class PublishInstance:\n",
    "    publisher_handle: int\n",
    "    timestamp: int\n",
    "    message: int\n",
    "\n",
    "    @property\n",
    "    def publisher(self) -> 'Publisher':\n",
    "        return publishers[self.publisher_handle]\n",
    "\n",
    "@dataclass\n",
    "class CallbackInstance:\n",
    "    callback_object: int\n",
    "    timestamp: int\n",
    "    duration: int\n",
    "    intra_process: bool\n",
    "\n",
    "    @property\n",
    "    def callback_obj(self) -> 'CallbackObject':\n",
    "        return callback_objects[self.callback_object]\n",
    "\n",
    "@dataclass\n",
    "class CallbackSymbol:\n",
    "    id: int  # callback_object\n",
    "    timestamp: int\n",
    "    symbol: str\n",
    "\n",
    "    @property\n",
    "    def callback_obj(self) -> 'CallbackObject':\n",
    "        cb_objs = list(filter(lambda cb_obj: cb_obj.callback_object == self.id, callback_objects.values()))\n",
    "        assert len(cb_objs) <= 1\n",
    "        return cb_objs[0] if cb_objs else None\n",
    "\n",
    "\n",
    "#######################################\n",
    "# Self-defined (not from ROS2DataModel)\n",
    "#######################################\n",
    "\n",
    "@dataclass\n",
    "class Topic:\n",
    "    name: str\n",
    "\n",
    "    @property\n",
    "    def publishers(self) -> List['Publisher']:\n",
    "        return list(filter(lambda pub: pub.topic_name == self.name, publishers.values()))\n",
    "    \n",
    "    @property\n",
    "    def subscriptions(self) -> List['Subscription']:\n",
    "        return list(filter(lambda sub: sub.topic_name == self.name, subscriptions.values()))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 85,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Processed 61       nodes\n",
      "Processed 264      publishers\n",
      "Processed 212      subscriptions\n",
      "Processed 67       timers\n",
      "Processed 31       timer-node links\n",
      "Processed 201      subscription objects\n",
      "Processed 483      callback objects\n",
      "Processed 483      callback symbols\n",
      "Processed 183512   publish instances\n",
      "Processed 390810   callback instances\n",
      "Processed 161      topics\n"
     ]
    }
   ],
   "source": [
    "\n",
    "#######################################\n",
    "# Instantiate collections\n",
    "#######################################\n",
    "\n",
    "nodes:                  Dict[int, 'Node']               = list_to_dict(df_to_type_list(handler.data.nodes,                  'Node'));               print(f\"Processed {len(nodes):<8d} nodes\")\n",
    "publishers:             Dict[int, 'Publisher']          = list_to_dict(df_to_type_list(handler.data.rcl_publishers,         'Publisher'));          print(f\"Processed {len(publishers):<8d} publishers\")\n",
    "subscriptions:          Dict[int, 'Subscription']       = list_to_dict(df_to_type_list(handler.data.rcl_subscriptions,      'Subscription'));       print(f\"Processed {len(subscriptions):<8d} subscriptions\")\n",
    "timers:                 Dict[int, 'Timer']              = list_to_dict(df_to_type_list(handler.data.timers,                 'Timer'));              print(f\"Processed {len(timers):<8d} timers\")\n",
    "timer_node_links:       Dict[int, 'TimerNodeLink']      = list_to_dict(df_to_type_list(handler.data.timer_node_links,       'TimerNodeLink'));      print(f\"Processed {len(timer_node_links):<8d} timer-node links\")\n",
    "subscription_objects:   Dict[int, 'SubscriptionObject'] = list_to_dict(df_to_type_list(handler.data.subscription_objects,   'SubscriptionObject')); print(f\"Processed {len(subscription_objects):<8d} subscription objects\")\n",
    "callback_objects:       Dict[int, 'CallbackObject']     = list_to_dict(df_to_type_list(handler.data.callback_objects,       'CallbackObject'));     print(f\"Processed {len(callback_objects):<8d} callback objects\")\n",
    "callback_symbols:       Dict[int, 'CallbackSymbol']     = list_to_dict(df_to_type_list(handler.data.callback_symbols,       'CallbackSymbol'));     print(f\"Processed {len(callback_symbols):<8d} callback symbols\")\n",
    "publish_instances:      List['PublishInstance']         =              df_to_type_list(handler.data.rcl_publish_instances,  'PublishInstance');     print(f\"Processed {len(publish_instances):<8d} publish instances\")\n",
    "callback_instances:     List['CallbackInstance']        =              df_to_type_list(handler.data.callback_instances,     'CallbackInstance');    print(f\"Processed {len(callback_instances):<8d} callback instances\")\n",
    "\n",
    "_unique_topic_names = {*(pub.topic_name for pub in publishers.values()), *(sub.topic_name for sub in subscriptions.values())}\n",
    "topics: Dict[str, 'Topic'] = list_to_dict(map(Topic, _unique_topic_names), key=\"name\"); print(f\"Processed {len(topics):<8d} topics\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Callback-Sub & Callback-Timer Links"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 83,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i=1026 Subscription n=system_monitor_container                 t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i=1042 Subscription n=robot_state_publisher                    t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i=1070 Subscription n=system_error_monitor                     t=/parameter_events             \n",
      "void (AutowareErrorMonitor::?)(diagnostic_msgs::msg::DiagnosticArrayconst*)                         : i=1780 Subscription n=system_error_monitor                     t=/diagnostics_agg              \n",
      "void (AutowareErrorMonitor::?)(tier4_control_msgs::msg::GateModeconst*)                             : i=1626 Subscription n=system_error_monitor                     t=/control/current_gate_mode    \n",
      "void (AutowareErrorMonitor::?)(autoware_auto_system_msgs::msg::AutowareStateconst*)                 : i=1772 Subscription n=system_error_monitor                     t=/autoware/state               \n",
      "void (AutowareErrorMonitor::?)(autoware_auto_vehicle_msgs::msg::ControlModeReportconst*)            : i=6466 Subscription n=system_error_monitor                     t=/vehicle/status/control_mode  \n",
      "void (robot_state_publisher::RobotStatePublisher::?)(sensor_msgs::msg::JointState*)                 : i=   0 Subscription n=robot_state_publisher                    t=/joint_states                 \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 950 Subscription n=aggregator_node                          t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i=1035 Subscription n=emergency_handler                        t=/parameter_events             \n",
      "void (EmergencyHandler::?)(autoware_auto_system_msgs::msg::HazardStatusStampedconst*)               : i=1626 Subscription n=emergency_handler                        t=/system/emergency/hazard_status\n",
      "void (EmergencyHandler::?)(autoware_auto_control_msgs::msg::AckermannControlCommandconst*)          : i=10468 Subscription n=emergency_handler                        t=/control/command/control_cmd  \n",
      "void (EmergencyHandler::?)(nav_msgs::msg::Odometryconst*)                                           : i=6466 Subscription n=emergency_handler                        t=/localization/kinematic_state \n",
      "void (EmergencyHandler::?)(autoware_auto_vehicle_msgs::msg::ControlModeReportconst*)                : i=6466 Subscription n=emergency_handler                        t=/vehicle/status/control_mode  \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i=1026 Subscription n=map_container                            t=/parameter_events             \n",
      "void (HeaderlessHeartbeatChecker<autoware_auto_system_msgs::msg::HazardStatusStamped>::?)(autowar...: i=1626 Subscription n=emergency_handler                        t=/system/emergency/hazard_status\n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i=1030 Subscription n=ad_service_state_monitor                 t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i=1026 Subscription n=transform_listener_impl_aaaab90b38b0     t=/parameter_events             \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=6466 Subscription n=transform_listener_impl_aaaab90b38b0     t=/tf                           \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=   3 Subscription n=transform_listener_impl_aaaab90b38b0     t=/tf_static                    \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 940 Subscription n=mission_planning_container               t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 907 Subscription n=scenario_selector                        t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 903 Subscription n=transform_listener_impl_aaaad6cbbcb8     t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 907 Subscription n=external_velocity_limit_selector         t=/parameter_events             \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=6466 Subscription n=transform_listener_impl_aaaad6cbbcb8     t=/tf                           \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=   3 Subscription n=transform_listener_impl_aaaad6cbbcb8     t=/tf_static                    \n",
      "void (AutowareStateMonitorNode::?)(autoware_auto_vehicle_msgs::msg::Engageconst*)                   : i=1626 Subscription n=ad_service_state_monitor                 t=/api/autoware/get/engage      \n",
      "void (AutowareStateMonitorNode::?)(autoware_auto_vehicle_msgs::msg::ControlModeReportconst*)        : i=6466 Subscription n=ad_service_state_monitor                 t=/vehicle/status/control_mode  \n",
      "void (AutowareStateMonitorNode::?)(autoware_auto_planning_msgs::msg::HADMapRouteconst*)             : i=   1 Subscription n=ad_service_state_monitor                 t=/planning/mission_planning/route\n",
      "void (AutowareStateMonitorNode::?)(nav_msgs::msg::Odometryconst*)                                   : i=6466 Subscription n=ad_service_state_monitor                 t=/localization/kinematic_state \n",
      "void (ScenarioSelectorNode::?)(autoware_auto_planning_msgs::msg::Trajectoryconst*)                  : i= 978 Subscription n=scenario_selector                        t=/planning/scenario_planning/lane_driving/trajectory\n",
      "void (ScenarioSelectorNode::?)(autoware_auto_planning_msgs::msg::Trajectoryconst*)                  : i= 101 Subscription n=scenario_selector                        t=/planning/scenario_planning/parking/trajectory\n",
      "void (ScenarioSelectorNode::?)(autoware_auto_mapping_msgs::msg::HADMapBinconst*)                    : i=   1 Subscription n=scenario_selector                        t=/map/vector_map               \n",
      "void (ScenarioSelectorNode::?)(autoware_auto_planning_msgs::msg::HADMapRouteconst*)                 : i=   1 Subscription n=scenario_selector                        t=/planning/mission_planning/route\n",
      "void (ScenarioSelectorNode::?)(nav_msgs::msg::Odometryconst*)                                       : i=6466 Subscription n=scenario_selector                        t=/localization/kinematic_state \n",
      "void (ExternalVelocityLimitSelectorNode::?)(tier4_planning_msgs::msg::VelocityLimitconst*)          : i=   0 Subscription n=external_velocity_limit_selector         t=/planning/scenario_planning/max_velocity_default\n",
      "void (ExternalVelocityLimitSelectorNode::?)(tier4_planning_msgs::msg::VelocityLimitconst*)          : i=   0 Subscription n=external_velocity_limit_selector         t=/planning/scenario_planning/max_velocity_candidates\n",
      "void (ExternalVelocityLimitSelectorNode::?)(tier4_planning_msgs::msg::VelocityLimitClearCommandco...: i=   0 Subscription n=external_velocity_limit_selector         t=/planning/scenario_planning/clear_velocity_limit\n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 934 Subscription n=motion_velocity_smoother                 t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 930 Subscription n=transform_listener_impl_aaaaf1071f80     t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 887 Subscription n=behavior_planning_container              t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 887 Subscription n=motion_planning_container                t=/parameter_events             \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=6466 Subscription n=transform_listener_impl_aaaaf1071f80     t=/tf                           \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=   3 Subscription n=transform_listener_impl_aaaaf1071f80     t=/tf_static                    \n",
      "void (motion_velocity_smoother::MotionVelocitySmootherNode::?)(autoware_auto_planning_msgs::msg::...: i=1079 Subscription n=motion_velocity_smoother                 t=/planning/scenario_planning/scenario_selector/trajectory\n",
      "void (motion_velocity_smoother::MotionVelocitySmootherNode::?)(nav_msgs::msg::Odometryconst*)       : i=6267 Subscription n=motion_velocity_smoother                 t=/localization/kinematic_state \n",
      "void (motion_velocity_smoother::MotionVelocitySmootherNode::?)(tier4_planning_msgs::msg::Velocity...: i=   0 Subscription n=motion_velocity_smoother                 t=/planning/scenario_planning/max_velocity\n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 887 Subscription n=parking_container                        t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 888 Subscription n=control_container                        t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 896 Subscription n=planning_error_monitor                   t=/parameter_events             \n",
      "void (planning_diagnostics::PlanningErrorMonitorNode::?)(autoware_auto_planning_msgs::msg::Trajec...: i=1078 Subscription n=planning_error_monitor                   t=/planning/scenario_planning/trajectory\n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 562 Subscription n=cpu_monitor                              t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 874 Subscription n=hdd_monitor                              t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 873 Subscription n=obstacle_avoidance_planner               t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 875 Subscription n=lateral_controller_node_exe              t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 863 Subscription n=transform_listener_impl_aaaaf51e51c0     t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 862 Subscription n=transform_listener_impl_aaaaf153e160     t=/parameter_events             \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=6465 Subscription n=transform_listener_impl_aaaaf51e51c0     t=/tf                           \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=6466 Subscription n=transform_listener_impl_aaaaf153e160     t=/tf                           \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=   3 Subscription n=transform_listener_impl_aaaaf51e51c0     t=/tf_static                    \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=   3 Subscription n=transform_listener_impl_aaaaf153e160     t=/tf_static                    \n",
      "void (ObstacleAvoidancePlanner::?)(autoware_auto_planning_msgs::msg::Path*)                         : i= 978 Subscription n=obstacle_avoidance_planner               t=/planning/scenario_planning/lane_driving/behavior_planning/path\n",
      "void (ObstacleAvoidancePlanner::?)(nav_msgs::msg::Odometry*)                                        : i=6141 Subscription n=obstacle_avoidance_planner               t=/localization/kinematic_state \n",
      "void (ObstacleAvoidancePlanner::?)(autoware_auto_perception_msgs::msg::PredictedObjects*)           : i=1582 Subscription n=obstacle_avoidance_planner               t=/perception/object_recognition/objects\n",
      "void (autoware::motion::control::trajectory_follower_nodes::LateralController::?)(autoware_auto_p...: i=1078 Subscription n=lateral_controller_node_exe              t=/planning/scenario_planning/trajectory\n",
      "void (ObstacleAvoidancePlanner::?)(tier4_planning_msgs::msg::EnableAvoidance*)                      : i=   0 Subscription n=obstacle_avoidance_planner               t=/planning/scenario_planning/lane_driving/obstacle_avoidance_approval\n",
      "void (autoware::motion::control::trajectory_follower_nodes::LateralController::?)(autoware_auto_v...: i=6285 Subscription n=lateral_controller_node_exe              t=/vehicle/status/steering_status\n",
      "void (autoware::motion::control::trajectory_follower_nodes::LateralController::?)(nav_msgs::msg::...: i=6285 Subscription n=lateral_controller_node_exe              t=/localization/kinematic_state \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 710 Subscription n=mem_monitor                              t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 703 Subscription n=lanelet2_map_loader                      t=/parameter_events             \n",
      "void (diagnostic_aggregator::Aggregator::?)(diagnostic_msgs::msg::DiagnosticArray*)                 : i=10587 Subscription n=aggregator_node                          t=/diagnostics                  \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 695 Subscription n=net_monitor                              t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 683 Subscription n=lanelet2_map_visualization               t=/parameter_events             \n",
      "void (Lanelet2MapVisualizationNode::?)(autoware_auto_mapping_msgs::msg::HADMapBinconst*)            : i=   1 Subscription n=lanelet2_map_visualization               t=/map/vector_map               \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 677 Subscription n=ntp_monitor                              t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 669 Subscription n=mission_planner                          t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 665 Subscription n=transform_listener_impl_aaab0bafc440     t=/parameter_events             \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=6466 Subscription n=transform_listener_impl_aaab0bafc440     t=/tf                           \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=   3 Subscription n=transform_listener_impl_aaab0bafc440     t=/tf_static                    \n",
      "void (mission_planner::MissionPlanner::?)(geometry_msgs::msg::PoseStampedconst*)                    : i=   1 Subscription n=mission_planner                          t=/planning/mission_planning/goal\n",
      "void (mission_planner::MissionPlanner::?)(geometry_msgs::msg::PoseStampedconst*)                    : i=   0 Subscription n=mission_planner                          t=/planning/mission_planning/checkpoint\n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 661 Subscription n=process_monitor                          t=/parameter_events             \n",
      "void (mission_planner::MissionPlannerLanelet2::?)(autoware_auto_mapping_msgs::msg::HADMapBinconst*) : i=   1 Subscription n=mission_planner                          t=/map/vector_map               \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 655 Subscription n=longitudinal_controller_node_exe         t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 651 Subscription n=transform_listener_impl_aaaaf52d4c30     t=/parameter_events             \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=6465 Subscription n=transform_listener_impl_aaaaf52d4c30     t=/tf                           \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=   3 Subscription n=transform_listener_impl_aaaaf52d4c30     t=/tf_static                    \n",
      "void (autoware::motion::control::trajectory_follower_nodes::LongitudinalController::?)(nav_msgs::...: i=6285 Subscription n=longitudinal_controller_node_exe         t=/localization/kinematic_state \n",
      "void (autoware::motion::control::trajectory_follower_nodes::LongitudinalController::?)(autoware_a...: i=1078 Subscription n=longitudinal_controller_node_exe         t=/planning/scenario_planning/trajectory\n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 603 Subscription n=gpu_monitor                              t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 581 Subscription n=goal_pose_visualizer                     t=/parameter_events             \n",
      "void (mission_planner::GoalPoseVisualizer::?)(autoware_auto_planning_msgs::msg::HADMapRouteconst*)  : i=   1 Subscription n=goal_pose_visualizer                     t=/planning/mission_planning/route\n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 569 Subscription n=pointcloud_map_loader                    t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 569 Subscription n=external_cmd_selector                    t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 553 Subscription n=behavior_path_planner                    t=/parameter_events             \n",
      "void (ExternalCmdSelector::?)(tier4_external_api_msgs::msg::ControlCommandStampedconst*)            : i=   0 Subscription n=external_cmd_selector                    t=/api/external/set/command/local/control\n",
      "void (ExternalCmdSelector::?)(tier4_external_api_msgs::msg::GearShiftStampedconst*)                 : i=   0 Subscription n=external_cmd_selector                    t=/api/external/set/command/local/shift\n",
      "void (ExternalCmdSelector::?)(tier4_external_api_msgs::msg::TurnSignalStampedconst*)                : i=   0 Subscription n=external_cmd_selector                    t=/api/external/set/command/local/turn_signal\n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 551 Subscription n=transform_listener_impl_ffff200c66b0     t=/parameter_events             \n",
      "void (ExternalCmdSelector::?)(tier4_external_api_msgs::msg::Heartbeatconst*)                        : i=   0 Subscription n=external_cmd_selector                    t=/api/external/set/command/local/heartbeat\n",
      "void (ExternalCmdSelector::?)(tier4_external_api_msgs::msg::ControlCommandStampedconst*)            : i=   0 Subscription n=external_cmd_selector                    t=/api/external/set/command/remote/control\n",
      "void (ExternalCmdSelector::?)(tier4_external_api_msgs::msg::GearShiftStampedconst*)                 : i=   0 Subscription n=external_cmd_selector                    t=/api/external/set/command/remote/shift\n",
      "void (ExternalCmdSelector::?)(tier4_external_api_msgs::msg::TurnSignalStampedconst*)                : i=   0 Subscription n=external_cmd_selector                    t=/api/external/set/command/remote/turn_signal\n",
      "void (ExternalCmdSelector::?)(tier4_external_api_msgs::msg::Heartbeatconst*)                        : i=   0 Subscription n=external_cmd_selector                    t=/api/external/set/command/remote/heartbeat\n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=6466 Subscription n=transform_listener_impl_ffff200c66b0     t=/tf                           \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=   3 Subscription n=transform_listener_impl_ffff200c66b0     t=/tf_static                    \n",
      "void (behavior_path_planner::BehaviorPathPlannerNode::?)(nav_msgs::msg::Odometryconst*)             : i=4320 Subscription n=behavior_path_planner                    t=/localization/kinematic_state \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 518 Subscription n=external_cmd_converter                   t=/parameter_events             \n",
      "void (behavior_path_planner::BehaviorPathPlannerNode::?)(autoware_auto_perception_msgs::msg::Pred...: i=1049 Subscription n=behavior_path_planner                    t=/perception/object_recognition/objects\n",
      "behavior_path_planner::BehaviorPathPlannerNode::BehaviorPathPlannerNode(rclcpp::NodeOptionsconst&...: i=1577 Subscription n=behavior_path_planner                    t=/planning/scenario_planning/scenario\n",
      "void (behavior_path_planner::BehaviorPathPlannerNode::?)(tier4_planning_msgs::msg::Approvalconst*)  : i=1171 Subscription n=behavior_path_planner                    t=/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/path_change_approval\n",
      "void (behavior_path_planner::BehaviorPathPlannerNode::?)(tier4_planning_msgs::msg::PathChangeModu...: i=   0 Subscription n=behavior_path_planner                    t=/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/path_change_force\n",
      "void (behavior_path_planner::BehaviorPathPlannerNode::?)(autoware_auto_mapping_msgs::msg::HADMapB...: i=   1 Subscription n=behavior_path_planner                    t=/map/vector_map               \n",
      "void (behavior_path_planner::BehaviorPathPlannerNode::?)(autoware_auto_planning_msgs::msg::HADMap...: i=   1 Subscription n=behavior_path_planner                    t=/planning/mission_planning/route\n",
      "void (external_cmd_converter::ExternalCmdConverterNode::?)(nav_msgs::msg::Odometryconst*)           : i=6285 Subscription n=external_cmd_converter                   t=/localization/kinematic_state \n",
      "void (external_cmd_converter::ExternalCmdConverterNode::?)(tier4_external_api_msgs::msg::ControlC...: i=   0 Subscription n=external_cmd_converter                   t=/external/selected/external_control_cmd\n",
      "void (behavior_path_planner::SideShiftModule::?)(tier4_planning_msgs::msg::LateralOffsetconst*)     : i=   0 Subscription n=behavior_path_planner                    t=/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/input/lateral_offset\n",
      "void (external_cmd_converter::ExternalCmdConverterNode::?)(autoware_auto_vehicle_msgs::msg::GearC...: i=   0 Subscription n=external_cmd_converter                   t=/external/selected/gear_cmd   \n",
      "void (external_cmd_converter::ExternalCmdConverterNode::?)(tier4_control_msgs::msg::GateModeconst*) : i=1626 Subscription n=external_cmd_converter                   t=/control/current_gate_mode    \n",
      "void (external_cmd_converter::ExternalCmdConverterNode::?)(tier4_external_api_msgs::msg::Heartbea...: i=   0 Subscription n=external_cmd_converter                   t=/external/selected/heartbeat  \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 469 Subscription n=costmap_generator                        t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 454 Subscription n=transform_listener_impl_aaaaf521b848     t=/parameter_events             \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=6466 Subscription n=transform_listener_impl_aaaaf521b848     t=/tf                           \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=   3 Subscription n=transform_listener_impl_aaaaf521b848     t=/tf_static                    \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 364 Subscription n=latlon_muxer_node_exe                    t=/parameter_events             \n",
      "void (autoware::motion::control::trajectory_follower_nodes::LatLonMuxer::?)(autoware_auto_control...: i=5235 Subscription n=latlon_muxer_node_exe                    t=/control/trajectory_follower/lateral/control_cmd\n",
      "void (autoware::motion::control::trajectory_follower_nodes::LatLonMuxer::?)(autoware_auto_control...: i=5235 Subscription n=latlon_muxer_node_exe                    t=/control/trajectory_follower/longitudinal/control_cmd\n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 358 Subscription n=lane_departure_checker_node              t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 354 Subscription n=transform_listener_impl_aaaaf55ae730     t=/parameter_events             \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=6465 Subscription n=transform_listener_impl_aaaaf55ae730     t=/tf                           \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=   3 Subscription n=transform_listener_impl_aaaaf55ae730     t=/tf_static                    \n",
      "void (lane_departure_checker::LaneDepartureCheckerNode::?)(nav_msgs::msg::Odometryconst*)           : i=6286 Subscription n=lane_departure_checker_node              t=/localization/kinematic_state \n",
      "void (lane_departure_checker::LaneDepartureCheckerNode::?)(autoware_auto_mapping_msgs::msg::HADMa...: i=   1 Subscription n=lane_departure_checker_node              t=/map/vector_map               \n",
      "void (lane_departure_checker::LaneDepartureCheckerNode::?)(autoware_auto_planning_msgs::msg::HADM...: i=   1 Subscription n=lane_departure_checker_node              t=/planning/mission_planning/route\n",
      "void (lane_departure_checker::LaneDepartureCheckerNode::?)(autoware_auto_planning_msgs::msg::Traj...: i=1078 Subscription n=lane_departure_checker_node              t=/planning/scenario_planning/trajectory\n",
      "void (lane_departure_checker::LaneDepartureCheckerNode::?)(autoware_auto_planning_msgs::msg::Traj...: i=5235 Subscription n=lane_departure_checker_node              t=/control/trajectory_follower/lateral/predicted_trajectory\n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 333 Subscription n=map_tf_generator                         t=/parameter_events             \n",
      "void (MapTFGeneratorNode::?)(sensor_msgs::msg::PointCloud2const*)                                   : i=   1 Subscription n=map_tf_generator                         t=/map/pointcloud_map           \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 322 Subscription n=surround_obstacle_checker                t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 318 Subscription n=transform_listener_impl_ffff5c00c708     t=/parameter_events             \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=6298 Subscription n=transform_listener_impl_ffff5c00c708     t=/tf                           \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=   3 Subscription n=transform_listener_impl_ffff5c00c708     t=/tf_static                    \n",
      "void (surround_obstacle_checker::SurroundObstacleCheckerNode::?)(sensor_msgs::msg::PointCloud2con...: i=1574 Subscription n=surround_obstacle_checker                t=/perception/obstacle_segmentation/pointcloud\n",
      "void (surround_obstacle_checker::SurroundObstacleCheckerNode::?)(autoware_auto_perception_msgs::m...: i=1572 Subscription n=surround_obstacle_checker                t=/perception/object_recognition/objects\n",
      "void (surround_obstacle_checker::SurroundObstacleCheckerNode::?)(nav_msgs::msg::Odometryconst*)     : i=6298 Subscription n=surround_obstacle_checker                t=/localization/kinematic_state \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 300 Subscription n=obstacle_stop_planner                    t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 296 Subscription n=transform_listener_impl_ffff3400eba0     t=/parameter_events             \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=6294 Subscription n=transform_listener_impl_ffff3400eba0     t=/tf                           \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=   3 Subscription n=transform_listener_impl_ffff3400eba0     t=/tf_static                    \n",
      "void (motion_planning::ObstacleStopPlannerNode::?)(sensor_msgs::msg::PointCloud2const*)             : i=1573 Subscription n=obstacle_stop_planner                    t=/perception/obstacle_segmentation/pointcloud\n",
      "void (motion_planning::ObstacleStopPlannerNode::?)(autoware_auto_planning_msgs::msg::Trajectoryco...: i= 978 Subscription n=obstacle_stop_planner                    t=/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory\n",
      "void (motion_planning::ObstacleStopPlannerNode::?)(nav_msgs::msg::Odometryconst*)                   : i=6293 Subscription n=obstacle_stop_planner                    t=/localization/kinematic_state \n",
      "void (motion_planning::ObstacleStopPlannerNode::?)(autoware_auto_perception_msgs::msg::PredictedO...: i=1571 Subscription n=obstacle_stop_planner                    t=/perception/object_recognition/objects\n",
      "void (motion_planning::ObstacleStopPlannerNode::?)(tier4_planning_msgs::msg::ExpandStopRangeconst*) : i=   0 Subscription n=obstacle_stop_planner                    t=/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/input/expand_stop_range\n",
      "void (CostmapGenerator::?)(autoware_auto_perception_msgs::msg::PredictedObjectsconst*)              : i=1228 Subscription n=costmap_generator                        t=/perception/object_recognition/objects\n",
      "void (CostmapGenerator::?)(sensor_msgs::msg::PointCloud2const*)                                     : i=1287 Subscription n=costmap_generator                        t=/perception/obstacle_segmentation/pointcloud\n",
      "void (CostmapGenerator::?)(autoware_auto_mapping_msgs::msg::HADMapBinconst*)                        : i=   1 Subscription n=costmap_generator                        t=/map/vector_map               \n",
      "void (CostmapGenerator::?)(tier4_planning_msgs::msg::Scenarioconst*)                                : i=1232 Subscription n=costmap_generator                        t=/planning/scenario_planning/scenario\n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 227 Subscription n=freespace_planner                        t=/parameter_events             \n",
      "void (freespace_planner::FreespacePlannerNode::?)(autoware_auto_planning_msgs::msg::HADMapRouteco...: i=   1 Subscription n=freespace_planner                        t=/planning/mission_planning/route\n",
      "void (freespace_planner::FreespacePlannerNode::?)(nav_msgs::msg::OccupancyGridconst*)               : i= 247 Subscription n=freespace_planner                        t=/planning/scenario_planning/parking/costmap_generator/occupancy_grid\n",
      "void (freespace_planner::FreespacePlannerNode::?)(tier4_planning_msgs::msg::Scenarioconst*)         : i=1232 Subscription n=freespace_planner                        t=/planning/scenario_planning/scenario\n",
      "void (freespace_planner::FreespacePlannerNode::?)(nav_msgs::msg::Odometryconst*)                    : i=4919 Subscription n=freespace_planner                        t=/localization/kinematic_state \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 189 Subscription n=transform_listener_impl_aaaaf5695dc0     t=/parameter_events             \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=6288 Subscription n=transform_listener_impl_aaaaf5695dc0     t=/tf                           \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=   3 Subscription n=transform_listener_impl_aaaaf5695dc0     t=/tf_static                    \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 188 Subscription n=shift_decider                            t=/parameter_events             \n",
      "void (ShiftDecider::?)(autoware_auto_control_msgs::msg::AckermannControlCommand*)                   : i=10468 Subscription n=shift_decider                            t=/control/trajectory_follower/control_cmd\n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 183 Subscription n=vehicle_cmd_gate                         t=/parameter_events             \n",
      "void (VehicleCmdGate::?)(autoware_auto_system_msgs::msg::EmergencyStateconst*)                      : i=1625 Subscription n=vehicle_cmd_gate                         t=/system/emergency/emergency_state\n",
      "void (VehicleCmdGate::?)(tier4_external_api_msgs::msg::Heartbeatconst*)                             : i=   0 Subscription n=vehicle_cmd_gate                         t=/external/selected/heartbeat  \n",
      "void (VehicleCmdGate::?)(tier4_control_msgs::msg::GateModeconst*)                                   : i=   0 Subscription n=vehicle_cmd_gate                         t=/control/gate_mode_cmd        \n",
      "void (VehicleCmdGate::?)(autoware_auto_vehicle_msgs::msg::Engageconst*)                             : i=   2 Subscription n=vehicle_cmd_gate                         t=/autoware/engage              \n",
      "void (VehicleCmdGate::?)(autoware_auto_vehicle_msgs::msg::SteeringReportconst*)                     : i=6282 Subscription n=vehicle_cmd_gate                         t=/vehicle/status/steering_status\n",
      "void (VehicleCmdGate::?)(autoware_auto_control_msgs::msg::AckermannControlCommandconst*)            : i=10468 Subscription n=vehicle_cmd_gate                         t=/control/trajectory_follower/control_cmd\n",
      "void (VehicleCmdGate::?)(autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommandconst*)              : i= 983 Subscription n=vehicle_cmd_gate                         t=/planning/turn_indicators_cmd \n",
      "void (VehicleCmdGate::?)(autoware_auto_vehicle_msgs::msg::HazardLightsCommandconst*)                : i= 983 Subscription n=vehicle_cmd_gate                         t=/planning/hazard_lights_cmd   \n",
      "void (VehicleCmdGate::?)(autoware_auto_vehicle_msgs::msg::GearCommandconst*)                        : i=1571 Subscription n=vehicle_cmd_gate                         t=/control/shift_decider/gear_cmd\n",
      "void (VehicleCmdGate::?)(autoware_auto_control_msgs::msg::AckermannControlCommandconst*)            : i=   0 Subscription n=vehicle_cmd_gate                         t=/external/selected/control_cmd\n",
      "void (VehicleCmdGate::?)(autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommandconst*)              : i=   0 Subscription n=vehicle_cmd_gate                         t=/external/selected/turn_indicators_cmd\n",
      "void (VehicleCmdGate::?)(autoware_auto_vehicle_msgs::msg::HazardLightsCommandconst*)                : i=   0 Subscription n=vehicle_cmd_gate                         t=/external/selected/hazard_lights_cmd\n",
      "void (VehicleCmdGate::?)(autoware_auto_vehicle_msgs::msg::GearCommandconst*)                        : i=   0 Subscription n=vehicle_cmd_gate                         t=/external/selected/gear_cmd   \n",
      "void (VehicleCmdGate::?)(autoware_auto_control_msgs::msg::AckermannControlCommandconst*)            : i=1625 Subscription n=vehicle_cmd_gate                         t=/system/emergency/control_cmd \n",
      "void (VehicleCmdGate::?)(autoware_auto_vehicle_msgs::msg::HazardLightsCommandconst*)                : i=1625 Subscription n=vehicle_cmd_gate                         t=/system/emergency/hazard_lights_cmd\n",
      "void (VehicleCmdGate::?)(autoware_auto_vehicle_msgs::msg::GearCommandconst*)                        : i=   0 Subscription n=vehicle_cmd_gate                         t=/system/emergency/gear_cmd    \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 151 Subscription n=behavior_velocity_planner                t=/parameter_events             \n",
      "void (rclcpp::TimeSource::?)(rcl_interfaces::msg::ParameterEventconst*)                             : i= 147 Subscription n=transform_listener_impl_ffff24598628     t=/parameter_events             \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=6070 Subscription n=transform_listener_impl_ffff24598628     t=/tf                           \n",
      "void (tf2_ros::TransformListener::?)(tf2_msgs::msg::TFMessage*,bool)                                : i=   3 Subscription n=transform_listener_impl_ffff24598628     t=/tf_static                    \n",
      "void (behavior_velocity_planner::BehaviorVelocityPlannerNode::?)(autoware_auto_planning_msgs::msg...: i= 981 Subscription n=behavior_velocity_planner                t=/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id\n",
      "void (behavior_velocity_planner::BehaviorVelocityPlannerNode::?)(autoware_auto_perception_msgs::m...: i=1515 Subscription n=behavior_velocity_planner                t=/perception/object_recognition/objects\n",
      "void (behavior_velocity_planner::BehaviorVelocityPlannerNode::?)(sensor_msgs::msg::PointCloud2con...: i=1517 Subscription n=behavior_velocity_planner                t=/perception/obstacle_segmentation/pointcloud\n",
      "void (behavior_velocity_planner::BehaviorVelocityPlannerNode::?)(nav_msgs::msg::Odometryconst*)     : i=6069 Subscription n=behavior_velocity_planner                t=/localization/kinematic_state \n",
      "void (behavior_velocity_planner::BehaviorVelocityPlannerNode::?)(autoware_auto_mapping_msgs::msg:...: i=   1 Subscription n=behavior_velocity_planner                t=/map/vector_map               \n",
      "void (behavior_velocity_planner::BehaviorVelocityPlannerNode::?)(autoware_auto_perception_msgs::m...: i=   0 Subscription n=behavior_velocity_planner                t=/perception/traffic_light_recognition/traffic_signals\n",
      "void (behavior_velocity_planner::BehaviorVelocityPlannerNode::?)(tier4_api_msgs::msg::CrosswalkSt...: i=   0 Subscription n=behavior_velocity_planner                t=/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/input/external_crosswalk_states\n",
      "void (behavior_velocity_planner::BehaviorVelocityPlannerNode::?)(tier4_api_msgs::msg::Intersectio...: i=   0 Subscription n=behavior_velocity_planner                t=/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/input/external_intersection_states\n",
      "void (behavior_velocity_planner::BehaviorVelocityPlannerNode::?)(tier4_planning_msgs::msg::Veloci...: i=   0 Subscription n=behavior_velocity_planner                t=/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/input/external_velocity_limit_mps\n",
      "void (behavior_velocity_planner::BehaviorVelocityPlannerNode::?)(autoware_auto_perception_msgs::m...: i=   0 Subscription n=behavior_velocity_planner                t=/external/traffic_light_recognition/traffic_signals\n",
      "void (behavior_velocity_planner::BehaviorVelocityPlannerNode::?)(tier4_v2x_msgs::msg::VirtualTraf...: i=   0 Subscription n=behavior_velocity_planner                t=/awapi/tmp/virtual_traffic_light_states\n",
      "void (behavior_velocity_planner::BehaviorVelocityPlannerNode::?)(nav_msgs::msg::OccupancyGridconst*): i=1515 Subscription n=behavior_velocity_planner                t=/perception/occupancy_grid_map/map\n",
      "void (AutowareErrorMonitor::?)()                                                                    : i=1783 Timer        n=system_error_monitor                     p=100 ms \n",
      "void (EmergencyHandler::?)()                                                                        : i=1782 Timer        n=emergency_handler                        p=100 ms \n",
      "void (diagnostic_updater::Updater::?)()                                                             : i= 178 Timer        n=ad_service_state_monitor                 p=1000 ms\n",
      "void (ScenarioSelectorNode::?)()                                                                    : i=1672 Timer        n=scenario_selector                        p=100 ms \n",
      "void (diagnostic_updater::Updater::?)()                                                             : i= 178 Timer        n=planning_error_monitor                   p=1000 ms\n",
      "void (planning_diagnostics::PlanningErrorMonitorNode::?)()                                          : i=1781 Timer        n=planning_error_monitor                   p=100 ms \n",
      "void (diagnostic_updater::Updater::?)()                                                             : i= 174 Timer        n=cpu_monitor                              p=1000 ms\n",
      "void (diagnostic_updater::Updater::?)()                                                             : i= 178 Timer        n=hdd_monitor                              p=1000 ms\n",
      "void (autoware::motion::control::trajectory_follower_nodes::LateralController::?)()                 : i=5433 Timer        n=lateral_controller_node_exe              p=30 ms  \n",
      "void (HDDMonitor::?)()                                                                              : i= 178 Timer        n=hdd_monitor                              p=1000 ms\n",
      "void (diagnostic_updater::Updater::?)()                                                             : i= 178 Timer        n=mem_monitor                              p=1000 ms\n",
      "void (diagnostic_aggregator::Aggregator::?)()                                                       : i=1780 Timer        n=aggregator_node                          p=100 ms \n",
      "void (diagnostic_updater::Updater::?)()                                                             : i= 178 Timer        n=net_monitor                              p=1000 ms\n",
      "void (diagnostic_updater::Updater::?)()                                                             : i= 177 Timer        n=ntp_monitor                              p=1000 ms\n",
      "void (diagnostic_updater::Updater::?)()                                                             : i= 121 Timer        n=process_monitor                          p=1000 ms\n",
      "void (autoware::motion::control::trajectory_follower_nodes::LongitudinalController::?)()            : i=5427 Timer        n=longitudinal_controller_node_exe         p=30 ms  \n",
      "void (diagnostic_updater::Updater::?)()                                                             : i= 177 Timer        n=gpu_monitor                              p=1000 ms\n",
      "void (diagnostic_updater::Updater::?)()                                                             : i= 163 Timer        n=external_cmd_selector                    p=1000 ms\n",
      "void (ExternalCmdSelector::?)()                                                                     : i=1628 Timer        n=external_cmd_selector                    p=100 ms \n",
      "void (diagnostic_updater::Updater::?)()                                                             : i= 163 Timer        n=external_cmd_converter                   p=1000 ms\n",
      "void (external_cmd_converter::ExternalCmdConverterNode::?)()                                        : i=1629 Timer        n=external_cmd_converter                   p=100 ms \n",
      "void (diagnostic_updater::Updater::?)()                                                             : i= 163 Timer        n=lane_departure_checker_node              p=1000 ms\n",
      "void (AutowareStateMonitorNode::?)()                                                                : i=1772 Timer        n=ad_service_state_monitor                 p=100 ms \n",
      "void (surround_obstacle_checker::SurroundObstacleCheckerNode::?)()                                  : i=1630 Timer        n=surround_obstacle_checker                p=100 ms \n",
      "void (CostmapGenerator::?)()                                                                        : i=1339 Timer        n=costmap_generator                        p=100 ms \n",
      "void (freespace_planner::FreespacePlannerNode::?)()                                                 : i=1332 Timer        n=freespace_planner                        p=100 ms \n",
      "void (lane_departure_checker::LaneDepartureCheckerNode::?)()                                        : i=1627 Timer        n=lane_departure_checker_node              p=100 ms \n",
      "void (ShiftDecider::?)()                                                                            : i=1627 Timer        n=shift_decider                            p=100 ms \n",
      "void (diagnostic_updater::Updater::?)()                                                             : i= 162 Timer        n=vehicle_cmd_gate                         p=1000 ms\n",
      "void (VehicleCmdGate::?)()                                                                          : i=1626 Timer        n=vehicle_cmd_gate                         p=100 ms \n",
      "void (behavior_path_planner::BehaviorPathPlannerNode::?)()                                          : i= 984 Timer        n=behavior_path_planner                    p=100 ms \n"
     ]
    }
   ],
   "source": [
    "import re\n",
    "\n",
    "sym_table = []\n",
    "\n",
    "for sym in callback_symbols.values():\n",
    "    try:\n",
    "        cbo = list(filter(lambda val: val.callback_object==sym.id, callback_objects.values()))\n",
    "        assert len(cbo) == 1\n",
    "        cbo = cbo[0]\n",
    "    except:\n",
    "        print(len(cbo))\n",
    "        continue\n",
    "    owner_info = cbo.owner_info\n",
    "\n",
    "    if None in owner_info: continue\n",
    "    type, info = owner_info\n",
    "    sym_table.append((sym, type, info))\n",
    "\n",
    "sym_table.sort(key=lambda tup: tup[1])\n",
    "\n",
    "def trim(string, length):\n",
    "    if len(string) > length:\n",
    "        return f\"{string[:length-3]}...\"\n",
    "    return string\n",
    "\n",
    "for sym, type, info in sym_table:\n",
    "    sym: CallbackSymbol\n",
    "    pretty_sym = util._prettify(sym.symbol)\n",
    "    pretty_sym = re.sub(r\"std::shared_ptr<(.*?)>\", r\"\\1*\", pretty_sym)\n",
    "    try:\n",
    "        i = len(sym.callback_obj.callback_instances)\n",
    "    except KeyError:\n",
    "        i = -1\n",
    "    print(f\"{trim(pretty_sym, 100):100s}: i={i:>4d} {type:12s} n={info['node']:40s}\", end=' ') \n",
    "    if type == 'Timer':\n",
    "        print(f\"p={info['period']:7s}\")\n",
    "    elif type == 'Subscription':\n",
    "        print(f\"t={info['topic']:30s}\")\n",
    "    else:\n",
    "        print()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Topic-Node Mapping"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "161 topics were aggregated into 78 cohorts\n"
     ]
    }
   ],
   "source": [
    "# Aggregate topics that have the same pubs and subs\n",
    "topic_cohorts = {}\n",
    "for topic in topics.values():\n",
    "    key = (frozenset({*(pub.node_handle for pub in topic.publishers)}), frozenset({*(sub.node_handle for sub in topic.subscriptions)}))\n",
    "    if key not in topic_cohorts:\n",
    "        topic_cohorts[key] = []\n",
    "    topic_cohorts[key].append(topic)\n",
    "\n",
    "print(f\"{len(topics)} topics were aggregated into {len(topic_cohorts)} cohorts\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Timer-Node Mapping"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "/system/system_error_monitor                                                              :    10.00Hz\n",
      "/system/emergency_handler                                                                 :    10.00Hz\n",
      "/system/ad_service_state_monitor                                                          :     1.00Hz\n",
      "/planning/scenario_planning/scenario_selector                                             :    10.00Hz\n",
      "/planning/planning_diagnostics/planning_error_monitor                                     :     1.00Hz\n",
      "/planning/planning_diagnostics/planning_error_monitor                                     :    10.00Hz\n",
      "/system/system_monitor/cpu_monitor                                                        :     1.00Hz\n",
      "/system/system_monitor/hdd_monitor                                                        :     1.00Hz\n",
      "/control/trajectory_follower/lateral_controller_node_exe                                  :    33.33Hz\n",
      "/system/system_monitor/hdd_monitor                                                        :     1.00Hz\n",
      "/system/system_monitor/mem_monitor                                                        :     1.00Hz\n",
      "/aggregator_node                                                                          :    10.00Hz\n",
      "/system/system_monitor/net_monitor                                                        :     1.00Hz\n",
      "/system/system_monitor/ntp_monitor                                                        :     1.00Hz\n",
      "/system/system_monitor/process_monitor                                                    :     1.00Hz\n",
      "/control/trajectory_follower/longitudinal_controller_node_exe                             :    33.33Hz\n",
      "/system/system_monitor/gpu_monitor                                                        :     1.00Hz\n",
      "/control/external_cmd_selector                                                            :     1.00Hz\n",
      "/control/external_cmd_selector                                                            :    10.00Hz\n",
      "/control/external_cmd_converter                                                           :     1.00Hz\n",
      "/control/external_cmd_converter                                                           :    10.00Hz\n",
      "/control/trajectory_follower/lane_departure_checker_node                                  :     1.00Hz\n",
      "/system/ad_service_state_monitor                                                          :    10.00Hz\n",
      "/planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker        :    10.00Hz\n",
      "/planning/scenario_planning/parking/costmap_generator                                     :    10.00Hz\n",
      "/planning/scenario_planning/parking/freespace_planner                                     :    10.00Hz\n",
      "/control/trajectory_follower/lane_departure_checker_node                                  :    10.00Hz\n",
      "/control/shift_decider                                                                    :    10.00Hz\n",
      "/control/vehicle_cmd_gate                                                                 :     1.00Hz\n",
      "/control/vehicle_cmd_gate                                                                 :    10.00Hz\n",
      "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner          :    10.00Hz\n",
      "UNKNOWN (x34)                                                                             :     1.00Hz\n",
      "UNKNOWN (x1)                                                                              :     2.00Hz\n",
      "UNKNOWN (x1)                                                                              :    10.00Hz\n",
      "Found 31 timers with a recorded node, 36 without.\n"
     ]
    }
   ],
   "source": [
    "unknowns = {}\n",
    "\n",
    "print_node_timer = lambda node_path, period: print(f\"{node_path:<90s}: {1/(period*1e-9):8.2f}Hz\")\n",
    "\n",
    "for timer in timers.values():\n",
    "    timer_nodes = timer.nodes\n",
    "    if not timer_nodes:\n",
    "        if timer.period not in unknowns:\n",
    "            unknowns[timer.period] = 0\n",
    "        unknowns[timer.period] += 1\n",
    "\n",
    "    for node in timer_nodes: print_node_timer(node.path, timer.period)\n",
    "        \n",
    "for period, count in unknowns.items():\n",
    "    print_node_timer(f\"UNKNOWN (x{count})\", period)\n",
    "\n",
    "n_unknown = sum(unknowns.values())  # Values are counts per period\n",
    "print(f\"Found {len(timers) - n_unknown} timers with a recorded node, {n_unknown} without.\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Measure Frequency Deviations"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "219 unknown publisher handles (176 known ones)\n"
     ]
    }
   ],
   "source": [
    "# Get Publisher frequencies\n",
    "df_publications = handler.data.rcl_publish_instances\n",
    "pub_stats = {}\n",
    "unknown = 0\n",
    "for pi in publish_instances:\n",
    "    try:\n",
    "        pub = pi.publisher\n",
    "    except KeyError:\n",
    "        unknown += 1\n",
    "        continue\n",
    "    if pub.id not in pub_stats:\n",
    "        pub_stats[pub.id] = {'times': []}\n",
    "    pub_stats[pub.id]['times'].append(pi.timestamp*1e-9) # Nanoseconds to seconds float\n",
    "\n",
    "print(f\"{unknown} unknown publisher handles ({len(pub_stats)} known ones)\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Plot Frequency Deviations"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "fig_dirname = \"fig_frequency\"\n",
    "os.makedirs(fig_dirname, exist_ok=True)\n",
    "for i, (k, v) in enumerate(sorted(pub_stats.items(), key=lambda kv: len(kv[1]['times']), reverse=True)):\n",
    "    pub_time_diff = np.diff(np.array(v['times']))\n",
    "    v['period'] = pub_time_diff.mean()\n",
    "    v['period_std'] = pub_time_diff.std()\n",
    "    v['frequency'] = 1 / v['period']\n",
    "    v['frequency_std'] = (1/pub_time_diff).std()\n",
    "\n",
    "    try:\n",
    "        publisher = publishers[k]\n",
    "        publisher_node = publisher.node\n",
    "        topic_name = publisher.topic_name\n",
    "        node_path = publisher_node.path\n",
    "    except Exception:\n",
    "        topic_name=\"UNKNOWN\"\n",
    "        node_path=\"UNKNOWN\"\n",
    "    \n",
    "    fig = plt.figure(figsize=(15,5))\n",
    "    ax = fig.add_subplot()\n",
    "    ax.hist(1/pub_time_diff)\n",
    "    ax.set_xlabel(\"Publication Frequency [Hz]\")\n",
    "    ax.set_ylabel(\"#Publications\")\n",
    "    ax.set_title(f\"{node_path} =({v['frequency']:.2f}Hz)=> {topic_name}\")\n",
    "    plt.savefig(os.path.join(fig_dirname, f\"{i:06}{node_path}__{topic_name}\".replace('/','-')))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Data Flow Graph"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "node_filters = [\"transform_listener_impl\", \"_monitor\"]\n",
    "topic_filters = [\"/rosout\", \"/parameter_events\", \"/diagnostics\"]\n",
    "\n",
    "from pyvis.network import Network\n",
    "net = Network(notebook=True, height='750px', width='100%', bgcolor='#ffffff', font_color='#000000')\n",
    "\n",
    "net.add_node(\"INPUT\", label=\"Input\", size=100, color=\"green\", physics=False, x=0, y=0)\n",
    "net.add_node(\"OUTPUT\", label=\"Output\", size=100, color=\"red\", physics=False, x=6000, y=0)\n",
    "\n",
    "\n",
    "for node in nodes.values():\n",
    "    if any(f in node.path for f in node_filters): \n",
    "        continue\n",
    "    net.add_node(node.id, label=node.name, title=node.path, size=20, color=\"#333\")\n",
    "\n",
    "for cohort_key, cohort_topics in topic_cohorts.items():\n",
    "    cohort_topic_names = [topic.name for topic in cohort_topics if not any(f in topic.name for f in topic_filters)]\n",
    "    if not cohort_topic_names: \n",
    "        continue\n",
    "    cohort_id=\"\\n\".join(cohort_topic_names)\n",
    "    cohort_weight=len(cohort_topic_names)\n",
    "    net.add_node(cohort_id, label=\" \", title=cohort_id, size=5, color=\"#333\")\n",
    "    \n",
    "    pubs = cohort_key[0]\n",
    "    subs = cohort_key[1]\n",
    "    n_pubs = len(pubs)\n",
    "    n_subs = len(subs)\n",
    "    \n",
    "    try:\n",
    "        if not n_pubs:\n",
    "            net.add_edge(\"INPUT\", cohort_id, arrows=\"to\", color=\"green\", weight=cohort_weight)\n",
    "        if not n_subs:\n",
    "            net.add_edge(cohort_id, \"OUTPUT\", arrows=\"to\", color=\"red\", weight=cohort_weight)\n",
    "\n",
    "        for pub in pubs:\n",
    "            net.add_edge(pub, cohort_id, arrows=\"to\", color=\"green\", weight=cohort_weight)\n",
    "        for sub in subs:\n",
    "            net.add_edge(cohort_id, sub, arrows=\"to\", color=\"red\", weight=cohort_weight)\n",
    "    except:\n",
    "        continue\n",
    "\n",
    "net.toggle_physics(True)\n",
    "net.show_buttons()\n",
    "net.show(\"graph.html\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Pub-Use Latencies\n",
    "Compute for each node and its data dependencies the list of pub-use delays (per-topic-per-node list of pub-use delays)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 107,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "/launch_ros_9807                                                                                has 0 timer callbacks,  0 subscription callbacks,  0 subscribed topics.\n",
      "/system/system_monitor/system_monitor/system_monitor_container                                  has 0 timer callbacks,  1 subscription callbacks,  1 subscribed topics.\n",
      "/robot_state_publisher                                                                          has 0 timer callbacks,  2 subscription callbacks,  2 subscribed topics.\n",
      "/system/system_error_monitor                                                                    has 1 timer callbacks,  5 subscription callbacks,  5 subscribed topics.\n",
      "/aggregator_node                                                                                has 1 timer callbacks,  2 subscription callbacks,  2 subscribed topics.\n",
      "/system/emergency_handler                                                                       has 1 timer callbacks,  6 subscription callbacks,  5 subscribed topics.\n",
      "/map/map_container                                                                              has 0 timer callbacks,  1 subscription callbacks,  1 subscribed topics.\n",
      "/system/ad_service_state_monitor                                                                has 2 timer callbacks,  5 subscription callbacks, 15 subscribed topics.\n",
      "/system/transform_listener_impl_aaaab90b38b0                                                    has 0 timer callbacks,  3 subscription callbacks,  3 subscribed topics.\n",
      "/planning/mission_planning/mission_planning_container                                           has 0 timer callbacks,  1 subscription callbacks,  1 subscribed topics.\n",
      "/planning/scenario_planning/scenario_selector                                                   has 1 timer callbacks,  6 subscription callbacks,  6 subscribed topics.\n",
      "/planning/scenario_planning/external_velocity_limit_selector                                    has 0 timer callbacks,  4 subscription callbacks,  4 subscribed topics.\n",
      "/planning/scenario_planning/transform_listener_impl_aaaad6cbbcb8                                has 0 timer callbacks,  3 subscription callbacks,  3 subscribed topics.\n",
      "/planning/scenario_planning/motion_velocity_smoother                                            has 0 timer callbacks,  4 subscription callbacks,  4 subscribed topics.\n",
      "/planning/scenario_planning/transform_listener_impl_aaaaf1071f80                                has 0 timer callbacks,  3 subscription callbacks,  3 subscribed topics.\n",
      "/planning/scenario_planning/lane_driving/behavior_planning/behavior_planning_container          has 0 timer callbacks,  1 subscription callbacks,  1 subscribed topics.\n",
      "/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container              has 0 timer callbacks,  1 subscription callbacks,  1 subscribed topics.\n",
      "/planning/scenario_planning/parking/parking_container                                           has 0 timer callbacks,  1 subscription callbacks,  1 subscribed topics.\n",
      "/planning/planning_diagnostics/planning_error_monitor                                           has 2 timer callbacks,  2 subscription callbacks,  2 subscribed topics.\n",
      "/control/control_container                                                                      has 0 timer callbacks,  1 subscription callbacks,  1 subscribed topics.\n",
      "/system/system_monitor/cpu_monitor                                                              has 1 timer callbacks,  1 subscription callbacks,  1 subscribed topics.\n",
      "/control/trajectory_follower/lateral_controller_node_exe                                        has 1 timer callbacks,  4 subscription callbacks,  4 subscribed topics.\n",
      "/system/system_monitor/hdd_monitor                                                              has 2 timer callbacks,  1 subscription callbacks,  1 subscribed topics.\n",
      "/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner             has 0 timer callbacks,  5 subscription callbacks,  5 subscribed topics.\n",
      "/planning/scenario_planning/lane_driving/motion_planning/transform_listener_impl_aaaaf153e160   has 0 timer callbacks,  3 subscription callbacks,  3 subscribed topics.\n",
      "/control/transform_listener_impl_aaaaf51e51c0                                                   has 0 timer callbacks,  3 subscription callbacks,  3 subscribed topics.\n",
      "/map/map_hash_generator                                                                         has 0 timer callbacks,  0 subscription callbacks,  0 subscribed topics.\n",
      "/system/system_monitor/mem_monitor                                                              has 1 timer callbacks,  1 subscription callbacks,  1 subscribed topics.\n",
      "/map/lanelet2_map_loader                                                                        has 0 timer callbacks,  1 subscription callbacks,  1 subscribed topics.\n",
      "/system/system_monitor/net_monitor                                                              has 1 timer callbacks,  1 subscription callbacks,  1 subscribed topics.\n",
      "/map/lanelet2_map_visualization                                                                 has 0 timer callbacks,  2 subscription callbacks,  2 subscribed topics.\n",
      "/system/system_monitor/ntp_monitor                                                              has 1 timer callbacks,  1 subscription callbacks,  1 subscribed topics.\n",
      "/planning/mission_planning/mission_planner                                                      has 0 timer callbacks,  4 subscription callbacks,  4 subscribed topics.\n",
      "/system/system_monitor/process_monitor                                                          has 1 timer callbacks,  1 subscription callbacks,  1 subscribed topics.\n",
      "/planning/mission_planning/transform_listener_impl_aaab0bafc440                                 has 0 timer callbacks,  3 subscription callbacks,  3 subscribed topics.\n",
      "/control/trajectory_follower/longitudinal_controller_node_exe                                   has 1 timer callbacks,  3 subscription callbacks,  3 subscribed topics.\n",
      "/planning/mission_planning/goal_pose_visualizer                                                 has 0 timer callbacks,  2 subscription callbacks,  2 subscribed topics.\n",
      "/system/system_monitor/gpu_monitor                                                              has 1 timer callbacks,  1 subscription callbacks,  1 subscribed topics.\n",
      "/control/transform_listener_impl_aaaaf52d4c30                                                   has 0 timer callbacks,  3 subscription callbacks,  3 subscribed topics.\n",
      "/map/pointcloud_map_loader                                                                      has 0 timer callbacks,  1 subscription callbacks,  1 subscribed topics.\n",
      "/control/external_cmd_selector                                                                  has 2 timer callbacks,  9 subscription callbacks,  9 subscribed topics.\n",
      "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner                has 1 timer callbacks,  9 subscription callbacks,  9 subscribed topics.\n",
      "/planning/scenario_planning/lane_driving/behavior_planning/transform_listener_impl_ffff200c66b0 has 0 timer callbacks,  3 subscription callbacks,  3 subscribed topics.\n",
      "/control/external_cmd_converter                                                                 has 2 timer callbacks,  6 subscription callbacks,  6 subscribed topics.\n",
      "/planning/scenario_planning/parking/costmap_generator                                           has 1 timer callbacks,  5 subscription callbacks,  5 subscribed topics.\n",
      "/planning/scenario_planning/parking/transform_listener_impl_aaaaf521b848                        has 0 timer callbacks,  3 subscription callbacks,  3 subscribed topics.\n",
      "/control/trajectory_follower/latlon_muxer_node_exe                                              has 0 timer callbacks,  3 subscription callbacks,  3 subscribed topics.\n",
      "/control/trajectory_follower/lane_departure_checker_node                                        has 2 timer callbacks,  6 subscription callbacks,  6 subscribed topics.\n",
      "/control/transform_listener_impl_aaaaf55ae730                                                   has 0 timer callbacks,  3 subscription callbacks,  3 subscribed topics.\n",
      "/map/map_tf_generator                                                                           has 0 timer callbacks,  2 subscription callbacks,  2 subscribed topics.\n",
      "/_ros2cli_9870                                                                                  has 0 timer callbacks,  0 subscription callbacks,  0 subscribed topics.\n",
      "/planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker              has 1 timer callbacks,  4 subscription callbacks,  4 subscribed topics.\n",
      "/planning/scenario_planning/lane_driving/motion_planning/transform_listener_impl_ffff5c00c708   has 0 timer callbacks,  3 subscription callbacks,  3 subscribed topics.\n",
      "/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner                  has 0 timer callbacks,  6 subscription callbacks,  6 subscribed topics.\n",
      "/planning/scenario_planning/lane_driving/motion_planning/transform_listener_impl_ffff3400eba0   has 0 timer callbacks,  3 subscription callbacks,  3 subscribed topics.\n",
      "/planning/scenario_planning/parking/freespace_planner                                           has 1 timer callbacks,  5 subscription callbacks,  5 subscribed topics.\n",
      "/planning/scenario_planning/parking/transform_listener_impl_aaaaf5695dc0                        has 0 timer callbacks,  3 subscription callbacks,  3 subscribed topics.\n",
      "/control/shift_decider                                                                          has 1 timer callbacks,  2 subscription callbacks,  2 subscribed topics.\n",
      "/control/vehicle_cmd_gate                                                                       has 2 timer callbacks, 17 subscription callbacks, 17 subscribed topics.\n",
      "/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner            has 0 timer callbacks, 13 subscription callbacks, 13 subscribed topics.\n",
      "/planning/scenario_planning/lane_driving/behavior_planning/transform_listener_impl_ffff24598628 has 0 timer callbacks,  3 subscription callbacks,  3 subscribed topics.\n"
     ]
    }
   ],
   "source": [
    "def filter_none(ls):\n",
    "    return filter(lambda x: x is not None, ls)\n",
    "\n",
    "def safe_map(func, ls):\n",
    "    def safe_func(arg):\n",
    "        try:\n",
    "            return func(arg)\n",
    "        except:\n",
    "            return None\n",
    "    \n",
    "    return map(safe_func, ls)\n",
    "\n",
    "pub_use_delays = {node.id: {\n",
    "    'pubs': {}, \n",
    "    'invocations': {}, \n",
    "    'n_unknown_invocations': 0, \n",
    "    'n_pub_timestamps': 0\n",
    "    } for node in nodes.values()}\n",
    "\n",
    "for node in nodes.values():\n",
    "    node_pub_use_dict = pub_use_delays[node.id]\n",
    "    timestamp_min = np.inf; timestamp_max = 0\n",
    "\n",
    "    n_pub_timestamps = 0\n",
    "    for sub in node.subscriptions:\n",
    "        node_pub_use_dict['pubs'][sub.topic_name] = {}\n",
    "        for pub in sub.publishers:\n",
    "            pub_timestamps = [inst.timestamp for inst in pub.instances]\n",
    "\n",
    "            try:\n",
    "                pub_t_min = min(pub_timestamps); pub_t_max = max(pub_timestamps)\n",
    "            except ValueError:\n",
    "                pub_t_min = np.inf; pub_t_max = 0\n",
    "            \n",
    "            if pub_t_min < timestamp_min: timestamp_min = pub_t_min\n",
    "            if pub_t_max > timestamp_max: timestamp_max = pub_t_max\n",
    "\n",
    "            node_pub_use_dict['pubs'][sub.topic_name][pub.node.path] = pub_timestamps\n",
    "            node_pub_use_dict['n_pub_timestamps'] += len(pub_timestamps)\n",
    "\n",
    "    timer_cb_objs = list(filter_none(safe_map(lambda timer: timer.callback_object, node.timers)))\n",
    "    subsc_cb_objs = list(filter_none(safe_map(lambda subsc: subsc.subscription_object.callback_object, node.subscriptions)))\n",
    "\n",
    "    print(f\"{node.path:95s} has {len(timer_cb_objs):1d} timer callbacks, {len(subsc_cb_objs):2d} subscription callbacks, {len(node_pub_use_dict['pubs']):2d} subscribed topics.\")\n",
    "\n",
    "    node_invocations = node_pub_use_dict['invocations']\n",
    "\n",
    "    for cb_obj in timer_cb_objs + subsc_cb_objs:\n",
    "        cb_invocations = []\n",
    "        for inst in cb_obj.callback_instances:\n",
    "            cb_invocations.append((inst.timestamp, inst.duration))\n",
    "\n",
    "        node_invocations[cb_obj.id] = cb_invocations"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 117,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Ignoring 0 nodes due to filters.\n",
      "/launch_ros_9807                                                                                has no invocations, skipping.\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "<ipython-input-117-38f5f659767b>:26: RuntimeWarning: More than 20 figures have been opened. Figures created through the pyplot interface (`matplotlib.pyplot.figure`) are retained until explicitly closed and may consume too much memory. (To control this warning, see the rcParam `figure.max_open_warning`).\n",
      "  fig = plt.figure(figsize=(15,5))\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "/map/map_hash_generator                                                                         has no invocations, skipping.\n",
      "/_ros2cli_9870                                                                                  has no invocations, skipping.\n"
     ]
    },
    {
     "data": {
      "image/png": "[encoded data, 20584 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 23784 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 50104 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 31348 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 63400 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 17964 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 80060 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 29608 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 19612 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 59984 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 46332 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 30780 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 47852 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 31104 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 21668 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 21072 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 20372 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 37008 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 18268 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 22544 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 53364 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 24620 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 52588 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 31864 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 29492 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 22200 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 18524 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 22216 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 25972 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 21848 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 40004 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 22640 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 30844 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 46084 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 28268 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 22400 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 29772 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 18500 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 88284 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 101964 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 32100 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 68256 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 52492 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 31476 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 44384 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 71464 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 29748 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 23892 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 53300 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 32220 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 68088 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 32184 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 57552 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 31320 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 32296 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 141152 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 137504 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "data": {
      "image/png": "[encoded data, 32416 characters]",
      "text/plain": [
       "<Figure size 1080x360 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "from matplotlib import cm\n",
    "\n",
    "fig_dirname = \"fig_pub_use\"\n",
    "os.makedirs(fig_dirname, exist_ok=True)\n",
    "plt.close('all')\n",
    "\n",
    "node_filters=[]#\"transform_listener_impl\",]\n",
    "\n",
    "nodes_filtered = [node for node in nodes.values() if not any(f in node.path for f in node_filters)]\n",
    "print(f\"Ignoring {len(nodes.values()) - len(nodes_filtered)} nodes due to filters.\")\n",
    "\n",
    "common_offset = min(map(lambda cb_inst: cb_inst.timestamp.timestamp(), callback_instances))\n",
    "\n",
    "zero_color = cm.get_cmap('viridis')(0.0)\n",
    "\n",
    "for node_i, (node, node_path, node_pub_use_dict) in enumerate(map(lambda node: (node, node.path, pub_use_delays[node.id]), nodes_filtered)):\n",
    "\n",
    "    if not node_pub_use_dict['invocations']:\n",
    "        print(f\"{node_path:95s} has no invocations, skipping.\")\n",
    "        continue\n",
    "\n",
    "    if len(node_pub_use_dict['pubs']) == 0:\n",
    "        print(f\"Skipping {node_path}, no publications\")\n",
    "        continue\n",
    "\n",
    "    fig = plt.figure(figsize=(15,5))\n",
    "    ax: plt.Axes = fig.add_subplot()\n",
    "\n",
    "    max_pubs_per_topic = max(len(pubs) for pubs in node_pub_use_dict['pubs'].values())\n",
    "    topic_names, topic_pubs = (zip(*node_pub_use_dict['pubs'].items()))\n",
    "\n",
    "    vmin = 0; vmax = max_pubs_per_topic\n",
    "\n",
    "    y_labels = []\n",
    "    current_y = 0\n",
    "\n",
    "    for invoc_i, (cb_obj_id, cb_invocations) in enumerate(node_pub_use_dict['invocations'].items()):\n",
    "        try:\n",
    "            cb_obj = callback_objects[cb_obj_id]\n",
    "            sym = callback_symbols[cb_obj.callback_object].symbol\n",
    "            sym = util._prettify(sym)\n",
    "            sym = re.sub(r\"std::shared_ptr<(.*?)>\", r\"\\1*\", sym)\n",
    "\n",
    "            cb_owner = cb_obj.owner\n",
    "            if isinstance(cb_owner, Timer):\n",
    "                cb_type = \"T\"\n",
    "            elif isinstance(cb_owner, SubscriptionObject):\n",
    "                cb_type = \"S\"\n",
    "        except KeyError or AttributeError:\n",
    "            sym = \"UNKNOWN\"\n",
    "            cb_type = \"U\"\n",
    "        \n",
    "        y_labels.append(f\"{sym} {cb_type}\")\n",
    "        n_markers = len(cb_invocations)\n",
    "\n",
    "        points_x = []; points_y = []\n",
    "        for time, dur in cb_invocations:\n",
    "            time = time.timestamp() - common_offset; dur = dur.total_seconds()\n",
    "            points_x += [time, time+dur, None]\n",
    "            points_y += [current_y, current_y, 0.0]\n",
    "        \n",
    "        ax.plot(points_x,points_y, marker='.', c=zero_color)\n",
    "        current_y += 1\n",
    "\n",
    "    n_cbs = current_y\n",
    "\n",
    "    for topic_i, (topic_name, pubs) in enumerate(zip(topic_names, topic_pubs)):\n",
    "        for pub_i, (pub_name, timestamps) in enumerate(pubs.items()):\n",
    "            n_markers = len(timestamps)\n",
    "            ax.scatter(np.array(timestamps)*1e-9 - common_offset, (current_y,) * n_markers, marker='.', c=(pub_i,) * n_markers, vmin=vmin, vmax=vmax)\n",
    "        \n",
    "        y_labels.append(topic_name)\n",
    "        current_y += 1\n",
    "    \n",
    "    trigger_strs = []\n",
    "    t = node.timers\n",
    "    if t:\n",
    "        n_timers = len(t)\n",
    "        freqs = map(lambda timer: 1 / (timer.period*1e-9), t)\n",
    "        trigger_strs.append(f\"{n_timers} timer{'s' if n_timers != 1 else ''}, {'Hz, '.join((f'{freq:.0f}' for freq in freqs))}Hz\")\n",
    "    if node.subscriptions:\n",
    "        n_subs = len(node.subscriptions)\n",
    "        trigger_strs.append(f\"{n_subs} subscription{'s' if n_subs != 1 else ''}\")\n",
    "\n",
    "    ax.set_xlabel(\"Publication / Invocation Timestamp [s]\")\n",
    "    ax.set_ylabel(\"Topic\")\n",
    "    ax.set_yticks(range(current_y))\n",
    "    ax.set_yticklabels(y_labels)\n",
    "    ax.set_ylim(0 - .1, current_y - 1 + .1)\n",
    "    ax.set_title(f\"{node_path} ({'; '.join(trigger_strs)})\")\n",
    "    ax.set_xlim(50, 50.25)\n",
    "\n",
    "    ax.hlines(n_cbs - 0.5, *ax.get_xlim(), linestyles='dashed')\n",
    "    plt.savefig(os.path.join(fig_dirname, f\"{node_i:06}{node_path}\".replace('/','-')))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "916dbcbb3f70747c44a77c7bcd40155683ae19c65e1c03b4aa3499c5328201f1"
  },
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
