From 0780b447d44825e9c85c3d15a23d2092c7ccaac2 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Sat, 28 Oct 2023 16:14:17 -0300 Subject: [PATCH] Add nominal beluga vs nav2 benchmark Signed-off-by: Michel Hidalgo --- src/benchmarks/beluga_vs_nav2/CMakeLists.txt | 3 +- src/benchmarks/beluga_vs_nav2/Dockerfile | 2 +- .../launch/beluga_vs_nav2.launch | 11 +- .../beluga_vs_nav2/params/amcl.yaml | 24 ++- .../reports/nominal_report/conf.py | 73 +++++++ .../reports/nominal_report/index.rst | 201 ++++++++++++++++++ .../conf.py | 0 .../index.rst | 0 .../beluga_vs_nav2/scripts/nominal.robot | 62 ++++++ .../{beluga_vs_nav2.robot => swept.robot} | 7 +- 10 files changed, 365 insertions(+), 18 deletions(-) create mode 100644 src/benchmarks/beluga_vs_nav2/reports/nominal_report/conf.py create mode 100644 src/benchmarks/beluga_vs_nav2/reports/nominal_report/index.rst rename src/benchmarks/beluga_vs_nav2/reports/{beluga_vs_nav2_report => swept_report}/conf.py (100%) rename src/benchmarks/beluga_vs_nav2/reports/{beluga_vs_nav2_report => swept_report}/index.rst (100%) create mode 100755 src/benchmarks/beluga_vs_nav2/scripts/nominal.robot rename src/benchmarks/beluga_vs_nav2/scripts/{beluga_vs_nav2.robot => swept.robot} (91%) diff --git a/src/benchmarks/beluga_vs_nav2/CMakeLists.txt b/src/benchmarks/beluga_vs_nav2/CMakeLists.txt index 6e66db2c..2bfab4be 100644 --- a/src/benchmarks/beluga_vs_nav2/CMakeLists.txt +++ b/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( diff --git a/src/benchmarks/beluga_vs_nav2/Dockerfile b/src/benchmarks/beluga_vs_nav2/Dockerfile index 98ae1a03..23ee8dd5 100644 --- a/src/benchmarks/beluga_vs_nav2/Dockerfile +++ b/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 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 64a60fea..523dbf1a 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,7 @@ - + + @@ -10,8 +11,8 @@ - - + + @@ -19,8 +20,8 @@ - - + + diff --git a/src/benchmarks/beluga_vs_nav2/params/amcl.yaml b/src/benchmarks/beluga_vs_nav2/params/amcl.yaml index 06924d24..2d5abbb6 100644 --- a/src/benchmarks/beluga_vs_nav2/params/amcl.yaml +++ b/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,11 +38,11 @@ # 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. @@ -50,7 +50,7 @@ # 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. diff --git a/src/benchmarks/beluga_vs_nav2/reports/nominal_report/conf.py b/src/benchmarks/beluga_vs_nav2/reports/nominal_report/conf.py new file mode 100644 index 00000000..6df5e59c --- /dev/null +++ b/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) diff --git a/src/benchmarks/beluga_vs_nav2/reports/nominal_report/index.rst b/src/benchmarks/beluga_vs_nav2/reports/nominal_report/index.rst new file mode 100644 index 00000000..a260d529 --- /dev/null +++ b/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 `_ and `Nav2 AMCL `_ localization systems in terms of localization and computational performance for the nominal case. + +Methodology +----------- + +Dataset +^^^^^^^ + +For this report, `Magazino datasets `_, 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 diff --git a/src/benchmarks/beluga_vs_nav2/reports/beluga_vs_nav2_report/conf.py b/src/benchmarks/beluga_vs_nav2/reports/swept_report/conf.py similarity index 100% rename from src/benchmarks/beluga_vs_nav2/reports/beluga_vs_nav2_report/conf.py rename to src/benchmarks/beluga_vs_nav2/reports/swept_report/conf.py diff --git a/src/benchmarks/beluga_vs_nav2/reports/beluga_vs_nav2_report/index.rst b/src/benchmarks/beluga_vs_nav2/reports/swept_report/index.rst similarity index 100% rename from src/benchmarks/beluga_vs_nav2/reports/beluga_vs_nav2_report/index.rst rename to src/benchmarks/beluga_vs_nav2/reports/swept_report/index.rst diff --git a/src/benchmarks/beluga_vs_nav2/scripts/nominal.robot b/src/benchmarks/beluga_vs_nav2/scripts/nominal.robot new file mode 100755 index 00000000..3e43f0d7 --- /dev/null +++ b/src/benchmarks/beluga_vs_nav2/scripts/nominal.robot @@ -0,0 +1,62 @@ +#!/usr/bin/env -S shepherd robot -f + +# Copyright 2022 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. + + +*** Settings *** +Documentation Nominal Beluga AMCL vs Nav2 AMCL benchmark using 2D datasets. + +Resource lambkin/shepherd/robot/resources/all.resource + +Suite Setup Setup Beluga vs Nav2 benchmark suite +Suite Teardown Teardown Beluga vs Nav2 benchmark suite +Test Template Run Beluga vs Nav2 benchmark case for each ${dataset} ${laser_model} + + +*** Variables *** +@{LASER_MODELS} beam likelihood_field + + +*** Test Cases *** DATASET LASER_MODEL +Hallway Localization hallway_localization ${{LASER_MODELS}} +Hallway Return hallway_return ${{LASER_MODELS}} + + +*** Keywords *** +Beluga vs Nav2 benchmark suite + Extends ROS 2 system benchmark suite + Extends ROS 2 2D SLAM system benchmark suite + Generates latexpdf report from nominal_report template in beluga_vs_nav2 ROS 2 package + +Beluga vs Nav2 benchmark case + Extends ROS 2 system 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 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 + # Setup benchmark rig + Uses beluga_vs_nav2.launch in beluga_vs_nav2 ROS package as rig + Sets map_filename launch argument to ${dataset_path}/map.yaml + Sets laser_model_type launch argument to ${laser_model} + Sets use_sim_time launch argument to true + # Setup benchmark analysis + Tracks /nav2_amcl/pose /beluga_amcl/pose trajectories + Uses ${dataset_path}/groundtruth.tum as trajectory groundtruth + Performs trajectory corrections align=yes t_max_diff=${0.1} + + Uses 10 iterations diff --git a/src/benchmarks/beluga_vs_nav2/scripts/beluga_vs_nav2.robot b/src/benchmarks/beluga_vs_nav2/scripts/swept.robot similarity index 91% rename from src/benchmarks/beluga_vs_nav2/scripts/beluga_vs_nav2.robot rename to src/benchmarks/beluga_vs_nav2/scripts/swept.robot index b434162d..d92fbb2c 100755 --- a/src/benchmarks/beluga_vs_nav2/scripts/beluga_vs_nav2.robot +++ b/src/benchmarks/beluga_vs_nav2/scripts/swept.robot @@ -16,7 +16,7 @@ *** Settings *** -Documentation Beluga AMCL vs Nav2 AMCL benchmarks for 2D datasets. +Documentation Beluga AMCL vs Nav2 AMCL benchmark over configuration space. Resource lambkin/shepherd/robot/resources/all.resource @@ -41,7 +41,7 @@ Beluga vs Nav2 benchmark suite Extends ROS 2 system benchmark suite Extends ROS 2 2D SLAM system benchmark suite Extends generic resource usage benchmark suite - Generates latexpdf report from beluga_vs_nav2_report template in beluga_vs_nav2 ROS 2 package + Generates latexpdf report from swept_report template in beluga_vs_nav2 ROS 2 package Beluga vs Nav2 benchmark case Extends generic resource usage benchmark case @@ -57,7 +57,8 @@ Beluga vs Nav2 benchmark case Uses beluga_vs_nav2.launch in beluga_vs_nav2 ROS package as rig Sets map_filename launch argument to ${dataset_path}/map.yaml Sets laser_model_type launch argument to ${laser_model} - Sets num_particles launch argument to ${num_particles} + Sets max_particles launch argument to ${num_particles} + Sets min_particles launch argument to ${num_particles} Sets execution_policy launch argument to ${execution_policy} Sets use_sim_time launch argument to true # Setup benchmark profiling