dataflow-analysis/trace-analysis.ipynb

{
 "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\n",
    "\n",
    "from dataclasses import dataclass\n",
    "from typing import List, Dict, Set, Union, Tuple\n",
    "from functools import cached_property\n",
    "import pickle\n",
    "import re\n",
    "\n",
    "from utils import ProgressPrinter"
   ]
  },
  {
   "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",
      " [100%] [Ros2Handler]\n"
     ]
    }
   ],
   "source": [
    "def pkl_filename_from_file_timestamp(file_path):\n",
    "    if os.path.exists(file_path):\n",
    "        timestamp = os.path.getmtime(file_path)\n",
    "        pkl_filename = f\"ros_objects_{hash(timestamp)}.pkl\"\n",
    "        return pkl_filename, os.path.exists(pkl_filename)\n",
    "    return None, False\n",
    "\n",
    "path = os.path.expanduser(\"~/.ros/tracing/autoware-trace/\")\n",
    "path_converted = os.path.join(path, 'converted')\n",
    "pkl_filename, pkl_exists = pkl_filename_from_file_timestamp(path_converted)\n",
    "\n",
    "if not pkl_exists:\n",
    "    file = load_file(path)\n",
    "    handler = Ros2Handler.process(file)\n",
    "    util = Ros2DataModelUtil(handler)\n",
    "    pkl_filename, pkl_exists = pkl_filename_from_file_timestamp(path_converted)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "if False:\n",
    "    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": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "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",
    "    @cached_property\n",
    "    def path(self) -> str:\n",
    "        return os.path.join(self.namespace, self.name)\n",
    "\n",
    "    @cached_property\n",
    "    def publishers(self) -> List['Publisher']:\n",
    "        return list(filter(lambda pub: pub.node_handle == self.id, publishers.values()))\n",
    "\n",
    "    @cached_property\n",
    "    def subscriptions(self) -> List['Subscription']:\n",
    "        return list(filter(lambda sub: sub.node_handle == self.id, subscriptions.values()))\n",
    "    \n",
    "    @cached_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",
    "    def __hash__(self):\n",
    "        return hash(self.id)\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",
    "    @cached_property\n",
    "    def subscriptions(self) -> List['Subscription']:\n",
    "        return list(filter(lambda sub: sub.topic_name == self.topic_name, subscriptions.values()))\n",
    "\n",
    "    @cached_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",
    "    def __hash__(self):\n",
    "        return hash(self.id)\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",
    "    @cached_property\n",
    "    def publishers(self) -> List['Publisher']:\n",
    "        return list(filter(lambda pub: pub.topic_name == self.topic_name, publishers.values()))\n",
    "    \n",
    "    @cached_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",
    "    def __hash__(self):\n",
    "        return hash(self.id)\n",
    "    \n",
    "@dataclass\n",
    "class Timer:\n",
    "    id: int\n",
    "    timestamp: int\n",
    "    period: int\n",
    "    tid: int\n",
    "\n",
    "    @cached_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",
    "    def __hash__(self):\n",
    "        return hash(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",
    "    def __hash__(self):\n",
    "        return hash((self.id, self.timestamp, self.subscription_handle))\n",
    "\n",
    "@dataclass\n",
    "class CallbackObject:\n",
    "    id: int             # (reference) = subscription_object.id | timer.id | ....\n",
    "    timestamp: int\n",
    "    callback_object: int\n",
    "\n",
    "    @cached_property\n",
    "    def callback_instances(self) -> List['CallbackInstance']:\n",
    "        return list(filter(lambda inst: inst.callback_object == self.callback_object, callback_instances))\n",
    "\n",
    "    @cached_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",
    "    @cached_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",
    "    def __hash__(self):\n",
    "        return hash((self.id, self.timestamp, self.callback_object))\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",
    "    def __hash__(self):\n",
    "        return hash((self.publisher_handle, self.timestamp, self.message))\n",
    "\n",
    "@dataclass\n",
    "class CallbackInstance:\n",
    "    callback_object: int\n",
    "    timestamp: pd.Timestamp\n",
    "    duration: pd.Timedelta\n",
    "    intra_process: bool\n",
    "\n",
    "    @property\n",
    "    def callback_obj(self) -> 'CallbackObject':\n",
    "        return callback_objects[self.callback_object]\n",
    "\n",
    "    def __hash__(self):\n",
    "        return hash((self.callback_object, self.timestamp, self.duration))\n",
    "\n",
    "@dataclass\n",
    "class CallbackSymbol:\n",
    "    id: int  # callback_object\n",
    "    timestamp: int\n",
    "    symbol: str\n",
    "\n",
    "    @cached_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",
    "    def __hash__(self):\n",
    "        return hash((self.id, self.timestamp, self.symbol))\n",
    "\n",
    "\n",
    "#######################################\n",
    "# Self-defined (not from ROS2DataModel)\n",
    "#######################################\n",
    "\n",
    "@dataclass\n",
    "class Topic:\n",
    "    name: str\n",
    "\n",
    "    @cached_property\n",
    "    def publishers(self) -> List['Publisher']:\n",
    "        return list(filter(lambda pub: pub.topic_name == self.name, publishers.values()))\n",
    "    \n",
    "    @cached_property\n",
    "    def subscriptions(self) -> List['Subscription']:\n",
    "        return list(filter(lambda sub: sub.topic_name == self.name, subscriptions.values()))\n",
    "\n",
    "    def __hash__(self):\n",
    "        return hash(self.name)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Did not find pickled ROS objects, extracting...\n",
      "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",
      "Caching dynamic properties...\n",
      "Cached node properties\n",
      "Cached publisher properties\n",
      "Cached subscription properties\n",
      "Cached timer properties\n",
      "Cached callback object properties\n",
      "Cached callback symbol properties\n",
      "Cached topic properties\n",
      "Pickling...\n",
      "Done.\n"
     ]
    }
   ],
   "source": [
    "\n",
    "if not pkl_exists:\n",
    "    print(\"Did not find pickled ROS objects, extracting...\")\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\")\n",
    "\n",
    "    print(\"Caching dynamic properties...\")\n",
    "\n",
    "    [(o.path, o.publishers, o.subscriptions, o.timers) for o in nodes.values()]         ; print(\"Cached node properties\")\n",
    "    [(o.instances, o.subscriptions) for o in publishers.values()]                       ; print(\"Cached publisher properties\")\n",
    "    [(o.publishers, o.subscription_object) for o in subscriptions.values()]             ; print(\"Cached subscription properties\")\n",
    "    [(o.nodes) for o in timers.values()]                                                ; print(\"Cached timer properties\")\n",
    "    [(o.callback_instances, o.owner, o.owner_info) for o in callback_objects.values()]  ; print(\"Cached callback object properties\")\n",
    "    [(o.callback_obj) for o in callback_symbols.values()]                               ; print(\"Cached callback symbol properties\")\n",
    "    [(o.publishers, o.subscriptions) for o in topics.values()]                          ; print(\"Cached topic properties\")\n",
    "\n",
    "    fields_to_pickle = [\n",
    "        \"nodes\",\n",
    "        \"publishers\",\n",
    "        \"subscriptions\",\n",
    "        \"timers\",\n",
    "        \"timer_node_links\",\n",
    "        \"subscription_objects\",\n",
    "        \"callback_objects\",\n",
    "        \"callback_symbols\",\n",
    "        \"publish_instances\",\n",
    "        \"callback_instances\",\n",
    "        \"topics\"\n",
    "    ]\n",
    "\n",
    "    pkl_dict = {key: globals()[key] for key in fields_to_pickle}\n",
    "\n",
    "    print(\"Pickling...\")\n",
    "    with open(pkl_filename, \"wb\") as f:\n",
    "        pickle.dump(pkl_dict, f)\n",
    "else:\n",
    "    print(\"Found pickled ROS objects from previous session, restoring...\")\n",
    "    with open(pkl_filename, \"rb\") as f:\n",
    "        pkl_dict = pickle.load(f)\n",
    "        for k, v in pkl_dict.items():\n",
    "            globals()[k] = v\n",
    "\n",
    "print(\"Done.\")\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Callback-Sub & Callback-Timer Links"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'util' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-6-8d4109750ed4>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m     26\u001b[0m \u001b[0;32mfor\u001b[0m \u001b[0msym\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mtype\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0minfo\u001b[0m \u001b[0;32min\u001b[0m \u001b[0msym_table\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     27\u001b[0m     \u001b[0msym\u001b[0m\u001b[0;34m:\u001b[0m \u001b[0mCallbackSymbol\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 28\u001b[0;31m     \u001b[0mpretty_sym\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mutil\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0m_prettify\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0msym\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msymbol\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m     29\u001b[0m     \u001b[0mpretty_sym\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mre\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msub\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34mr\"std::shared_ptr<(.*?)>\"\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34mr\"\\1*\"\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mpretty_sym\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     30\u001b[0m     \u001b[0;32mtry\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mNameError\u001b[0m: name 'util' is not defined"
     ]
    }
   ],
   "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": 6,
   "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": null,
   "metadata": {},
   "outputs": [],
   "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 = Ros2DataModelUtil._prettify(None, 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": "markdown",
   "metadata": {},
   "source": [
    "# E2E Latency Calculation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(483/483) Processing done.                                                                                                                   annerNode*, std::_Placeholder<1>))(std::shared_ptr<nav_msgs::msg::OccupancyGrid_<std::allocator<void> > const>)>cator<void> > const>)>t>)>::shared_ptr<rmw_request_id_t>, std::shared_ptr<rcl_interfaces::srv::ListParameters_Request_<std::allocator<void> > >, std::shared_ptr<rcl_interfaces::srv::ListParameters_Response_<std::allocator<void> > >)#6} > >)#5}d> > >)#4}\n"
     ]
    }
   ],
   "source": [
    "#################################################\n",
    "# Data structures & helpers\n",
    "#################################################\n",
    "\n",
    "@dataclass\n",
    "class LatencyStats:\n",
    "    pub_use_latencies: np.ndarray\n",
    "\n",
    "@dataclass\n",
    "class LatencyGraph:\n",
    "    verts: Set[CallbackObject]\n",
    "    edges: Dict[Tuple[CallbackObject, CallbackObject], Tuple[Topic, LatencyStats]]\n",
    "    starts: Set[CallbackObject]\n",
    "    ends: Set[CallbackObject]\n",
    "\n",
    "def pub_use_latencies(cb_instances: List[CallbackInstance], pub_instances: List[PublishInstance]):\n",
    "    if not pub_instances:\n",
    "        return np.full(len(cb_instances), np.nan)\n",
    "\n",
    "    cb_times = sorted([inst.timestamp.timestamp() for inst in cb_instances])\n",
    "    pub_times = np.array(sorted([pub.timestamp * 1e-9 for pub in pub_instances]))\n",
    "\n",
    "    pub_use_lats = np.array([cb_time - np.max(pub_times[pub_times < cb_time], initial=-np.inf) for cb_time in cb_times])\n",
    "    pub_use_lats[np.isposinf(pub_use_lats)] = np.nan\n",
    "    return pub_use_lats\n",
    "\n",
    "#################################################\n",
    "# Identify input and output topics\n",
    "#################################################\n",
    "\n",
    "in_topics = [t for t in topics.values() if not t.publishers]\n",
    "out_topics = [t for t in topics.values() if not t.subscriptions]\n",
    "\n",
    "#################################################\n",
    "# For each node, work out dependencies and\n",
    "# publications of each callback\n",
    "#################################################\n",
    "\n",
    "cb_to_scored_topic: Dict[CallbackObject, Set[Tuple[Topic, float]]] = {}\n",
    "topic_to_cb: Dict[Topic, Set[CallbackObject]] = {}\n",
    "pub_cb_to_lat_stats: Dict[Tuple[Publisher, CallbackObject], LatencyStats] = {}\n",
    "\n",
    "with ProgressPrinter(\"Processing\", len(callback_objects)) as p:\n",
    "    for cb in callback_objects.values():\n",
    "        p.step(callback_symbols[cb.callback_object].symbol if cb.callback_object in callback_symbols else str(cb.id))\n",
    "        # Find topics the callback depends on (HEURISTICALLY!)\n",
    "        # - Timer callbacks: assume that the callback depends on every subscribed topic of the node\n",
    "        # - Subscription callbacks: assume that the callback only depends on the subscribed topic\n",
    "\n",
    "        if type(cb.owner).__name__ == Timer.__name__:\n",
    "            owner_nodes = cb.owner.nodes\n",
    "            if len(owner_nodes) != 1:\n",
    "                raise(ValueError(\"Timer has more than one owner!\"))\n",
    "            owner_node = owner_nodes[0]\n",
    "            dep_topics = [sub.topic for sub in owner_node.subscriptions]\n",
    "        elif type(cb.owner).__name__ == SubscriptionObject.__name__:\n",
    "            owner_node = cb.owner.subscription.node\n",
    "            dep_topics = [cb.owner.subscription.topic,]\n",
    "        elif cb.owner is None:\n",
    "            continue\n",
    "        else:\n",
    "            raise RuntimeError(f\"Callback owners other than timers/subscriptions cannot be handled: {cb.owner} {cb.owner_info}\")\n",
    "\n",
    "        for topic in dep_topics: \n",
    "            if topic not in topic_to_cb:\n",
    "                topic_to_cb[topic] = set()\n",
    "            topic_to_cb[topic].add(cb)\n",
    "\n",
    "            for pub in topic.publishers:\n",
    "                pub_cb_to_lat_stats[(pub, cb)] = LatencyStats(pub_use_latencies(cb.callback_instances, pub.instances))\n",
    "\n",
    "        # Find topics the callback publishes to (HEURISTICALLY!)\n",
    "        # For topics published to during the runtime of the callback's instances, \n",
    "        # assume that they are published by the callback\n",
    "\n",
    "        def inst_runtime_interval(cb_inst):\n",
    "            inst_t_min = cb_inst.timestamp.timestamp()\n",
    "            inst_t_max = inst_t_min + cb_inst.duration.total_seconds()\n",
    "            return (inst_t_min, inst_t_max)\n",
    "        \n",
    "        def filter_pub_insts_by_interval(cb_intervals: List[Tuple[float, float]], pub_insts: List[PublishInstance]):\n",
    "            \"\"\"\n",
    "            Counts number of publication instancess that lie within one of the cb_intervals.\n",
    "            \"\"\"\n",
    "            pub_timestamps = [inst.timestamp * 1e-9 for inst in pub_insts]\n",
    "            \n",
    "            # Algorithm: Two-pointer method\n",
    "            # With both the pub_timestamps and cb_intervals sorted ascending,\n",
    "            # we can cut down the O(m*n) comparisons to O(m+n).\n",
    "            pub_timestamps.sort()\n",
    "            cb_intervals.sort(key=lambda tup: tup[0])\n",
    "\n",
    "            n_overlaps = 0\n",
    "            cb_iter = iter(cb_intervals)\n",
    "            pub_iter = iter(pub_timestamps)\n",
    "            (t_min, t_max) = next(cb_iter, (None, None))\n",
    "            t_pub = next(pub_iter, None)\n",
    "\n",
    "            while t_pub is not None and t_min is not None:\n",
    "                if t_min <= t_pub <= t_max:  # If publication in interval, increase counter, go to next pub (multiple pubs can be within one interval)\n",
    "                    n_overlaps += 1\n",
    "                    t_pub = next(pub_iter, None)\n",
    "                elif t_pub < t_min:  # If publication before interval, increase pub\n",
    "                    t_pub = next(pub_iter, None)\n",
    "                else:  # If interval before publication, increase interval\n",
    "                    (t_min, t_max) = next(cb_iter, (None, None))\n",
    "\n",
    "            return n_overlaps\n",
    "\n",
    "        cb_runtime_intervals = [inst_runtime_interval(inst) for inst in cb.callback_instances]\n",
    "        cb_pub_overlap_counts = [filter_pub_insts_by_interval(cb_runtime_intervals, pub.instances) for pub in owner_node.publishers]\n",
    "\n",
    "        for pub, olap_count in zip(owner_node.publishers, cb_pub_overlap_counts):\n",
    "            if olap_count == 0 or not pub.instances:\n",
    "                continue\n",
    "            score = olap_count / len(pub.instances)\n",
    "\n",
    "            if cb not in cb_to_scored_topic: \n",
    "                cb_to_scored_topic[cb] = set()\n",
    "            cb_to_scored_topic[cb].add((pub.topic, score))\n",
    "\n",
    "#################################################\n",
    "# For each callback, compute pub-use latency \n",
    "# and runtime\n",
    "#################################################\n",
    "\n",
    "\n",
    "\n",
    "#################################################\n",
    "# Transitively add latencies to get E2E latency\n",
    "#################################################"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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
      "text/plain": [
       "<Figure size 1080x720 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "fig = plt.figure(figsize=(15, 10))\n",
    "ax = fig.add_subplot()\n",
    "\n",
    "for lat_stats in pub_cb_to_lat_stats.values():\n",
    "    ax.plot(np.where(np.isnan(lat_stats.pub_use_latencies), 0, lat_stats.pub_use_latencies))\n",
    "\n",
    "ax.set_ylim(4)"
   ]
  },
  {
   "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
}