diff --git a/src/benchmarks/beluga_vs_nav2/Dockerfile b/src/benchmarks/beluga_vs_nav2/Dockerfile index bfca6970..98ae1a03 100644 --- a/src/benchmarks/beluga_vs_nav2/Dockerfile +++ b/src/benchmarks/beluga_vs_nav2/Dockerfile @@ -22,7 +22,9 @@ RUN mkdir -p /workspace/src && chown -R $USERNAME:$USERNAME /workspace RUN apt-get update && apt upgrade -y && rm -rf /var/lib/apt/lists/* USER $USERNAME -RUN git -C /workspace/src clone https://github.com/Ekumen-OS/beluga +RUN cd /workspace/src && git clone https://github.com/Ekumen-OS/beluga +RUN cd /workspace/src && git clone --depth 1 --no-checkout https://github.com/ros-planning/navigation2.git && \ + cd navigation2 && git sparse-checkout set nav2_amcl && git checkout RUN --mount=type=bind,source=.,target=/workspace/src/beluga_vs_nav2,readonly cd /workspace && sudo apt update && rosdep update && \ rosdep install -i -y --from-paths src --skip-keys 'lambkin-shepherd flatland_server flatland_plugins' && \ diff --git a/src/benchmarks/beluga_vs_nav2/launch/beluga_vs_nav2.launch b/src/benchmarks/beluga_vs_nav2/launch/beluga_vs_nav2.launch index b6047d6a..64a60fea 100755 --- a/src/benchmarks/beluga_vs_nav2/launch/beluga_vs_nav2.launch +++ b/src/benchmarks/beluga_vs_nav2/launch/beluga_vs_nav2.launch @@ -1,6 +1,5 @@ - diff --git a/src/benchmarks/beluga_vs_nav2/reports/beluga_vs_nav2_report/conf.py b/src/benchmarks/beluga_vs_nav2/reports/beluga_vs_nav2_report/conf.py index 9183a77d..0e5497a4 100644 --- a/src/benchmarks/beluga_vs_nav2/reports/beluga_vs_nav2_report/conf.py +++ b/src/benchmarks/beluga_vs_nav2/reports/beluga_vs_nav2_report/conf.py @@ -66,6 +66,5 @@ (master_doc, 'report.tex', project, author, 'manual'), ] - def setup(app): app.add_config_value('sysroot', os.path.relpath('/', app.srcdir), rebuild=False) diff --git a/src/benchmarks/beluga_vs_nav2/reports/beluga_vs_nav2_report/index.rst b/src/benchmarks/beluga_vs_nav2/reports/beluga_vs_nav2_report/index.rst index 355659bc..ea399a35 100644 --- a/src/benchmarks/beluga_vs_nav2/reports/beluga_vs_nav2_report/index.rst +++ b/src/benchmarks/beluga_vs_nav2/reports/beluga_vs_nav2_report/index.rst @@ -3,6 +3,7 @@ import lambkin.shepherd.data as lks import pandas as pd import seaborn as sns + import numpy as np from matplotlib import pyplot as plt sns.set_theme(style='darkgrid') @@ -97,7 +98,7 @@ Results Nominal setup ^^^^^^^^^^^^^ -In the following, for a nominal setup this report assumes :math:`N = 2000` particles, a likelihood field sensor model, and a sequential (i.e. single-threaded) execution policy. +In the following, for a nominal setup this report assumes :math:`N = 2000` particles (fixed), a likelihood field sensor model, and a sequential (i.e. single-threaded) execution policy. .. repl-quiet:: @@ -116,7 +117,7 @@ In the following, for a nominal setup this report assumes :math:`N = 2000` parti lks.evo.series('/nav2_amcl/pose', 'ape', target_iterations=nominal_case, normalization='long'), lks.evo.series('/beluga_amcl/pose', 'rpe', target_iterations=nominal_case, normalization='long'), lks.evo.series('/nav2_amcl/pose', 'rpe', target_iterations=nominal_case, normalization='long') - ]).sample(frac=0.25).sort_index() + ]) data = data.replace({'metric.name': {'ape': 'APE', 'rpe': 'RPE'}}) @@ -129,7 +130,6 @@ In the following, for a nominal setup this report assumes :math:`N = 2000` parti grid.set_titles('{row_name} @ {col_name}') grid.set_axis_labels('Error (m)') grid._legend.set_title('Trajectory') - grid.tight_layout() plt.show() @@ -140,7 +140,7 @@ In the following, for a nominal setup this report assumes :math:`N = 2000` parti data = pd.concat([ lks.evo.series('/beluga_amcl/pose', 'ape', target_iterations=nominal_case, normalization='long'), lks.evo.series('/nav2_amcl/pose', 'ape', target_iterations=nominal_case, normalization='long') - ]).sample(frac=0.25).sort_index() + ]) grid = sns.relplot( data=data, @@ -148,12 +148,14 @@ In the following, for a nominal setup this report assumes :math:`N = 2000` parti y='metric.series.value', hue='trajectory.name', col='variation.parameters.dataset', - kind='line', n_boot=20 + kind='line', n_boot=20, + facet_kws=dict(sharex='col') ) grid.figure.suptitle('Absolute Pose Error (APE) over time') grid.set_axis_labels('Time (s)', 'APE (m)') grid.set_titles('{col_name} dataset') grid._legend.set_title('Trajectory') + grid.set(ylim=(0, None)) grid.tight_layout() plt.show() @@ -164,7 +166,7 @@ In the following, for a nominal setup this report assumes :math:`N = 2000` parti data = pd.concat([ lks.evo.series('/beluga_amcl/pose', 'rpe', target_iterations=nominal_case, normalization='long'), lks.evo.series('/nav2_amcl/pose', 'rpe', target_iterations=nominal_case, normalization='long') - ]).sample(frac=0.25).sort_index() + ]) grid = sns.relplot( data=data, @@ -172,12 +174,14 @@ In the following, for a nominal setup this report assumes :math:`N = 2000` parti y='metric.series.value', hue='trajectory.name', col='variation.parameters.dataset', - kind='line', n_boot=20 + kind='line', n_boot=20, + facet_kws=dict(sharex='col') ) grid.figure.suptitle('Relative Pose Error (RPE) over time') grid.set_axis_labels('Time (s)', 'RPE (m)') grid.set_titles('{col_name} dataset') grid._legend.set_title('Trajectory') + grid.set(ylim=(0, None)) grid.tight_layout() plt.show() @@ -191,7 +195,7 @@ In the following, for a nominal setup this report assumes :math:`N = 2000` parti trajectory_name_format='path', trajectory_file_format='tum'), lks.evo.trajectory('/beluga_amcl/pose', target_iterations=nominal_case), lks.evo.trajectory('/nav2_amcl/pose', target_iterations=nominal_case) - ]).sample(frac=0.25).sort_index() + ]) grid = sns.relplot( data=data, @@ -217,7 +221,9 @@ In the following, for a nominal setup this report assumes :math:`N = 2000` parti trajectory_name_format='path', trajectory_file_format='tum'), lks.evo.trajectory('/beluga_amcl/pose', target_iterations=nominal_case), lks.evo.trajectory('/nav2_amcl/pose', target_iterations=nominal_case) - ]).sample(frac=0.25).sort_index() + ]) + + data['trajectory.yaw'] = np.degrees(data['trajectory.yaw']) data = pd.melt( data, @@ -227,7 +233,7 @@ In the following, for a nominal setup this report assumes :math:`N = 2000` parti ) data = data.replace({'trajectory.coordinate.name': { - 'trajectory.x': 'x (m)', 'trajectory.y': 'y (m)', 'trajectory.yaw': 'yaw (rad)' + 'trajectory.x': 'x (m)', 'trajectory.y': 'y (m)', 'trajectory.yaw': 'yaw (deg)' }}) grid = sns.relplot( @@ -256,14 +262,14 @@ In the following, for a nominal setup this report assumes :math:`N = 2000` parti Setup sweep ^^^^^^^^^^^ -In the following, this report sweeps over parameterizations to compare the resulting performance. +In the following, this report sweeps over parameterizations to compare the resulting performance. Error bars represent bootstrapped 95% confidence intervals. .. repl-quiet:: data = pd.concat([ lks.evo.series('/beluga_amcl/pose', 'ape', normalization='long'), lks.evo.series('/nav2_amcl/pose', 'ape', normalization='long') - ]).sample(frac=0.25).sort_index() + ]) data['trajectory.qualified_name'] = data.apply( lambda row: '{} ({}, {})'.format( @@ -273,19 +279,33 @@ In the following, this report sweeps over parameterizations to compare the resul if 'beluga' in row['trajectory.name'] else 'seq' ), axis=1 ) + + data['variation.parameters.num_particles'] = pd.to_numeric( + data['variation.parameters.num_particles'] + ) + + data = data.sort_values([ + 'variation.parameters.dataset' + 'trajectory.qualified_name', + 'variation.parameters.num_particles' + ]) + grid = sns.relplot( data=data, y='metric.series.value', x='variation.parameters.num_particles', hue='trajectory.qualified_name', row='variation.parameters.dataset', - kind='line', n_boot=20, - height=4, aspect=2.5, + marker='o', kind='line', n_boot=20, + height=4, aspect=2.5, err_style='bars', facet_kws=dict(sharey=False) ) + xticks = np.sort(np.unique(data['variation.parameters.num_particles'])) + grid.set_xticks(xticks, labels=list(map(str, xticks))) grid.figure.suptitle('Absolute Pose Error (APE) with particle filter size') grid.set_axis_labels('Particle count', 'Error (m)') grid.set_titles('{row_name} dataset') grid._legend.set_title('Trajectory') + grid.set(ylim=(0, None)) grid.tight_layout() plt.show() @@ -296,7 +316,7 @@ In the following, this report sweeps over parameterizations to compare the resul data = pd.concat([ lks.evo.series('/beluga_amcl/pose', 'rpe', normalization='long'), lks.evo.series('/nav2_amcl/pose', 'rpe', normalization='long') - ]).sample(frac=0.25).sort_index() + ]) data['trajectory.qualified_name'] = data.apply( lambda row: '{} ({}, {})'.format( @@ -306,19 +326,33 @@ In the following, this report sweeps over parameterizations to compare the resul if 'beluga' in row['trajectory.name'] else 'seq' ), axis=1 ) + + data['variation.parameters.num_particles'] = pd.to_numeric( + data['variation.parameters.num_particles'] + ) + + data = data.sort_values([ + 'variation.parameters.dataset' + 'trajectory.qualified_name', + 'variation.parameters.num_particles' + ]) + grid = sns.relplot( data=data, y='metric.series.value', x='variation.parameters.num_particles', hue='trajectory.qualified_name', row='variation.parameters.dataset', - kind='line', n_boot=20, - height=4, aspect=2.5, + marker='o', kind='line', n_boot=20, + height=4, aspect=2.5, err_style='bars', facet_kws=dict(sharey=False) ) + xticks = np.sort(np.unique(data['variation.parameters.num_particles'])) + grid.set_xticks(xticks, labels=list(map(str, xticks))) grid.figure.suptitle('Relative Pose Error (RPE) with particle filter size') grid.set_axis_labels('Particle count', 'Error (m)') grid.set_titles('{row_name} dataset') grid._legend.set_title('Trajectory') + grid.set(ylim=(0, None)) grid.tight_layout() plt.show() @@ -329,7 +363,7 @@ In the following, this report sweeps over parameterizations to compare the resul data = pd.concat([ lks.timem.summary('beluga_amcl', 'cpu_util', normalization='long'), lks.timem.summary('nav2_amcl', 'cpu_util', normalization='long') - ]).sample(frac=0.25).sort_index() + ]) data = lks.pandas.rescale(data, {'process.summary.cpu_util': 100}) data['process.qualified_name'] = data.apply( @@ -340,19 +374,33 @@ In the following, this report sweeps over parameterizations to compare the resul if 'beluga' in row['process.name'] else 'seq' ), axis=1 ) + + data['variation.parameters.num_particles'] = pd.to_numeric( + data['variation.parameters.num_particles'] + ) + + data = data.sort_values([ + 'variation.parameters.dataset' + 'process.qualified_name', + 'variation.parameters.num_particles' + ]) + grid = sns.relplot( data=data, y='process.summary.cpu_util', x='variation.parameters.num_particles', hue='process.qualified_name', row='variation.parameters.dataset', - kind='line', n_boot=20, - height=4, aspect=2.5, + marker='o', kind='line', n_boot=20, + height=4, aspect=2.5, err_style='bars', facet_kws=dict(sharey=False) ) + xticks = np.sort(np.unique(data['variation.parameters.num_particles'])) + grid.set_xticks(xticks, labels=list(map(str, xticks))) grid.figure.suptitle('CPU usage with particle filter size') grid.set_axis_labels('Particle count', 'Usage (%)') grid.set_titles('{row_name} dataset') grid._legend.set_title('Process') + grid.set(ylim=(0, None)) grid.tight_layout() plt.show() @@ -363,7 +411,7 @@ In the following, this report sweeps over parameterizations to compare the resul data = pd.concat([ lks.timem.summary('beluga_amcl', 'peak_rss', normalization='long'), lks.timem.summary('nav2_amcl', 'peak_rss', normalization='long') - ]).sample(frac=0.25).sort_index() + ]) data = lks.pandas.rescale(data, {'process.summary.peak_rss': 1 / 8e6}) data['process.qualified_name'] = data.apply( @@ -375,19 +423,32 @@ In the following, this report sweeps over parameterizations to compare the resul ), axis=1 ) + data['variation.parameters.num_particles'] = pd.to_numeric( + data['variation.parameters.num_particles'] + ) + + data = data.sort_values([ + 'variation.parameters.dataset' + 'process.qualified_name', + 'variation.parameters.num_particles' + ]) + grid = sns.relplot( data=data, y='process.summary.peak_rss', x='variation.parameters.num_particles', hue='process.qualified_name', row='variation.parameters.dataset', - kind='line', n_boot=20, - height=4, aspect=2.5, + kind='line', marker='o', n_boot=20, + height=4, aspect=2.5, err_style='bars', facet_kws=dict(sharey=False) ) + xticks = np.sort(np.unique(data['variation.parameters.num_particles'])) + grid.set_xticks(xticks, labels=list(map(str, xticks))) grid.figure.suptitle('Peak RSS with particle filter size') grid.set_axis_labels('Particle count', 'Memory (MB)') grid.set_titles('{row_name} dataset') grid._legend.set_title('Process') + grid.set(ylim=(0, None)) grid.tight_layout() plt.show() diff --git a/src/benchmarks/beluga_vs_nav2/scripts/beluga_vs_nav2.robot b/src/benchmarks/beluga_vs_nav2/scripts/beluga_vs_nav2.robot index 277ac572..b434162d 100755 --- a/src/benchmarks/beluga_vs_nav2/scripts/beluga_vs_nav2.robot +++ b/src/benchmarks/beluga_vs_nav2/scripts/beluga_vs_nav2.robot @@ -26,7 +26,7 @@ Test Template Run Beluga vs Nav2 benchmark case for each ${dataset} ${lase *** Variables *** -@{NUM_PARTICLES} 250 300 400 500 750 1000 2000 5000 10000 20000 50000 100000 200000 +@{NUM_PARTICLES} 250 300 400 500 750 1000 2000 3000 4000 5000 @{LASER_MODELS} beam likelihood_field @{EXECUTION_POLICIES} seq par @@ -49,7 +49,7 @@ Beluga vs Nav2 benchmark case Extends ROS 2 2D SLAM system benchmark case # Setup benchmark inputs ${dataset_path} = Set Variable ${EXECDIR}/beluga-datasets/${dataset} - Uses ${dataset_path}/ROS2/${dataset}_bag at 2x as input to ROS 2 system + Uses ${dataset_path}/ROS2/${dataset}_bag as input to ROS 2 system ${package_share_path} = Find ROS 2 Package beluga_vs_nav2 share=yes ${qos_override_path} = Join Path ${package_share_path} config qos_override.yml Configures QoS overrides from ${qos_override_path} for input to ROS 2 system diff --git a/src/core/lambkin-shepherd/src/lambkin/shepherd/data/evo.py b/src/core/lambkin-shepherd/src/lambkin/shepherd/data/evo.py index a644faac..544eeca1 100644 --- a/src/core/lambkin-shepherd/src/lambkin/shepherd/data/evo.py +++ b/src/core/lambkin-shepherd/src/lambkin/shepherd/data/evo.py @@ -207,15 +207,15 @@ def _to_dict(trajectory: SomeTrajectoryType) -> Mapping: qx = trajectory.orientations_quat_wxyz[:, 1] qy = trajectory.orientations_quat_wxyz[:, 2] qz = trajectory.orientations_quat_wxyz[:, 3] - roll = np.arctan2( + roll = np.unwrap(np.arctan2( 2 * (qw * qx + qy * qz), - 1 - 2 * (qx**2 + qy**2)) - pitch = -np.pi / 2 + 2 * np.arctan2( + 1 - 2 * (qx**2 + qy**2))) + pitch = np.unwrap(-np.pi / 2 + 2 * np.arctan2( np.sqrt(1 + 2 * (qw * qy - qx * qz)), - np.sqrt(1 - 2 * (qw * qy - qx * qz))) - yaw = np.arctan2( + np.sqrt(1 - 2 * (qw * qy - qx * qz)))) + yaw = np.unwrap(np.arctan2( 2 * (qw * qz + qx * qy), - 1 - 2 * (qy**2 + qz**2)) + 1 - 2 * (qy**2 + qz**2))) return { 'time': time, 'time_since_epoch': time_since_epoch,