Add Lambkin benchmark for Beluga AMCL

README.md

@@ -43,6 +43,7 @@ This repository contains the following packages:
 | [`beluga_amcl`](beluga_amcl)                 | A ROS wrapper, providing an executable node and component (or nodelet).<br> It provides interface parity with `nav2_amcl` (and `amcl`). |
 | [`beluga_example`](beluga_example)           | Example launch files, showing how to run Beluga-based nodes.                                                            |
 | [`beluga_system_tests`](beluga_system_tests) | System integration tests for Beluga.                                                                                    |
+| [`beluga_lambkin`](beluga_lambkin) | Lambkin-based benchmark for `beluga_amcl`. |
 | [`beluga_vdb`](beluga_vdb) | A library extension for `beluga` facilitating the use of OpenVDB for 3D localization.                                                                                    |
 
 ## ⚙️ First Steps

beluga_lambkin/README.md

@@ -0,0 +1,50 @@
+# Beluga Lambkin Benchmark
+
+Benchmarks [`beluga_amcl`](../beluga_amcl) with [Lambkin](https://github.com/Ekumen-OS/lambkin).
+
+## Contents
+
+- `beluga_benchmark.py`
+- `requirements.txt` (Lambkin pin)
+- `beluga_inference_ros2/` (launch + params used by the benchmark)
+
+## Environment
+
+Lambkin is installed in the jazzy, kilted and lyrical dev images.
+
+Start the container with:
+```bash
+ROSDISTRO=jazzy docker/run.sh --build     # or kilted / lyrical
+```
+
+## Data
+
+Needs an MCAP dataset and a map YAML. Reference data: https://huggingface.co/datasets/ekumenlabs/lambkin-beluga-datasets
+
+Mount them with a local `docker/docker-compose.override.yml` (not committed), which `docker/run.sh` picks up automatically:
+```yaml
+services:
+  dev:
+    volumes:
+      - /path/to/datasets:/data/datasets
+      - /path/to/maps:/data/maps
+```
+
+## Run
+
+Inside the container, from `/ws`:
+```bash
+colcon build --packages-up-to beluga_inference_ros2
+source install/setup.bash
+python3 src/beluga/beluga_lambkin/beluga_benchmark.py --dataset /data/datasets/input --map /data/maps/map.yaml
+```
+
+## Outputs
+
+- `output.ape.zip`
+- `plots.png`
+- RMSE/mean/max stats in the log
+
+## Updating Lambkin
+
+Change the commit SHA in `requirements.txt`, then rebuild the image with `--build`.

beluga_lambkin/beluga_benchmark.py

@@ -0,0 +1,113 @@
+# Copyright 2026 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.
+"""Nominal benchmark for the Beluga AMCL localization system."""
+
+import matplotlib.pyplot as plt
+
+import lambkin
+
+
+@lambkin.benchmark(
+    variants=lambkin.common.named_product(
+        laser_model_type=["beam", "likelihood_field"], max_particles=[10, 20, 100]
+    ),
+    num_iterations=2,
+)
+@lambkin.option("--clock-rate", default=1.0)
+@lambkin.option("--sensor-topic", default="scan_front")
+@lambkin.option("--dataset", default="/data/datasets/input")
+@lambkin.option("--map", default="/data/maps/map.yaml")
+def nominal(ctx):
+    """Run a nominal Beluga AMCL benchmark across sensor models and particle counts.
+
+    Launches the Beluga localization stack, plays back a ROS 2 bag,
+    and evaluates trajectory accuracy with evo_ape.
+
+    Args:
+        ctx: Lambkin context with variant, options, source, and shell access.
+    """
+    lambkin.logger.info(
+        "Running benchmark with variant: %s, iteration: %d", ctx.variant, ctx.iteration
+    )
+
+    with lambkin.process.background(
+        ctx.shell.ros2.bag.record,
+        "--output",
+        "output",
+        "-a",
+    ):
+        with lambkin.process.background(
+            ctx.shell.ros2.launch,
+            "beluga_inference_ros2",
+            "beluga.launch.py",
+            f"laser_model_type:={ctx.variant.laser_model_type}",
+            f"max_particles:={ctx.variant.max_particles}",
+            f"scan_topic:={ctx.options.sensor_topic}",
+            f"map_path:={ctx.inputs.map}",
+        ):
+            ctx.shell.ros2.bag.play(
+                ctx.inputs.dataset, "--clock", "-r", ctx.options.clock_rate
+            )
+    ctx.shell.evo_ape.bag2(
+        "output",
+        "/ground_truth",
+        "/pose",
+        "--t_max_diff",
+        "0.5",
+        "--save_results",
+        "output.ape.zip",
+    )
+
+
+@nominal.input
+def dataset(ctx):
+    """Return the path to the MCAP dataset used as input for the benchmark."""
+    return ctx.options.dataset
+
+
+@nominal.input
+def map(ctx):
+    """Return the path to the map file used for localization."""
+    return ctx.options.map
+
+
+@nominal.output
+def plots(ctx):
+    """Save APE timeseries plot for all iterations."""
+    for entry in lambkin.data.evo.series(ctx, "output.ape.zip"):
+        plt.plot(
+            entry.time, entry.error, label=f"{entry.variant} / iter {entry.iteration}"
+        )
+    plt.xlabel("Time (s)")
+    plt.ylabel("Error (m)")
+    plt.legend()
+    plt.savefig(ctx.base_dir / "plots.png")
+
+
+@nominal.output
+def stats(ctx):
+    """Log RMSE, mean, and max APE for all iterations."""
+    for entry in lambkin.data.evo.stats(ctx, "output.ape.zip"):
+        lambkin.logger.info(
+            "%s iter %d: rmse=%.4f mean=%.4f max=%.4f",
+            entry.variant,
+            entry.iteration,
+            entry.rmse,
+            entry.mean,
+            entry.max,
+        )
+
+
+if __name__ == "__main__":
+    nominal()

beluga_lambkin/beluga_inference_ros2/CMakeLists.txt

@@ -0,0 +1,25 @@
+# Copyright 2026 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.
+
+cmake_minimum_required(VERSION 3.16)
+project(beluga_inference_ros2)
+find_package(ament_cmake REQUIRED)
+find_package(beluga_amcl REQUIRED)
+find_package(nav2_map_server REQUIRED)
+find_package(nav2_lifecycle_manager REQUIRED)
+
+install(DIRECTORY launch params DESTINATION share/${PROJECT_NAME})
+
+ament_export_dependencies(launch launch_ros)
+ament_package()

beluga_lambkin/beluga_inference_ros2/launch/beluga.launch.py

@@ -0,0 +1,107 @@
+# Copyright 2026 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.
+
+"""Launch file for the Beluga AMCL benchmarking environment."""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+pkg_dir = get_package_share_directory("beluga_inference_ros2")
+
+default_yaml_path = os.path.join(pkg_dir, "params", "default.ros2.yaml")
+
+
+def generate_launch_description():
+    """Generate the launch description for the Beluga AMCL benchmarking environment.
+
+    Declares launch arguments for the map path, laser model type,
+    maximum particles, and scan topic, and configures the required ROS 2 nodes:
+    beluga_amcl, map_server, and lifecycle_manager.
+
+    Returns:
+        LaunchDescription: The complete ROS 2 launch description object.
+    """
+    config_file_arg = DeclareLaunchArgument(
+        "params_file",
+        default_value=default_yaml_path,
+        description="Absolute YAML file path",
+    )
+
+    map_path_arg = DeclareLaunchArgument("map_path", description="Absolute map path")
+
+    laser_model_arg = DeclareLaunchArgument(
+        "laser_model_type",
+        default_value="likelihood_field",
+        description="Sensor model",
+        choices=["likelihood_field", "beam"],
+    )
+
+    max_particles_arg = DeclareLaunchArgument(
+        "max_particles", default_value="2000", description="Max number of particles"
+    )
+
+    scan_topic_arg = DeclareLaunchArgument(
+        "scan_topic",
+        default_value="scan_front",
+        description="Sensor scan topic",
+    )
+
+    beluga_node = Node(
+        package="beluga_amcl",
+        executable="amcl_node",
+        name="beluga_amcl",
+        output="screen",
+        parameters=[
+            LaunchConfiguration("params_file"),
+            {
+                "laser_model_type": LaunchConfiguration("laser_model_type"),
+                "max_particles": LaunchConfiguration("max_particles"),
+                "scan_topic": LaunchConfiguration("scan_topic"),
+            },
+        ],
+    )
+
+    map_server_node = Node(
+        package="nav2_map_server",
+        executable="map_server",
+        name="map_server",
+        output="screen",
+        parameters=[{"yaml_filename": LaunchConfiguration("map_path")}],
+    )
+
+    lifecycle_manager_node = Node(
+        package="nav2_lifecycle_manager",
+        executable="lifecycle_manager",
+        name="lifecycle_manager_localization",
+        output="screen",
+        parameters=[{"autostart": True, "node_names": ["map_server", "beluga_amcl"]}],
+    )
+
+    return LaunchDescription(
+        [
+            config_file_arg,
+            map_path_arg,
+            laser_model_arg,
+            max_particles_arg,
+            scan_topic_arg,
+            beluga_node,
+            map_server_node,
+            lifecycle_manager_node,
+        ]
+    )

beluga_lambkin/beluga_inference_ros2/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>beluga_inference_ros2</name>
+  <version>1.0.0</version>
+  <description>ROS2 launch for Beluga Localization</description>
+
+  <maintainer email="glpuga@ekumenlabs.com">Gerardo Puga</maintainer>
+  <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
+  <maintainer email="nespinosa@ekumenlabs.com">Nahuel Espinosa</maintainer>
+  <maintainer email="andres.brumovsky@ekumenlabs.com">Andres Brumovsky</maintainer>
+  <license>Apache-2.0</license>
+
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>beluga_amcl</exec_depend>
+  <exec_depend>nav2_map_server</exec_depend>
+  <exec_depend>nav2_lifecycle_manager</exec_depend>
+
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>