Wrap up Beluga vs Nav2 benchmark report

Signed-off-by: Michel Hidalgo <[email protected]>
Ekumen-OS · Oct 29, 2023 · 970f4d8 · 970f4d8
1 parent 3a8ce1b
commit 970f4d8
Show file tree

Hide file tree

Showing 6 changed files with 95 additions and 34 deletions.
diff --git 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
@@ -1,6 +1,5 @@
 <launch>
   <arg name="map_filename"/>
-  <!-- <arg name="location_params_filename"/> -->
   <arg name="num_particles" default="1000"/>
   <arg name="laser_model_type" default="beam"/>
   <arg name="execution_policy" default="seq"/>

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
@@ -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
@@ -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,20 +140,22 @@ 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,
         x='metric.series.time',
         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,20 +166,22 @@ 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,
         x='metric.series.time',
         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
@@ -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
@@ -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,