diff --git a/tracetools_analysis/analysis/lifecycle_states.ipynb b/tracetools_analysis/analysis/lifecycle_states.ipynb new file mode 100644 index 0000000..17de46a --- /dev/null +++ b/tracetools_analysis/analysis/lifecycle_states.ipynb @@ -0,0 +1,162 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Lifecycle node states\n", + "#\n", + "# Get trace data using the provided launch file:\n", + "# $ ros2 launch tracetools_analysis lifecycle_states.launch.py" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "path = '~/.ros/tracing/lifecycle-node-state/'" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import sys\n", + "# Assuming a workspace with:\n", + "# src/tracetools_analysis/\n", + "# src/micro-ROS/ros_tracing/ros2_tracing/tracetools_read/\n", + "sys.path.insert(0, '../')\n", + "sys.path.insert(0, '../../../micro-ROS/ros_tracing/ros2_tracing/tracetools_read/')\n", + "import datetime as dt\n", + "\n", + "from bokeh.palettes import Category10\n", + "from bokeh.plotting import figure\n", + "from bokeh.plotting import output_notebook\n", + "from bokeh.io import show\n", + "from bokeh.layouts import row\n", + "from bokeh.models import ColumnDataSource\n", + "from bokeh.models import DatetimeTickFormatter\n", + "from bokeh.models import PrintfTickFormatter\n", + "import numpy as np\n", + "import pandas as pd\n", + "\n", + "from tracetools_analysis.loading import load_file\n", + "from tracetools_analysis.processor.ros2 import Ros2Handler\n", + "from tracetools_analysis.utils.ros2 import Ros2DataModelUtil" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Process\n", + "events = load_file(path)\n", + "handler = Ros2Handler.process(events)\n", + "#handler.data.print_data()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "data_util = Ros2DataModelUtil(handler.data)\n", + "\n", + "state_intervals = data_util.get_lifecycle_node_state_intervals()\n", + "for handle, states in state_intervals.items():\n", + " print(handle)\n", + " print(states.to_string())\n", + "\n", + "output_notebook()\n", + "psize = 450" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Plot\n", + "colors = Category10[10]\n", + "\n", + "lifecycle_node_names = {\n", + " handle: data_util.get_lifecycle_node_handle_info(handle)['lifecycle node'] for handle in state_intervals.keys()\n", + "}\n", + "states_labels = []\n", + "start_times = []\n", + "\n", + "fig = figure(\n", + " y_range=list(lifecycle_node_names.values()),\n", + " title='Lifecycle states over time',\n", + " y_axis_label='node',\n", + " plot_width=psize*2, plot_height=psize,\n", + ")\n", + "\n", + "for lifecycle_node_handle, states in state_intervals.items():\n", + " lifecycle_node_name = lifecycle_node_names[lifecycle_node_handle]\n", + "\n", + " start_times.append(states['start_timestamp'].iloc[0])\n", + " for index, row in states.iterrows():\n", + " # TODO fix end\n", + " if index == max(states.index):\n", + " continue\n", + " start = row['start_timestamp']\n", + " end = row['end_timestamp']\n", + " state = row['state']\n", + " if state not in states_labels:\n", + " states_labels.append(state)\n", + " state_index = states_labels.index(state)\n", + " fig.line(\n", + " x=[start, end],\n", + " y=[lifecycle_node_name]*2,\n", + " line_width=10.0,\n", + " line_color=colors[state_index],\n", + " legend_label=state,\n", + " )\n", + "\n", + "fig.title.align = 'center'\n", + "fig.xaxis[0].formatter = DatetimeTickFormatter(seconds=['%Ss'])\n", + "fig.xaxis[0].axis_label = 'time (' + min(start_times).strftime('%Y-%m-%d %H:%M') + ')'\n", + "show(fig)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "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.2" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/tracetools_analysis/launch/lifecycle_states.launch.py b/tracetools_analysis/launch/lifecycle_states.launch.py new file mode 100644 index 0000000..9f67802 --- /dev/null +++ b/tracetools_analysis/launch/lifecycle_states.launch.py @@ -0,0 +1,38 @@ +# Copyright 2020 Christophe Bedard +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Example launch file for a lifecycle node state analysis.""" + +from launch import LaunchDescription +from launch_ros.actions import Node +from tracetools_launch.action import Trace + + +def generate_launch_description(): + return LaunchDescription([ + Trace( + session_name='lifecycle-node-state', + events_kernel=[], + ), + Node( + package='tracetools_test', + executable='test_lifecycle_node', + output='screen', + ), + Node( + package='tracetools_test', + executable='test_lifecycle_client', + output='screen', + ), + ]) diff --git a/tracetools_analysis/tracetools_analysis/data_model/ros2.py b/tracetools_analysis/tracetools_analysis/data_model/ros2.py index 70a80b9..75f4c6f 100644 --- a/tracetools_analysis/tracetools_analysis/data_model/ros2.py +++ b/tracetools_analysis/tracetools_analysis/data_model/ros2.py @@ -1,4 +1,5 @@ # Copyright 2019 Robert Bosch GmbH +# Copyright 2020 Christophe Bedard # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -86,12 +87,20 @@ class Ros2DataModel(DataModel): 'timestamp', 'symbol']) self.callback_symbols.set_index(['callback_object'], inplace=True, drop=True) + self.lifecycle_state_machines = pd.DataFrame(columns=['state_machine_handle', + 'node_handle']) + self.lifecycle_state_machines.set_index(['state_machine_handle'], inplace=True, drop=True) # Events (multiple instances, may not have a meaningful index) self.callback_instances = pd.DataFrame(columns=['callback_object', 'timestamp', 'duration', 'intra_process']) + # Lifecycle state transitions (may not have a meaningful index) + self.lifecycle_transitions = pd.DataFrame(columns=['state_machine_handle', + 'start_label', + 'goal_label', + 'timestamp']) def add_context( self, context_handle, timestamp, pid, version @@ -154,6 +163,22 @@ class Ros2DataModel(DataModel): } self.callback_instances = self.callback_instances.append(data, ignore_index=True) + def add_lifecycle_state_machine( + self, handle, node_handle + ) -> None: + self.lifecycle_state_machines.loc[handle] = [node_handle] + + def add_lifecycle_state_transition( + self, state_machine_handle, start_label, goal_label, timestamp + ) -> None: + data = { + 'state_machine_handle': state_machine_handle, + 'start_label': start_label, + 'goal_label': goal_label, + 'timestamp': timestamp, + } + self.lifecycle_transitions = self.lifecycle_transitions.append(data, ignore_index=True) + def print_data(self) -> None: print('====================ROS 2 DATA MODEL===================') print('Contexts:') @@ -188,4 +213,11 @@ class Ros2DataModel(DataModel): print() print('Callback instances:') print(self.callback_instances.to_string()) + print() + print('Lifecycle state machines:') + print() + print(self.lifecycle_state_machines.to_string()) + print() + print('Lifecycle transitions:') + print(self.lifecycle_transitions.to_string()) print('==================================================') diff --git a/tracetools_analysis/tracetools_analysis/processor/ros2.py b/tracetools_analysis/tracetools_analysis/processor/ros2.py index 8fbcc28..d340e05 100644 --- a/tracetools_analysis/tracetools_analysis/processor/ros2.py +++ b/tracetools_analysis/tracetools_analysis/processor/ros2.py @@ -1,4 +1,5 @@ # Copyright 2019 Robert Bosch GmbH +# Copyright 2020 Christophe Bedard # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -68,6 +69,10 @@ class Ros2Handler(EventHandler): self._handle_callback_start, 'ros2:callback_end': self._handle_callback_end, + 'ros2:rcl_lifecycle_state_machine_init': + self._handle_rcl_lifecycle_state_machine_init, + 'ros2:rcl_lifecycle_transition': + self._handle_rcl_lifecycle_transition, } super().__init__( handler_map=handler_map, @@ -226,3 +231,19 @@ class Ros2Handler(EventHandler): bool(is_intra_process)) else: print(f'No matching callback start for callback object "{callback_object}"') + + def _handle_rcl_lifecycle_state_machine_init( + self, event: Dict, metadata: EventMetadata, + ) -> None: + node_handle = get_field(event, 'node_handle') + state_machine = get_field(event, 'state_machine') + self.data.add_lifecycle_state_machine(state_machine, node_handle) + + def _handle_rcl_lifecycle_transition( + self, event: Dict, metadata: EventMetadata, + ) -> None: + timestamp = metadata.timestamp + state_machine = get_field(event, 'state_machine') + start_label = get_field(event, 'start_label') + goal_label = get_field(event, 'goal_label') + self.data.add_lifecycle_state_transition(state_machine, start_label, goal_label, timestamp) diff --git a/tracetools_analysis/tracetools_analysis/utils/__init__.py b/tracetools_analysis/tracetools_analysis/utils/__init__.py index 3074c3a..0d55138 100644 --- a/tracetools_analysis/tracetools_analysis/utils/__init__.py +++ b/tracetools_analysis/tracetools_analysis/utils/__init__.py @@ -19,6 +19,7 @@ from typing import List from typing import Optional from typing import Union +import numpy as np from pandas import DataFrame from ..data_model import DataModel @@ -72,12 +73,12 @@ class DataModelUtil(): # Convert from ns to ms if len(columns_ns_to_ms) > 0: df[columns_ns_to_ms] = df[columns_ns_to_ms].applymap( - lambda t: t / 1000000.0 + lambda t: t / 1000000.0 if not np.isnan(t) else t ) # Convert from ns to ms + ms to datetime, as UTC if len(columns_ns_to_datetime) > 0: df[columns_ns_to_datetime] = df[columns_ns_to_datetime].applymap( - lambda t: dt.utcfromtimestamp(t / 1000000000.0) + lambda t: dt.utcfromtimestamp(t / 1000000000.0) if not np.isnan(t) else t ) return df diff --git a/tracetools_analysis/tracetools_analysis/utils/ros2.py b/tracetools_analysis/tracetools_analysis/utils/ros2.py index b4934b9..761b3e5 100644 --- a/tracetools_analysis/tracetools_analysis/utils/ros2.py +++ b/tracetools_analysis/tracetools_analysis/utils/ros2.py @@ -21,6 +21,8 @@ from typing import Mapping from typing import Optional from typing import Union +import numpy as np +from pandas import concat from pandas import DataFrame from . import DataModelUtil @@ -369,6 +371,81 @@ class Ros2DataModelUtil(DataModelUtil): tid = self.data.nodes.loc[node_handle, 'tid'] return {'node': node_name, 'tid': tid} + def get_lifecycle_node_handle_info( + self, + lifecycle_node_handle: int, + ) -> Optional[Mapping[str, Any]]: + """ + Get information about a lifecycle node handle. + + :param lifecycle_node_handle: the lifecycle node handle value + :return: a dictionary with name:value info, or `None` if it fails + """ + node_info = self.get_node_handle_info(lifecycle_node_handle) + if not node_info: + return None + # TODO(christophebedard) validate that it is an actual lifecycle node and not just a node + node_info['lifecycle node'] = node_info.pop('node') # type: ignore + return node_info + + def get_lifecycle_node_state_intervals( + self, + ) -> DataFrame: + """ + Get state intervals (start, end) for all lifecycle nodes. + + The returned dictionary contains a dataframe for each lifecycle node handle: + (lifecycle node handle -> [state string, start timestamp, end timestamp]) + + In cases where there is no explicit timestamp (e.g. end of state), + `np.nan` is used instead. + The node creation timestamp is used as the start timestamp of the first state. + TODO(christophebedard) do the same with context shutdown for the last end time + + :return: dictionary with a dataframe (with each row containing state interval information) + for each lifecycle node + """ + data = {} + lifecycle_transitions = self.data.lifecycle_transitions.copy() + state_machine_handles = set(lifecycle_transitions['state_machine_handle']) + for state_machine_handle in state_machine_handles: + transitions = lifecycle_transitions.loc[ + lifecycle_transitions.loc[:, 'state_machine_handle'] == state_machine_handle, + ['start_label', 'goal_label', 'timestamp'] + ] + # Get lifecycle node handle from state machine handle + lifecycle_node_handle = self.data.lifecycle_state_machines.loc[ + state_machine_handle, 'node_handle' + ] + + # Infer first start time from node creation timestamp + node_creation_timestamp = self.data.nodes.loc[lifecycle_node_handle, 'timestamp'] + + # Add initial and final timestamps + # Last states has an unknown end timestamp + first_state_label = transitions.loc[0, 'start_label'] + last_state_label = transitions.loc[transitions.index[-1], 'goal_label'] + transitions.loc[-1] = ['', first_state_label, node_creation_timestamp] + transitions.index = transitions.index + 1 + transitions.sort_index(inplace=True) + transitions.loc[transitions.index.max() + 1] = [last_state_label, '', np.nan] + + # Process transitions to get start/end timestamp of each instance of a state + end_timestamps = transitions[['timestamp']].shift(periods=-1) + end_timestamps.rename( + columns={end_timestamps.columns[0]: 'end_timestamp'}, inplace=True) + states = concat([transitions, end_timestamps], axis=1) + states.drop(['start_label'], axis=1, inplace=True) + states.rename( + columns={'goal_label': 'state', 'timestamp': 'start_timestamp'}, inplace=True) + states.drop(states.tail(1).index, inplace=True) + + # Convert time columns + self.convert_time_columns(states, [], ['start_timestamp', 'end_timestamp'], True) + + data[lifecycle_node_handle] = states + return data + def format_info_dict( self, info_dict: Mapping[str, Any],