diff --git a/trace-analysis.ipynb b/trace-analysis.ipynb index 16ad94e..c6cb25d 100644 --- a/trace-analysis.ipynb +++ b/trace-analysis.ipynb @@ -23,14 +23,15 @@ "from tracetools_analysis.utils.ros2 import Ros2DataModelUtil\n", "\n", "from dataclasses import dataclass\n", - "from typing import List, Dict\n", + "from typing import List, Dict, Set, Union, Tuple\n", "from functools import cached_property\n", - "import pickle" + "import pickle\n", + "import re" ] }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 2, "metadata": {}, "outputs": [], "source": [ @@ -45,7 +46,7 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": 3, "metadata": {}, "outputs": [], "source": [ @@ -57,7 +58,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 4, "metadata": {}, "outputs": [], "source": [ @@ -121,7 +122,7 @@ }, { "cell_type": "code", - "execution_count": 5, + "execution_count": 7, "metadata": {}, "outputs": [], "source": [ @@ -177,6 +178,9 @@ " 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", @@ -202,6 +206,9 @@ " 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", @@ -229,6 +236,9 @@ " @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", @@ -246,6 +256,9 @@ " 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", @@ -266,6 +279,9 @@ " 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 CallbackObject:\n", " id: int # (reference) = subscription_object.id | timer.id | ....\n", @@ -301,6 +317,9 @@ " 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)\n", + "\n", "@dataclass\n", "class PublishInstance:\n", " publisher_handle: int\n", @@ -311,11 +330,14 @@ " def publisher(self) -> 'Publisher':\n", " return publishers[self.publisher_handle]\n", "\n", + " def __hash__(self):\n", + " return hash((self.publisher_handle))\n", + "\n", "@dataclass\n", "class CallbackInstance:\n", " callback_object: int\n", - " timestamp: int\n", - " duration: int\n", + " timestamp: pd.Timestamp\n", + " duration: pd.Timedelta\n", " intra_process: bool\n", "\n", " @property\n", @@ -354,7 +376,7 @@ }, { "cell_type": "code", - "execution_count": 7, + "execution_count": 6, "metadata": {}, "outputs": [ { @@ -978,7 +1000,7 @@ }, { "cell_type": "code", - "execution_count": 107, + "execution_count": 11, "metadata": {}, "outputs": [ { @@ -1151,7 +1173,7 @@ " try:\n", " cb_obj = callback_objects[cb_obj_id]\n", " sym = callback_symbols[cb_obj.callback_object].symbol\n", - " sym = util._prettify(sym)\n", + " sym = Ros2DataModelUtil._prettify(None, sym)\n", " sym = re.sub(r\"std::shared_ptr<(.*?)>\", r\"\\1*\", sym)\n", "\n", " cb_owner = cb_obj.owner\n", @@ -1211,7 +1233,101 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "# Pub-Use Latency Calculation" + "# E2E Latency Calculation" + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[Topic(name='/api/autoware/get/map/info/hash'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/markers'), Topic(name='/rosout'), Topic(name='/control/command/gear_cmd'), Topic(name='/planning/scenario_planning/status/infrastructure_commands'), Topic(name='/control/trajectory_follower/longitudinal/diagnostic'), Topic(name='/robot_description'), Topic(name='/planning/mission_planning/route_marker'), Topic(name='/control/trajectory_follower/lane_departure_checker_node/debug/deviation/yaw'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/running_modules'), Topic(name='/planning/scenario_planning/status/stop_reason'), Topic(name='/planning/scenario_planning/motion_velocity_smoother/debug/forward_filtered_trajectory'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_boundary'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance_debug_message_array'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/ready_module'), Topic(name='/control/command/emergency_cmd'), Topic(name='/planning/scenario_planning/motion_velocity_smoother/debug/backward_filtered_trajectory'), Topic(name='/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/mpt_ref_traj'), Topic(name='/map/vector_map_marker'), Topic(name='/planning/scenario_planning/motion_velocity_smoother/stop_speed_exceeded'), Topic(name='/planning/scenario_planning/motion_velocity_smoother/closest_max_velocity'), Topic(name='/control/trajectory_follower/lane_departure_checker_node/debug/marker_array'), Topic(name='/planning/scenario_planning/status/stop_reasons'), Topic(name='/planning/scenario_planning/motion_velocity_smoother/closest_jerk'), Topic(name='/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/clearance_map'), Topic(name='/api/autoware/get/emergency'), Topic(name='/planning/scenario_planning/parking/freespace_planner/debug/pose_array'), Topic(name='/diagnostics_toplevel_state'), Topic(name='/planning/mission_planning/echo_back_goal_pose'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection'), Topic(name='/control/trajectory_follower/longitudinal/slope_angle'), Topic(name='/planning/planning_diagnostics/planning_error_monitor/debug/marker'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/drivable_area'), Topic(name='/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/calculation_time'), Topic(name='/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/marker'), Topic(name='/planning/scenario_planning/motion_velocity_smoother/debug/merged_filtered_trajectory'), Topic(name='/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_external_velocity_limited'), Topic(name='/planning/scenario_planning/motion_velocity_smoother/closest_acceleration'), Topic(name='/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/area_with_objects'), Topic(name='/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/mpt_fixed_traj'), Topic(name='/control/command/turn_indicators_cmd'), Topic(name='/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_raw'), Topic(name='/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/path_for_visualize'), Topic(name='/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop/debug_values'), Topic(name='/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/object_clearance_map'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/force_available'), Topic(name='/planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array'), Topic(name='/control/external_cmd_selector/current_selector_mode'), Topic(name='/planning/scenario_planning/motion_velocity_smoother/closest_velocity'), Topic(name='/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_time_resampled'), Topic(name='/planning/scenario_planning/status/no_start_reason'), Topic(name='/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/extended_non_fixed_traj'), Topic(name='/control/trajectory_follower/lane_departure_checker_node/debug/deviation/lateral'), Topic(name='/planning/scenario_planning/motion_velocity_smoother/calculation_time'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/debug/traffic_signal'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area'), Topic(name='/api/external/get/command/selected/control'), Topic(name='/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/extended_fixed_traj'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk'), Topic(name='/planning/scenario_planning/parking/costmap_generator/grid_map'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/merge_from_private'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/occlusion_spot'), Topic(name='/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/wall_marker'), Topic(name='/control/trajectory_follower/lane_departure_checker_node/debug/processing_time_ms'), Topic(name='/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/mpt_traj'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/output/stop_reason'), Topic(name='/control/trajectory_follower/lane_departure_checker_node/debug/deviation/yaw_deg'), Topic(name='/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_lateral_acc_filtered'), Topic(name='/planning/scenario_planning/current_max_velocity'), Topic(name='/system/system_monitor/cpu_monitor/cpu_usage'), Topic(name='/control/command/hazard_lights_cmd'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/output/path_candidate'), Topic(name='/diagnostics_err'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light'), Topic(name='/planning/scenario_planning/motion_velocity_smoother/closest_merged_velocity'), Topic(name='/planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker'), Topic(name='/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control/debug_values'), Topic(name='/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot'), Topic(name='/control/trajectory_follower/lateral/diagnostic'), Topic(name='/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/eb_trajectory'), Topic(name='/planning/scenario_planning/motion_velocity_smoother/distance_to_stopline')]\n" + ] + } + ], + "source": [ + "#################################################\n", + "# Data structures & helpers\n", + "#################################################\n", + "\n", + "@dataclass\n", + "class LatencyStats:\n", + " pub_use_latencies: np.ndarray\n", + " exec_latencies: np.ndarray\n", + "\n", + " @cached_property\n", + " def total_latencies(self):\n", + " return self.pub_use_latencies + self.exec_latencies\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", + "\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", + "for cb in callback_objects.values():\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 isinstance(cb.owner, Timer):\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", + "\n", + " dep_topics = [sub.topic for sub in owner_node.subscriptions]\n", + " elif isinstance(cb.owner, SubscriptionObject):\n", + " owner_node = cb.owner.subscription.node\n", + " dep_topics = cb.owner.subscription.topic\n", + " else:\n", + " raise RuntimeError(f\"Callback owners other than timers/subscriptions cannot be handled: {cb.owner_info}\")\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 = inst.timestamp.timestamp()\n", + " inst_t_max = inst_t_min + inst.duration.total_seconds()\n", + " return (inst_t_min, inst_t_max)\n", + " \n", + " def filter_pub_insts_by_interval(cb_intervals, pub_insts: List[PublishInstance]):\n", + " return [inst for inst in pub_insts for (t_min, t_max) in cb_intervals if t_min < inst.timestamp * 1e-9 < t_max]\n", + "\n", + " cb_runtime_intervals = [inst_runtime_interval(inst) for inst in cb.callback_instances]\n", + " cb_pub_overlaps = [filter_pub_insts_by_interval(cb_runtime_intervals, pub.instances) for pub in owner_node.publishers]\n", + " cb_pub_scores = [len(pub_overlaps) / len(pub.instances) for pub, pub_overlaps in zip(owner_node.publishers, cb_pub_overlaps)]\n", + " print(cb_pub_scores)\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", + "#################################################" ] }, { @@ -1219,11 +1335,7 @@ "execution_count": null, "metadata": {}, "outputs": [], - "source": [ - "#################################################\n", - "# \n", - "#################################################" - ] + "source": [] } ], "metadata": {