Add nominal beluga vs nav2 benchmark

Signed-off-by: Michel Hidalgo <[email protected]>

src/benchmarks/beluga_vs_nav2/CMakeLists.txt

@@ -6,7 +6,8 @@ find_package(ament_cmake_python REQUIRED)
 
 install(
   PROGRAMS
-    scripts/beluga_vs_nav2.robot
+    scripts/nominal.robot
+    scripts/swept.robot
   DESTINATION lib/${PROJECT_NAME})
 
 install(

src/benchmarks/beluga_vs_nav2/Dockerfile

@@ -30,7 +30,7 @@ RUN --mount=type=bind,source=.,target=/workspace/src/beluga_vs_nav2,readonly cd
     rosdep install -i -y --from-paths src --skip-keys 'lambkin-shepherd flatland_server flatland_plugins' && \
     sudo rm -rf /var/lib/apt/lists/*
 
-RUN sudo pip install sphinxcontrib.datatemplates sphinxcontrib-repl
+RUN sudo pip install linuxdoc sphinxcontrib.datatemplates sphinxcontrib-repl
 
 FROM development AS release
 

src/benchmarks/beluga_vs_nav2/launch/beluga_vs_nav2.launch

@@ -1,6 +1,7 @@
 <launch>
   <arg name="map_filename"/>
-  <arg name="num_particles" default="1000"/>
+  <arg name="max_particles" default="2000"/>
+  <arg name="min_particles" default="500"/>
   <arg name="laser_model_type" default="beam"/>
   <arg name="execution_policy" default="seq"/>
   <arg name="use_sim_time" default="false"/>
@@ -10,17 +11,17 @@
   <node pkg="nav2_amcl" exec="amcl" name="nav2_amcl" launch-prefix="$(env nav2_amcl_PREFIX '')">
     <param from="$(find-pkg-share beluga_vs_nav2)/params/amcl.yaml"/>
     <param name="laser_model_type" value="$(var laser_model_type)"/>
-    <param name="max_particles" value="$(var num_particles)"/>
-    <param name="min_particles" value="$(var num_particles)"/>
+    <param name="max_particles" value="$(var max_particles)"/>
+    <param name="min_particles" value="$(var min_particles)"/>
     <remap from="amcl_pose" to="/nav2_amcl/pose"/>
     <remap from="particle_cloud" to="/nav2_amcl/particle_cloud"/>
   </node>
 
   <node pkg="beluga_amcl" exec="amcl_node" name="beluga_amcl" launch-prefix="$(env beluga_amcl_PREFIX '')">
     <param from="$(find-pkg-share beluga_vs_nav2)/params/amcl.yaml"/>
     <param name="laser_model_type" value="$(var laser_model_type)"/>
-    <param name="max_particles" value="$(var num_particles)"/>
-    <param name="min_particles" value="$(var num_particles)"/>
+    <param name="max_particles" value="$(var max_particles)"/>
+    <param name="min_particles" value="$(var min_particles)"/>
     <param name="execution_policy" value="$(var execution_policy)"/>
     <remap from="pose" to="/beluga_amcl/pose"/>
     <remap from="particle_cloud" to="/beluga_amcl/particle_cloud"/>

src/benchmarks/beluga_vs_nav2/params/amcl.yaml

@@ -3,15 +3,15 @@
     # Odometry motion model type.
     robot_model_type: nav2_amcl::DifferentialMotionModel
     # Expected process noise in odometry’s rotation estimate from rotation.
-    alpha1: 0.2
+    alpha1: 0.1
     # Expected process noise in odometry’s rotation estimate from translation.
-    alpha2: 0.2
+    alpha2: 0.05
     # Expected process noise in odometry’s translation estimate from translation.
-    alpha3: 0.2
+    alpha3: 0.1
     # Expected process noise in odometry’s translation estimate from rotation.
-    alpha4: 0.2
+    alpha4: 0.05
     # Expected process noise in odometry's strafe estimate from translation.
-    alpha5: 0.2
+    alpha5: 0.1
     # The name of the coordinate frame published by the localization system.
     global_frame_id: map
     # The name of the coordinate frame published by the odometry system.
@@ -38,19 +38,19 @@
     # Fast exponential filter constant, used to filter the average particles weights.
     # Random particles are added if the fast filter result is larger than the slow filter result
     # allowing the particle filter to recover from a bad approximation.
-    recovery_alpha_fast: 0.
+    recovery_alpha_fast: 0.1
     # Slow exponential filter constant, used to filter the average particles weights.
     # Random particles are added if the fast filter result is larger than the slow filter result
     # allowing the particle filter to recover from a bad approximation.
-    recovery_alpha_slow: 0.
+    recovery_alpha_slow: 0.001
     # Resample will happen after the amount of updates specified here happen.
     resample_interval: 1
     # Minimum angle difference from last resample for resampling to happen again.
     update_min_a: 0.2
     # Maximum angle difference from last resample for resampling to happen again.
     update_min_d: 0.25
     # Laser sensor model type.
-    laser_model_type: beam
+    laser_model_type: likelihood_field
     # Maximum distance of an obstacle (if the distance is higher, this one will be used in the likelihood map).
     laser_likelihood_max_dist: 2.0
     # Maximum range of the laser.
@@ -61,6 +61,10 @@
     z_hit: 0.5
     # Weight used to combine the probability of random noise in perception.
     z_rand: 0.5
+    # Weight used to combine the probability of getting short readings.
+    z_short: 0.05
+    # Weight used to combine the probability of getting max range readings.
+    z_max: 0.05
     # Standard deviation of a gaussian centered arounds obstacles.
     sigma_hit: 0.2
     # Whether to broadcast map to odom transform or not.
@@ -73,6 +77,10 @@
     # Whether to set initial pose based on parameters.
     # When enabled, particles will be initialized with the specified pose coordinates and covariance.
     set_initial_pose: true
+    # If false, AMCL will use the last known pose to initialize when a new map is received.
+    always_reset_initial_pose: false
+    # Set this to true when you want to load only the first published map from map_server and ignore subsequent ones.
+    first_map_only: false
     # Initial pose x coordinate.
     initial_pose.x: 0.0
     # Initial pose y coordinate.

src/benchmarks/beluga_vs_nav2/reports/nominal_report/conf.py

@@ -0,0 +1,73 @@
+# -*- coding: utf-8 -*-
+#
+# Copyright 2023 Ekumen, Inc.
+#
+# 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.
+#
+# Configuration file for the Sphinx documentation builder.
+
+import os
+
+import ament_index_python
+
+# -- Project information -----------------------------------------------------
+
+project = 'Nominal Beluga AMCL Benchmark Report'
+copyright = '2023, Ekumen Inc.'
+author = 'Ekumen Inc.'
+
+version = '0.1.0'
+release = '0.1.0-alpha'
+
+# -- General configuration ---------------------------------------------------
+
+extensions = [
+    'linuxdoc.rstFlatTable',
+    'sphinxcontrib.datatemplates',
+    'sphinxcontrib.repl'
+]
+
+# The suffix(es) of source filenames.
+source_suffix = '.rst'
+
+# The master toctree document.
+master_doc = 'index'
+
+# The language for content autogenerated by Sphinx.
+language = 'en'
+
+# List of patterns, relative to source directory, that match files and
+# directories to ignore when looking for source files.
+exclude_patterns = ['build']
+
+# The name of the Pygments (syntax highlighting) style to use.
+pygments_style = None
+
+# -- Options for LaTeX output ------------------------------------------------
+
+latex_elements = {
+    'papersize': 'a4paper',
+    'pointsize': '10pt',
+    'extraclassoptions': 'openany,oneside'
+}
+
+latex_table_style = []
+
+# Grouping the document tree into LaTeX files.
+latex_documents = [
+    # (source, target name, title, author, documentclass)
+    (master_doc, 'report.tex', project, author, 'manual'),
+]
+
+def setup(app):
+    app.add_config_value('sysroot', os.path.relpath('/', app.srcdir), rebuild=False)

src/benchmarks/beluga_vs_nav2/reports/nominal_report/index.rst

@@ -0,0 +1,201 @@
+.. repl-quiet::
+
+    import lambkin.shepherd.data as lks
+    import pandas as pd
+    import numpy as np
+    import os
+
+    os.makedirs('_generated', exist_ok=True)
+
+
+Nominal Beluga AMCL vs Nav2 AMCL benchmark
+==========================================
+
+.. toctree::
+   :maxdepth: 2
+   :caption: Contents:
+
+Objective
+---------
+
+Compare `Beluga AMCL <https://github.com/Ekumen-OS/beluga>`_ and `Nav2 AMCL <https://github.com/ros-planning/navigation2/tree/main/nav2_amcl>`_ localization systems in terms of localization and computational performance for the nominal case.
+
+Methodology
+-----------
+
+Dataset
+^^^^^^^
+
+For this report, `Magazino datasets <https://google-cartographer-ros.readthedocs.io/en/latest/data.html#magazino>`_, published with the Cartographer Public Data set under Apache License v2.0, were chosen. As these datasets are distributed in rosbag format, equivalent datasets in rosbag2 format were recreated. As both localization systems need a map to work with, and a groundtruth is necessary for performance evaluation, offline mapping was conducted using Cartographer ROS.
+
+Configuration
+^^^^^^^^^^^^^
+
+For this report, the following baseline configuration:
+
+.. datatemplate:import-module:: ament_index_python
+
+    {% set package_path = data.get_package_share_directory('beluga_vs_nav2') %}
+
+    .. literalinclude:: {{config.sysroot}}/{{ package_path }}/params/amcl.yaml
+        :language: yaml
+
+was modified for each benchmark case in terms of:
+
+* the laser sensor model, to assess both beam and likelihood models (see sections 6.3 and 6.4 of Probabilistic Robotics, by Thrun et al);
+* the execution policy, to compare single-threaded and multi-threaded performance. Note this feature is only provided by Beluga AMCL.
+
+so as to have a reasonably complete picture of how both localization systems perform.
+
+Platform
+^^^^^^^^
+
+.. datatemplate:import-module:: lambkin.clerk
+
+    * Hardware
+      {% set cpu_info = data.hardware.cpu_info() %}
+        * CPU: {{ cpu_info.description }}
+            {% for cache in cpu_info.caches %}
+            * {{ cache }}
+            {% endfor %}
+        {% set memory_info = data.hardware.memory_info() %}
+        * Memory: {{ '{:~P}'.format(memory_info.ram_size.to('MB')) }}
+    * Software
+      {% set os_distribution_info = data.os.distribution_info() %}
+        * OS: {{ os_distribution_info.description  }}
+        * ROS:
+            {% set ros_distribution_info = data.ros2.distribution_info() %}
+            * Distribution: {{ ros_distribution_info.name }}
+            * Packages:
+              {% for name in ('beluga_amcl', 'nav2_amcl') %}
+              {% set pkg_info = data.ros2.package_info(name) %}
+                * ``{{ pkg_info.name }}`` {{ pkg_info.version }}
+              {% endfor %}
+
+Metrics
+^^^^^^^
+
+To characterize the localization performance of both systems, this report uses:
+
+* **APE**. The Absolute Pose Error is the difference between estimated and reference trajectories after alignment when taken as a whole. It is a measure of global accuracy and consistency.
+
+Metrics are aggregated across multiple runs of each parameter variation to ensure statistical significance.
+
+Results
+-------
+
+.. repl-quiet::
+
+    data = pd.concat([
+        lks.evo.series('/beluga_amcl/pose', 'ape', normalization='long'),
+        lks.evo.series('/nav2_amcl/pose', 'ape', normalization='long')
+    ])
+
+    data = data[[
+        'variation.parameters.dataset',
+        'variation.parameters.laser_model',
+        'trajectory.name', 'metric.series.value'
+    ]]
+
+    def rms(x):
+        return np.sqrt(np.mean(np.power(x, 2.)))
+
+    data = data.groupby([
+        'variation.parameters.dataset',
+        'trajectory.name',
+        'variation.parameters.laser_model',
+    ])['metric.series.value']
+
+    ape = data.agg([rms, 'mean', 'std', 'max']).stack().round(3)
+    ape.to_pickle('_generated/ape.pkl')
+
+
+.. datatemplate:import-module:: pandas
+
+    {% set data = data.read_pickle('_generated/ape.pkl') %}
+    {% set ape = data.loc['hallway_localization'] %}
+
+    .. flat-table:: APE metrics for ``hallway_localization`` trajectories
+       :header-rows: 2
+       :name: hallway-localization-ape-comparison
+
+       *
+         - :rspan:`1`  :cspan:`1`  Implementation
+         - :cspan:`3` Likelihood field sensor model
+         - :cspan:`3` Beam sensor model
+
+       *
+         - rms
+         - mean
+         - stddev
+         - max
+         - rms
+         - mean
+         - stddev
+         - max
+
+       *
+         - :cspan:`1` Beluga AMCL
+         - {{ ape.loc['/beluga_amcl/pose', 'likelihood_field', 'rms'] }} m
+         - {{ ape.loc['/beluga_amcl/pose', 'likelihood_field', 'mean'] }} m
+         - {{ ape.loc['/beluga_amcl/pose', 'likelihood_field', 'std'] }} m
+         - {{ ape.loc['/beluga_amcl/pose', 'likelihood_field', 'max'] }} m
+         - {{ ape.loc['/beluga_amcl/pose', 'beam', 'rms'] }} m
+         - {{ ape.loc['/beluga_amcl/pose', 'beam', 'mean'] }} m
+         - {{ ape.loc['/beluga_amcl/pose', 'beam', 'std'] }} m
+         - {{ ape.loc['/beluga_amcl/pose', 'beam', 'max'] }} m
+
+       *
+         - :cspan:`1` Nav2 AMCL
+         - {{ ape.loc['/nav2_amcl/pose', 'likelihood_field', 'rms'] }} m
+         - {{ ape.loc['/nav2_amcl/pose', 'likelihood_field', 'mean'] }} m
+         - {{ ape.loc['/nav2_amcl/pose', 'likelihood_field', 'std'] }} m
+         - {{ ape.loc['/nav2_amcl/pose', 'likelihood_field', 'max'] }} m
+         - {{ ape.loc['/nav2_amcl/pose', 'beam', 'rms'] }} m
+         - {{ ape.loc['/nav2_amcl/pose', 'beam', 'mean'] }} m
+         - {{ ape.loc['/nav2_amcl/pose', 'beam', 'std'] }} m
+         - {{ ape.loc['/nav2_amcl/pose', 'beam', 'max'] }} m
+
+
+    {% set ape = data.loc['hallway_return'] %}
+
+    .. flat-table:: APE metrics for ``hallway_return`` trajectories
+       :header-rows: 2
+       :name: hallway-return-ape-comparison
+
+       *
+         - :rspan:`1`  :cspan:`1`  Implementation
+         - :cspan:`3` Likelihood field sensor model
+         - :cspan:`3` Beam sensor model
+
+       *
+         - rms
+         - mean
+         - stddev
+         - max
+         - rms
+         - mean
+         - stddev
+         - max
+
+       *
+         - :cspan:`1` Beluga AMCL
+         - {{ ape.loc['/beluga_amcl/pose', 'likelihood_field', 'rms'] }} m
+         - {{ ape.loc['/beluga_amcl/pose', 'likelihood_field', 'mean'] }} m
+         - {{ ape.loc['/beluga_amcl/pose', 'likelihood_field', 'std'] }} m
+         - {{ ape.loc['/beluga_amcl/pose', 'likelihood_field', 'max'] }} m
+         - {{ ape.loc['/beluga_amcl/pose', 'beam', 'rms'] }} m
+         - {{ ape.loc['/beluga_amcl/pose', 'beam', 'mean'] }} m
+         - {{ ape.loc['/beluga_amcl/pose', 'beam', 'std'] }} m
+         - {{ ape.loc['/beluga_amcl/pose', 'beam', 'max'] }} m
+
+       *
+         - :cspan:`1` Nav2 AMCL
+         - {{ ape.loc['/nav2_amcl/pose', 'likelihood_field', 'rms'] }} m
+         - {{ ape.loc['/nav2_amcl/pose', 'likelihood_field', 'mean'] }} m
+         - {{ ape.loc['/nav2_amcl/pose', 'likelihood_field', 'std'] }} m
+         - {{ ape.loc['/nav2_amcl/pose', 'likelihood_field', 'max'] }} m
+         - {{ ape.loc['/nav2_amcl/pose', 'beam', 'rms'] }} m
+         - {{ ape.loc['/nav2_amcl/pose', 'beam', 'mean'] }} m
+         - {{ ape.loc['/nav2_amcl/pose', 'beam', 'std'] }} m
+         - {{ ape.loc['/nav2_amcl/pose', 'beam', 'max'] }} m