Add package to evaluate the impact of the beam skipping feature in nav2

Signed-off-by: Gerardo Puga <[email protected]>

src/benchmarks/beam_skipping_evaluation/.devcontainer/devcontainer.json

@@ -0,0 +1,18 @@
+{
+    "image": "ekumenlabs/nav2-beam-skipping-eval:dev",
+
+    "mounts": [{"source": "${localEnv:DEVCONTAINER_DATA:/tmp}", "target": "/data", "type": "bind"}],
+    "workspaceMount": "source=${localWorkspaceFolder}/../..,target=/workspace/src/lambkin,type=bind,consistency=cached",
+    "workspaceFolder": "/workspace",
+
+	"customizations": {
+		"vscode": {
+			"extensions": [
+				"tumit.vscode-rf-formatter",
+				"robocorp.robotframework-lsp"
+			]
+		}
+	},
+
+    "shutdownAction":"none"
+}

src/benchmarks/beam_skipping_evaluation/.devcontainer/docker-compose.yml

@@ -0,0 +1,16 @@
+services:
+  devcontainer:
+    image: ekumenlabs/nav2-beam-skipping-eval:dev
+    container_name: nav2-beam-skipping-eval-dev
+    environment:
+      - DISPLAY
+      - QT_X11_NO_MITSHM=1
+      - XAUTHORITY=/tmp/.docker.xauth
+    stdin_open: true
+    tty: true
+    privileged: ${PRIVILEGED:-false}
+    volumes:
+      - /tmp/.X11-unix:/tmp/.X11-unix:rw
+      - /tmp/.docker.xauth:/tmp/.docker.xauth
+      - ../../..:/workspace/src/lambkin
+    working_dir: /workspace

src/benchmarks/beam_skipping_evaluation/.dockerignore

@@ -0,0 +1 @@
+playground

src/benchmarks/beam_skipping_evaluation/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.16)
+project(beam_skipping_evaluation)
+
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+
+install(
+  PROGRAMS
+    scripts/nominal.robot
+  DESTINATION lib/${PROJECT_NAME})
+
+install(
+  DIRECTORY
+    config
+    params
+    launch
+    reports
+    scripts
+  DESTINATION share/${PROJECT_NAME})
+
+
+ament_package()

src/benchmarks/beam_skipping_evaluation/DEFAULT_FASTRTPS_PROFILES.xml

@@ -0,0 +1,14 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<dds xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
+    <profiles>
+        <publisher profile_name="service">
+            <qos>
+                <reliability>
+                    <max_blocking_time>
+                        <sec>1</sec>
+                    </max_blocking_time>
+                </reliability>
+            </qos>
+        </publisher>
+    </profiles>
+</dds>

src/benchmarks/beam_skipping_evaluation/Earthfile

@@ -0,0 +1,83 @@
+# Copyright 2024 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.
+
+VERSION 0.8
+
+IMPORT ../../external/os AS os
+IMPORT ../../.. AS lambkin
+
+devel:
+    ARG distro=jammy
+    ARG rosdistro  # forward
+    ARG user=lambkin
+    ARG uid=1000
+    ARG gid=1000
+    FROM lambkin+embed-ubuntu-devel --distro=${distro}  --rosdistro=${rosdistro}
+    RUN mkdir -p /workspace/src
+    WORKDIR /workspace
+    COPY package.xml src/lambkin/benchmarks/beam_skipping_evaluation/package.xml
+    RUN . /etc/profile && apt update && rosdep update && \
+        rosdep install -y -i --from-paths src \
+            --skip-keys 'lambkin-shepherd lambkin-clerk' && \
+        apt clean && rm -rf /var/lib/apt/lists/*
+    RUN pip install linuxdoc sphinxcontrib.datatemplates sphinxcontrib-repl
+    COPY DEFAULT_FASTRTPS_PROFILES.xml /
+    ENV ROS2_DEFAULT_FASTRTPS_PROFILES_FILE=/DEFAULT_FASTRTPS_PROFILES.xml
+    DO os+ADDUSER --user=${user} --uid=${uid} --gid=${gid} --workdir=/workspace
+    SAVE IMAGE ekumenlabs/nav2-beam-skipping-eval:dev
+
+local-devel:
+    LOCALLY
+    ARG distro=jammy
+    ARG rosdistro  # forward
+    LET user="$(whoami)"
+    LET uid="$(id -u)"
+    LET gid="$(id -g)"
+    BUILD +devel --distro=${distro} --rosdistro=${rosdistro} --user=${user} --uid=${uid} --gid=${gid}
+
+build:
+    ARG distro=jammy
+    ARG rosdistro  # forward
+    FROM lambkin+embed-ubuntu-devel --distro=${distro} --rosdistro=${rosdistro} --components="external/ros2"
+    RUN mkdir -p /workspace/src
+    WORKDIR /workspace
+    COPY . src/beam_skipping_evaluation
+    RUN . /etc/profile && apt update && rosdep update && \
+        rosdep install -y -i --from-paths src -t build -t buildtool -t test \
+            --skip-keys 'lambkin-shepherd lambkin-clerk' && \
+        apt clean && rm -rf /var/lib/apt/lists/*
+    RUN . /etc/profile && colcon build --merge-install --install-base /opt/ros/application
+    LET content = "
+    source /opt/ros/application/setup.bash
+    if [ \$# -ne 0 ]; then
+        ros2 run beam_skipping_evaluation \$@
+    else
+        bash
+    fi
+    "
+    RUN echo "${content}" > /opt/ros/application/entrypoint.bash
+    SAVE ARTIFACT /opt/ros/application
+
+release:
+    ARG distro=jammy
+    ARG rosdistro  # forward
+    FROM lambkin+embed-ubuntu-release --distro=${distro} --rosdistro=${rosdistro}
+    COPY (+build/application --distro=${distro} --rosdistro=${rosdistro}) /opt/ros/application
+    RUN . /etc/profile && apt update && rosdep update && \
+        rosdep install -i -y --from-path /opt/ros/application \
+            -t exec --skip-keys 'lambkin-shepherd lambkin-clerk' && \
+        apt clean && rm -rf /var/lib/apt/lists/*
+    RUN pip install linuxdoc sphinxcontrib.datatemplates sphinxcontrib-repl
+    ENTRYPOINT ["/bin/bash", "--login", "/opt/ros/application/entrypoint.bash"]
+    SAVE IMAGE ekumenlabs/nav2-beam-skipping-eval:latest

src/benchmarks/beam_skipping_evaluation/README.md

@@ -0,0 +1,45 @@
+# beam_skipping_evaluation package
+
+## Instructions
+
+- Cd to `lambkin` root directory.
+```sh
+cd {LAMBKIN_ROOT_DIR}
+```
+- Build the beam_skipping_evaluation `devcontainer` image
+```sh
+./tools/setup.sh
+./tools/earthly ./src/benchmarks/beam_skipping_evaluation+local-devel
+```
+- Clone `beluga-datasets`. **This repository uses LFS**, so make sure you have it installed.
+```bash
+cd src/benchmarks/beam_skipping_evaluation \
+    && mkdir -p playground && cd playground
+```
+- Copy in that folder the datasets from the `/srv/datasets/beluga_evaluation_datasets` directory in the beefy machine.
+```sh
+- Open beam_skipping_evaluation `devcontainer` using either its CLI or `vscode`
+```
+docker compose -f .devcontainer/docker-compose.yml run devcontainer
+```
+- Build
+```sh
+BUILD_DOCUMENTATION=false BUILD_TESTING=false colcon build --packages-up-to beam_skipping_evaluation --symlink-install
+source install/setup.bash
+```
+- Go to the `playgrounds` directory, where you've downloaded the datasets
+```sh
+cd src/lambkin/benchmarks/beam_skipping_evaluation/playground
+```
+- Run the benchmark itself.
+```sh
+ros2 run beam_skipping_evaluation nominal.robot
+```
+- Inspect the results using the same name of the launch file.
+```sh
+ls nominal
+```
+- The report can be found in the `report` directory.
+```sh
+ls nominal/report/build/latex/report.pdf
+```

src/benchmarks/beam_skipping_evaluation/config/qos_override.yml

@@ -0,0 +1,6 @@
+# durability_override.yml
+# Need to override static transform QoS, to reflect that is a latched topic.
+# This is not handled properly by ROS1 to ROS2 bag converting tools. 
+/tf_static:
+  durability: transient_local
+  history: keep_all

src/benchmarks/beam_skipping_evaluation/launch/beam_skipping_evaluation.launch

@@ -0,0 +1,100 @@
+<launch>
+  <arg name="map_filename" />
+  <arg name="global_frame_id" />
+  <arg name="odom_frame_id" />
+  <arg name="base_frame_id" />
+  <arg name="scan_topic" />
+
+  <arg name="use_sim_time" default="true" />
+
+  <arg name="initial_pose_x" default="0.0" />
+  <arg name="initial_pose_y" default="0.0" />
+  <arg name="initial_pose_yaw" default="0.0" />
+
+  <arg name="robot_model_type" />
+
+  <!-- don't change this because it's hardcoded in the robotframework stuff and you too will waste
+  time -->
+  <arg name="map_topic" default="map" />
+
+  <set_parameter name="use_sim_time" value="$(var use_sim_time)" />
+
+  <node pkg="nav2_map_server" exec="map_server" name="map_server">
+    <param name="yaml_filename" value="$(var map_filename)" />
+    <remap from="map" to="$(var map_topic)" />
+  </node>
+
+  <node pkg="nav2_lifecycle_manager" exec="lifecycle_manager" name="lifecycle_manager">
+    <param name="autostart" value="true" />
+    <param name="node_names"
+      value="[map_server, nav2_amcl_likelihood, nav2_amcl_likelihood_prob, nav2_amcl_likelihood_beam_skip]" />
+  </node>
+
+  <!-- nav2_amcl_likelihood -->
+
+  <node pkg="nav2_amcl" exec="amcl" name="nav2_amcl_likelihood"
+    launch-prefix="$(env nav2_amcl_likelihood_PREFIX '')">
+    <param from="$(find-pkg-share beam_skipping_evaluation)/params/nav2_amcl_likelihood.yaml" />
+
+    <param name="global_frame_id" value="$(var global_frame_id)" />
+    <param name="odom_frame_id" value="$(var odom_frame_id)" />
+    <param name="base_frame_id" value="$(var base_frame_id)" />
+    <param name="map_topic" value="$(var map_topic)" />
+    <param name="scan_topic" value="$(var scan_topic)" />
+
+    <param name="initial_pose.x" value="$(var initial_pose_x)" />
+    <param name="initial_pose.y" value="$(var initial_pose_y)" />
+    <param name="initial_pose.yaw" value="$(var initial_pose_yaw)" />
+
+    <param name="robot_model_type" value="$(var robot_model_type)" />
+
+    <remap from="amcl_pose" to="/nav2_amcl_likelihood/pose" />
+    <remap from="particle_cloud" to="/nav2_amcl_likelihood/particle_cloud" />
+  </node>
+
+  <!-- nav2_amcl_likelihood_prob -->
+
+  <node pkg="nav2_amcl" exec="amcl" name="nav2_amcl_likelihood_prob"
+    launch-prefix="$(env nav2_amcl_likelihood_prob_PREFIX '')">
+    <param from="$(find-pkg-share beam_skipping_evaluation)/params/nav2_amcl_likelihood_prob.yaml" />
+
+    <param name="global_frame_id" value="$(var global_frame_id)" />
+    <param name="odom_frame_id" value="$(var odom_frame_id)" />
+    <param name="base_frame_id" value="$(var base_frame_id)" />
+    <param name="map_topic" value="$(var map_topic)" />
+    <param name="scan_topic" value="$(var scan_topic)" />
+
+    <param name="initial_pose.x" value="$(var initial_pose_x)" />
+    <param name="initial_pose.y" value="$(var initial_pose_y)" />
+    <param name="initial_pose.yaw" value="$(var initial_pose_yaw)" />
+
+    <param name="robot_model_type" value="$(var robot_model_type)" />
+
+    <remap from="amcl_pose" to="/nav2_amcl_likelihood_prob/pose" />
+    <remap from="particle_cloud" to="/nav2_amcl_likelihood_prob/particle_cloud" />
+  </node>
+
+  <!-- nav2_amcl_likelihood_beam_skip -->
+
+  <node pkg="nav2_amcl" exec="amcl" name="nav2_amcl_likelihood_beam_skip"
+    launch-prefix="$(env nav2_amcl_likelihood_beam_skip_PREFIX '')">
+    <param
+      from="$(find-pkg-share beam_skipping_evaluation)/params/nav2_amcl_likelihood_beam_skip.yaml" />
+
+    <param name="global_frame_id" value="$(var global_frame_id)" />
+    <param name="odom_frame_id" value="$(var odom_frame_id)" />
+    <param name="base_frame_id" value="$(var base_frame_id)" />
+    <param name="map_topic" value="$(var map_topic)" />
+    <param name="scan_topic" value="$(var scan_topic)" />
+
+    <param name="initial_pose.x" value="$(var initial_pose_x)" />
+    <param name="initial_pose.y" value="$(var initial_pose_y)" />
+    <param name="initial_pose.yaw" value="$(var initial_pose_yaw)" />
+
+    <param name="robot_model_type" value="$(var robot_model_type)" />
+
+    <remap from="amcl_pose" to="/nav2_amcl_likelihood_beam_skip/pose" />
+    <remap from="particle_cloud" to="/nav2_amcl_likelihood_beam_skip/particle_cloud" />
+  </node>
+
+</launch>

src/benchmarks/beam_skipping_evaluation/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>beam_skipping_evaluation</name>
+  <version>1.0.0</version>
+  <description>LAMBKIN powered benchmarks for comparing Beluga AMCL to Nav2 AMCL.</description>
+
+  <maintainer email="[email protected]">Gerardo Puga</maintainer>
+
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <exec_depend>launch_ros</exec_depend>
+
+  <exec_depend>lambkin-clerk</exec_depend>
+  <exec_depend>lambkin-shepherd</exec_depend>
+
+  <exec_depend>python3-matplotlib</exec_depend>
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-pandas</exec_depend>
+  <exec_depend>python3-tk</exec_depend>
+
+  <exec_depend>nav2_amcl</exec_depend>
+  <exec_depend>nav2_lifecycle_manager</exec_depend>
+  <exec_depend>nav2_map_server</exec_depend>
+
+  <exec_depend>rosbag2_storage_default_plugins</exec_depend>
+  <exec_depend>rosbag2_storage_mcap</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>