{ "cells": [ { "cell_type": "code", "execution_count": null, "metadata": { "pycharm": { "name": "#%%\n" } }, "outputs": [], "source": [ "import glob\n", "import json\n", "import os\n", "import pickle\n", "import re\n", "import sys\n", "\n", "import numpy as np\n", "import pandas as pd\n", "from matplotlib import pyplot as plt\n", "\n", "from clang_interop.cl_types import ClContext\n", "from clang_interop.process_clang_output import process_clang_output\n", "\n", "sys.path.append(\"../autoware/build/tracetools_read/\")\n", "sys.path.append(\"../autoware/build/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 tracing_interop.tr_types import TrTimer, TrTopic, TrPublisher, TrPublishInstance, TrCallbackInstance, \\\n", "TrCallbackSymbol, TrCallbackObject, TrSubscriptionObject, TrContext\n", "from misc.utils import ProgressPrinter, cached\n", "\n", "%load_ext pyinstrument\n", "%matplotlib inline" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "pycharm": { "name": "#%%\n" } }, "outputs": [], "source": [ "TR_PATH = os.path.expanduser(\"data/awsim-trace/ust\")\n", "CL_PATH = os.path.expanduser(\"~/Projects/llvm-project/clang-tools-extra/ros2-internal-dependency-checker/output\")" ] }, { "cell_type": "markdown", "source": [ "# Organize Trace Data" ], "metadata": { "collapsed": false, "pycharm": { "name": "#%% md\n" } } }, { "cell_type": "code", "execution_count": null, "outputs": [], "source": [ "def _load_traces():\n", " file = load_file(TR_PATH)\n", " handler = Ros2Handler.process(file)\n", " return TrContext(handler)\n", "\n", "\n", "_tracing_context = cached(\"tr_objects\", _load_traces, [TR_PATH])\n", "_tr_globals = [\"nodes\", \"publishers\", \"subscriptions\", \"timers\", \"timer_node_links\", \"subscription_objects\",\n", " \"callback_objects\", \"callback_symbols\", \"publish_instances\", \"callback_instances\", \"topics\"]\n", "\n", "# Help the IDE recognize those identifiers\n", "nodes = publishers = subscriptions = timers = timer_node_links = subscription_objects = callback_objects = callback_symbols = publish_instances = callback_instances = topics = None\n", "\n", "for name in _tr_globals:\n", " globals()[name] = getattr(_tracing_context, name)\n", "\n", "print(\"Done.\")\n" ], "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } } }, { "cell_type": "markdown", "source": [ "# E2E Latency Calculation" ], "metadata": { "collapsed": false, "pycharm": { "name": "#%% md\n" } } }, { "cell_type": "code", "execution_count": null, "outputs": [], "source": [ "from latency_graph import latency_graph as lg\n", "\n", "lat_graph = lg.LatencyGraph(_tracing_context)\n", "\n", "import pickle\n", "\n", "with open(\"lat_graph.pkl\", \"wb\") as f:\n", " pickle.dump(lat_graph, f)\n", "#with open(\"lat_graph.pkl\", \"rb\") as f:\n", "# lat_graph = pickle.load(f)" ], "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } } }, { "cell_type": "code", "execution_count": null, "outputs": [], "source": [ "len(lat_graph.edges)" ], "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } } }, { "cell_type": "code", "execution_count": null, "outputs": [], "source": [ "from matching.subscriptions import sanitize\n", "from typing import Iterable, Sized\n", "from tracing_interop.tr_types import TrNode, TrCallbackObject, TrCallbackSymbol, TrSubscriptionObject\n", "\n", "#################################################\n", "# Plot DFG\n", "#################################################\n", "\n", "# Compare with: https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/node-diagram/\n", "node_colors = {\n", " \"sensing\": {\"fill\": \"#e1d5e7\", \"stroke\": \"#9673a6\"},\n", " \"localization\": {\"fill\": \"#dae8fc\", \"stroke\": \"#6c8ebf\"},\n", " \"perception\": {\"fill\": \"#d5e8d4\", \"stroke\": \"#82b366\"},\n", " \"planning\": {\"fill\": \"#fff2cc\", \"stroke\": \"#d6b656\"},\n", " \"control\": {\"fill\": \"#ffe6cc\", \"stroke\": \"#d79b00\"},\n", " \"system\": {\"fill\": \"#f8cecc\", \"stroke\": \"#b85450\"},\n", " \"vehicle_interface\": {\"fill\": \"#b0e3e6\", \"stroke\": \"#0e8088\"},\n", " None: {\"fill\": \"#f5f5f5\", \"stroke\": \"#666666\"}\n", "}\n", "\n", "node_namespace_mapping = {\n", " 'perception': 'perception',\n", " 'sensing': 'sensing',\n", " 'planning': 'planning',\n", " 'control': 'control',\n", " 'awapi': None,\n", " 'autoware_api': None,\n", " 'map': None,\n", " 'system': 'system',\n", " 'localization': 'localization',\n", " 'robot_state_publisher': None,\n", " 'aggregator_node': None,\n", " 'pointcloud_container': 'sensing',\n", "}\n", "\n", "import graphviz as gv\n", "\n", "g = gv.Digraph('G', filename=\"latency_graph.gv\",\n", " node_attr={'shape': 'plain'},\n", " graph_attr={'pack': '1'})\n", "g.graph_attr['rankdir'] = 'LR'\n", "\n", "def plot_hierarchy(gv_parent, lg_node: lg.LGHierarchyLevel, **subgraph_kwargs):\n", " if lg_node.name == \"[NONE]\":\n", " return\n", "\n", " print(f\"{' ' * lg_node.full_name.count('/')}Processing {lg_node.name}: {len(lg_node.callbacks)}\")\n", " with gv_parent.subgraph(name=f\"cluster_{lg_node.full_name.replace('/', '__')}\", **subgraph_kwargs) as c:\n", " c.attr(label=lg_node.name)\n", " for cb in lg_node.callbacks:\n", " if isinstance(cb, lg.LGTrCallback):\n", " tr_cb = cb.cb\n", " try:\n", " sym = _tracing_context.callback_symbols.by_id.get(tr_cb.callback_object)\n", " pretty_sym = repr(sanitize(sym.symbol))\n", " except KeyError:\n", " pretty_sym = cb.name\n", " except TypeError:\n", " pretty_sym = cb.name\n", " else:\n", " pretty_sym = cb.name\n", "\n", " pretty_sym = pretty_sym.replace(\"&\", \"&\").replace(\"<\", \"<\").replace(\">\", \">\")\n", "\n", " c.node(cb.id(),\n", " f'<
{pretty_sym}
>')\n", "\n", " for ch in lg_node.children:\n", " plot_hierarchy(c, ch, **subgraph_kwargs)\n", "\n", "def plot_lg(graph: lg.LatencyGraph):\n", " for top_level_node in graph.top_node.children:\n", " colors = node_colors[node_namespace_mapping.get(top_level_node.name)]\n", " plot_hierarchy(g, top_level_node, graph_attr={'bgcolor': colors[\"fill\"], 'pencolor': colors[\"stroke\"]})\n", "\n", " for edge in graph.edges:\n", " g.edge(f\"{edge.start.id()}:out\", f\"{edge.end.id()}:in\")\n", "\n", "plot_lg(lat_graph)\n", "\n", "g.save(\"latency_graph.gv\")\n", "g.render(\"latency_graph.svg\")\n", "\n", "g" ], "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } } }, { "cell_type": "code", "execution_count": null, "outputs": [], "source": [ "import re\n", "import math\n", "\n", "##################################################\n", "# Compute in/out topics for hierarchy level X\n", "##################################################\n", "\n", "HIER_LEVEL = 100\n", "\n", "input_node_patterns = [r\"^/sensing\"]\n", "output_node_patterns = [r\"^/awapi\", r\"^/control/external_cmd_converter\"]\n", "\n", "node_excluded_patterns = [r\"^/rviz2\", r\"transform_listener_impl\"]\n", "\n", "def get_nodes_on_level(lat_graph: lg.LatencyGraph):\n", " def _traverse_node(node: lg.LGHierarchyLevel, cur_lvl=0):\n", " if cur_lvl == HIER_LEVEL:\n", " return [node]\n", "\n", " if not node.children and cur_lvl < HIER_LEVEL:\n", " return [node]\n", "\n", " collected_nodes = []\n", " for ch in node.children:\n", " collected_nodes += _traverse_node(ch, cur_lvl + 1)\n", " return collected_nodes\n", "\n", " return _traverse_node(lat_graph.top_node)\n", "\n", "lvl_nodes = get_nodes_on_level(lat_graph)\n", "lvl_nodes = [n for n in lvl_nodes if not any(re.search(p, n.full_name) for p in node_excluded_patterns)]\n", "\n", "input_nodes = [n.full_name for n in lvl_nodes if any(re.search(p, n.full_name) for p in input_node_patterns)]\n", "output_nodes = [n.full_name for n in lvl_nodes if any(re.search(p, n.full_name) for p in output_node_patterns)]\n", "\n", "print(', '.join(map(lambda n: n, input_nodes)))\n", "print(', '.join(map(lambda n: n, output_nodes)))\n", "print(', '.join(map(lambda n: n.full_name, lvl_nodes)))\n", "\n", "def _collect_callbacks(n: lg.LGHierarchyLevel):\n", " callbacks = []\n", " callbacks += n.callbacks\n", " for ch in n.children:\n", " callbacks += _collect_callbacks(ch)\n", " return callbacks\n", "\n", "\n", "cb_to_node_map = {}\n", "for n in lvl_nodes:\n", " cbs = _collect_callbacks(n)\n", " for cb in cbs:\n", " cb_to_node_map[cb.id()] = n\n", "\n", "\n", "edges_between_nodes = {}\n", "for edge in lat_graph.edges:\n", " from_node = cb_to_node_map.get(edge.start.id())\n", " to_node = cb_to_node_map.get(edge.end.id())\n", "\n", " if from_node is None or to_node is None:\n", " continue\n", "\n", " if from_node.full_name == to_node.full_name:\n", " continue\n", "\n", " k = (from_node.full_name, to_node.full_name)\n", "\n", " if k not in edges_between_nodes:\n", " edges_between_nodes[k] = []\n", "\n", " edges_between_nodes[k].append(edge)\n", "\n", "g = gv.Digraph('G', filename=\"latency_graph.gv\",\n", " node_attr={'shape': 'plain'},\n", " graph_attr={'pack': '1'})\n", "g.graph_attr['rankdir'] = 'LR'\n", "\n", "for n in lvl_nodes:\n", " colors = node_colors[node_namespace_mapping.get(n.full_name.strip(\"/\").split(\"/\")[0])]\n", " peripheries = \"1\" if n.full_name not in output_nodes else \"2\"\n", " g.node(n.full_name, label=n.full_name, fillcolor=colors[\"fill\"], color=colors[\"stroke\"],\n", " shape=\"box\", style=\"filled\", peripheries=peripheries)\n", "\n", " if n.full_name in input_nodes:\n", " helper_node_name = f\"{n.full_name}__before\"\n", " g.node(helper_node_name, label=\"\", shape=\"none\", height=\"0\", width=\"0\")\n", " g.edge(helper_node_name, n.full_name)\n", "\n", "def compute_e2e_paths(start_nodes, end_nodes, edges):\n", " frontier_paths = [[n] for n in start_nodes]\n", " final_paths = []\n", "\n", " while frontier_paths:\n", " frontier_paths_new = []\n", "\n", " for path in frontier_paths:\n", " head = path[-1]\n", " if head in end_nodes:\n", " final_paths.append(path)\n", " continue\n", "\n", " out_nodes = [n_to for n_from, n_to in edges if n_from == head if n_to not in path]\n", " new_paths = [path + [n] for n in out_nodes]\n", " frontier_paths_new += new_paths\n", "\n", " frontier_paths = frontier_paths_new\n", "\n", " final_paths = [[(n_from, n_to)\n", " for n_from, n_to in zip(path[:-1], path[1:])]\n", " for path in final_paths]\n", " return final_paths\n", "\n", "e2e_paths = compute_e2e_paths(input_nodes, output_nodes, edges_between_nodes)\n", "\n", "for (src_name, dst_name), edges in edges_between_nodes.items():\n", " print(src_name, dst_name, len(edges))\n", " color = \"black\" if any((src_name, dst_name) in path for path in e2e_paths) else \"tomato\"\n", " g.edge(src_name, dst_name, penwidth=str(math.log(len(edges)) * 2 + .2), color=color)\n", "\n", "g.save(\"level_graph.gv\")\n", "g.render(\"level_graph.svg\")\n", "\n", "g" ], "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } } }, { "cell_type": "code", "execution_count": null, "outputs": [], "source": [ "from latency_graph.message_tree import DepTree\n", "from tqdm.notebook import tqdm\n", "from bisect import bisect\n", "\n", "topic_name_filter = [\"/control/trajectory_follower/control_cmd\"]\n", "\n", "\n", "def inst_get_dep_msg(inst: TrCallbackInstance):\n", " if inst.callback_object not in _tracing_context.callback_objects.by_callback_object:\n", " # print(\"Callback not found (2)\")\n", " return None\n", "\n", " if not isinstance(inst.callback_obj.owner, TrSubscriptionObject):\n", " # print(f\"Wrong type: {type(inst.callback_obj.owner)}\")\n", " return None\n", "\n", " sub_obj: TrSubscriptionObject = inst.callback_obj.owner\n", " if sub_obj and sub_obj.subscription and sub_obj.subscription.topic:\n", " # print(f\"Subscription has no topic\")\n", " pubs = sub_obj.subscription.topic.publishers\n", " else:\n", " pubs = []\n", "\n", " def _pub_latest_msg_before(pub: TrPublisher, inst):\n", " i_latest_msg = bisect(pub.instances, inst.timestamp, key=lambda x: x.timestamp) - 1\n", " if i_latest_msg < 0 or i_latest_msg >= len(pub.instances):\n", " return None\n", " latest_msg = pub.instances[i_latest_msg]\n", " if latest_msg.timestamp >= inst.timestamp:\n", " return None\n", "\n", " return latest_msg\n", "\n", " msgs = [_pub_latest_msg_before(pub, inst) for pub in pubs]\n", " msgs = [msg for msg in msgs if msg is not None]\n", " msgs.sort(key=lambda i: i.timestamp, reverse=True)\n", " if msgs:\n", " msg = msgs[0]\n", " return msg\n", "\n", " # print(f\"No messages found for topic {sub_obj.subscription.topic}\")\n", " return None\n", "\n", "def inst_get_dep_insts(inst: TrCallbackInstance):\n", " if inst.callback_object not in _tracing_context.callback_objects.by_callback_object:\n", " # print(\"Callback not found\")\n", " return []\n", " dep_cbs = get_cb_dep_cbs(inst.callback_obj)\n", "\n", " def _cb_to_chronological_inst(cb: TrCallbackObject, inst):\n", " i_inst_latest = bisect(cb.callback_instances, inst.timestamp, key=lambda x: x.timestamp)\n", "\n", " for inst_before in cb.callback_instances[i_inst_latest::-1]:\n", " if lg.inst_runtime_interval(inst_before)[-1] < inst.timestamp:\n", " return inst_before\n", "\n", " return None\n", "\n", " insts = [_cb_to_chronological_inst(cb, inst) for cb in dep_cbs]\n", " insts = [inst for inst in insts if inst is not None]\n", " return insts\n", "\n", "def get_cb_dep_cbs(cb: TrCallbackObject):\n", " match cb.owner:\n", " case TrSubscriptionObject() as sub_obj:\n", " sub_obj: TrSubscriptionObject\n", " owner = sub_obj.subscription.node\n", " case TrTimer() as tmr:\n", " tmr: TrTimer\n", " owner = tmr.node\n", " case _:\n", " raise RuntimeError(f\"Encountered {cb.owner} as callback owner\")\n", "\n", " owner: TrNode\n", " dep_sub_objs = {sub.subscription_object for sub in owner.subscriptions}\n", " dep_cbs = {callback_objects.by_id.get(sub_obj.id) for sub_obj in dep_sub_objs if sub_obj is not None}\n", " dep_cbs |= {callback_objects.by_id.get(tmr.id) for tmr in owner.timers}\n", " dep_cbs.discard(cb)\n", " dep_cbs.discard(None)\n", "\n", " return dep_cbs\n", "\n", "\n", "def get_msg_dep_cb(msg: TrPublishInstance):\n", " \"\"\"\n", " For a given message instance `msg`, find the publishing callback,\n", " as well as the message instances that callback depends on (transitively within its TrNode).\n", " \"\"\"\n", "\n", " # Find CB instance that published msg\n", " # print(f\"PUB {msg.publisher.node.path if msg.publisher.node is not None else '??'} ==> {msg.publisher.topic_name}\")\n", " pub_cbs = lat_graph.pub_cbs.get(msg.publisher)\n", " if pub_cbs is None:\n", " # print(\"Publisher unknown to lat graph. Skipping.\")\n", " return None\n", "\n", " # print(f\"Found {len(pub_cbs)} pub cbs\")\n", " cb_inst_candidates = []\n", " for cb in pub_cbs:\n", " # print(f\" > CB ({len(cb.callback_instances)} instances): {cb.callback_symbol.symbol if cb.callback_symbol else cb.id}\")\n", " i_inst_after = bisect(cb.callback_instances, msg.timestamp, key=lambda x: x.timestamp)\n", "\n", " for inst in cb.callback_instances[:i_inst_after]:\n", " inst_start, inst_end = lg.inst_runtime_interval(inst)\n", " if msg.timestamp > inst_end:\n", " continue\n", "\n", " assert inst_start <= msg.timestamp <= inst_end\n", "\n", " cb_inst_candidates.append(inst)\n", "\n", " if len(cb_inst_candidates) > 1:\n", " # print(\"Found multiple possible callbacks\")\n", " return None\n", " if not cb_inst_candidates:\n", " # print(\"Found no possible callbacks\")\n", " return None\n", "\n", " dep_inst = cb_inst_candidates[0]\n", " return dep_inst\n", "\n", "\n", "def get_dep_tree(inst: TrPublishInstance | TrCallbackInstance, lvl=0, visited_topics=None, is_dep_cb=False):\n", " if visited_topics is None:\n", " visited_topics = set()\n", "\n", " children_are_dep_cbs = False\n", "\n", " match inst:\n", " case TrPublishInstance(publisher=pub):\n", " if pub.topic_name in visited_topics:\n", " return None\n", "\n", " visited_topics.add(pub.topic_name)\n", " deps = [get_msg_dep_cb(inst)]\n", " case TrCallbackInstance() as cb_inst:\n", " deps = [inst_get_dep_msg(cb_inst)]\n", " if not is_dep_cb:\n", " deps += inst_get_dep_insts(cb_inst)\n", " children_are_dep_cbs = True\n", " case _:\n", " raise TypeError(f\"Expected inst to be of type TrPublishInstance or TrCallbackInstance, got {type(inst).__name__}\")\n", "\n", " # print(\"Rec level\", lvl)\n", " deps = [dep for dep in deps if dep is not None]\n", " deps = [get_dep_tree(dep, lvl + 1, set(visited_topics), is_dep_cb=children_are_dep_cbs) for dep in deps]\n", " deps = [dep for dep in deps if dep is not None]\n", " return DepTree(inst, deps)" ], "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } } }, { "cell_type": "code", "execution_count": null, "outputs": [], "source": [ "\n", "#for path in e2e_topic_paths:\n", "# end_topics = path[-1]\n", "end_topics = [t for t in _tracing_context.topics if any(f in t.name for f in topic_name_filter)]\n", "for end_topic in end_topics:\n", " end_topic: TrTopic\n", "\n", " pubs = end_topic.publishers\n", " for pub in pubs:\n", " depths = []\n", " sizes = []\n", " e2e_lats = []\n", " trees = []\n", " msgs = pub.instances\n", " for msg in tqdm(msgs, desc=f\"Building message chains for topic {end_topic.name}\"):\n", " msg: TrPublishInstance\n", " tree = get_dep_tree(msg)\n", " depths.append(tree.depth())\n", " sizes.append(tree.size())\n", " e2e_lats.append(tree.e2e_lat())\n", " trees.append(tree)\n", " if depths:\n", " print(f\"Depth: min={min(depths)} avg={sum(depths) / len(depths)} max={max(depths)}\")\n", " print(f\"Size: min={min(sizes)} avg={sum(sizes) / len(sizes)} max={max(sizes)}\")\n", " print(f\"E2E Lat: min={min(e2e_lats)*1000:.3f}ms avg={sum(e2e_lats) / len(sizes)*1000:.3f}ms max={max(e2e_lats)*1000:.3f}ms\")\n", "\n", "with open(\"trees.pkl\", \"wb\") as f:\n", " pickle.dump(trees, f)\n" ], "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } } }, { "cell_type": "code", "execution_count": null, "outputs": [], "source": [ "with open(\"trees.pkl\", \"rb\") as f:\n", " trees = pickle.load(f)" ], "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } } }, { "cell_type": "code", "execution_count": null, "outputs": [], "source": [ "import glob\n", "\n", "\n", "def parse_bytes(string):\n", " match string[-1]:\n", " case 'K':\n", " exponent = 1e3\n", " case 'M':\n", " exponent = 1e6\n", " case _:\n", " exponent = 1\n", "\n", " num = float(string.split(\" \")[0])\n", " return num * exponent\n", "\n", "\n", "def bytes_str(bytes):\n", " if bytes >= 1024**2:\n", " return f\"{bytes/(1024**2):.2f} MiB\"\n", " if bytes >= 1024:\n", " return f\"{bytes/1024:.2f} KiB\"\n", " return f\"{bytes:.0f} B\"\n", "\n", "\n", "BW_PATH = \"../ma-hw-perf-tools/data/results\"\n", "bw_files = glob.glob(os.path.join(BW_PATH, \"*.log\"))\n", "msg_sizes = {}\n", "for bw_file in bw_files:\n", " with open(bw_file) as f:\n", " lines = f.readlines()\n", " topic = os.path.splitext(os.path.split(bw_file)[1])[0].replace(\"__\", \"/\")\n", "\n", " if not lines or re.match(f\"^\\s*$\", lines[-1]):\n", " #print(f\"No data for {topic}\")\n", " continue\n", "\n", " line_pattern = re.compile(r\"(?P[0-9.]+ [KM]?)B/s from (?P[0-9.]+) messages --- Message size mean: (?P[0-9.]+ [KM]?)B min: (?P[0-9.]+ [KM]?)B max: (?P[0-9.]+ [KM]?)B\\n\")\n", " m = re.fullmatch(line_pattern, lines[-1])\n", " if m is None:\n", " print(f\"Line could not be parsed in {topic}: '{lines[-1]}'\")\n", " continue\n", "\n", " msg_sizes[topic] = {'bw': parse_bytes(m.group(\"bw\")),\n", " 'min': parse_bytes(m.group(\"min\")),\n", " 'mean': parse_bytes(m.group(\"mean\")),\n", " 'max': parse_bytes(m.group(\"max\"))}\n" ], "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } } }, { "cell_type": "code", "execution_count": null, "outputs": [], "source": [ "from typing import List\n", "from latency_graph.message_tree import DepTree\n", "\n", "start_topic_filters = [\"/vehicle/status/\", \"/sensing/imu\"]\n", "\n", "def leaf_topics(tree: DepTree, lvl=0):\n", " ret_list = []\n", " match tree.head:\n", " case TrPublishInstance(publisher=pub):\n", " if pub:\n", " ret_list += [(lvl, pub.topic_name)]\n", " ret_list += [(lvl, None)]\n", "\n", " for dep in tree.deps:\n", " ret_list += leaf_topics(dep, lvl+1)\n", " return ret_list\n", "\n", "#all_topics = set()\n", "\n", "#for tree in trees:\n", "# for d, t in leaf_topics(tree):\n", "# if t in [\"/parameter_events\", \"/clock\"]:\n", "# continue\n", "# all_topics.add(t)\n", "\n", "#def critical_path(self: DepTree, start_topic_filters: List[str]):\n", "# if not self.deps:\n", "# return [self.head]\n", "#\n", "# return [self.head, *max(map(DepTree.critical_path, self.deps), key=lambda ls: ls[-1].timestamp)]\n", "\n", "E2E_TIME_LIMIT_S = 2\n", "\n", "def all_e2es(tree: DepTree, t_start=None):\n", " if t_start is None:\n", " t_start = tree.head.timestamp\n", "\n", " if not tree.deps:\n", " return [t_start - tree.head.timestamp]\n", "\n", " ret_list = []\n", " for dep in tree.deps:\n", " ret_list += all_e2es(dep, t_start)\n", " return ret_list\n", "\n", "def relevant_e2es(tree: DepTree, start_topic_filters, t_start=None, path=None):\n", " if t_start is None:\n", " t_start = tree.head.timestamp\n", "\n", " if path is None:\n", " path = []\n", "\n", " latency = t_start - tree.head.timestamp\n", " if latency > E2E_TIME_LIMIT_S:\n", " return []\n", "\n", " new_path = [tree.head] + path\n", "\n", " if not tree.deps:\n", " match tree.head:\n", " case TrPublishInstance(publisher=pub):\n", " if pub and any(f in pub.topic_name for f in start_topic_filters):\n", " return [(latency,new_path)]\n", "\n", " ret_list = []\n", " for dep in tree.deps:\n", " ret_list += relevant_e2es(dep, start_topic_filters, t_start, new_path)\n", " return ret_list\n", "\n", "\n", "e2ess = []\n", "e2e_pathss = []\n", "for tree in trees:\n", " e2es, e2e_paths = zip(*relevant_e2es(tree, start_topic_filters))\n", " e2ess.append(e2es)\n", " e2e_pathss.append(e2e_paths)" ], "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } } }, { "cell_type": "code", "execution_count": null, "outputs": [], "source": [ "from matplotlib.animation import FuncAnimation\n", "from IPython import display\n", "\n", "fig, ax = plt.subplots(figsize=(16, 9))\n", "ax: plt.Axes\n", "ax.set_xlim(0, 4)\n", "\n", "ax.hist([], bins=200, range=(0, 4), histtype='stepfilled')\n", "ax.set_title(\"Time: 0.000000s\")\n", "\n", "def anim(frame):\n", " print(frame, end='\\r')\n", " ax.clear()\n", " ax.hist(e2es[frame], bins=200, range=(0, 4), histtype='stepfilled')\n", " ax.set_title(f\"Time: {(trees[frame].head.timestamp - trees[0].head.timestamp):.6}s\")\n", "\n", "\n", "anim_created = FuncAnimation(fig, anim, min(len(trees), 10000), interval=16, repeat_delay=200)\n", "\n", "video = anim_created.save(\"anim.mp4\", dpi=120)\n", "\n", "#for tree in trees:\n", "# path = tree.critical_path(start_topic_filters)\n", "# for i, inst in enumerate(path[::-1]):\n", "# match inst:\n", "# case TrPublishInstance(publisher=pub):\n", "# print(f\" {i:>3d}: T\", pub.topic_name)\n", "# case TrCallbackInstance(callback_obj=cb):\n", "# match cb.owner:\n", "# case TrSubscriptionObject(subscription=sub):\n", "# node = sub.node\n", "# case TrTimer() as tmr:\n", "# node = tmr.node\n", "# case _:\n", "# raise ValueError(f\"Callback owner type not recognized: {type(cb.owner).__name__}\")\n", "#\n", "# print(f\" {i:>3d}: N\", node.path)\n", "# print(\"==================\")\n" ], "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } } }, { "cell_type": "code", "execution_count": null, "outputs": [], "source": [ "fig, ax = plt.subplots(figsize=(18, 9), num=\"e2e_plot\")\n", "DS=1\n", "times = [tree.head.timestamp - trees[0].head.timestamp for tree in trees[::DS]]\n", "\n", "ax2 = ax.twinx()\n", "ax2.plot(times, list(map(lambda paths: sum(map(len, paths)) / len(paths), e2e_pathss[::DS])), color=\"orange\")\n", "ax2.fill_between(times,\n", " list(map(lambda paths: min(map(len, paths)), e2e_pathss[::DS])),\n", " list(map(lambda paths: max(map(len, paths)), e2e_pathss[::DS])),\n", " alpha=.3, color=\"orange\")\n", "\n", "ax.plot(times, [np.mean(e2es) for e2es in e2ess[::DS]])\n", "ax.fill_between(times, [np.min(e2es) for e2es in e2ess[::DS]], [np.max(e2es) for e2es in e2ess[::DS]], alpha=.3)\n", "\n", "def scatter_topic(topic_name, y=0, **scatter_kwargs):\n", " for pub in topics.by_name[topic_name].publishers:\n", " if not pub:\n", " continue\n", "\n", " inst_timestamps = [inst.timestamp - trees[0].head.timestamp for inst in pub.instances if inst.timestamp >= trees[0].head.timestamp]\n", " scatter_kwargs_default = {\"marker\": \"x\", \"color\": \"indianred\"}\n", " scatter_kwargs_default.update(scatter_kwargs)\n", " ax.scatter(inst_timestamps, np.full(len(inst_timestamps), fill_value=y), **scatter_kwargs_default)\n", "\n", "scatter_topic(\"/autoware/engage\")\n", "scatter_topic(\"/planning/scenario_planning/parking/trajectory\", y=-.04, color=\"cadetblue\")\n", "scatter_topic(\"/planning/scenario_planning/lane_driving/trajectory\", y=-.08, color=\"darkgreen\")\n", "scatter_topic(\"/initialpose2d\", y=-.12, color=\"orange\")\n", "\n", "ax.set_xlabel(\"Simulation time [s]\")\n", "ax.set_ylabel(\"End-to-End latency [s]\")\n", "ax2.set_ylabel(\"End-to-End path length\")\n", "None" ], "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } } }, { "cell_type": "code", "execution_count": null, "outputs": [], "source": [ "def critical_path(self):\n", " return [self.head, *min(map(critical_path, self.deps), key=lambda ls: ls[-1].timestamp)]\n", "\n", "\n", "def e2e_lat(self):\n", " return self.head.timestamp - critical_path(self)[-1].timestamp\n", "\n", "\n", "def get_relevant_tree(tree: DepTree, accept_leaf=lambda x: True, root=None):\n", " if root is None:\n", " root = tree.head\n", " if not tree.deps:\n", " if accept_leaf(tree.head, root):\n", " return tree\n", " return None\n", "\n", " relevant_deps = [get_relevant_tree(dep, accept_leaf, root) for dep in tree.deps]\n", " if not any(relevant_deps):\n", " return None\n", "\n", " return DepTree(tree.head, [dep for dep in relevant_deps if dep])\n", "\n", "\n", "def fanout(self):\n", " if not self.deps:\n", " return 1\n", "\n", " return sum(map(fanout, self.deps))\n", "\n", "\n", "def sort_subtree(subtree: DepTree, sort_func=lambda t: t.head.timestamp - e2e_lat(t)):\n", " subtree.deps.sort(key=sort_func)\n", " for dep in subtree.deps:\n", " sort_subtree(dep, sort_func)\n", " return subtree\n", "\n", "def _leaf_filter(inst, root):\n", " if root.timestamp - inst.timestamp > E2E_TIME_LIMIT_S:\n", " return False\n", "\n", " match inst:\n", " case TrPublishInstance(publisher=pub):\n", " return pub and any(f in pub.topic_name for f in start_topic_filters)\n", " return False\n", "\n", "\n", "relevant_trees = [get_relevant_tree(tree, _leaf_filter) for tree in trees]" ], "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } } }, { "cell_type": "code", "execution_count": null, "outputs": [], "source": [ "from cycler import cycler\n", "\n", "def dict_safe_append(dictionary, key, value):\n", " if key not in dictionary:\n", " dictionary[key] = []\n", " dictionary[key].append(value)\n", "\n", "\n", "fig, (ax, ax_rel) = plt.subplots(2, 1, sharex=True, figsize=(60, 30), num=\"crit_plot\")\n", "ax.set_prop_cycle(cycler('color', [plt.cm.nipy_spectral(i/4) for i in range(5)]))\n", "ax_rel.set_prop_cycle(cycler('color', [plt.cm.nipy_spectral(i/4) for i in range(5)]))\n", "\n", "critical_paths = [critical_path(tree) for tree in relevant_trees[::DS]]\n", "\n", "time_breakdown = {}\n", "\n", "for path in critical_paths:\n", " tmr_cb_calc_time = 0.0\n", " sub_cb_calc_time = 0.0\n", " tmr_cb_relevant_time = 0.0\n", " sub_cb_relevant_time = 0.0\n", " dds_time = 0.0\n", " idle_time = 0.0\n", "\n", " last_pub_time = None\n", " last_cb_time = None\n", " for inst in path:\n", " match inst:\n", " case TrPublishInstance(timestamp=t):\n", " assert last_pub_time is None, \"Two publication without callback inbetween\"\n", "\n", " if last_cb_time is not None:\n", " dds_time += last_cb_time - t\n", "\n", " last_pub_time = t\n", " last_cb_time = None\n", " case TrCallbackInstance(callback_obj=cb, timestamp=t, duration=d):\n", " if last_pub_time is not None:\n", " assert last_pub_time <= t+d, \"Publication out of CB instance timeframe\"\n", "\n", " match cb.owner:\n", " case TrTimer():\n", " tmr_cb_calc_time += d\n", " tmr_cb_relevant_time += last_pub_time - t\n", " case TrSubscriptionObject():\n", " sub_cb_calc_time += d\n", " sub_cb_relevant_time += last_pub_time - t\n", " elif last_cb_time is not None:\n", " idle_time += last_cb_time - (t + d)\n", " last_pub_time = None\n", " last_cb_time = t\n", "\n", " #dict_safe_append(time_breakdown, \"tmr_cb_calc_time\", tmr_cb_calc_time)\n", " #dict_safe_append(time_breakdown, \"sub_cb_calc_time\", sub_cb_calc_time)\n", " dict_safe_append(time_breakdown, \"Timer CB\", tmr_cb_relevant_time)\n", " dict_safe_append(time_breakdown, \"Subscription CB\", sub_cb_relevant_time)\n", " dict_safe_append(time_breakdown, \"DDS\", dds_time)\n", " dict_safe_append(time_breakdown, \"Idle\", idle_time)\n", "\n", "time_breakdown = {k: np.array(v) for k, v in time_breakdown.items()}\n", "\n", "timer_cb_times = [sum(inst.duration for inst in path if isinstance(inst, TrCallbackInstance) and isinstance(inst.callback_obj, TrTimer)) for path in critical_paths]\n", "sub_cb_times = [sum(inst.duration for inst in path if isinstance(inst, TrCallbackInstance)) for path in critical_paths]\n", "\n", "labels, values = list(zip(*time_breakdown.items()))\n", "\n", "#ax.plot(range(len(relevant_trees[::DS])), [e2e_lat(tree) for tree in relevant_trees[::DS]], label=\"Total E2E\")\n", "ax.stackplot(range(len(relevant_trees[::DS])), values, labels=labels)\n", "ax.legend()\n", "ax.set_title(\"End-to-End Latency Breakdown\")\n", "ax.set_ylabel(\"End-to-End Latency [s]\")\n", "\n", "timestep_mags = np.array([sum(vs) for vs in zip(*values)])\n", "ax_rel.stackplot(range(len(relevant_trees[::DS])), [val / timestep_mags for val in values], labels=labels)\n", "ax_rel.set_title(\"End-to-End Latency Breakdown (relative)\")\n", "ax_rel.set_ylabel(\"End-to-End Latency Fraction\")\n", "ax_rel.set_xlabel(\"Timestep\")\n", "ax_rel.legend()\n", "\n", "None" ], "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } } }, { "cell_type": "code", "execution_count": null, "outputs": [], "source": [ "from scipy import stats\n", "\n", "fig, ax = plt.subplots(figsize=(60, 15), num=\"crit_pdf\")\n", "ax.set_prop_cycle(cycler('color', [plt.cm.nipy_spectral(i/4) for i in range(5)]))\n", "\n", "kde = stats.gaussian_kde(timestep_mags)\n", "xs = np.linspace(values.min(), values.max(), 1000)\n", "ax.plot(xs, kde(xs), label=\"End-to-End Latency\")\n", "perc = 90\n", "ax.axvline(np.percentile(timestep_mags, perc), label=f\"{perc}th percentile\")\n", "\n", "ax2 = ax.twinx()\n", "ax2.hist(timestep_mags, 200)\n", "ax2.set_ylim(0, ax2.get_ylim()[1])\n", "\n", "ax.set_title(\"Time Distribution for E2E Breakdown\")\n", "ax.set_xlabel(\"Time [s]\")\n", "ax.set_ylabel(\"Frequency\")\n", "ax.set_xlim(0, 2.01)\n", "ax.set_ylim(0, ax.get_ylim()[1])\n", "ax.legend()\n", "\n", "None" ], "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } } }, { "cell_type": "code", "execution_count": null, "outputs": [], "source": [ "from tracing_interop.tr_types import TrSubscription\n", "from matching.subscriptions import sanitize\n", "import matplotlib.patches as mpatch\n", "from matplotlib.text import Text\n", "import math\n", "\n", "i = 3900\n", "tree = trees[i]\n", "e2es = e2ess[i]\n", "e2e_paths = e2e_pathss[i]\n", "margin_y = .2\n", "margin_x=0\n", "arr_width= 1 - margin_y\n", "\n", "def cb_str(inst: TrCallbackInstance):\n", " cb: TrCallbackObject = inst.callback_obj\n", " if not cb:\n", " return None\n", " ret_str = f\"- {inst.duration*1e3:07.3f}ms \"\n", " #if cb.callback_symbol:\n", " # ret_str = repr(sanitize(cb.callback_symbol.symbol))\n", "\n", " match cb.owner:\n", " case TrSubscriptionObject(subscription=sub):\n", " sub: TrSubscription\n", " ret_str = f\"{ret_str}{sub.node.path if sub.node else None} <- {sub.topic_name}\"\n", " case TrTimer(period=p, node=node):\n", " p: int\n", " node: TrNode\n", " ret_str = f\"{ret_str}{node.path if node else None} <- @{1/(p*1e-9):.2f}Hz\"\n", " return ret_str\n", "\n", "\n", "def ts_str(inst, prev):\n", " return f\"{(inst.timestamp - prev)*1e3:+09.3f}ms\"\n", "\n", "def bw_str(bw):\n", " return bytes_str(bw['mean'])\n", "\n", "def trunc_chars(string, w, e2e):\n", " if w < e2e * .005:\n", " return \"\"\n", " n_chars = max(math.floor(w / (e2e * .17) * 65) - 5, 0)\n", " if n_chars < 4:\n", " return \"\"\n", "\n", " return \"...\" + string[-n_chars:] if n_chars < len(string) else string\n", "\n", "#e2e_paths = sorted(e2e_paths, key=lambda path: path[-1].timestamp - path[0].timestamp, reverse=True)\n", "#for y, e2e_path in reversed(list(enumerate(e2e_paths))):\n", "# last_pub_ts = None\n", "# last_cb_end = None\n", "# print(f\"=== {y}:\")\n", "# for inst in e2e_path:\n", "\n", "tree = sort_subtree(get_relevant_tree(tree, _leaf_filter))\n", "\n", "t_start = tree.head.timestamp\n", "t_min = t_start - e2e_lat(tree)\n", "t_e2e = t_start - t_min\n", "\n", "legend_entries = {}\n", "\n", "def plot_subtree(subtree: DepTree, ax: plt.Axes, y_labels, y=0, next_cb_start=0):\n", " height = fanout(subtree)\n", " inst = subtree.head\n", "\n", " match inst:\n", " case TrCallbackInstance(timestamp=t_cb, duration=d_cb):\n", " is_sub = isinstance(inst.callback_obj.owner, TrSubscriptionObject)\n", "\n", " r_x = t_cb - t_start + margin_x / 2\n", " r_y = y + margin_y / 2\n", " r_w = max(d_cb - margin_x, 0)\n", " r_h = height - margin_y\n", "\n", " r = mpatch.Rectangle((r_x, r_y), r_w, r_h,\n", " ec=\"cadetblue\" if is_sub else \"indianred\", fc=\"lightblue\" if is_sub else \"lightcoral\", zorder=9000)\n", " ax.add_artist(r)\n", "\n", " text = repr(sanitize(inst.callback_obj.callback_symbol.symbol)) if inst.callback_obj and inst.callback_obj.callback_symbol else \"??\"\n", " text = trunc_chars(text, r_w, t_e2e)\n", " if text:\n", " ax.text(r_x + r_w / 2, r_y + r_h / 2, text, ha=\"center\", va=\"center\", backgroundcolor=(1,1,1,.5), zorder=11000)\n", "\n", " if is_sub and \"Subscription CB\" not in legend_entries:\n", " legend_entries[\"Subscription CB\"] = r\n", " elif not is_sub and \"Timer CB\" not in legend_entries:\n", " legend_entries[\"Timer CB\"] = r\n", "\n", " if next_cb_start is not None:\n", " r_x = t_cb - t_start + d_cb - margin_x / 2\n", " r_y = y + .5 - arr_width/2\n", " r_w = next_cb_start - (t_cb + d_cb) + margin_x\n", " r_h = arr_width\n", " r = mpatch.Rectangle((r_x, r_y), r_w, r_h, color=\"orange\")\n", " ax.add_artist(r)\n", "\n", " if is_sub:\n", " node = inst.callback_obj.owner.subscription.node\n", " else:\n", " node = inst.callback_obj.owner.node\n", " text = node.path\n", "\n", " text = trunc_chars(text, r_w, t_e2e)\n", " if text:\n", " ax.text(r_x + r_w / 2, r_y + r_h / 2, text, ha=\"center\", va=\"center\", backgroundcolor=(1,1,1,.5), zorder=11000)\n", "\n", " if \"Idle\" not in legend_entries:\n", " legend_entries[\"Idle\"] = r\n", "\n", " next_cb_start = t_cb\n", " case TrPublishInstance(timestamp=t_pub, publisher=pub):\n", " if not subtree.deps:\n", " y_labels.append(pub.topic_name if pub else None)\n", "\n", " scatter = ax.scatter(t_pub - t_start, y+.5, color=\"cyan\", marker=\".\", zorder=10000)\n", "\n", " if \"Publication\" not in legend_entries:\n", " legend_entries[\"Publication\"] = scatter\n", "\n", " if next_cb_start is not None:\n", " r_x = t_pub - t_start\n", " r_y = y + .5 - arr_width/2\n", " r_w = max(next_cb_start - t_pub + margin_x / 2, 0)\n", " r = mpatch.Rectangle((r_x, r_y), r_w, arr_width, color=\"lightgreen\")\n", " ax.add_artist(r)\n", " if pub:\n", " text = pub.topic_name\n", " text = trunc_chars(text, r_w, t_e2e)\n", " if text:\n", " ax.text(r_x + r_w / 2, r_y + arr_width / 2, text, ha=\"center\", va=\"center\", backgroundcolor=(1,1,1,.5), zorder=11000)\n", "\n", " if \"DDS\" not in legend_entries:\n", " legend_entries[\"DDS\"] = r\n", "\n", " topic_stats = msg_sizes.get(pub.topic_name)\n", " if topic_stats:\n", " size_str = bw_str(topic_stats)\n", " ax.text(r_x + r_w / 2, r_y + arr_width + margin_y, size_str, ha=\"center\", backgroundcolor=(1,1,1,.5), zorder=11000)\n", " else:\n", " print(\"[WARN] Tried to publish to another PublishInstance\")\n", " next_cb_start = None\n", "\n", " acc_fanout = 0\n", " for dep in subtree.deps:\n", " acc_fanout += plot_subtree(dep, ax, y_labels, y + acc_fanout, next_cb_start)\n", " return height\n", "\n", "\n", "\n", "fig, ax = plt.subplots(figsize=(36, 20), num=\"path_viz\")\n", "ax.set_ylim(0, len(e2es))\n", "\n", "y_labels = []\n", "plot_subtree(tree, ax, y_labels)\n", "\n", "tree_e2e = e2e_lat(tree)\n", "plot_margin_x = .01 * tree_e2e\n", "ax.set_xlim(-tree_e2e - plot_margin_x, plot_margin_x)\n", "ax.set_yticks(np.array(range(len(y_labels))) + .5, y_labels)\n", "ax.set_title(f\"Timestep {i}: {(tree.head.timestamp - trees[0].head.timestamp):10.6f}s\")\n", "ax.set_xlabel(\"Time relative to output message [s]\")\n", "ax.set_ylabel(\"Start topic\")\n", "\n", "labels, handles = list(zip(*legend_entries.items()))\n", "ax.legend(handles, labels)\n", "print(len(y_labels))" ], "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } } }, { "cell_type": "code", "execution_count": null, "outputs": [], "source": [], "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } } } ], "metadata": { "interpreter": { "hash": "e7370f93d1d0cde622a1f8e1c04877d8463912d04d973331ad4851f04de6915a" }, "kernelspec": { "display_name": "Python 3.8.10 64-bit", "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 }