Add script to run benchmarks (#117)

Related to #35.

Adds a script that allows running a rosbag repeatedly, configuring amcl
with different number of particles.
It records another bagfile with the result, the results of timem, and
optionally perf events data.

It also adds postprocessing scripts, and instructions of how to run them.

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>

beluga_amcl/src/amcl_node.cpp

@@ -610,7 +610,7 @@ void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map)
 
     mean.x() = this->get_parameter("initial_pose.x").as_double();
     mean.y() = this->get_parameter("initial_pose.y").as_double();
-    mean.z() = this->get_parameter("initial_pose.z").as_double();
+    mean.z() = this->get_parameter("initial_pose.yaw").as_double();
     covariance.coeffRef(0, 0) = this->get_parameter("initial_pose.covariance_x").as_double();
     covariance.coeffRef(1, 1) = this->get_parameter("initial_pose.covariance_y").as_double();
     covariance.coeffRef(2, 2) = this->get_parameter("initial_pose.covariance_yaw").as_double();
@@ -752,7 +752,7 @@ void AmclNode::laser_callback(
 
   {
     auto message = geometry_msgs::msg::PoseWithCovarianceStamped{};
-    message.header.stamp = now();
+    message.header.stamp = laser_scan->header.stamp;
     message.header.frame_id = get_parameter("global_frame_id").as_string();
     tf2::toMsg(pose, message.pose.pose);
     message.pose.covariance = tf2::covarianceEigenToRowMajor(covariance);

beluga_benchmark/CMakeLists.txt

@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 3.16)
+project(beluga_benchmark)
+
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(beluga_amcl REQUIRED)
+
+ament_python_install_package(${PROJECT_NAME})
+
+install(DIRECTORY
+  scripts/profiling
+  scripts/benchmarking
+  DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

beluga_benchmark/beluga_benchmark/__init__.py

@@ -0,0 +1,23 @@
+# 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.
+
+from .exceptions import ScriptError
+from . import compare_results
+from . import timem_results
+
+__all__ = [
+    'ScriptError',
+    'compare_results',
+    'timem_results',
+]

beluga_benchmark/beluga_benchmark/compare_results.py

@@ -0,0 +1,147 @@
+# 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.
+
+import argparse
+from pathlib import Path
+import re
+
+from evo import main_ape
+from evo.tools import file_interface
+import matplotlib.pyplot as plt
+from rosbag2_py._info import Info as Bag2Info
+import pandas as pd
+
+from . import timem_results
+from .exceptions import ScriptError
+
+DIR_NAME_PATTERN = 'benchmark_([0-9]+)_particles_output'
+SUPPORTED_TOPIC_NAMES = ('/pose', '/amcl_pose')
+EVO_RESULTS_FILE_NAME = 'evo_results.zip'
+
+
+def get_bag_est_topic(bag_path: Path):
+    metadata = Bag2Info().read_metadata(str(bag_path), '')
+    for topic_name in SUPPORTED_TOPIC_NAMES:
+        if any(
+            topic_info.topic_metadata.name == topic_name
+            for topic_info in metadata.topics_with_message_count
+        ):
+            return topic_name
+    raise ScriptError(
+        f'Estimate pose topic was not found in bag file, expected names: {SUPPORTED_TOPIC_NAMES}'
+    )
+
+
+def run_evo_ape(dir_path: Path):
+    evo_results_path = dir_path / EVO_RESULTS_FILE_NAME
+    if evo_results_path.exists():
+        return
+    bag_path = dir_path / 'rosbag'
+    est_topic = get_bag_est_topic(bag_path)
+    arg_parser = main_ape.parser()
+    args = arg_parser.parse_args(
+        [
+            'bag2',
+            str(bag_path),
+            '/odometry/ground_truth',
+            est_topic,
+            '--save_results',
+            str(evo_results_path.resolve()),
+        ]
+    )
+    main_ape.run(args)
+
+
+def read_evo_stats(zip_file_path):
+    # keys: "rmse", "mean", "median", "std", "min", "max", "sse"
+    return file_interface.load_res_file(zip_file_path).stats
+
+
+def create_parameterized_series(results_path: Path):
+    particles = []
+    peak_rss = []
+    cpu_usage = []
+    ape_rmse = []
+    ape_mean = []
+    ape_median = []
+    ape_max = []
+
+    for dir in results_path.iterdir():
+        if not dir.is_dir():
+            continue
+        m = re.match(DIR_NAME_PATTERN, dir.name)
+        if not m:
+            continue
+        particles.append(int(m[1]))
+        timem_results_output = timem_results.read_timem_output(dir)
+        rss, cpu = timem_results.get_timem_metrics_values(timem_results_output)
+        peak_rss.append(rss)
+        cpu_usage.append(cpu)
+        run_evo_ape(dir)
+        evo_results_path = dir / EVO_RESULTS_FILE_NAME
+        stats = read_evo_stats(evo_results_path)
+        ape_rmse.append(stats["rmse"])
+        ape_max.append(stats["max"])
+        ape_mean.append(stats["mean"])
+        ape_median.append(stats["median"])
+    return (
+        pd.DataFrame(
+            {
+                'particles_n': particles,
+                'peak_rss': peak_rss,
+                'cpu_usage': cpu_usage,
+                'ape_rmse': ape_rmse,
+                'ape_mean': ape_mean,
+                'ape_median': ape_median,
+                'ape_max': ape_max,
+            }
+        )
+        .set_index('particles_n')
+        .sort_index()
+    )
+
+
+def main():
+    arg_parser = argparse.ArgumentParser(
+        description='Script to compare parameterized run results of beluga with another'
+        ' implementation (e.g. nav2_amcl, another beluga version, etc).'
+    )
+    arg_parser.add_argument(
+        '--beluga-results',
+        '-b',
+        type=Path,
+        help='Folder with parameterized beluga benchmark results',
+        required=True,
+    )
+    arg_parser.add_argument(
+        '--other-results',
+        '-o',
+        type=Path,
+        help='Folder with parameterized benchmark results for the other implementation',
+        required=True,
+    )
+    args = arg_parser.parse_args()
+    beluga_series = create_parameterized_series(args.beluga_results).add_prefix(
+        'beluga_'
+    )
+    other_series = create_parameterized_series(args.other_results).add_prefix('other_')
+
+    ax = beluga_series.plot(subplots=True, color='red')
+    other_series.plot(ax=ax, subplots=True, color='blue')
+    for ax in plt.gcf().axes:
+        ax.legend(fontsize='small', loc='upper left', bbox_to_anchor=(1.01, 1))
+        current_bounds = ax.get_position().bounds
+        new_bounds = (0.05, *current_bounds[1:])
+        ax.set_position(new_bounds)
+    plt.show()

beluga_benchmark/beluga_benchmark/exceptions.py

@@ -0,0 +1,17 @@
+# 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.
+
+
+class ScriptError(Exception):
+    pass

beluga_benchmark/beluga_benchmark/timem_results.py

@@ -0,0 +1,98 @@
+# 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.
+
+import argparse
+import json
+from pathlib import Path
+
+import matplotlib.pyplot as plt
+import numpy as np
+import pandas as pd
+
+from .exceptions import ScriptError
+
+DEFAULT_FILE_NAME = "timem-output.json"
+
+
+def read_timem_output(dir_path: Path):
+    file = dir_path / DEFAULT_FILE_NAME
+    try:
+        with open(file, mode='r') as f:
+            output = json.load(f)
+    except (FileExistsError, FileNotFoundError) as ex:
+        raise ScriptError(f"Failed to open file '{file.absolute()}': {ex}")
+    return output['timemory']['timem'][0]
+
+
+def get_value_with_unit(data):
+    return f"{data['value']:.2f}{data['unit_repr']}"
+
+
+def get_timem_metrics(output):
+    return output['peak_rss'], output['cpu_util']
+
+
+def get_timem_metrics_values(output):
+    return tuple(data['value'] for data in get_timem_metrics(output))
+
+
+def get_timem_metrics_with_units(output):
+    return tuple(get_value_with_unit(data) for data in get_timem_metrics(output))
+
+
+def create_timeseries(output):
+    data = output['wall_clock']
+    elapsed_time = data['value']
+    time = np.arange(0.0, elapsed_time, 0.2)
+    wall_clock_to_time_offset = output['history'][0]['wall_clock']['value']
+    wall_clock_to_time_scale = elapsed_time / (
+        output['history'][-1]['wall_clock']['value'] - wall_clock_to_time_offset
+    )
+    time = []
+    rss = []
+    virtual_memory = []
+    for sample in output['history']:
+        time.append(
+            (sample['wall_clock']['value'] - wall_clock_to_time_offset)
+            * wall_clock_to_time_scale
+        )
+        rss.append(sample['page_rss']['value'])
+        virtual_memory.append(sample['virtual_memory']['value'])
+
+    series = pd.DataFrame(
+        {'time': time, 'rss': rss, 'virtual_memory': virtual_memory}
+    ).set_index('time')
+    return series
+
+
+def main():
+    arg_parser = argparse.ArgumentParser(
+        description='Script to postprocess timem results.'
+    )
+    arg_parser.add_argument(
+        'dir', help='Directory with timem-output.json file', type=Path
+    )
+    args = arg_parser.parse_args()
+    dir_path = Path(args.dir)
+    name = dir_path.name
+    output = read_timem_output(dir_path)
+    peak_rss_str, cpu_usage_str = get_timem_metrics_with_units(output)
+    series = create_timeseries(output)
+    elapsed_time_str = get_value_with_unit(output['wall_clock'])
+    print(f"timem metrics for run '{name}':")
+    print(f"\telapsed_time: {elapsed_time_str}")
+    print(f"\tcpu_usage: {cpu_usage_str}")
+    print(f"\tpeak_rss: {peak_rss_str}")
+    series.plot(subplots=True)
+    plt.show()