diff --git a/src/benchmarks/beam_skipping_evaluation/.devcontainer/devcontainer.json b/src/benchmarks/beam_skipping_evaluation/.devcontainer/devcontainer.json new file mode 100644 index 0000000..720d5f5 --- /dev/null +++ b/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" +} diff --git a/src/benchmarks/beam_skipping_evaluation/.devcontainer/docker-compose.yml b/src/benchmarks/beam_skipping_evaluation/.devcontainer/docker-compose.yml new file mode 100644 index 0000000..283d4af --- /dev/null +++ b/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 diff --git a/src/benchmarks/beam_skipping_evaluation/.dockerignore b/src/benchmarks/beam_skipping_evaluation/.dockerignore new file mode 100644 index 0000000..2c28fff --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/.dockerignore @@ -0,0 +1 @@ +playground diff --git a/src/benchmarks/beam_skipping_evaluation/CMakeLists.txt b/src/benchmarks/beam_skipping_evaluation/CMakeLists.txt new file mode 100644 index 0000000..41d793b --- /dev/null +++ b/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() diff --git a/src/benchmarks/beam_skipping_evaluation/DEFAULT_FASTRTPS_PROFILES.xml b/src/benchmarks/beam_skipping_evaluation/DEFAULT_FASTRTPS_PROFILES.xml new file mode 100644 index 0000000..8f49e50 --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/DEFAULT_FASTRTPS_PROFILES.xml @@ -0,0 +1,14 @@ + + + + + + + + 1 + + + + + + \ No newline at end of file diff --git a/src/benchmarks/beam_skipping_evaluation/Earthfile b/src/benchmarks/beam_skipping_evaluation/Earthfile new file mode 100644 index 0000000..a0e55f1 --- /dev/null +++ b/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 FASTRTPS_DEFAULT_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 diff --git a/src/benchmarks/beam_skipping_evaluation/README.md b/src/benchmarks/beam_skipping_evaluation/README.md new file mode 100644 index 0000000..49ccd00 --- /dev/null +++ b/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 +``` diff --git a/src/benchmarks/beam_skipping_evaluation/config/display.rviz b/src/benchmarks/beam_skipping_evaluation/config/display.rviz new file mode 100644 index 0000000..129b5bb --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/config/display.rviz @@ -0,0 +1,639 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 85 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1 + Splitter Ratio: 0.5 + Tree Height: 875 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 0.10000000149011612 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + donatello/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_accel_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_accel_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_gyro_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_gyro_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra1_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra2_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/arm_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/arm_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/arm_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + donatello/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + donatello/camera_support_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/chassis_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/controller_support_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/cpu_lidar_shelves: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/depth_camera_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + donatello/end_point_link: + Alpha: 1 + Show Axes: false + Show Trail: false + donatello/endpoint_bracket_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/extension_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/front_hit_sensor_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/front_led_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/front_left_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/front_right_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/gripper_link_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/gripper_m_link: + Alpha: 1 + Show Axes: false + Show Trail: false + donatello/gripper_t_link: + Alpha: 1 + Show Axes: false + Show Trail: false + donatello/left_gripper_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/left_gripper_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/left_gripper_link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/left_gripper_link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/left_gripper_link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/left_gripper_link_7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/left_hit_sensor_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/left_led_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/lidar_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + donatello/rear_hit_sensor_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/rear_led_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/rear_left_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/rear_right_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/right_gripper_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/right_gripper_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/right_gripper_link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/right_gripper_link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/right_gripper_link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/right_gripper_link_7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/right_hit_sensor_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/right_led_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/rod_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/rod_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/rod_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/rod_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + donatello/triangle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_mesh_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_rplidar_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + donatello/base_link: + Value: true + camera_accel_frame: + Value: true + camera_accel_optical_frame: + Value: true + camera_bottom_screw_frame: + Value: true + camera_color_frame: + Value: true + camera_color_optical_frame: + Value: true + camera_depth_frame: + Value: true + camera_depth_optical_frame: + Value: true + camera_gyro_frame: + Value: true + camera_gyro_optical_frame: + Value: true + camera_infra1_frame: + Value: true + camera_infra1_optical_frame: + Value: true + camera_infra2_frame: + Value: true + camera_infra2_optical_frame: + Value: true + camera_link: + Value: true + donatello/arm_1_link: + Value: true + donatello/arm_2_link: + Value: true + donatello/arm_base_link: + Value: true + donatello/base_link: + Value: true + donatello/camera_link: + Value: true + donatello/camera_optical_link: + Value: true + donatello/camera_support_link: + Value: true + donatello/chassis_base_link: + Value: true + donatello/controller_support_link: + Value: true + donatello/cpu_lidar_shelves: + Value: true + donatello/depth_camera_mount_link: + Value: true + donatello/end_point_link: + Value: true + donatello/endpoint_bracket_link: + Value: true + donatello/extension_base_link: + Value: true + donatello/front_hit_sensor_link: + Value: true + donatello/front_led_link: + Value: true + donatello/front_left_wheel_link: + Value: true + donatello/front_right_wheel_link: + Value: true + donatello/gripper_link_link: + Value: true + donatello/gripper_m_link: + Value: true + donatello/gripper_t_link: + Value: true + donatello/left_gripper_link_1: + Value: true + donatello/left_gripper_link_2: + Value: true + donatello/left_gripper_link_4: + Value: true + donatello/left_gripper_link_5: + Value: true + donatello/left_gripper_link_6: + Value: true + donatello/left_gripper_link_7: + Value: true + donatello/left_hit_sensor_link: + Value: true + donatello/left_led_link: + Value: true + donatello/lidar_base_link: + Value: true + donatello/rear_hit_sensor_link: + Value: true + donatello/rear_led_link: + Value: true + donatello/rear_left_wheel_link: + Value: true + donatello/rear_right_wheel_link: + Value: true + donatello/right_gripper_link_1: + Value: true + donatello/right_gripper_link_2: + Value: true + donatello/right_gripper_link_4: + Value: true + donatello/right_gripper_link_5: + Value: true + donatello/right_gripper_link_6: + Value: true + donatello/right_gripper_link_7: + Value: true + donatello/right_hit_sensor_link: + Value: true + donatello/right_led_link: + Value: true + donatello/rod_1_link: + Value: true + donatello/rod_2_link: + Value: true + donatello/rod_3_link: + Value: true + donatello/rod_link: + Value: true + donatello/triangle_link: + Value: true + rear_mesh_link: + Value: true + rear_rplidar_optical_link: + Value: true + Marker Scale: 0.15000000596046448 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + donatello/base_link: + donatello/base_link: + donatello/chassis_base_link: + donatello/arm_base_link: + donatello/arm_1_link: + donatello/arm_2_link: + donatello/camera_link: + donatello/camera_optical_link: + {} + donatello/endpoint_bracket_link: + donatello/end_point_link: + {} + donatello/gripper_link_link: + donatello/camera_support_link: + donatello/depth_camera_mount_link: + camera_bottom_screw_frame: + camera_link: + camera_accel_frame: + camera_accel_optical_frame: + {} + camera_color_frame: + camera_color_optical_frame: + {} + camera_depth_frame: + camera_depth_optical_frame: + {} + camera_gyro_frame: + camera_gyro_optical_frame: + {} + camera_infra1_frame: + camera_infra1_optical_frame: + {} + camera_infra2_frame: + camera_infra2_optical_frame: + {} + donatello/gripper_m_link: + donatello/left_gripper_link_1: + donatello/left_gripper_link_2: + donatello/left_gripper_link_4: + donatello/left_gripper_link_5: + donatello/left_gripper_link_6: + donatello/left_gripper_link_7: + {} + donatello/right_gripper_link_1: + donatello/right_gripper_link_2: + donatello/right_gripper_link_4: + donatello/right_gripper_link_5: + donatello/right_gripper_link_6: + donatello/right_gripper_link_7: + {} + donatello/gripper_t_link: + {} + donatello/rod_1_link: + {} + donatello/triangle_link: + {} + donatello/controller_support_link: + {} + donatello/cpu_lidar_shelves: + donatello/lidar_base_link: + rear_mesh_link: + {} + rear_rplidar_optical_link: + {} + donatello/rod_2_link: + {} + donatello/rod_link: + donatello/rod_3_link: + {} + donatello/extension_base_link: + {} + donatello/front_hit_sensor_link: + donatello/front_led_link: + {} + donatello/front_left_wheel_link: + {} + donatello/front_right_wheel_link: + {} + donatello/left_hit_sensor_link: + donatello/left_led_link: + {} + donatello/rear_hit_sensor_link: + donatello/rear_led_link: + {} + donatello/rear_left_wheel_link: + {} + donatello/rear_right_wheel_link: + {} + donatello/right_hit_sensor_link: + donatello/right_led_link: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: donatello/base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 0.553801417350769 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.0911332219839096 + Y: -0.022594859823584557 + Z: 0.15257664024829865 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.38979730010032654 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.254964351654053 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1123 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000001000000000000015a00000401fc0200000002fb000000100044006900730070006c006100790073010000004400000401000000eb00fffffffb0000000a0056006900650077007300000002730000016a000000b900ffffff000005dd0000040100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1854 + X: 66 + Y: 40 diff --git a/src/benchmarks/beam_skipping_evaluation/config/qos_override.yml b/src/benchmarks/beam_skipping_evaluation/config/qos_override.yml new file mode 100644 index 0000000..b6a54a2 --- /dev/null +++ b/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 diff --git a/src/benchmarks/beam_skipping_evaluation/launch/beam_skipping_evaluation.launch b/src/benchmarks/beam_skipping_evaluation/launch/beam_skipping_evaluation.launch new file mode 100755 index 0000000..259ecba --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/launch/beam_skipping_evaluation.launch @@ -0,0 +1,110 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/benchmarks/beam_skipping_evaluation/package.xml b/src/benchmarks/beam_skipping_evaluation/package.xml new file mode 100644 index 0000000..be0f970 --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/package.xml @@ -0,0 +1,35 @@ + + + beam_skipping_evaluation + 1.0.0 + LAMBKIN powered benchmarks for comparing Beluga AMCL to Nav2 AMCL. + + Gerardo Puga + + Apache-2.0 + + ament_cmake + launch_ros + + lambkin-clerk + lambkin-shepherd + + python3-matplotlib + python3-numpy + python3-pandas + python3-tk + + nav2_amcl + nav2_lifecycle_manager + nav2_map_server + + rosbag2_storage_default_plugins + rosbag2_storage_mcap + + rviz2 + nav2_rviz_plugins + + + ament_cmake + + diff --git a/src/benchmarks/beam_skipping_evaluation/params/nav2_amcl_likelihood.yaml b/src/benchmarks/beam_skipping_evaluation/params/nav2_amcl_likelihood.yaml new file mode 100644 index 0000000..2d5abbb --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/params/nav2_amcl_likelihood.yaml @@ -0,0 +1,101 @@ +/**: + ros__parameters: + # Odometry motion model type. + robot_model_type: nav2_amcl::DifferentialMotionModel + # Expected process noise in odometry’s rotation estimate from rotation. + alpha1: 0.1 + # Expected process noise in odometry’s rotation estimate from translation. + alpha2: 0.05 + # Expected process noise in odometry’s translation estimate from translation. + alpha3: 0.1 + # Expected process noise in odometry’s translation estimate from rotation. + alpha4: 0.05 + # Expected process noise in odometry's strafe estimate from translation. + 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. + odom_frame_id: odom + # The name of the coordinate frame of the robot base. + base_frame_id: base_link + # The name of the topic where the map is published by the map server. + map_topic: map + # The name of the topic where scans are being published. + scan_topic: scan_front + # The name of the topic where an initial pose can be published. + # The particle filter will be reset using the provided pose with covariance. + initial_pose_topic: initialpose + # Maximum number of particles that will be used. + max_particles: 2000 + # Minimum number of particles that will be used. + min_particles: 500 + # Error allowed by KLD criteria. + pf_err: 0.05 + # KLD criteria parameter. + # Upper standard normal quantile for the probability that the error in the + # estimated distribution is less than pf_err. + pf_z: 3.0 + # 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.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.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: 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. + laser_max_range: 100.0 + # Maximum number of beams to use in the likelihood field sensor model. + max_beams: 100 + # Weight used to combine the probability of hitting an obstacle. + 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. + tf_broadcast: false + # Transform tolerance allowed. + transform_tolerance: 1.0 + # Execution policy used to apply the motion update and importance weight steps. + # Valid options: "seq", "par". + execution_policy: seq + # 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. + initial_pose.y: 0.0 + # Initial pose yaw coordinate. + initial_pose.yaw: 0.0 + # Initial pose xx covariance. + initial_pose.covariance_x: 0.25 + # Initial pose yy covariance. + initial_pose.covariance_y: 0.25 + # Initial pose yawyaw covariance. + initial_pose.covariance_yaw: 0.0685 + # Initial pose xy covariance. + initial_pose.covariance_xy: 0.0 + # Initial pose xyaw covariance. + initial_pose.covariance_xyaw: 0.0 + # Initial pose yyaw covariance. + initial_pose.covariance_yyaw: 0.0 diff --git a/src/benchmarks/beam_skipping_evaluation/params/nav2_amcl_likelihood_beam_skip.yaml b/src/benchmarks/beam_skipping_evaluation/params/nav2_amcl_likelihood_beam_skip.yaml new file mode 100644 index 0000000..6f3c27a --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/params/nav2_amcl_likelihood_beam_skip.yaml @@ -0,0 +1,103 @@ +/**: + ros__parameters: + # Odometry motion model type. + robot_model_type: nav2_amcl::DifferentialMotionModel + # Expected process noise in odometry’s rotation estimate from rotation. + alpha1: 0.1 + # Expected process noise in odometry’s rotation estimate from translation. + alpha2: 0.05 + # Expected process noise in odometry’s translation estimate from translation. + alpha3: 0.1 + # Expected process noise in odometry’s translation estimate from rotation. + alpha4: 0.05 + # Expected process noise in odometry's strafe estimate from translation. + 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. + odom_frame_id: odom + # The name of the coordinate frame of the robot base. + base_frame_id: base_link + # The name of the topic where the map is published by the map server. + map_topic: map + # The name of the topic where scans are being published. + scan_topic: scan_front + # The name of the topic where an initial pose can be published. + # The particle filter will be reset using the provided pose with covariance. + initial_pose_topic: initialpose + # Maximum number of particles that will be used. + max_particles: 2000 + # Minimum number of particles that will be used. + min_particles: 500 + # Error allowed by KLD criteria. + pf_err: 0.05 + # KLD criteria parameter. + # Upper standard normal quantile for the probability that the error in the + # estimated distribution is less than pf_err. + pf_z: 3.0 + # 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.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.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: likelihood_field_prob + # Beam skipping parameters. + do_beamskip: true + # 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. + laser_max_range: 100.0 + # Maximum number of beams to use in the likelihood field sensor model. + max_beams: 100 + # Weight used to combine the probability of hitting an obstacle. + 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. + tf_broadcast: false + # Transform tolerance allowed. + transform_tolerance: 1.0 + # Execution policy used to apply the motion update and importance weight steps. + # Valid options: "seq", "par". + execution_policy: seq + # 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. + initial_pose.y: 0.0 + # Initial pose yaw coordinate. + initial_pose.yaw: 0.0 + # Initial pose xx covariance. + initial_pose.covariance_x: 0.25 + # Initial pose yy covariance. + initial_pose.covariance_y: 0.25 + # Initial pose yawyaw covariance. + initial_pose.covariance_yaw: 0.0685 + # Initial pose xy covariance. + initial_pose.covariance_xy: 0.0 + # Initial pose xyaw covariance. + initial_pose.covariance_xyaw: 0.0 + # Initial pose yyaw covariance. + initial_pose.covariance_yyaw: 0.0 diff --git a/src/benchmarks/beam_skipping_evaluation/params/nav2_amcl_likelihood_prob.yaml b/src/benchmarks/beam_skipping_evaluation/params/nav2_amcl_likelihood_prob.yaml new file mode 100644 index 0000000..a1787f7 --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/params/nav2_amcl_likelihood_prob.yaml @@ -0,0 +1,103 @@ +/**: + ros__parameters: + # Odometry motion model type. + robot_model_type: nav2_amcl::DifferentialMotionModel + # Expected process noise in odometry’s rotation estimate from rotation. + alpha1: 0.1 + # Expected process noise in odometry’s rotation estimate from translation. + alpha2: 0.05 + # Expected process noise in odometry’s translation estimate from translation. + alpha3: 0.1 + # Expected process noise in odometry’s translation estimate from rotation. + alpha4: 0.05 + # Expected process noise in odometry's strafe estimate from translation. + 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. + odom_frame_id: odom + # The name of the coordinate frame of the robot base. + base_frame_id: base_link + # The name of the topic where the map is published by the map server. + map_topic: map + # The name of the topic where scans are being published. + scan_topic: scan_front + # The name of the topic where an initial pose can be published. + # The particle filter will be reset using the provided pose with covariance. + initial_pose_topic: initialpose + # Maximum number of particles that will be used. + max_particles: 2000 + # Minimum number of particles that will be used. + min_particles: 500 + # Error allowed by KLD criteria. + pf_err: 0.05 + # KLD criteria parameter. + # Upper standard normal quantile for the probability that the error in the + # estimated distribution is less than pf_err. + pf_z: 3.0 + # 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.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.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: likelihood_field_prob + # Beam skipping parameters. + do_beamskip: false + # 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. + laser_max_range: 100.0 + # Maximum number of beams to use in the likelihood field sensor model. + max_beams: 100 + # Weight used to combine the probability of hitting an obstacle. + 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. + tf_broadcast: false + # Transform tolerance allowed. + transform_tolerance: 1.0 + # Execution policy used to apply the motion update and importance weight steps. + # Valid options: "seq", "par". + execution_policy: seq + # 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. + initial_pose.y: 0.0 + # Initial pose yaw coordinate. + initial_pose.yaw: 0.0 + # Initial pose xx covariance. + initial_pose.covariance_x: 0.25 + # Initial pose yy covariance. + initial_pose.covariance_y: 0.25 + # Initial pose yawyaw covariance. + initial_pose.covariance_yaw: 0.0685 + # Initial pose xy covariance. + initial_pose.covariance_xy: 0.0 + # Initial pose xyaw covariance. + initial_pose.covariance_xyaw: 0.0 + # Initial pose yyaw covariance. + initial_pose.covariance_yyaw: 0.0 diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/bookstore_simulated_world.png b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/bookstore_simulated_world.png new file mode 100644 index 0000000..a41814a Binary files /dev/null and b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/bookstore_simulated_world.png differ diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/hq_simulated_environment.jpg b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/hq_simulated_environment.jpg new file mode 100644 index 0000000..26cbeab Binary files /dev/null and b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/hq_simulated_environment.jpg differ diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/representative_cartographer_magazino_map.png b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/representative_cartographer_magazino_map.png new file mode 100644 index 0000000..6ecdfbf Binary files /dev/null and b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/representative_cartographer_magazino_map.png differ diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/representative_diff_drive_sim_24hs_map.png b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/representative_diff_drive_sim_24hs_map.png new file mode 100644 index 0000000..eceb6c7 Binary files /dev/null and b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/representative_diff_drive_sim_24hs_map.png differ diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/representative_omni_drive_sim_24hs_map.png b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/representative_omni_drive_sim_24hs_map.png new file mode 100644 index 0000000..6304d71 Binary files /dev/null and b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/representative_omni_drive_sim_24hs_map.png differ diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/representative_openloris_office_map.png b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/representative_openloris_office_map.png new file mode 100644 index 0000000..65ac8b6 Binary files /dev/null and b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/representative_openloris_office_map.png differ diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/representative_torwic_mapping_map.png b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/representative_torwic_mapping_map.png new file mode 100644 index 0000000..e376465 Binary files /dev/null and b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/representative_torwic_mapping_map.png differ diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/representative_torwic_slam_map.png b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/representative_torwic_slam_map.png new file mode 100644 index 0000000..0a99007 Binary files /dev/null and b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/representative_torwic_slam_map.png differ diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/representative_willow_garage_map.png b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/representative_willow_garage_map.png new file mode 100644 index 0000000..fc62081 Binary files /dev/null and b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/assets/representative_willow_garage_map.png differ diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/conf.py b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/conf.py new file mode 100644 index 0000000..bff95b1 --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/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 = 'Nav2 Beam Skipping Feature Evaluation' +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/beam_skipping_evaluation/reports/nominal_report/index.rst b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/index.rst new file mode 100644 index 0000000..c6ccb81 --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/index.rst @@ -0,0 +1,27 @@ +.. repl-quiet:: + + import lambkin.shepherd.data as lks + import pandas as pd + import numpy as np + import os + + os.makedirs('_generated', exist_ok=True) + + +Nav2 Beam Skipping Feature Evaluation +===================================== + +.. toctree:: + :maxdepth: 2 + :caption: Contents: + +.. + .. include:: sections/objective.inc + +.. + .. include:: sections/platform.inc + +.. include:: sections/main_corpus.inc + +.. + .. include:: sections/appendices.inc diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/sections/appendices.inc b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/sections/appendices.inc new file mode 100644 index 0000000..c684a38 --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/sections/appendices.inc @@ -0,0 +1,35 @@ +Appendix I: Lambkin Configuration +--------------------------------- + +For this report, the following benchmarking script was used: + +.. datatemplate:import-module:: ament_index_python + + {% set package_path = data.get_package_share_directory('beam_skipping_evaluation') %} + + .. literalinclude:: {{config.sysroot}}/{{ package_path }}/scripts/nominal.robot + :language: robotframework + + +Appendix II: AMCL Node Configuration +------------------------------------ + +In all cases, the configuration of both the Beluga AMCL and Nav2 AMCL nodes was kept as close as +possible to the default configuration provided by the respective packages. This configuration is +detailed below. + +The only deviations on top of this configuration were the following settings, which were modified +according to the requirements of each particular dataset and sensor model configuration used: + +- The laser sensor model, to assess both beam and likelihood models. +- The name of the base link frame. +- The name of the map frame. +- The name of the odom frame. +- The 2D scan topic name. + +.. datatemplate:import-module:: ament_index_python + + {% set package_path = data.get_package_share_directory('beam_skipping_evaluation') %} + + .. literalinclude:: {{config.sysroot}}/{{ package_path }}/params/amcl.yaml + :language: yaml diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/sections/datasets/diff_drive_sim_60m_description.inc b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/sections/datasets/diff_drive_sim_60m_description.inc new file mode 100644 index 0000000..f248d7f --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/sections/datasets/diff_drive_sim_60m_description.inc @@ -0,0 +1,34 @@ +Dataset information +^^^^^^^^^^^^^^^^^^^ + +This is a synthetic dataset created recording a simulated robot wandering around a +:math:`450 \mathrm{m}^2` office environment for 24 hours. + +The simulated diff-drive robot is modelled after a `Kobuki platform `_ featuring an `RPLidar A1 `_, an 8m range 2D lidar. The +simulation was performed using Gazebo Classic. + +The dataset was recorded in a single run, and it consists of a single continuous bagfile with a duration of 24 hours. +During this time the robot is periodically sent navigation goals to reachable locations to ensure it keeps moving +around the environment. + +The robot covers approximately 24 km during those 24 hours, with an average speed of about 0.28 m/s. + +The robot model was configured to introduce small imperfections in the odometry to cause a small amount of drift and +emulate conditions closer to real-world. + +The simulated environment is populated with static obstacles (furniture), but the localization map intentionally +excludes them to evaluate the systems under test in a condition where unmapped obstacles perturb the estimation. + +The localization map used to evaluate the localization performance with this dataset is the one shown below. + +.. figure:: assets/representative_diff_drive_sim_24hs_map.png + :scale: 99 % + + Localization map used to evaluate the localization performance with the Diff Drive Sim 60m dataset. + +The simulated environment used to record the Diff Drive Sim 60m dataset is shown below. + +.. figure:: assets/hq_simulated_environment.jpg + :scale: 99 % + + The simulated environment used to record the Diff Drive Sim 60m dataset. diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/sections/datasets/omni_drive_sim_30m_description.inc b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/sections/datasets/omni_drive_sim_30m_description.inc new file mode 100644 index 0000000..3f3081d --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/sections/datasets/omni_drive_sim_30m_description.inc @@ -0,0 +1,35 @@ +Dataset information +^^^^^^^^^^^^^^^^^^^ + +This is a synthetic dataset created recording a simulated omni-drive robot wandering around the +`AWS Robomaker Bookstore World `_ for 24 hours. + +The simulated omni-drive robot is modelled after a customized `Robomaster EP `_ featuring an `RPLidar A2 `_, a 12m range 2D lidar. +The simulation was performed using Gazebo Sim. + +The dataset was recorded in a single run, and it consists of a single continuous bagfile with a duration of 24 hours. +During this time the robot is periodically sent navigation goals to reachable locations within the map to ensure it +keeps moving. + +The robot covers approximately 45 km during those 24 hours, with an average speed of about 0.5 m/s. + +The robot model was configured to introduce small imperfections in the odometry to cause a small amount of drift and +emulate conditions closer to real-world. + +Most but not all obstacles and furniture are mapped, and one of the walls is windowed and therefore +invisible to the simulated lidar. + +The localization world used to evaluate the localization performance with this dataset is the one shown below. + +.. figure:: assets/representative_omni_drive_sim_24hs_map.png + :scale: 99 % + + Localization world used to evaluate the localization performance with the Omni Drive Sim 30m dataset. + +The simulated world used to record the Omni Drive Sim 30m dataset is shown below. It differs from the localization world in that it is populated with obstacles and furniture. +in that a few of the obstacles and furniture present in the simulated world are not present. + +.. figure:: assets/bookstore_simulated_world.png + :scale: 99 % + + The simulated environment used to record the Omni Drive Sim 30m dataset. diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/sections/main_corpus.inc b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/sections/main_corpus.inc new file mode 100644 index 0000000..d6fe4c4 --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/sections/main_corpus.inc @@ -0,0 +1,304 @@ + +.. repl-quiet:: + + def get_bagfile_info(bagpath): + """TODO: replace this with rosbag2_py.Info().read_metadata(bag_path)""" + import subprocess + process = subprocess.Popen(['ros2', 'bag', 'info', f'{bagpath}'], stdout=subprocess.PIPE) + output, error = process.communicate() + return output.decode('utf-8') + + data = pd.concat([ + lks.evo.series('/nav2_amcl_likelihood/pose', 'ape', normalization='long'), + lks.evo.series('/nav2_amcl_likelihood_prob/pose', 'ape', normalization='long'), + lks.evo.series('/nav2_amcl_likelihood_beam_skip/pose', 'ape', normalization='long'), + ]) + + data = data[[ + 'case.name', + 'variation.parameters.dataset', + 'trajectory.name', + 'variation.parameters.basedir', + 'metric.series.value' + ]] + + data = data.rename(columns = { + 'case.name': 'case_name', + 'variation.parameters.dataset': 'bagfile', + 'trajectory.name': 'implementation', + 'variation.parameters.basedir': 'basedir', + 'metric.series.value': 'ape', + }) + + def build_bootstrapped_estimator(name, func, p = 50, n_bootstraps=500): + def bootstrap_confidence_interval(data, name, func, p, n_bootstraps): + bootstrapped = np.array([func(data.sample(frac=1, replace=True)) for _ in range(n_bootstraps)]) + return np.percentile(bootstrapped, [p])[0] + return lambda data: bootstrap_confidence_interval(data, name, func, p, n_bootstraps) + + bootstrapped_mean_05 = build_bootstrapped_estimator('mean', np.mean, 5) + bootstrapped_mean_95 = build_bootstrapped_estimator('mean', np.mean, 95) + bootstrapped_median_05 = build_bootstrapped_estimator('median', np.median, 5) + bootstrapped_median_95 = build_bootstrapped_estimator('median', np.median, 95) + + ape = data.groupby([ + 'case_name', + 'bagfile', + 'implementation', + ])['ape'].agg([ + ('median', np.median), + ('median_05', bootstrapped_median_05), + ('median_95', bootstrapped_median_95), + ('mean', np.mean), + ('mean_05', bootstrapped_mean_05), + ('mean_95', bootstrapped_mean_95), + 'std', + 'max', + ]) + + bagfile_location_data = data.groupby([ + 'case_name', + 'bagfile', + ])['basedir'].agg(['first']).rename(columns={'first': 'basedir'}) + + prof_data = pd.concat([ + lks.timem.summary('nav2_amcl_likelihood', 'cpu_util'), + lks.timem.summary('nav2_amcl_likelihood', 'peak_rss'), + lks.timem.summary('nav2_amcl_likelihood_prob', 'cpu_util'), + lks.timem.summary('nav2_amcl_likelihood_prob', 'peak_rss'), + lks.timem.summary('nav2_amcl_likelihood_beam_skip', 'cpu_util'), + lks.timem.summary('nav2_amcl_likelihood_beam_skip', 'peak_rss'), + ]) + + prof_data = lks.pandas.rescale(prof_data, { + 'nav2_amcl_likelihood.summary.cpu_util': 100., + 'nav2_amcl_likelihood.summary.peak_rss': 1 / 8e6, + 'nav2_amcl_likelihood_prob.summary.cpu_util': 100., + 'nav2_amcl_likelihood_prob.summary.peak_rss': 1 / 8e6, + 'nav2_amcl_likelihood_beam_skip.summary.cpu_util': 100., + 'nav2_amcl_likelihood_beam_skip.summary.peak_rss': 1 / 8e6, + }) + + prof_data = prof_data[[ + 'case.name', + 'variation.parameters.dataset', + 'nav2_amcl_likelihood.summary.peak_rss', + 'nav2_amcl_likelihood.summary.cpu_util', + 'nav2_amcl_likelihood_prob.summary.peak_rss', + 'nav2_amcl_likelihood_prob.summary.cpu_util', + 'nav2_amcl_likelihood_beam_skip.summary.peak_rss', + 'nav2_amcl_likelihood_beam_skip.summary.cpu_util', + ]] + + prof_data = prof_data.rename(columns = { + 'case.name': 'case_name', + 'variation.parameters.dataset': 'bagfile', + 'nav2_amcl_likelihood.summary.cpu_util': 'nav2_amcl_likelihood_cpu_util', + 'nav2_amcl_likelihood.summary.peak_rss': 'nav2_amcl_likelihood_peak_rss', + 'nav2_amcl_likelihood_prob.summary.cpu_util': 'nav2_amcl_likelihood_prob_cpu_util', + 'nav2_amcl_likelihood_prob.summary.peak_rss': 'nav2_amcl_likelihood_prob_peak_rss', + 'nav2_amcl_likelihood_beam_skip.summary.cpu_util': 'nav2_amcl_likelihood_beam_skip_cpu_util', + 'nav2_amcl_likelihood_beam_skip.summary.peak_rss': 'nav2_amcl_likelihood_beam_skip_peak_rss', + }) + + prof_data = prof_data.groupby([ + 'case_name', + 'bagfile', + ]) + + nav2_amcl_likelihood_cpu_util_data = prof_data['nav2_amcl_likelihood_cpu_util'].agg(['max']) + nav2_amcl_likelihood_peak_rss_data = prof_data['nav2_amcl_likelihood_peak_rss'].agg(['max']) + nav2_amcl_likelihood_prob_cpu_util_data = prof_data['nav2_amcl_likelihood_prob_cpu_util'].agg(['max']) + nav2_amcl_likelihood_prob_peak_rss_data = prof_data['nav2_amcl_likelihood_prob_peak_rss'].agg(['max']) + nav2_amcl_likelihood_beam_skip_cpu_util_data = prof_data['nav2_amcl_likelihood_beam_skip_cpu_util'].agg(['max']) + nav2_amcl_likelihood_beam_skip_peak_rss_data = prof_data['nav2_amcl_likelihood_beam_skip_peak_rss'].agg(['max']) + + from ament_index_python import get_package_share_directory + + this_pkg_dir = get_package_share_directory('beam_skipping_evaluation') + + with open(f'{this_pkg_dir}/reports/nominal_report/templates/template_table_header.txt', 'r') as f: + TABLE_HEADER_TEMPLATE = f.read() + + with open(f'{this_pkg_dir}/reports/nominal_report/templates/template_table_row.txt', 'r') as f: + ROW_TEMPLATE = f.read() + + with open(f'{this_pkg_dir}/reports/nominal_report/templates/template_dataset_chapter.txt', 'r') as f: + DATASET_CHAPTER_TEMPLATE = f.read() + + with open(f'{this_pkg_dir}/reports/nominal_report/templates/template_bagfile_section.txt', 'r') as f: + BAGFILE_SECTION_TEMPLATE = f.read() + + with open(f'{this_pkg_dir}/reports/nominal_report/templates/template_resource_table_header.txt', 'r') as f: + RESOURCE_TABLE_HEADER_TEMPLATE = f.read() + + with open(f'{this_pkg_dir}/reports/nominal_report/templates/template_resource_table_row.txt', 'r') as f: + RESOURCE_TABLE_ROW_TEMPLATE = f.read() + + output = [] + + def round_digits(value, digits=3): + # if the value is not a number, just pass through whatever it is + try: + output_str = f'{value:.{digits}f}' + except: + output_str = str(value) + return output_str + + desired_section_order = [ + 'Willow Garage', + 'TorWIC SLAM', + 'TorWIC Mapping', + 'Magazino Datasets', + 'Openloris Office', + 'Omni Drive Sim 24hs', + ] + datasets_in_data = ape.index.get_level_values(0).unique() + missing_datasets = [x for x in datasets_in_data if x not in desired_section_order] + desired_section_order = desired_section_order + missing_datasets + datasets_to_process = [x for x in desired_section_order if x in datasets_in_data] + + for case_name in datasets_to_process: + output.append(DATASET_CHAPTER_TEMPLATE.format(dataset_name=case_name)) + case_name_str = case_name.replace(' ', '_').lower() + output.append(f'.. include:: sections/datasets/{case_name_str}_description.inc\n\n') + data_for_case = ape.loc[case_name] + for bagfile in data_for_case.index.get_level_values(0).unique(): + metadata = "" + mapfile = "" + try: + bagpath = bagfile_location_data.loc[case_name].loc[bagfile].loc['basedir'] + metadata = get_bagfile_info(os.path.join('..', '..', bagpath, 'bagfiles', bagfile)) + metadata = [x for x in metadata.split('\n') if "Topic:" not in x] + metadata = " " + "\n ".join(metadata) + mapfile = os.path.join('..', '..', bagpath, 'artifacts', bagfile, 'map.pgm') + except: + pass + output.append(BAGFILE_SECTION_TEMPLATE.format(bagfile_name=bagfile, metadata=metadata)) + data_for_bagfile = data_for_case.loc[bagfile] + try: + nav2_amcl_likelihood_data = data_for_bagfile.loc['/nav2_amcl_likelihood/pose'] + nav2_amcl_likelihood_median = nav2_amcl_likelihood_data['median'] + nav2_amcl_likelihood_median_05 = nav2_amcl_likelihood_data['median_05'] + nav2_amcl_likelihood_median_95 = nav2_amcl_likelihood_data['median_95'] + nav2_amcl_likelihood_mean = nav2_amcl_likelihood_data['mean'] + nav2_amcl_likelihood_mean_05 = nav2_amcl_likelihood_data['mean_05'] + nav2_amcl_likelihood_mean_95 = nav2_amcl_likelihood_data['mean_95'] + nav2_amcl_likelihood_std = nav2_amcl_likelihood_data['std'] + nav2_amcl_likelihood_max = nav2_amcl_likelihood_data['max'] + """ """ + nav2_amcl_likelihood_prob_data = data_for_bagfile.loc['/nav2_amcl_likelihood_prob/pose'] + nav2_amcl_likelihood_prob_median = nav2_amcl_likelihood_prob_data['median'] + nav2_amcl_likelihood_prob_median_05 = nav2_amcl_likelihood_prob_data['median_05'] + nav2_amcl_likelihood_prob_median_95 = nav2_amcl_likelihood_prob_data['median_95'] + nav2_amcl_likelihood_prob_mean = nav2_amcl_likelihood_prob_data['mean'] + nav2_amcl_likelihood_prob_mean_05 = nav2_amcl_likelihood_prob_data['mean_05'] + nav2_amcl_likelihood_prob_mean_95 = nav2_amcl_likelihood_prob_data['mean_95'] + nav2_amcl_likelihood_prob_std = nav2_amcl_likelihood_prob_data['std'] + nav2_amcl_likelihood_prob_max = nav2_amcl_likelihood_prob_data['max'] + """ """ + nav2_amcl_likelihood_beam_skip_data = data_for_bagfile.loc['/nav2_amcl_likelihood_beam_skip/pose'] + nav2_amcl_likelihood_beam_skip_median = nav2_amcl_likelihood_beam_skip_data['median'] + nav2_amcl_likelihood_beam_skip_median_05 = nav2_amcl_likelihood_beam_skip_data['median_05'] + nav2_amcl_likelihood_beam_skip_median_95 = nav2_amcl_likelihood_beam_skip_data['median_95'] + nav2_amcl_likelihood_beam_skip_mean = nav2_amcl_likelihood_beam_skip_data['mean'] + nav2_amcl_likelihood_beam_skip_mean_05 = nav2_amcl_likelihood_beam_skip_data['mean_05'] + nav2_amcl_likelihood_beam_skip_mean_95 = nav2_amcl_likelihood_beam_skip_data['mean_95'] + nav2_amcl_likelihood_beam_skip_std = nav2_amcl_likelihood_beam_skip_data['std'] + nav2_amcl_likelihood_beam_skip_max = nav2_amcl_likelihood_beam_skip_data['max'] + except: + nav2_amcl_likelihood_median = '???' + nav2_amcl_likelihood_median_05 = '???' + nav2_amcl_likelihood_median_95 = '???' + nav2_amcl_likelihood_mean = '???' + nav2_amcl_likelihood_mean_05 = '???' + nav2_amcl_likelihood_mean_95 = '???' + nav2_amcl_likelihood_std = '???' + nav2_amcl_likelihood_max = '???' + """ """ + nav2_amcl_likelihood_prob_median = '???' + nav2_amcl_likelihood_prob_median_05 = '???' + nav2_amcl_likelihood_prob_median_95 = '???' + nav2_amcl_likelihood_prob_mean = '???' + nav2_amcl_likelihood_prob_mean_05 = '???' + nav2_amcl_likelihood_prob_mean_95 = '???' + nav2_amcl_likelihood_prob_std = '???' + nav2_amcl_likelihood_prob_max = '???' + """ """ + nav2_amcl_likelihood_beam_skip_median = '???' + nav2_amcl_likelihood_beam_skip_median_05 = '???' + nav2_amcl_likelihood_beam_skip_median_95 = '???' + nav2_amcl_likelihood_beam_skip_mean = '???' + nav2_amcl_likelihood_beam_skip_mean_05 = '???' + nav2_amcl_likelihood_beam_skip_mean_95 = '???' + nav2_amcl_likelihood_beam_skip_std = '???' + nav2_amcl_likelihood_beam_skip_max = '???' + try: + nav2_amcl_likelihood_cpu_util = nav2_amcl_likelihood_cpu_util_data.loc[case_name].loc[bagfile]['max'] + nav2_amcl_likelihood_peak_rss = nav2_amcl_likelihood_peak_rss_data.loc[case_name].loc[bagfile]['max'] + """ """ + nav2_amcl_likelihood_prob_cpu_util = nav2_amcl_likelihood_prob_cpu_util_data.loc[case_name].loc[bagfile]['max'] + nav2_amcl_likelihood_prob_peak_rss = nav2_amcl_likelihood_prob_peak_rss_data.loc[case_name].loc[bagfile]['max'] + """ """ + nav2_amcl_likelihood_beam_skip_cpu_util = nav2_amcl_likelihood_beam_skip_cpu_util_data.loc[case_name].loc[bagfile]['max'] + nav2_amcl_likelihood_beam_skip_peak_rss = nav2_amcl_likelihood_beam_skip_peak_rss_data.loc[case_name].loc[bagfile]['max'] + except: + nav2_amcl_likelihood_cpu_util = nav2_amcl_likelihood_peak_rss = "???" + nav2_amcl_likelihood_prob_cpu_util = nav2_amcl_likelihood_prob_peak_rss = "???" + nav2_amcl_likelihood_beam_skip_cpu_util = nav2_amcl_likelihood_beam_skip_peak_rss = "???" + """ """ + output.append(TABLE_HEADER_TEMPLATE.format( + trajectory_name=bagfile, + sensor_model='Likelihood field', + nrows=len(data_for_bagfile))) + """ """ + output.append(ROW_TEMPLATE.format( + implementation_name="Likelihood Field", + median=round_digits(nav2_amcl_likelihood_median), + median_05=round_digits(nav2_amcl_likelihood_median_05), + median_95=round_digits(nav2_amcl_likelihood_median_95), + mean=round_digits(nav2_amcl_likelihood_mean), + mean_05=round_digits(nav2_amcl_likelihood_mean_05), + mean_95=round_digits(nav2_amcl_likelihood_mean_95), + worst=round_digits(nav2_amcl_likelihood_max))) + output.append(ROW_TEMPLATE.format( + implementation_name="Likelihood Prob", + median=round_digits(nav2_amcl_likelihood_prob_median), + median_05=round_digits(nav2_amcl_likelihood_prob_median_05), + median_95=round_digits(nav2_amcl_likelihood_prob_median_95), + mean=round_digits(nav2_amcl_likelihood_prob_mean), + mean_05=round_digits(nav2_amcl_likelihood_prob_mean_05), + mean_95=round_digits(nav2_amcl_likelihood_prob_mean_95), + worst=round_digits(nav2_amcl_likelihood_prob_max))) + output.append(ROW_TEMPLATE.format( + implementation_name="Beam Skip", + median=round_digits(nav2_amcl_likelihood_beam_skip_median), + median_05=round_digits(nav2_amcl_likelihood_beam_skip_median_05), + median_95=round_digits(nav2_amcl_likelihood_beam_skip_median_95), + mean=round_digits(nav2_amcl_likelihood_beam_skip_mean), + mean_05=round_digits(nav2_amcl_likelihood_beam_skip_mean_05), + mean_95=round_digits(nav2_amcl_likelihood_beam_skip_mean_95), + worst=round_digits(nav2_amcl_likelihood_beam_skip_max))) + """ """ + output.append(RESOURCE_TABLE_HEADER_TEMPLATE.format( + trajectory_name=bagfile, + nrows=3)) + output.append(RESOURCE_TABLE_ROW_TEMPLATE.format( + implementation_name="Likelihood Field", + likelihood_field_peak_cpu=round_digits(nav2_amcl_likelihood_cpu_util, 1), + likelihood_field_peak_rss=round_digits(nav2_amcl_likelihood_peak_rss, 0))) + output.append(RESOURCE_TABLE_ROW_TEMPLATE.format( + implementation_name="Likelihood Prob", + likelihood_field_peak_cpu=round_digits(nav2_amcl_likelihood_prob_cpu_util, 1), + likelihood_field_peak_rss=round_digits(nav2_amcl_likelihood_prob_peak_rss, 0))) + output.append(RESOURCE_TABLE_ROW_TEMPLATE.format( + implementation_name="Beam Skip", + likelihood_field_peak_cpu=round_digits(nav2_amcl_likelihood_beam_skip_cpu_util, 1), + likelihood_field_peak_rss=round_digits(nav2_amcl_likelihood_beam_skip_peak_rss, 0))) + + output = '\n\n'.join(output) + + with open('_generated/generated_section.inc', 'w') as f: + f.write(output) + +.. include:: _generated/generated_section.inc diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/sections/objective.inc b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/sections/objective.inc new file mode 100644 index 0000000..705516d --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/sections/objective.inc @@ -0,0 +1,17 @@ +Introduction +------------ + +Objective +^^^^^^^^^ + +TBD + +Scope +^^^^^ + +This document is limited to the presentation of the methodology and the results used to gather the performance data. + +No root-cause analysis of issues or performance tuning is performed in this document. + +For the sake of brevity, a limited number of configuration variants are used in the experiments: the sensor model and the motion model. +The rest of the configuration parameters are kept at their default values, which are the same for both AMCL implementations. diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/sections/platform.inc b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/sections/platform.inc new file mode 100644 index 0000000..86e7460 --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/sections/platform.inc @@ -0,0 +1,129 @@ +Methodology +----------- + +Datasets used +^^^^^^^^^^^^^ + +A number of pre-recorded datasets were selected to evaluate the performance of the Beluga AMCL and Nav2 AMCL localization systems. +A number of public datasets were considered for this evaluation, looking for a diversity of environments and robots that were representative of a wide range of real-world scenarios. + +The following datasets were selected based on their characteristics and availability for this evaluation: + +.. list-table:: Real robot datasets + :widths: 25 70 + :header-rows: 1 + + * - Name + - Description + * - `Willow Garage dataset `_ + - Large office environment with multiple recordings along different trajectories and times, including multiple rooms and hallways. + * - `TorWIC SLAM dataset `_ + - Warehouse environment with multiple recordings along different trajectories and times. + * - `TorWIC Mapping dataset `_ + - Trajectories in a synthetic warehouse-like environment undergoing incremental changes. + * - `Cartographer Magazino `_ + - Two sample datasets for mapping and localization that were recorded in a hallway. + * - `OpenLORIS-Scene dataset (Office set) `_ + - Short trajectories in an office environment. + +None of these datasets were originally conceived for the evaluation of 2D localization systems, +and therefore none of them provided a complete ground-truth that included both the real-world trajectory and the environment's +occupancy grid map. + +For this reason, ground-truth data was generated locally from the bagfiles themselves using Cartographer SLAM to create mutually +consistent ground-truth trajectory and occupancy grid map for each bagfile. + +Note that this approach has some drawbacks that need to be taken into account when interpreting the results later: + +- Both the space and time diversity of the original datasets is lost, as the ground-truth is generated for each individual bagfile representing the state of the environment as perceived when the data was recorded. +- All of the objects present in the environment are part of the generated occupancy grid map; i.e. there are no unmapped objects. + +These real-world datasets have two more limitations worth mentioning: + +- The longest run-time of any of the real-world datasets listed above is around 30 minutes, which may fail to flag longer-term resource usage issues, such as memory leaks. +- In all cases the datasets were recorded using differential-drive robots. + +To somewhat mitigate these issues two additional synthetic datasets were added to the evaluation: + +.. list-table:: Synthetic datasets + :widths: 25 70 + :header-rows: 1 + + * - Name + - Description + * - Diff Drive Sim 60m + - Gazebo Classic simulation of a diff-drive robot randomly wandering around a 450m^2 office environment with both mapped and unmapped furniture for just over 30m. The simulated robot is modelled on a Kobuki platform with a mounted RPLidar A1. + * - Omni Drive Sim 30m + - Gazebo Sim simulation of a omni-drive robot randomly wandering around the `AWS Robomaker Bookstore World `_ with both mapped and unmapped furniture for just over 30m. The simulated robot is modelled after a customized Robomaster EP with a mounted RPLidar A2 M12. + +In both cases small imperfections were added to the simulated models to cause a small amount of drift in the odometry. Ground-truth was provided by Gazebo plugins from +world state information, and the occupancy grid maps were generated with using `SLAM Toolbox `_. + + + +Alterations to the real robot datasets +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +A number of transformations were applied to the original real robot datasets to make them compatible +with the systems under test. The most important of all is that they needed to be converted from ROS 1 to ROS 2 using the +`rosbags `_ conversion tool. + +In some cases, important frame transforms were missing in the original datasets, such as the odom/base_link transform, +and they were generated from other information available in the dataset (e.g. odometry messages). Redundant information, +such as map/odom transforms, were removed to prevent conflicts with the systems under test. + +Additionally, all topics not related to lidar-based 2D localization were removed to reduce file size. This was needed because +the evaluation process is very intensive in terms of storage requirements. + +The two synthetic datasets were constructed for this evaluation and therefore were ROS 2 native with no missing data. + +Evaluation procedure +^^^^^^^^^^^^^^^^^^^^ + +The evaluation was performed using the `LAMBKIN `_ framework, +which is tool described as "a mixture of automation and conventions to facilitate reproducible +benchmarking and evaluation of localization and mapping systems". + +For each bagfile in each dataset and each tested configuration (likelihood or beam), LAMBKIN +replays the data through both Beluga AMCL and Nav2 AMCL at the same time, recording the output of both +in a new bagfile. This bagfile is then processed using the `evo `_ tool to +summarize the localization performance of both systems against the ground-truth data. + +During execution LAMBKIN also instruments both localization nodes using `timememory `_ to +collect resource usage metrics such as CPU time, memory usage, and other system-level metrics. This information is stored along with the +results of the evo. + +Each evaluation can be iterated multiple times to improve the statistical significance of the results. This comes +at the expense of increased execution time and storage requirements for the results, which can be substantial. As a +compromise, the results in this report are based on 3 iterations of each bagfile/configuration combination for real robot datasets, +and a single iteration for the synthetic datasets accounting for their very long length. + +For each bagfile/configuration combination, the resulting APE metrics for all iterations are processed to produce +the median, mean, standard deviation and worst-case value. These are the values reported in the following pages. + +We also reported the peak RSS (Resident Set Size) and CPU use across all iterations for each bagfile/configuration combination, as +recorded by timememory. + + +Evaluation Host 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() %} + {% set ros_distribution_info = data.ros2.distribution_info() %} + * OS: {{ os_distribution_info.description }} + * ROS distribution: {{ ros_distribution_info.name }} + * Localization packages: + {% set pkg_info = data.ros2.package_info('nav2_amcl') %} + * ``{{ pkg_info.name }}`` {{ pkg_info.version }} + diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/templates/template_bagfile_section.txt b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/templates/template_bagfile_section.txt new file mode 100644 index 0000000..9e20b3c --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/templates/template_bagfile_section.txt @@ -0,0 +1,10 @@ +{bagfile_name} +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + + +Bagfile metadata:: + +{metadata} + +Evaluation results: + diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/templates/template_dataset_chapter.txt b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/templates/template_dataset_chapter.txt new file mode 100644 index 0000000..5c5e19e --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/templates/template_dataset_chapter.txt @@ -0,0 +1,2 @@ +Dataset: {dataset_name} +------------------------------------------- diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/templates/template_figure_insert.txt b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/templates/template_figure_insert.txt new file mode 100644 index 0000000..5c40611 --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/templates/template_figure_insert.txt @@ -0,0 +1,5 @@ + +.. figure:: assets/hq4_office.png + :scale: 99 % + :alt: {dataset} localization map + diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/templates/template_resource_table_header.txt b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/templates/template_resource_table_header.txt new file mode 100644 index 0000000..8abf0c5 --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/templates/template_resource_table_header.txt @@ -0,0 +1,10 @@ +.. flat-table:: Average CPU and peak Resident Set Size (RSS) values. + :header-rows: {nrows} + + * + - :rspan:`1` :cspan:`1` Implementation + - :cspan:`1` Likelihood Field + + * + - cpu + - rss diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/templates/template_resource_table_row.txt b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/templates/template_resource_table_row.txt new file mode 100644 index 0000000..8e2d6a0 --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/templates/template_resource_table_row.txt @@ -0,0 +1,4 @@ + * + - :cspan:`1` {implementation_name} + - {likelihood_field_peak_cpu}% + - {likelihood_field_peak_rss} MB diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/templates/template_table_header.txt b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/templates/template_table_header.txt new file mode 100644 index 0000000..93398e8 --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/templates/template_table_header.txt @@ -0,0 +1,13 @@ +.. flat-table:: APE metrics aggregated across all iterations of ``{trajectory_name}`` for the {sensor_model} sensor model. + :header-rows: {nrows} + + * + - :rspan:`1` :cspan:`1` Implementation + - :cspan:`5` {sensor_model} sensor model + + * + - median + - median c.i. + - mean + - mean c.i. + - worst-case diff --git a/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/templates/template_table_row.txt b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/templates/template_table_row.txt new file mode 100644 index 0000000..8f60151 --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/reports/nominal_report/templates/template_table_row.txt @@ -0,0 +1,7 @@ + * + - :cspan:`1` {implementation_name} + - {median} m + - {median_05}-{median_95} m + - {mean} m + - {mean_05}-{mean_95} m + - {worst} m diff --git a/src/benchmarks/beam_skipping_evaluation/scripts/nominal.robot b/src/benchmarks/beam_skipping_evaluation/scripts/nominal.robot new file mode 100755 index 0000000..b663c53 --- /dev/null +++ b/src/benchmarks/beam_skipping_evaluation/scripts/nominal.robot @@ -0,0 +1,262 @@ +#!/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 Nav2 beam skipping feature evaluation. + +Resource lambkin/shepherd/robot/resources/all.resource + +Suite Setup Setup Nav2 Beam Skipping Evaluation benchmark suite +Suite Teardown Teardown Nav2 Beam Skipping Evaluation benchmark suite +Test Template Run Nav2 Beam Skipping Evaluation benchmark case for each ${dataset} ${basedir} ${odom_frame} ${map_frame} ${base_frame} ${scan_topic} ${iterations} ${initial_pose_x} ${initial_pose_y} ${initial_pose_yaw} ${robot_model_type} + +*** Variables *** + +@{MAGAZINO_DIR} magazzino_ros2_localization_only +@{MAGAZINO_BAGS} hallway_localization +... hallway_return + +@{OPENLORIS_DIR} openloris_office_ros2_localization_only +@{OPENLORIS_OFFICE_BAGS} office1-1 +... office1-2 +... office1-3 +... office1-4 +... office1-5 +... office1-6 +... office1-7 + +${TORWIC_MAPPING_DIR} torwic_mapping_dataset_ros2_localization_only +@{TORWIC_MAPPING_BAGS} baseline +... 1-1_top_row_shift +... 1-2_larger_shift +... 1-3_longitudinal_shift +... 1-4_all_boxes_shift +... 1-5_all_boxes_rotate +... 1-6_inner_shrink +... 1-7_all_shrink +... 1-8_alternating_in_and_out +... 1-9_stepping_outward +... 2-1_remove_top_row +... 2-2_remove_corners +... 2-3_remove_all_boxes +... 2-4_remove_fences +... 3-1_top_row_shift_with_fences +... 3-2_top_row_shift_with_sides +... 3-3_fences_move_outward +... 4-1_cover_the_fences_with_boxes + +${TORWIC_SLAM_DIR} torwic_slam_dataset_ros2_localization_only +@{TORWIC_SLAM_BAGS} 2022-06-15_aisle_ccw_run_1 +... 2022-06-15_aisle_ccw_run_2 +... 2022-06-15_aisle_cw_run_1 +... 2022-06-15_aisle_cw_run_2 +... 2022-06-15_hallway_full_cw_parts_1_and_2 +... 2022-06-15_hallway_full_ccw_parts_1_and_2 +... 2022-06-15_hallway_straight_ccw_parts_1_and_2 +... 2022-06-23_aisle_ccw_run_1 +... 2022-06-23_aisle_ccw_run_2 +... 2022-06-23_aisle_cw_run_1 +... 2022-06-23_aisle_cw_run_2 +... 2022-06-23_hallway_full_cw_parts_1_and_2 +... 2022-06-23_hallway_straight_ccw_parts_1_and_2 +... 2022-06-23_hallway_straight_cw_parts_1_and_2 +... 2022-10-12_aisle_ccw +... 2022-10-12_aisle_cw +... 2022-10-12_hallway_full_cw_run1 +... 2022-10-12_hallway_full_cw_run2 +... 2022-10-12_hallway_straight_ccw +... 2022-10-12_hallway_straight_cw + +${WILLOW_DSET_DIR} willow_garage_dataset_ros2_localization_only +# # 2011-08-03-16-16-43 is not in the list because it's missing the scan topic +@{WILLOW_DSET_BAGS} 2011-08-03-20-03-22 +... 2011-08-04-12-16-23 +... 2011-08-04-14-27-40 +... 2011-08-04-23-46-28 +... 2011-08-05-09-27-53 +... 2011-08-05-12-58-41 +... 2011-08-05-23-19-43 +... 2011-08-08-09-48-17 +... 2011-08-08-14-26-55 +... 2011-08-08-23-29-37 +# ... 2011-08-09-08-49-52 +# ... 2011-08-09-14-32-35 +# ... 2011-08-09-22-31-30 +# ... 2011-08-10-09-36-26 +# ... 2011-08-10-14-48-32 +# ... 2011-08-11-01-31-15 +# ... 2011-08-11-08-36-01 +# ... 2011-08-11-14-27-41 +# ... 2011-08-11-22-03-37 +# ... 2011-08-12-09-06-48 +# ... 2011-08-12-16-39-48 +# ... 2011-08-12-22-46-34 +# ... 2011-08-15-17-22-26 +# ... 2011-08-15-21-26-26 +# ... 2011-08-16-09-20-08 +# ... 2011-08-16-18-40-52 +# ... 2011-08-16-20-59-00 +# ... 2011-08-17-15-51-51 +# ... 2011-08-17-21-17-05 +# ... 2011-08-18-20-33-16 +# ... 2011-08-18-20-52-30 +# ... 2011-08-19-10-12-20 +# ... 2011-08-19-14-17-55 +# ... 2011-08-19-21-35-17 +# ... 2011-08-22-10-02-27 +# ... 2011-08-22-14-53-33 +# ... 2011-08-23-01-11-53 +# ... 2011-08-23-09-21-17 +# ... 2011-08-24-09-52-14 +# ... 2011-08-24-15-01-39 +# ... 2011-08-24-19-47-10 +# ... 2011-08-25-09-31-05 +# ... 2011-08-25-20-14-56 +# ... 2011-08-25-20-38-39 +# ... 2011-08-26-09-58-19 +# ... 2011-08-29-15-48-07 +# ... 2011-08-29-21-14-07 +# ... 2011-08-30-08-55-28 +# ... 2011-08-30-20-49-42 +# ... 2011-08-30-21-17-56 +# ... 2011-08-31-20-29-19 +# ... 2011-08-31-20-44-19 +# ... 2011-09-01-08-21-33 +# ... 2011-09-02-09-20-25 +# ... 2011-09-06-09-04-41 +# ... 2011-09-06-13-20-36 +# ... 2011-09-08-13-14-39 +# ... 2011-09-09-13-22-57 +# ... 2011-09-11-07-34-22 +# ... 2011-09-11-09-43-46 +# ... 2011-09-12-14-18-56 +# ... 2011-09-12-14-47-01 +# ... 2011-09-13-10-23-31 +# ... 2011-09-13-13-44-21 +# ... 2011-09-14-10-19-20 +# ... 2011-09-15-08-32-46 + +@{LONG_DURATION_DIR} long_duration_bags_ros2_localization_only +@{OMNI_DRIVE_SIM_BAGS} simulated_bookstore_robomaster_24hs + +@{HQ_SIMULATION_DIR} hq_simulation +# @{HQ_SIMULATION_BAGS} hq_simulation_segment_0 +# ... hq_simulation_segment_1 +# ... hq_simulation_segment_2 +# ... hq_simulation_segment_3 +# ... hq_simulation_segment_4 +# ... hq_simulation_segment_5 +# ... hq_simulation_segment_6 +# ... hq_simulation_segment_7 +# ... hq_simulation_segment_8 +# ... hq_simulation_segment_9 +# ... hq_simulation_segment_10 +# ... hq_simulation_segment_11 +# ... hq_simulation_segment_12 +# ... hq_simulation_segment_13 +# ... hq_simulation_segment_14 +# ... hq_simulation_segment_15 +# ... hq_simulation_segment_16 +# ... hq_simulation_segment_17 +# ... hq_simulation_segment_18 +# ... hq_simulation_segment_19 +# ... hq_simulation_segment_20 +# ... hq_simulation_segment_21 +# ... hq_simulation_segment_22 +# ... hq_simulation_segment_23 +# ... hq_simulation_segment_24 +# ... hq_simulation_segment_25 +# ... hq_simulation_segment_26 +# ... hq_simulation_segment_27 +# ... hq_simulation_segment_28 +# ... hq_simulation_segment_29 +# ... hq_simulation_segment_30 +# ... hq_simulation_segment_31 +# ... hq_simulation_segment_32 +# ... hq_simulation_segment_33 +# ... hq_simulation_segment_34 +# ... hq_simulation_segment_35 +# ... hq_simulation_segment_36 +# ... hq_simulation_segment_37 +# ... hq_simulation_segment_38 +# ... hq_simulation_segment_39 +# ... hq_simulation_segment_40 +# ... hq_simulation_segment_41 +# ... hq_simulation_segment_42 +# ... hq_simulation_segment_43 +# ... hq_simulation_segment_44 +# ... hq_simulation_segment_45 +# ... hq_simulation_segment_46 +# ... hq_simulation_segment_47 +# ... hq_simulation_segment_48 +@{HQ_SIMULATION_BAGS} hq_simulation_segment_5 + +# problematicos: +# - hq_simulation_segment_4 + + +*** Test Cases *** DATASET BASEDIR ODOM_FRAME MAP_FRAME BASE_FRAME SCAN_TOPIC ITERATION INITIAL_POSE_X INITIAL_POSE_Y INITIAL_POSE_YAW ROBOT_MODEL_TYPE +# Magazino Datasets ${{MAGAZINO_BAGS}} ${{MAGAZINO_DIR}} odom map base_footprint /scan_front 1 0.0 0.0 0.0 nav2_amcl::DifferentialMotionModel +# Openloris Office ${{OPENLORIS_OFFICE_BAGS}} ${{OPENLORIS_DIR}} base_odom map base_link /scan 5 0.0 0.0 0.0 nav2_amcl::DifferentialMotionModel +# TorWIC Mapping ${{TORWIC_MAPPING_BAGS}} ${{TORWIC_MAPPING_DIR}} odom map base_link /front/scan 1 0.0 0.0 0.0 nav2_amcl::DifferentialMotionModel +# TorWIC SLAM ${{TORWIC_SLAM_BAGS}} ${{TORWIC_SLAM_DIR}} odom map base_link /front/scan 1 0.0 0.0 0.0 nav2_amcl::DifferentialMotionModel +# Willow Garage ${{WILLOW_DSET_BAGS}} ${{WILLOW_DSET_DIR}} odom_combined map base_footprint /base_scan 1 0.0 0.0 0.0 nav2_amcl::OmniMotionModel +# Omni Drive Sim 24hs ${{OMNI_DRIVE_SIM_BAGS}} ${{LONG_DURATION_DIR}} odom map base_link /scan 1 3.2 9.0 0.7 nav2_amcl::OmniMotionModel +HQ Simulation ${{HQ_SIMULATION_BAGS}} ${{HQ_SIMULATION_DIR}} odom map base_link /scan 1 0.0 0.0 0.0 nav2_amcl::DifferentialMotionModel + +*** Keywords *** +Nav2 Beam Skipping Evaluation 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 nominal_report template in beam_skipping_evaluation ROS 2 package + +Nav2 Beam Skipping Evaluation benchmark case + Extends generic resource usage benchmark case + Extends ROS 2 system benchmark case + Extends ROS 2 2D SLAM system benchmark case + # Setup benchmark inputs + ${bagfile_path} = Set Variable ${EXECDIR}/${basedir}/bagfiles/${dataset} + ${artifacts_path} = Set Variable ${EXECDIR}/${basedir}/artifacts/${dataset} + Uses ${bagfile_path} at 4x as input to ROS 2 system + ${package_share_path} = Find ROS 2 Package beam_skipping_evaluation 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 beam_skipping_evaluation.launch in beam_skipping_evaluation ROS package as rig + Sets map_filename launch argument to ${artifacts_path}/map.yaml + Sets global_frame_id launch argument to ${map_frame} + Sets odom_frame_id launch argument to ${odom_frame} + Sets base_frame_id launch argument to ${base_frame} + Sets scan_topic launch argument to ${scan_topic} + Sets initial_pose_x launch argument to ${initial_pose_x} + Sets initial_pose_y launch argument to ${initial_pose_y} + Sets initial_pose_yaw launch argument to ${initial_pose_yaw} + Sets robot_model_type launch argument to ${robot_model_type} + Sets use_sim_time launch argument to true + # Setup benchmark profiling + Uses timemory-timem to sample nav2_amcl_likelihood performance + Uses timemory-timem to sample nav2_amcl_likelihood_prob performance + Uses timemory-timem to sample nav2_amcl_likelihood_beam_skip performance + # Setup tracking + Tracks /nav2_amcl_likelihood/pose trajectories + Tracks /nav2_amcl_likelihood_prob/pose trajectories + Tracks /nav2_amcl_likelihood_beam_skip/pose trajectories + # Setup benchmark analysis + Uses ${artifacts_path}/groundtruth.tum as trajectory groundtruth + Performs trajectory corrections align=yes t_max_diff=${0.1} + Uses ${iterations} iterations diff --git a/src/benchmarks/beluga_vs_nav2/package.xml b/src/benchmarks/beluga_vs_nav2/package.xml index f19f032..9773f50 100644 --- a/src/benchmarks/beluga_vs_nav2/package.xml +++ b/src/benchmarks/beluga_vs_nav2/package.xml @@ -21,6 +21,7 @@ nav2_amcl nav2_lifecycle_manager + nav2_map_server beluga_amcl