From 8332d4ab65421b24a6d710579e8a871b4e99ef51 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Tue, 15 Oct 2024 21:10:21 +0800 Subject: [PATCH 01/43] Adding dockerfiles for building tests Signed-off-by: Aaron Chong --- .github/minimal-nav2/Dockerfile | 18 ++++++++++++++++++ .github/minimal-zenoh-bridge/Dockerfile | 22 ++++++++++++++++++++++ 2 files changed, 40 insertions(+) create mode 100644 .github/minimal-nav2/Dockerfile create mode 100644 .github/minimal-zenoh-bridge/Dockerfile diff --git a/.github/minimal-nav2/Dockerfile b/.github/minimal-nav2/Dockerfile new file mode 100644 index 0000000..0d0de56 --- /dev/null +++ b/.github/minimal-nav2/Dockerfile @@ -0,0 +1,18 @@ +ARG ROS_DISTRO=jazzy +FROM docker.io/ros:$ROS_DISTRO-ros-base + +RUN apt update && apt install -y curl ros-$ROS_DISTRO-nav2-bringup ros-$ROS_DISTRO-rmw-cyclonedds-cpp + +RUN mkdir -p /tb3 && cd /tb3 \ + && curl -sL https://github.com/ROBOTIS-GIT/turtlebot3_simulations/archive/refs/heads/master.tar.gz -o turtlebot3_simulations.tar.gz \ + && mkdir -p /tb3/turtlebot3_simulations && tar zxf turtlebot3_simulations.tar.gz -C /tb3/turtlebot3_simulations --strip-components=1 && rm turtlebot3_simulations.tar.gz + +ENV RMW_IMPLEMENTATION rmw_cyclonedds_cpp +ENV GAZEBO_MODEL_PATH "$GAZEBO_MODEL_PATH:/tb3/turtlebot3_simulations/turtlebot3_gazebo/models" + +RUN rm -rf \ + /var/lib/apt/lists \ + /dist + +# TODO(ac): set up proper parameter file +ENTRYPOINT ["bash", "-c", ". /opt/ros/$ROS_DISTRO/setup.bash && ros2 launch nav2_bringup tb3_simulation_launch.py"] diff --git a/.github/minimal-zenoh-bridge/Dockerfile b/.github/minimal-zenoh-bridge/Dockerfile new file mode 100644 index 0000000..9c0e11b --- /dev/null +++ b/.github/minimal-zenoh-bridge/Dockerfile @@ -0,0 +1,22 @@ +ARG ROS_DISTRO=jazzy +FROM docker.io/ros:$ROS_DISTRO-ros-base +ARG ZENOH_VERSION=0.11.0 +ARG BRANCH=efc/ci + +RUN apt update && apt install -y wget unzip ros-jazzy-rmw-cyclonedds-cpp + +RUN mkdir -p /zenoh-bridge && cd /zenoh-bridge \ + && wget https://github.com/eclipse-zenoh/zenoh-plugin-ros2dds/releases/download/$ZENOH_VERSION/zenoh-plugin-ros2dds-$ZENOH_VERSION-x86_64-unknown-linux-gnu-standalone.zip \ + && unzip zenoh-plugin-ros2dds-$ZENOH_VERSION-x86_64-unknown-linux-gnu-standalone.zip \ + && rm zenoh-plugin-ros2dds-$ZENOH_VERSION-x86_64-unknown-linux-gnu-standalone.zip \ + && wget https://github.com/open-rmf/free_fleet/archive/$BRANCH.tar.gz -o free_fleet.tar.gz \ + && mkdir -p /zenoh-bridge/free_fleet && tar zxf free_fleet.tar.gz -C /zenoh-bridge/free_fleet --strip-components=1 \ + && rm free_fleet.tar.gz + +ENV RMW_IMPLEMENTATION rmw_cyclonedds_cpp + +RUN rm -rf \ + /var/lib/apt/lists \ + /dist + +ENTRYPOINT ["bash", "-c", ". /opt/ros/$ROS_DISTRO/setup.bash && . /zenoh-bridge/zenoh-bridge-ros2dds -c /zenoh-brdige/free_fleet/free_fleet_examples/zenoh_configs/turtlebot3_1_zenoh_config.json5"] From 603c86ec4d8dac3216a1c570512b83caf7220e17 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Tue, 5 Nov 2024 15:07:36 +0800 Subject: [PATCH 02/43] Initial docker compose nightly workflow Signed-off-by: Aaron Chong --- .github/integration-tests/docker-compose.yaml | 30 ++++++++++++++++ .github/minimal-zenoh-bridge/Dockerfile | 2 +- .github/workflows/nightly.yaml | 34 +++++++++++++++++++ 3 files changed, 65 insertions(+), 1 deletion(-) create mode 100644 .github/integration-tests/docker-compose.yaml create mode 100644 .github/workflows/nightly.yaml diff --git a/.github/integration-tests/docker-compose.yaml b/.github/integration-tests/docker-compose.yaml new file mode 100644 index 0000000..3e21ff0 --- /dev/null +++ b/.github/integration-tests/docker-compose.yaml @@ -0,0 +1,30 @@ +version: "3" + +services: + minimal-nav2: + image: minimal-nav2 + build: + context: .github/minimal-nav2 + # dockerfile: .github/minimal-nav2 + container_name: minimal-nav2 + stop_signal: SIGINT + network_mode: host + privileged: true + stdin_open: true + tty: true + environment: + - ROS_DOMAIN_ID=42 + # command: ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False + + zenoh-bridge: + image: minimal-zenoh-bridge + build: + context: .github/minimal-zenoh-bridge + # dockerfile: docker/task_manager/Dockerfile + container_name: minimal-zenoh-bridge + network_mode: host + stdin_open: true + tty: true + environment: + - ROS_DOMAIN_ID=42 + # command: ros2 launch task_manager task_manager.launch.py params_file:=/examples/nav2_example_params.yaml diff --git a/.github/minimal-zenoh-bridge/Dockerfile b/.github/minimal-zenoh-bridge/Dockerfile index 9c0e11b..f07260f 100644 --- a/.github/minimal-zenoh-bridge/Dockerfile +++ b/.github/minimal-zenoh-bridge/Dockerfile @@ -1,6 +1,6 @@ ARG ROS_DISTRO=jazzy FROM docker.io/ros:$ROS_DISTRO-ros-base -ARG ZENOH_VERSION=0.11.0 +ARG ZENOH_VERSION=1.0.0 ARG BRANCH=efc/ci RUN apt update && apt install -y wget unzip ros-jazzy-rmw-cyclonedds-cpp diff --git a/.github/workflows/nightly.yaml b/.github/workflows/nightly.yaml new file mode 100644 index 0000000..9834669 --- /dev/null +++ b/.github/workflows/nightly.yaml @@ -0,0 +1,34 @@ +name: Nightly + +on: push + +jobs: + integration-tests: + timeout-minutes: 10 + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v1 + + - name: Install dependencies + run: | + sudo apt update && sudo apt install docker-compose -y + + - name: Start containers + run: docker-compose -f ".github/integration-tests/docker-compose.yaml" up -d --build + + # - name: Install node + # uses: actions/setup-node@v1 + # with: + # node-version: 14.x + + # - name: Install dependencies + # run: npm install + + # - name: Run tests + # run: npm run test + + - name: Stop containers + if: always() + run: docker-compose -f ".github/integration-tests/docker-compose.yaml" down From f8631fe4d2e0e7c6fe7724792e9433fb8c5804f4 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Tue, 5 Nov 2024 15:21:45 +0800 Subject: [PATCH 03/43] Using new docker compose command Signed-off-by: Aaron Chong --- .github/integration-tests/docker-compose.yaml | 12 ++++++------ .github/workflows/nightly.yaml | 6 +----- 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/.github/integration-tests/docker-compose.yaml b/.github/integration-tests/docker-compose.yaml index 3e21ff0..5763cb0 100644 --- a/.github/integration-tests/docker-compose.yaml +++ b/.github/integration-tests/docker-compose.yaml @@ -4,9 +4,9 @@ services: minimal-nav2: image: minimal-nav2 build: - context: .github/minimal-nav2 - # dockerfile: .github/minimal-nav2 - container_name: minimal-nav2 + context: ../../ + dockerfile: .github/minimal-nav2/Dockerfile + # container_name: minimal-nav2 stop_signal: SIGINT network_mode: host privileged: true @@ -19,9 +19,9 @@ services: zenoh-bridge: image: minimal-zenoh-bridge build: - context: .github/minimal-zenoh-bridge - # dockerfile: docker/task_manager/Dockerfile - container_name: minimal-zenoh-bridge + context: ../../ + dockerfile: .github/minimal-zenoh-bridge/Dockerfile + # container_name: minimal-zenoh-bridge network_mode: host stdin_open: true tty: true diff --git a/.github/workflows/nightly.yaml b/.github/workflows/nightly.yaml index 9834669..5e258e0 100644 --- a/.github/workflows/nightly.yaml +++ b/.github/workflows/nightly.yaml @@ -11,12 +11,8 @@ jobs: - name: Checkout uses: actions/checkout@v1 - - name: Install dependencies - run: | - sudo apt update && sudo apt install docker-compose -y - - name: Start containers - run: docker-compose -f ".github/integration-tests/docker-compose.yaml" up -d --build + run: docker compose -f ".github/integration-tests/docker-compose.yaml" up -d --build # - name: Install node # uses: actions/setup-node@v1 From ec8c75d65968cff7121002986d555a9d6f4a3b64 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Tue, 5 Nov 2024 15:26:04 +0800 Subject: [PATCH 04/43] Fix zenoh bridge image branch Signed-off-by: Aaron Chong --- .github/minimal-zenoh-bridge/Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/minimal-zenoh-bridge/Dockerfile b/.github/minimal-zenoh-bridge/Dockerfile index f07260f..a4a1905 100644 --- a/.github/minimal-zenoh-bridge/Dockerfile +++ b/.github/minimal-zenoh-bridge/Dockerfile @@ -1,7 +1,7 @@ ARG ROS_DISTRO=jazzy FROM docker.io/ros:$ROS_DISTRO-ros-base ARG ZENOH_VERSION=1.0.0 -ARG BRANCH=efc/ci +ARG BRANCH=efc/integration-testing RUN apt update && apt install -y wget unzip ros-jazzy-rmw-cyclonedds-cpp From 6b2feecbdb89ddb5cd7ffa650b532aa05767df83 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Tue, 5 Nov 2024 15:34:44 +0800 Subject: [PATCH 05/43] Use curl for repo tar Signed-off-by: Aaron Chong --- .github/minimal-zenoh-bridge/Dockerfile | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/.github/minimal-zenoh-bridge/Dockerfile b/.github/minimal-zenoh-bridge/Dockerfile index a4a1905..5e172e1 100644 --- a/.github/minimal-zenoh-bridge/Dockerfile +++ b/.github/minimal-zenoh-bridge/Dockerfile @@ -3,14 +3,16 @@ FROM docker.io/ros:$ROS_DISTRO-ros-base ARG ZENOH_VERSION=1.0.0 ARG BRANCH=efc/integration-testing -RUN apt update && apt install -y wget unzip ros-jazzy-rmw-cyclonedds-cpp +RUN apt update && apt install -y curl wget unzip ros-jazzy-rmw-cyclonedds-cpp RUN mkdir -p /zenoh-bridge && cd /zenoh-bridge \ && wget https://github.com/eclipse-zenoh/zenoh-plugin-ros2dds/releases/download/$ZENOH_VERSION/zenoh-plugin-ros2dds-$ZENOH_VERSION-x86_64-unknown-linux-gnu-standalone.zip \ && unzip zenoh-plugin-ros2dds-$ZENOH_VERSION-x86_64-unknown-linux-gnu-standalone.zip \ - && rm zenoh-plugin-ros2dds-$ZENOH_VERSION-x86_64-unknown-linux-gnu-standalone.zip \ - && wget https://github.com/open-rmf/free_fleet/archive/$BRANCH.tar.gz -o free_fleet.tar.gz \ - && mkdir -p /zenoh-bridge/free_fleet && tar zxf free_fleet.tar.gz -C /zenoh-bridge/free_fleet --strip-components=1 \ + && rm zenoh-plugin-ros2dds-$ZENOH_VERSION-x86_64-unknown-linux-gnu-standalone.zip + +RUN cd /zenoh-bridge && mkdir -p /zenoh-bridge/free_fleet \ + && curl -L https://github.com/open-rmf/free_fleet/archive/$BRANCH.tar.gz -o free_fleet.tar.gz \ + && tar zxf free_fleet.tar.gz -C /zenoh-bridge/free_fleet --strip-components=1 \ && rm free_fleet.tar.gz ENV RMW_IMPLEMENTATION rmw_cyclonedds_cpp From 6a7e35259e09c82ecfc8f638f69495e1d58e9094 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Tue, 5 Nov 2024 15:38:18 +0800 Subject: [PATCH 06/43] Use docker compose command for down too Signed-off-by: Aaron Chong --- .github/workflows/nightly.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/nightly.yaml b/.github/workflows/nightly.yaml index 5e258e0..9025369 100644 --- a/.github/workflows/nightly.yaml +++ b/.github/workflows/nightly.yaml @@ -27,4 +27,4 @@ jobs: - name: Stop containers if: always() - run: docker-compose -f ".github/integration-tests/docker-compose.yaml" down + run: docker compose -f ".github/integration-tests/docker-compose.yaml" down From 1b945df44aee46d373ff4a2308a3cbdb58cfc9b6 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Wed, 6 Nov 2024 12:34:39 +0800 Subject: [PATCH 07/43] Setup integration testing with flag, test with workflow Signed-off-by: Aaron Chong --- .github/minimal-nav2/Dockerfile | 8 ++- .github/minimal-zenoh-bridge/Dockerfile | 2 +- .github/workflows/build.yaml | 2 + .github/workflows/nightly.yaml | 52 +++++++++------ free_fleet_examples/CMakeLists.txt | 14 +++++ .../tests/integration/__init__.py | 0 .../tests/integration/test_tf.py | 63 +++++++++++++++++++ 7 files changed, 118 insertions(+), 23 deletions(-) create mode 100644 free_fleet_examples/tests/integration/__init__.py create mode 100644 free_fleet_examples/tests/integration/test_tf.py diff --git a/.github/minimal-nav2/Dockerfile b/.github/minimal-nav2/Dockerfile index 0d0de56..2d96ca6 100644 --- a/.github/minimal-nav2/Dockerfile +++ b/.github/minimal-nav2/Dockerfile @@ -14,5 +14,9 @@ RUN rm -rf \ /var/lib/apt/lists \ /dist -# TODO(ac): set up proper parameter file -ENTRYPOINT ["bash", "-c", ". /opt/ros/$ROS_DISTRO/setup.bash && ros2 launch nav2_bringup tb3_simulation_launch.py"] +# Modify existing params file to add initial pose +RUN mkdir -p /params && cd /params \ + && cp /opt/ros/$ROS_DISTRO/share/nav2_bringup/params/nav2_params.yaml . \ + && sed -z 's|amcl:\n ros__parameters:\n|amcl:\n ros__parameters:\n set_initial_pose: true\n initial_pose:{x: -2.0, y: -0.5, z: 0.0, yaw: 0.0}\n|' nav2_params.yaml > nav2_params_edited.yaml + +ENTRYPOINT ["bash", "-c", ". /opt/ros/$ROS_DISTRO/setup.bash && ros2 launch nav2_bringup tb3_simulation_launch.py params_file:=/params/nav2_params_edited.yaml"] diff --git a/.github/minimal-zenoh-bridge/Dockerfile b/.github/minimal-zenoh-bridge/Dockerfile index 5e172e1..7b6330e 100644 --- a/.github/minimal-zenoh-bridge/Dockerfile +++ b/.github/minimal-zenoh-bridge/Dockerfile @@ -1,6 +1,6 @@ ARG ROS_DISTRO=jazzy FROM docker.io/ros:$ROS_DISTRO-ros-base -ARG ZENOH_VERSION=1.0.0 +ARG ZENOH_VERSION=0.11.0 ARG BRANCH=efc/integration-testing RUN apt update && apt install -y curl wget unzip ros-jazzy-rmw-cyclonedds-cpp diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index c41a4ce..685dfe2 100644 --- a/.github/workflows/build.yaml +++ b/.github/workflows/build.yaml @@ -25,6 +25,8 @@ jobs: required-ros-distributions: ${{ matrix.ros_distribution }} - name: build and test uses: ros-tooling/action-ros-ci@v0.3 + env: + INTEGRATION_TESTING: false with: package-name: free_fleet free_fleet_adapter free_fleet_examples target-ros2-distro: ${{ matrix.ros_distribution }} diff --git a/.github/workflows/nightly.yaml b/.github/workflows/nightly.yaml index 9025369..ef58c6c 100644 --- a/.github/workflows/nightly.yaml +++ b/.github/workflows/nightly.yaml @@ -6,25 +6,37 @@ jobs: integration-tests: timeout-minutes: 10 runs-on: ubuntu-latest + container: + image: osrf/ros:${{ matrix.ros_distribution }}-desktop-noble + strategy: + matrix: + ros_distribution: + - jazzy steps: - - name: Checkout - uses: actions/checkout@v1 - - - name: Start containers - run: docker compose -f ".github/integration-tests/docker-compose.yaml" up -d --build - - # - name: Install node - # uses: actions/setup-node@v1 - # with: - # node-version: 14.x - - # - name: Install dependencies - # run: npm install - - # - name: Run tests - # run: npm run test - - - name: Stop containers - if: always() - run: docker compose -f ".github/integration-tests/docker-compose.yaml" down + - name: Install dependencies + run: | + sudo apt update && sudo apt install python3-pip -y + pip3 install eclipse-zenoh==0.11.0 pycdr2 --break-system-packages + + - uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: ${{ matrix.ros_distribution }} + + - name: Checkout + uses: actions/checkout@v1 + + - name: Start containers + run: docker compose -f ".github/integration-tests/docker-compose.yaml" up -d --build + + - name: build and test + uses: ros-tooling/action-ros-ci@v0.3 + env: + INTEGRATION_TESTING: true + with: + package-name: free_fleet free_fleet_adapter free_fleet_examples + target-ros2-distro: ${{ matrix.ros_distribution }} + + - name: Stop containers + if: always() + run: docker compose -f ".github/integration-tests/docker-compose.yaml" down diff --git a/free_fleet_examples/CMakeLists.txt b/free_fleet_examples/CMakeLists.txt index dd778fd..3997135 100644 --- a/free_fleet_examples/CMakeLists.txt +++ b/free_fleet_examples/CMakeLists.txt @@ -83,6 +83,20 @@ if(BUILD_TESTING) WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} ) endforeach() + + if (INTEGRATION_TESTING) + set(_pytest_tests + tests/integration/test_tf.py + ) + foreach(_test_path ${_pytest_tests}) + get_filename_component(_test_name ${_test_path} NAME_WE) + ament_add_pytest_test(${_test_name} ${_test_path} + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + TIMEOUT 60 + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + endforeach() + endif() endif() ament_package() diff --git a/free_fleet_examples/tests/integration/__init__.py b/free_fleet_examples/tests/integration/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/free_fleet_examples/tests/integration/test_tf.py b/free_fleet_examples/tests/integration/test_tf.py new file mode 100644 index 0000000..8ccaa6d --- /dev/null +++ b/free_fleet_examples/tests/integration/test_tf.py @@ -0,0 +1,63 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import time + +from free_fleet.convert import transform_stamped_to_ros2_msg +from free_fleet.types import TFMessage +from free_fleet.utils import namespacify +from rclpy.time import Time +from tf2_ros import Buffer + +import zenoh + + +def test_tf(): + # Open Zenoh Session + session = zenoh.open(zenoh.Config()) + + tf_buffer = Buffer() + + def tf_callback(sample: zenoh.Sample): + transform = TFMessage.deserialize(sample.payload) + for zt in transform.transforms: + t = transform_stamped_to_ros2_msg(zt) + tf_buffer.set_transform(t, 'free_fleet_examples_test_tf') + + # Subscribe to TF + pub = session.declare_subscriber( + namespacify('tf', 'turtlebot3_1'), + tf_callback + ) + + transform_exists = False + for i in range(10): + try: + transform = tf_buffer.lookup_transform( + 'base_footprint', + 'map', + Time() + ) + transform_exists = True + except Exception as err: + print(f'Unable to get transform between base_footprint and ' + f'map: {type(err)}: {err}') + + time.sleep(1) + pub.undeclare() + session.close() + + assert transform_exists From c1fc27db894d7419b3620675abe4a5d7e669b57b Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Wed, 6 Nov 2024 12:57:08 +0800 Subject: [PATCH 08/43] Install docker-compose in ros container Signed-off-by: Aaron Chong --- .github/workflows/nightly.yaml | 10 +++++----- free_fleet_examples/tests/integration/test_tf.py | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/nightly.yaml b/.github/workflows/nightly.yaml index ef58c6c..0d3b0e0 100644 --- a/.github/workflows/nightly.yaml +++ b/.github/workflows/nightly.yaml @@ -16,7 +16,7 @@ jobs: steps: - name: Install dependencies run: | - sudo apt update && sudo apt install python3-pip -y + sudo apt update && sudo apt install python3-pip docker-compose -y pip3 install eclipse-zenoh==0.11.0 pycdr2 --break-system-packages - uses: ros-tooling/setup-ros@v0.7 @@ -26,8 +26,8 @@ jobs: - name: Checkout uses: actions/checkout@v1 - - name: Start containers - run: docker compose -f ".github/integration-tests/docker-compose.yaml" up -d --build + - name: Start test fixture containers + run: docker-compose -f ".github/integration-tests/docker-compose.yaml" up -d --build - name: build and test uses: ros-tooling/action-ros-ci@v0.3 @@ -37,6 +37,6 @@ jobs: package-name: free_fleet free_fleet_adapter free_fleet_examples target-ros2-distro: ${{ matrix.ros_distribution }} - - name: Stop containers + - name: Stop test fixture containers if: always() - run: docker compose -f ".github/integration-tests/docker-compose.yaml" down + run: docker-compose -f ".github/integration-tests/docker-compose.yaml" down diff --git a/free_fleet_examples/tests/integration/test_tf.py b/free_fleet_examples/tests/integration/test_tf.py index 8ccaa6d..578eec3 100644 --- a/free_fleet_examples/tests/integration/test_tf.py +++ b/free_fleet_examples/tests/integration/test_tf.py @@ -54,7 +54,7 @@ def tf_callback(sample: zenoh.Sample): transform_exists = True except Exception as err: print(f'Unable to get transform between base_footprint and ' - f'map: {type(err)}: {err}') + f'map: {type(err)}: {err}') time.sleep(1) pub.undeclare() From d9c9bd9037505be99b1dfc8a52729ae43d0420d8 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Wed, 6 Nov 2024 13:04:50 +0800 Subject: [PATCH 09/43] Ommit setup-ros step and fix linting Signed-off-by: Aaron Chong --- .github/workflows/build.yaml | 3 --- .github/workflows/nightly.yaml | 4 ---- free_fleet_examples/tests/integration/test_tf.py | 2 +- 3 files changed, 1 insertion(+), 8 deletions(-) diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index 685dfe2..ab90e7e 100644 --- a/.github/workflows/build.yaml +++ b/.github/workflows/build.yaml @@ -20,9 +20,6 @@ jobs: run: | sudo apt update && sudo apt install python3-pip -y pip3 install eclipse-zenoh==0.11.0 pycdr2 --break-system-packages - - uses: ros-tooling/setup-ros@v0.7 - with: - required-ros-distributions: ${{ matrix.ros_distribution }} - name: build and test uses: ros-tooling/action-ros-ci@v0.3 env: diff --git a/.github/workflows/nightly.yaml b/.github/workflows/nightly.yaml index 0d3b0e0..3ef8063 100644 --- a/.github/workflows/nightly.yaml +++ b/.github/workflows/nightly.yaml @@ -19,10 +19,6 @@ jobs: sudo apt update && sudo apt install python3-pip docker-compose -y pip3 install eclipse-zenoh==0.11.0 pycdr2 --break-system-packages - - uses: ros-tooling/setup-ros@v0.7 - with: - required-ros-distributions: ${{ matrix.ros_distribution }} - - name: Checkout uses: actions/checkout@v1 diff --git a/free_fleet_examples/tests/integration/test_tf.py b/free_fleet_examples/tests/integration/test_tf.py index 578eec3..6d94dd7 100644 --- a/free_fleet_examples/tests/integration/test_tf.py +++ b/free_fleet_examples/tests/integration/test_tf.py @@ -46,7 +46,7 @@ def tf_callback(sample: zenoh.Sample): transform_exists = False for i in range(10): try: - transform = tf_buffer.lookup_transform( + tf_buffer.lookup_transform( 'base_footprint', 'map', Time() From f3143bb30dcbb79567deabd54f02ecbf27d3bbdf Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Wed, 6 Nov 2024 15:52:47 +0800 Subject: [PATCH 10/43] Revert setup ros step, use cmake -args Signed-off-by: Aaron Chong --- .github/workflows/build.yaml | 15 +++++++++++++-- .github/workflows/nightly.yaml | 14 ++++++++++++-- 2 files changed, 25 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index ab90e7e..6891b4b 100644 --- a/.github/workflows/build.yaml +++ b/.github/workflows/build.yaml @@ -20,10 +20,21 @@ jobs: run: | sudo apt update && sudo apt install python3-pip -y pip3 install eclipse-zenoh==0.11.0 pycdr2 --break-system-packages + + - uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: ${{ matrix.ros_distribution }} + - name: build and test uses: ros-tooling/action-ros-ci@v0.3 - env: - INTEGRATION_TESTING: false with: package-name: free_fleet free_fleet_adapter free_fleet_examples target-ros2-distro: ${{ matrix.ros_distribution }} + colcon-defaults: | + { + "build": { + "cmake-args": [ + "-DINTEGRATION_TESTING=OFF" + ] + } + } diff --git a/.github/workflows/nightly.yaml b/.github/workflows/nightly.yaml index 3ef8063..2c50961 100644 --- a/.github/workflows/nightly.yaml +++ b/.github/workflows/nightly.yaml @@ -25,13 +25,23 @@ jobs: - name: Start test fixture containers run: docker-compose -f ".github/integration-tests/docker-compose.yaml" up -d --build + - uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: ${{ matrix.ros_distribution }} + - name: build and test uses: ros-tooling/action-ros-ci@v0.3 - env: - INTEGRATION_TESTING: true with: package-name: free_fleet free_fleet_adapter free_fleet_examples target-ros2-distro: ${{ matrix.ros_distribution }} + colcon-defaults: | + { + "build": { + "cmake-args": [ + "-DINTEGRATION_TESTING=ON" + ] + } + } - name: Stop test fixture containers if: always() From 89c687d6bcd2c87a5da179b7baa6f260a55707ac Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 7 Nov 2024 00:37:28 +0800 Subject: [PATCH 11/43] Use 1.0.1 API Signed-off-by: Aaron Chong --- .github/minimal-zenoh-bridge/Dockerfile | 2 +- .github/workflows/nightly.yaml | 2 +- .../tests/integration/test_tf.py | 70 +++++++++---------- 3 files changed, 37 insertions(+), 37 deletions(-) diff --git a/.github/minimal-zenoh-bridge/Dockerfile b/.github/minimal-zenoh-bridge/Dockerfile index 7b6330e..17c434a 100644 --- a/.github/minimal-zenoh-bridge/Dockerfile +++ b/.github/minimal-zenoh-bridge/Dockerfile @@ -1,6 +1,6 @@ ARG ROS_DISTRO=jazzy FROM docker.io/ros:$ROS_DISTRO-ros-base -ARG ZENOH_VERSION=0.11.0 +ARG ZENOH_VERSION=1.0.1 ARG BRANCH=efc/integration-testing RUN apt update && apt install -y curl wget unzip ros-jazzy-rmw-cyclonedds-cpp diff --git a/.github/workflows/nightly.yaml b/.github/workflows/nightly.yaml index 2c50961..bdcab26 100644 --- a/.github/workflows/nightly.yaml +++ b/.github/workflows/nightly.yaml @@ -17,7 +17,7 @@ jobs: - name: Install dependencies run: | sudo apt update && sudo apt install python3-pip docker-compose -y - pip3 install eclipse-zenoh==0.11.0 pycdr2 --break-system-packages + pip3 install eclipse-zenoh==1.0.1 pycdr2 --break-system-packages - name: Checkout uses: actions/checkout@v1 diff --git a/free_fleet_examples/tests/integration/test_tf.py b/free_fleet_examples/tests/integration/test_tf.py index 6d94dd7..db4857b 100644 --- a/free_fleet_examples/tests/integration/test_tf.py +++ b/free_fleet_examples/tests/integration/test_tf.py @@ -26,38 +26,38 @@ def test_tf(): - # Open Zenoh Session - session = zenoh.open(zenoh.Config()) - - tf_buffer = Buffer() - - def tf_callback(sample: zenoh.Sample): - transform = TFMessage.deserialize(sample.payload) - for zt in transform.transforms: - t = transform_stamped_to_ros2_msg(zt) - tf_buffer.set_transform(t, 'free_fleet_examples_test_tf') - - # Subscribe to TF - pub = session.declare_subscriber( - namespacify('tf', 'turtlebot3_1'), - tf_callback - ) - - transform_exists = False - for i in range(10): - try: - tf_buffer.lookup_transform( - 'base_footprint', - 'map', - Time() - ) - transform_exists = True - except Exception as err: - print(f'Unable to get transform between base_footprint and ' - f'map: {type(err)}: {err}') - - time.sleep(1) - pub.undeclare() - session.close() - - assert transform_exists + zenoh.try_init_log_from_env() + with zenoh.open(zenoh.Config()) as session: + tf_buffer = Buffer() + + def tf_callback(sample: zenoh.Sample): + transform = TFMessage.deserialize(sample.payload.to_bytes()) + for zt in transform.transforms: + t = transform_stamped_to_ros2_msg(zt) + tf_buffer.set_transform(t, 'free_fleet_examples_test_tf') + + # Subscribe to TF + pub = session.declare_subscriber( + namespacify('tf', 'turtlebot3_1'), + tf_callback + ) + + transform_exists = False + for i in range(10): + try: + tf_buffer.lookup_transform( + 'base_footprint', + 'map', + Time() + ) + transform_exists = True + except Exception as err: + print(f'Unable to get transform between base_footprint and ' + f'map: {type(err)}: {err}') + + time.sleep(1) + + pub.undeclare() + session.close() + + assert transform_exists From 4b6abf551f88245cf681b6eafcc33b9c5821e29b Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 7 Nov 2024 01:09:42 +0800 Subject: [PATCH 12/43] Using client and router method Signed-off-by: Aaron Chong --- .github/integration-tests/docker-compose.yaml | 10 ++++++++++ .github/minimal-zenoh/Dockerfile | 8 ++++++++ .../zenoh_configs/turtlebot3_1_zenoh_config.json5 | 2 +- 3 files changed, 19 insertions(+), 1 deletion(-) create mode 100644 .github/minimal-zenoh/Dockerfile diff --git a/.github/integration-tests/docker-compose.yaml b/.github/integration-tests/docker-compose.yaml index 5763cb0..259d108 100644 --- a/.github/integration-tests/docker-compose.yaml +++ b/.github/integration-tests/docker-compose.yaml @@ -28,3 +28,13 @@ services: environment: - ROS_DOMAIN_ID=42 # command: ros2 launch task_manager task_manager.launch.py params_file:=/examples/nav2_example_params.yaml + + zenoh: + image: zenoh + build: + context: ../../ + dockerfile: ./github/minimal-zenoh/Dockerfile + network_mode: host + stdin_open: true + tty: true + command: zenohd diff --git a/.github/minimal-zenoh/Dockerfile b/.github/minimal-zenoh/Dockerfile new file mode 100644 index 0000000..6215b93 --- /dev/null +++ b/.github/minimal-zenoh/Dockerfile @@ -0,0 +1,8 @@ +ARG ROS_DISTRO=jazzy +FROM docker.io/ros:$ROS_DISTRO-ros-base + +RUN echo "deb [trusted=yes] https://download.eclipse.org/zenoh/debian-repo/ /" | tee -a /etc/apt/sources.list > /dev/null + +RUN apt update && apt install -y zenoh + +ENTRYPOINT ["bash", "-c", "zenohd"] diff --git a/free_fleet_examples/zenoh_configs/turtlebot3_1_zenoh_config.json5 b/free_fleet_examples/zenoh_configs/turtlebot3_1_zenoh_config.json5 index 330b3cf..f6e02a9 100644 --- a/free_fleet_examples/zenoh_configs/turtlebot3_1_zenoh_config.json5 +++ b/free_fleet_examples/zenoh_configs/turtlebot3_1_zenoh_config.json5 @@ -150,7 +150,7 @@ //// //// mode: The bridge's mode (router, peer or client) //// - //mode: "router", + mode: "client", //// //// Which endpoints to connect to. E.g. tcp/localhost:7447. From 3c32daf782ef92de17f9699f60f104d089dc441b Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 7 Nov 2024 01:17:20 +0800 Subject: [PATCH 13/43] Remove command in docker compose Signed-off-by: Aaron Chong --- .github/integration-tests/docker-compose.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/integration-tests/docker-compose.yaml b/.github/integration-tests/docker-compose.yaml index 259d108..530d024 100644 --- a/.github/integration-tests/docker-compose.yaml +++ b/.github/integration-tests/docker-compose.yaml @@ -33,8 +33,8 @@ services: image: zenoh build: context: ../../ - dockerfile: ./github/minimal-zenoh/Dockerfile + dockerfile: .github/minimal-zenoh/Dockerfile network_mode: host stdin_open: true tty: true - command: zenohd + # command: zenohd From 2d1a08b76297dfb6dafaddc87d96f94d9d17b8ab Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 7 Nov 2024 11:57:07 +0800 Subject: [PATCH 14/43] Working locally, commented out zenohd Signed-off-by: Aaron Chong --- .github/integration-tests/docker-compose.yaml | 25 ++++++++----------- .github/minimal-nav2/Dockerfile | 4 +-- .github/minimal-zenoh-bridge/Dockerfile | 2 +- .../turtlebot3_1_zenoh_config.json5 | 2 +- 4 files changed, 15 insertions(+), 18 deletions(-) diff --git a/.github/integration-tests/docker-compose.yaml b/.github/integration-tests/docker-compose.yaml index 530d024..c0bb0eb 100644 --- a/.github/integration-tests/docker-compose.yaml +++ b/.github/integration-tests/docker-compose.yaml @@ -6,7 +6,6 @@ services: build: context: ../../ dockerfile: .github/minimal-nav2/Dockerfile - # container_name: minimal-nav2 stop_signal: SIGINT network_mode: host privileged: true @@ -14,27 +13,25 @@ services: tty: true environment: - ROS_DOMAIN_ID=42 - # command: ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False - zenoh-bridge: + minimal-zenoh-bridge: image: minimal-zenoh-bridge build: context: ../../ dockerfile: .github/minimal-zenoh-bridge/Dockerfile - # container_name: minimal-zenoh-bridge network_mode: host stdin_open: true tty: true environment: - ROS_DOMAIN_ID=42 - # command: ros2 launch task_manager task_manager.launch.py params_file:=/examples/nav2_example_params.yaml - zenoh: - image: zenoh - build: - context: ../../ - dockerfile: .github/minimal-zenoh/Dockerfile - network_mode: host - stdin_open: true - tty: true - # command: zenohd + # zenoh: + # image: minimal-zenoh + # # build: + # # context: ../../ + # # dockerfile: .github/minimal-zenoh/Dockerfile + # container_name: minimal-zenoh + # network_mode: host + # stdin_open: true + # tty: true + # # command: zenohd diff --git a/.github/minimal-nav2/Dockerfile b/.github/minimal-nav2/Dockerfile index 2d96ca6..cfc66b8 100644 --- a/.github/minimal-nav2/Dockerfile +++ b/.github/minimal-nav2/Dockerfile @@ -17,6 +17,6 @@ RUN rm -rf \ # Modify existing params file to add initial pose RUN mkdir -p /params && cd /params \ && cp /opt/ros/$ROS_DISTRO/share/nav2_bringup/params/nav2_params.yaml . \ - && sed -z 's|amcl:\n ros__parameters:\n|amcl:\n ros__parameters:\n set_initial_pose: true\n initial_pose:{x: -2.0, y: -0.5, z: 0.0, yaw: 0.0}\n|' nav2_params.yaml > nav2_params_edited.yaml + && sed -z 's|amcl:\n ros__parameters:\n|amcl:\n ros__parameters:\n set_initial_pose: true\n initial_pose: {x: -2.0, y: -0.5, z: 0.0, yaw: 0.0}\n|' nav2_params.yaml > nav2_params_edited.yaml -ENTRYPOINT ["bash", "-c", ". /opt/ros/$ROS_DISTRO/setup.bash && ros2 launch nav2_bringup tb3_simulation_launch.py params_file:=/params/nav2_params_edited.yaml"] +ENTRYPOINT ["bash", "-c", ". /opt/ros/$ROS_DISTRO/setup.bash && ros2 launch nav2_bringup tb3_simulation_launch.py params_file:=/params/nav2_params_edited.yaml use_rviz:=False"] diff --git a/.github/minimal-zenoh-bridge/Dockerfile b/.github/minimal-zenoh-bridge/Dockerfile index 17c434a..89f1f18 100644 --- a/.github/minimal-zenoh-bridge/Dockerfile +++ b/.github/minimal-zenoh-bridge/Dockerfile @@ -21,4 +21,4 @@ RUN rm -rf \ /var/lib/apt/lists \ /dist -ENTRYPOINT ["bash", "-c", ". /opt/ros/$ROS_DISTRO/setup.bash && . /zenoh-bridge/zenoh-bridge-ros2dds -c /zenoh-brdige/free_fleet/free_fleet_examples/zenoh_configs/turtlebot3_1_zenoh_config.json5"] +ENTRYPOINT ["bash", "-c", ". /opt/ros/$ROS_DISTRO/setup.bash && /zenoh-bridge/zenoh-bridge-ros2dds -c /zenoh-bridge/free_fleet/free_fleet_examples/zenoh_configs/turtlebot3_1_zenoh_config.json5"] diff --git a/free_fleet_examples/zenoh_configs/turtlebot3_1_zenoh_config.json5 b/free_fleet_examples/zenoh_configs/turtlebot3_1_zenoh_config.json5 index f6e02a9..7f402c9 100644 --- a/free_fleet_examples/zenoh_configs/turtlebot3_1_zenoh_config.json5 +++ b/free_fleet_examples/zenoh_configs/turtlebot3_1_zenoh_config.json5 @@ -150,7 +150,7 @@ //// //// mode: The bridge's mode (router, peer or client) //// - mode: "client", + // mode: "router", //// //// Which endpoints to connect to. E.g. tcp/localhost:7447. From d209d50217380ac9c2f92040c78754398d04ab32 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 7 Nov 2024 12:15:17 +0800 Subject: [PATCH 15/43] Spinning up minimal-zenoh too Signed-off-by: Aaron Chong --- .github/integration-tests/docker-compose.yaml | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/.github/integration-tests/docker-compose.yaml b/.github/integration-tests/docker-compose.yaml index c0bb0eb..0175f63 100644 --- a/.github/integration-tests/docker-compose.yaml +++ b/.github/integration-tests/docker-compose.yaml @@ -25,13 +25,11 @@ services: environment: - ROS_DOMAIN_ID=42 - # zenoh: - # image: minimal-zenoh - # # build: - # # context: ../../ - # # dockerfile: .github/minimal-zenoh/Dockerfile - # container_name: minimal-zenoh - # network_mode: host - # stdin_open: true - # tty: true - # # command: zenohd + minimal-zenoh: + image: minimal-zenoh + build: + context: ../../ + dockerfile: .github/minimal-zenoh/Dockerfile + network_mode: host + stdin_open: true + tty: true From 75d738036500841eb4549a1678887ea7224fdc9a Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 7 Nov 2024 12:54:33 +0800 Subject: [PATCH 16/43] Moving docker files, trying with client mode Signed-off-by: Aaron Chong --- .../integration-tests/docker-compose.yaml | 20 ++++---- .../minimal-nav2-bringup}/Dockerfile | 0 .../docker/minimal-zenoh-bridge/Dockerfile | 22 +++++++++ .../docker/minimal-zenoh-router/Dockerfile | 12 +++++ .github/minimal-zenoh-bridge/Dockerfile | 24 ---------- .github/minimal-zenoh/Dockerfile | 8 ---- .github/workflows/nightly.yaml | 47 ++++++++++++++++++- .../turtlebot3_1_zenoh_config.json5 | 2 +- 8 files changed, 90 insertions(+), 45 deletions(-) rename .github/{ => docker}/integration-tests/docker-compose.yaml (52%) rename .github/{minimal-nav2 => docker/minimal-nav2-bringup}/Dockerfile (100%) create mode 100644 .github/docker/minimal-zenoh-bridge/Dockerfile create mode 100644 .github/docker/minimal-zenoh-router/Dockerfile delete mode 100644 .github/minimal-zenoh-bridge/Dockerfile delete mode 100644 .github/minimal-zenoh/Dockerfile diff --git a/.github/integration-tests/docker-compose.yaml b/.github/docker/integration-tests/docker-compose.yaml similarity index 52% rename from .github/integration-tests/docker-compose.yaml rename to .github/docker/integration-tests/docker-compose.yaml index 0175f63..564e868 100644 --- a/.github/integration-tests/docker-compose.yaml +++ b/.github/docker/integration-tests/docker-compose.yaml @@ -1,11 +1,11 @@ version: "3" services: - minimal-nav2: - image: minimal-nav2 + minimal-nav2-bringup: + image: minimal-nav2-bringup build: - context: ../../ - dockerfile: .github/minimal-nav2/Dockerfile + context: ../../../ + dockerfile: .github/docker/minimal-nav2-bringup/Dockerfile stop_signal: SIGINT network_mode: host privileged: true @@ -17,19 +17,19 @@ services: minimal-zenoh-bridge: image: minimal-zenoh-bridge build: - context: ../../ - dockerfile: .github/minimal-zenoh-bridge/Dockerfile + context: ../../../ + dockerfile: .github/docker/minimal-zenoh-bridge/Dockerfile network_mode: host stdin_open: true tty: true environment: - ROS_DOMAIN_ID=42 - minimal-zenoh: - image: minimal-zenoh + minimal-zenoh-router: + image: minimal-zenoh-router build: - context: ../../ - dockerfile: .github/minimal-zenoh/Dockerfile + context: ../../../ + dockerfile: .github/docker/minimal-zenoh-router/Dockerfile network_mode: host stdin_open: true tty: true diff --git a/.github/minimal-nav2/Dockerfile b/.github/docker/minimal-nav2-bringup/Dockerfile similarity index 100% rename from .github/minimal-nav2/Dockerfile rename to .github/docker/minimal-nav2-bringup/Dockerfile diff --git a/.github/docker/minimal-zenoh-bridge/Dockerfile b/.github/docker/minimal-zenoh-bridge/Dockerfile new file mode 100644 index 0000000..c8f4e05 --- /dev/null +++ b/.github/docker/minimal-zenoh-bridge/Dockerfile @@ -0,0 +1,22 @@ +ARG ROS_DISTRO=jazzy +FROM docker.io/ros:$ROS_DISTRO-ros-base +ARG ZENOH_VERSION=1.0.1 +ARG FREE_FLEET_BRANCH=efc/integration-testing + +RUN apt update && apt install -y wget unzip ros-jazzy-rmw-cyclonedds-cpp + +RUN mkdir -p /zenoh-bridge && cd /zenoh-bridge \ + && wget -O zenoh-plugin-ros2dds.zip https://github.com/eclipse-zenoh/zenoh-plugin-ros2dds/releases/download/$ZENOH_VERSION/zenoh-plugin-ros2dds-$ZENOH_VERSION-x86_64-unknown-linux-gnu-standalone.zip \ + && unzip zenoh-plugin-ros2dds.zip \ + && rm zenoh-plugin-ros2dds.zip + +RUN cd /zenoh-bridge \ + && wget -O turtlebot3_1_zenoh_config.json5 https://raw.githubusercontent.com/open-rmf/free_fleet/refs/heads/$FREE_FLEET_BRANCH/free_fleet_examples/zenoh_configs/turtlebot3_1_zenoh_config.json5 + +ENV RMW_IMPLEMENTATION rmw_cyclonedds_cpp + +RUN rm -rf \ + /var/lib/apt/lists \ + /dist + +ENTRYPOINT ["bash", "-c", ". /opt/ros/$ROS_DISTRO/setup.bash && /zenoh-bridge/zenoh-bridge-ros2dds -c /zenoh-bridge/turtlebot3_1_zenoh_config.json5"] diff --git a/.github/docker/minimal-zenoh-router/Dockerfile b/.github/docker/minimal-zenoh-router/Dockerfile new file mode 100644 index 0000000..472074b --- /dev/null +++ b/.github/docker/minimal-zenoh-router/Dockerfile @@ -0,0 +1,12 @@ +ARG ROS_DISTRO=jazzy +FROM docker.io/ros:$ROS_DISTRO-ros-base +ARG ZENOH_VERSION=1.0.1 + +RUN apt update && apt install -y wget unzip + +RUN mkdir -p /zenoh && cd /zenoh \ + && wget -O zenoh.zip https://github.com/eclipse-zenoh/zenoh/releases/download/1.0.1/zenoh-1.0.1-x86_64-unknown-linux-gnu-standalone.zip \ + && unzip zenoh.zip \ + && rm zenoh.zip + +ENTRYPOINT ["bash", "-c", "/zenoh/zenohd"] diff --git a/.github/minimal-zenoh-bridge/Dockerfile b/.github/minimal-zenoh-bridge/Dockerfile deleted file mode 100644 index 89f1f18..0000000 --- a/.github/minimal-zenoh-bridge/Dockerfile +++ /dev/null @@ -1,24 +0,0 @@ -ARG ROS_DISTRO=jazzy -FROM docker.io/ros:$ROS_DISTRO-ros-base -ARG ZENOH_VERSION=1.0.1 -ARG BRANCH=efc/integration-testing - -RUN apt update && apt install -y curl wget unzip ros-jazzy-rmw-cyclonedds-cpp - -RUN mkdir -p /zenoh-bridge && cd /zenoh-bridge \ - && wget https://github.com/eclipse-zenoh/zenoh-plugin-ros2dds/releases/download/$ZENOH_VERSION/zenoh-plugin-ros2dds-$ZENOH_VERSION-x86_64-unknown-linux-gnu-standalone.zip \ - && unzip zenoh-plugin-ros2dds-$ZENOH_VERSION-x86_64-unknown-linux-gnu-standalone.zip \ - && rm zenoh-plugin-ros2dds-$ZENOH_VERSION-x86_64-unknown-linux-gnu-standalone.zip - -RUN cd /zenoh-bridge && mkdir -p /zenoh-bridge/free_fleet \ - && curl -L https://github.com/open-rmf/free_fleet/archive/$BRANCH.tar.gz -o free_fleet.tar.gz \ - && tar zxf free_fleet.tar.gz -C /zenoh-bridge/free_fleet --strip-components=1 \ - && rm free_fleet.tar.gz - -ENV RMW_IMPLEMENTATION rmw_cyclonedds_cpp - -RUN rm -rf \ - /var/lib/apt/lists \ - /dist - -ENTRYPOINT ["bash", "-c", ". /opt/ros/$ROS_DISTRO/setup.bash && /zenoh-bridge/zenoh-bridge-ros2dds -c /zenoh-bridge/free_fleet/free_fleet_examples/zenoh_configs/turtlebot3_1_zenoh_config.json5"] diff --git a/.github/minimal-zenoh/Dockerfile b/.github/minimal-zenoh/Dockerfile deleted file mode 100644 index 6215b93..0000000 --- a/.github/minimal-zenoh/Dockerfile +++ /dev/null @@ -1,8 +0,0 @@ -ARG ROS_DISTRO=jazzy -FROM docker.io/ros:$ROS_DISTRO-ros-base - -RUN echo "deb [trusted=yes] https://download.eclipse.org/zenoh/debian-repo/ /" | tee -a /etc/apt/sources.list > /dev/null - -RUN apt update && apt install -y zenoh - -ENTRYPOINT ["bash", "-c", "zenohd"] diff --git a/.github/workflows/nightly.yaml b/.github/workflows/nightly.yaml index bdcab26..9871232 100644 --- a/.github/workflows/nightly.yaml +++ b/.github/workflows/nightly.yaml @@ -3,6 +3,49 @@ name: Nightly on: push jobs: + # build-minimal-docker-images: + # name: Push minimal-rmf Docker image to GitHub Packages + # runs-on: ubuntu-latest + # strategy: + # matrix: + # ros_distribution: [jazzy] + # steps: + # - uses: actions/checkout@v4 + # - name: Set up Docker Buildx + # uses: docker/setup-buildx-action@v3 + # - name: Login to docker + # uses: docker/login-action@v3 + # with: + # registry: ghcr.io + # username: ${{ github.actor }} + # password: ${{ secrets.GITHUB_TOKEN }} + # - name: Build and push minimal-nav2-bringup + # uses: docker/build-push-action@v5 + # with: + # push: true + # build-args: | + # ROS_DISTRO=${{ matrix.ros_distribution }} + # tags: ghcr.io/${{ github.repository }}/minimal-nav2-bringup:${{ matrix.ros_distribution }}-latest + # context: .github/docker/minimal-nav2-bringup + # - name: Build and push minimal-zenoh-bridge + # uses: docker/build-push-action@v5 + # with: + # push: true + # build-args: | + # ROS_DISTRO=${{ matrix.ros_distribution }} + # ZENOH_VERSION=1.0.1 + # FREE_FLEET_BRANCH=efc/integration-testing + # tags: ghcr.io/${{ github.repository }}/minimal-nav2-bringup:${{ matrix.ros_distribution }}-latest + # context: .github/docker/minimal-nav2-bringup + # - name: Build and push minimal-zenoh-router + # uses: docker/build-push-action@v5 + # with: + # push: true + # build-args: | + # ROS_DISTRO=${{ matrix.ros_distribution }} + # tags: ghcr.io/${{ github.repository }}/minimal-nav2-bringup:${{ matrix.ros_distribution }}-latest + # context: .github/docker/minimal-nav2-bringup + integration-tests: timeout-minutes: 10 runs-on: ubuntu-latest @@ -23,7 +66,7 @@ jobs: uses: actions/checkout@v1 - name: Start test fixture containers - run: docker-compose -f ".github/integration-tests/docker-compose.yaml" up -d --build + run: docker-compose -f ".github/docker/integration-tests/docker-compose.yaml" up -d --build - uses: ros-tooling/setup-ros@v0.7 with: @@ -45,4 +88,4 @@ jobs: - name: Stop test fixture containers if: always() - run: docker-compose -f ".github/integration-tests/docker-compose.yaml" down + run: docker-compose -f ".github/docker/integration-tests/docker-compose.yaml" down diff --git a/free_fleet_examples/zenoh_configs/turtlebot3_1_zenoh_config.json5 b/free_fleet_examples/zenoh_configs/turtlebot3_1_zenoh_config.json5 index 7f402c9..f6e02a9 100644 --- a/free_fleet_examples/zenoh_configs/turtlebot3_1_zenoh_config.json5 +++ b/free_fleet_examples/zenoh_configs/turtlebot3_1_zenoh_config.json5 @@ -150,7 +150,7 @@ //// //// mode: The bridge's mode (router, peer or client) //// - // mode: "router", + mode: "client", //// //// Which endpoints to connect to. E.g. tcp/localhost:7447. From 2fda75c48038e93257f14dcf9648321df2f96f0d Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 11 Nov 2024 00:36:53 +0800 Subject: [PATCH 17/43] Use official zenoh docker image and compose example, rename client zenoh config, update README, test build docker images Signed-off-by: Aaron Chong --- .../integration-tests/docker-compose.yaml | 8 +- .../docker/minimal-zenoh-bridge/Dockerfile | 4 +- .github/workflows/nightly.yaml | 146 +++++++++--------- README.md | 49 ++++-- ...=> turtlebot3_1_client_zenoh_config.json5} | 0 5 files changed, 112 insertions(+), 95 deletions(-) rename free_fleet_examples/zenoh_configs/{turtlebot3_1_zenoh_config.json5 => turtlebot3_1_client_zenoh_config.json5} (100%) diff --git a/.github/docker/integration-tests/docker-compose.yaml b/.github/docker/integration-tests/docker-compose.yaml index 564e868..566046d 100644 --- a/.github/docker/integration-tests/docker-compose.yaml +++ b/.github/docker/integration-tests/docker-compose.yaml @@ -26,10 +26,10 @@ services: - ROS_DOMAIN_ID=42 minimal-zenoh-router: - image: minimal-zenoh-router - build: - context: ../../../ - dockerfile: .github/docker/minimal-zenoh-router/Dockerfile + image: eclipse/zenoh + restart: unless-stopped network_mode: host stdin_open: true tty: true + environment: + - RUST_LOG=debug diff --git a/.github/docker/minimal-zenoh-bridge/Dockerfile b/.github/docker/minimal-zenoh-bridge/Dockerfile index c8f4e05..5997e58 100644 --- a/.github/docker/minimal-zenoh-bridge/Dockerfile +++ b/.github/docker/minimal-zenoh-bridge/Dockerfile @@ -11,7 +11,7 @@ RUN mkdir -p /zenoh-bridge && cd /zenoh-bridge \ && rm zenoh-plugin-ros2dds.zip RUN cd /zenoh-bridge \ - && wget -O turtlebot3_1_zenoh_config.json5 https://raw.githubusercontent.com/open-rmf/free_fleet/refs/heads/$FREE_FLEET_BRANCH/free_fleet_examples/zenoh_configs/turtlebot3_1_zenoh_config.json5 + && wget -O turtlebot3_1_client_zenoh_config.json5 https://raw.githubusercontent.com/open-rmf/free_fleet/refs/heads/$FREE_FLEET_BRANCH/free_fleet_examples/zenoh_configs/turtlebot3_1_client_zenoh_config.json5 ENV RMW_IMPLEMENTATION rmw_cyclonedds_cpp @@ -19,4 +19,4 @@ RUN rm -rf \ /var/lib/apt/lists \ /dist -ENTRYPOINT ["bash", "-c", ". /opt/ros/$ROS_DISTRO/setup.bash && /zenoh-bridge/zenoh-bridge-ros2dds -c /zenoh-bridge/turtlebot3_1_zenoh_config.json5"] +ENTRYPOINT ["bash", "-c", ". /opt/ros/$ROS_DISTRO/setup.bash && /zenoh-bridge/zenoh-bridge-ros2dds -c /zenoh-bridge/turtlebot3_1_client_zenoh_config.json5"] diff --git a/.github/workflows/nightly.yaml b/.github/workflows/nightly.yaml index 9871232..ad55db9 100644 --- a/.github/workflows/nightly.yaml +++ b/.github/workflows/nightly.yaml @@ -3,89 +3,81 @@ name: Nightly on: push jobs: - # build-minimal-docker-images: - # name: Push minimal-rmf Docker image to GitHub Packages - # runs-on: ubuntu-latest - # strategy: - # matrix: - # ros_distribution: [jazzy] - # steps: - # - uses: actions/checkout@v4 - # - name: Set up Docker Buildx - # uses: docker/setup-buildx-action@v3 - # - name: Login to docker - # uses: docker/login-action@v3 - # with: - # registry: ghcr.io - # username: ${{ github.actor }} - # password: ${{ secrets.GITHUB_TOKEN }} - # - name: Build and push minimal-nav2-bringup - # uses: docker/build-push-action@v5 - # with: - # push: true - # build-args: | - # ROS_DISTRO=${{ matrix.ros_distribution }} - # tags: ghcr.io/${{ github.repository }}/minimal-nav2-bringup:${{ matrix.ros_distribution }}-latest - # context: .github/docker/minimal-nav2-bringup - # - name: Build and push minimal-zenoh-bridge - # uses: docker/build-push-action@v5 - # with: - # push: true - # build-args: | - # ROS_DISTRO=${{ matrix.ros_distribution }} - # ZENOH_VERSION=1.0.1 - # FREE_FLEET_BRANCH=efc/integration-testing - # tags: ghcr.io/${{ github.repository }}/minimal-nav2-bringup:${{ matrix.ros_distribution }}-latest - # context: .github/docker/minimal-nav2-bringup - # - name: Build and push minimal-zenoh-router - # uses: docker/build-push-action@v5 - # with: - # push: true - # build-args: | - # ROS_DISTRO=${{ matrix.ros_distribution }} - # tags: ghcr.io/${{ github.repository }}/minimal-nav2-bringup:${{ matrix.ros_distribution }}-latest - # context: .github/docker/minimal-nav2-bringup - - integration-tests: - timeout-minutes: 10 + build-minimal-docker-images: + name: Push minimal docker images to GitHub Packages runs-on: ubuntu-latest - container: - image: osrf/ros:${{ matrix.ros_distribution }}-desktop-noble strategy: matrix: - ros_distribution: - - jazzy - + ros_distribution: [jazzy] steps: - - name: Install dependencies - run: | - sudo apt update && sudo apt install python3-pip docker-compose -y - pip3 install eclipse-zenoh==1.0.1 pycdr2 --break-system-packages + - uses: actions/checkout@v4 + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 + - name: Login to docker + uses: docker/login-action@v3 + with: + registry: ghcr.io + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + - name: Build and push minimal-nav2-bringup + uses: docker/build-push-action@v5 + with: + push: true + build-args: | + ROS_DISTRO=${{ matrix.ros_distribution }} + tags: ghcr.io/${{ github.repository }}/minimal-nav2-bringup:${{ matrix.ros_distribution }}-latest + context: .github/docker/minimal-nav2-bringup + - name: Build and push minimal-zenoh-bridge + uses: docker/build-push-action@v5 + with: + push: true + build-args: | + ROS_DISTRO=${{ matrix.ros_distribution }} + ZENOH_VERSION=1.0.1 + FREE_FLEET_BRANCH=efc/integration-testing + tags: ghcr.io/${{ github.repository }}/minimal-zenoh-bridge:${{ matrix.ros_distribution }}-latest + context: .github/docker/minimal-zenoh-bridge - - name: Checkout - uses: actions/checkout@v1 + # integration-tests: + # timeout-minutes: 10 + # runs-on: ubuntu-latest + # container: + # image: osrf/ros:${{ matrix.ros_distribution }}-desktop-noble + # strategy: + # matrix: + # ros_distribution: + # - jazzy - - name: Start test fixture containers - run: docker-compose -f ".github/docker/integration-tests/docker-compose.yaml" up -d --build + # steps: + # - name: Install dependencies + # run: | + # sudo apt update && sudo apt install python3-pip docker-compose -y + # pip3 install eclipse-zenoh==1.0.1 pycdr2 --break-system-packages - - uses: ros-tooling/setup-ros@v0.7 - with: - required-ros-distributions: ${{ matrix.ros_distribution }} + # - name: Checkout + # uses: actions/checkout@v1 - - name: build and test - uses: ros-tooling/action-ros-ci@v0.3 - with: - package-name: free_fleet free_fleet_adapter free_fleet_examples - target-ros2-distro: ${{ matrix.ros_distribution }} - colcon-defaults: | - { - "build": { - "cmake-args": [ - "-DINTEGRATION_TESTING=ON" - ] - } - } + # - name: Start test fixture containers + # run: docker-compose -f ".github/docker/integration-tests/docker-compose.yaml" up -d --build + + # - uses: ros-tooling/setup-ros@v0.7 + # with: + # required-ros-distributions: ${{ matrix.ros_distribution }} + + # - name: build and test + # uses: ros-tooling/action-ros-ci@v0.3 + # with: + # package-name: free_fleet free_fleet_adapter free_fleet_examples + # target-ros2-distro: ${{ matrix.ros_distribution }} + # colcon-defaults: | + # { + # "build": { + # "cmake-args": [ + # "-DINTEGRATION_TESTING=ON" + # ] + # } + # } - - name: Stop test fixture containers - if: always() - run: docker-compose -f ".github/docker/integration-tests/docker-compose.yaml" down + # - name: Stop test fixture containers + # if: always() + # run: docker-compose -f ".github/docker/integration-tests/docker-compose.yaml" down diff --git a/README.md b/README.md index 33bd2ae..d6939c8 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # Free Fleet - **[Introduction](#introduction)** -- **[Source build and setup](#source-build-and-setup)** +- **[Dependency installation, source build and setup](#dependency-installation-source-build-and-setup)** - **[Simulation examples](#simulation-examples)** - [Single turtlebot3 world](#single-turtlebot3-world) - [Multiple turtlebot3 world](#multiple-turtlebot3-world) @@ -24,18 +24,18 @@ Supports * [rmw-cyclonedds-cpp](https://github.com/ros2/rmw_cyclonedds) * [Open-RMF on main](https://github.com/open-rmf/rmf) * [zenoh-bridge-ros2dds v1.0.1](https://github.com/eclipse-zenoh/zenoh-plugin-ros2dds/releases/tag/1.0.1) -* [zenoh router v1.0.1](https://github.com/eclipse-zenoh/zenoh/releases/tag/1.0.1) +* [zenoh router](https://zenoh.io/docs/getting-started/installation/#ubuntu-or-any-debian) -We recommend setting up `zenoh-bridge-ros2dds` with the standalone binaries. After downloading the appropriate released version and platform, extract and use the standalone binaries as is. For source builds of `zenoh-bridge-ros2dds`, please follow the [official guides](https://github.com/eclipse-zenoh/zenoh-plugin-ros2dds). +We recommend setting up `zenoh-bridge-ros2dds` with the released standalone binaries. After downloading the appropriate released version and platform, extract and use the standalone binaries as is. For source builds of `zenoh-bridge-ros2dds`, please follow the [official guides](https://github.com/eclipse-zenoh/zenoh-plugin-ros2dds). Most of the tests have been performed using `rmw-cyclonedds-cpp`, while other RMW implementations have shown varying results. Support and testing with other RMW implementations will be set up down the road. > [!WARNING] > This has so far only been tested in simulation, and will undergo updates and changes as more testing is performed. Use at your own risk. For the legacy implementation, check out the [`legacy`](https://github.com/open-rmf/free_fleet/tree/legacy) branch. -## Source build and setup +## Dependency installation, source build and setup -Other system dependencies, +System dependencies, ```bash sudo apt update && sudo apt install python3-pip ros-jazzy-rmw-cyclonedds-cpp @@ -47,6 +47,8 @@ The dependencies `eclipse-zenoh` and `pycdr2` are available through `pip`. Users pip3 install pip install eclipse-zenoh==1.0.1 pycdr2 --break-system-packages ``` +Install `zenohd` from the [official guide](https://zenoh.io/docs/getting-started/installation/#ubuntu-or-any-debian). + > [!NOTE] > If an Open-RMF workspace has already been set up, users can choose to only set up an overlay workspace, which reduces build time. The following steps will assume a fresh new workspace is required. @@ -68,18 +70,19 @@ rosdep install --from-paths src --ignore-src --rosdistro $ROS_DISTRO -yr colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release ``` -Download and extract standalone binaries for `zenohd` and `zenoh-bridge-ros2dds` with the correct architecture, system setup and version. The following example instructions are for `x86_64-unknown-linux-gnu`, +Download and extract standalone binaries for `zenoh-bridge-ros2dds` (optionally `zenohd` if a non-latest version is desired) with the correct architecture, system setup and version. The following example instructions are for `x86_64-unknown-linux-gnu`, ```bash +# Change preferred zenoh version here export ZENOH_VERSION=1.0.1 -# Download and extract Zenoh release -wget -O zenoh.zip https://github.com/eclipse-zenoh/zenoh/releases/download/$ZENOH_VERSION/zenoh-$ZENOH_VERSION-x86_64-unknown-linux-gnu-standalone.zip -unzip zenoh.zip - # Download and extract zenoh-bridge-ros2dds release wget -O zenoh-plugin-ros2dds.zip https://github.com/eclipse-zenoh/zenoh-plugin-ros2dds/releases/download/$ZENOH_VERSION/zenoh-plugin-ros2dds-$ZENOH_VERSION-x86_64-unknown-linux-gnu-standalone.zip unzip zenoh-plugin-ros2dds.zip + +# If using released standalone binaries of zenoh router, download and extract the release +# wget -O zenoh.zip https://github.com/eclipse-zenoh/zenoh/releases/download/$ZENOH_VERSION/zenoh-$ZENOH_VERSION-x86_64-unknown-linux-gnu-standalone.zip +# unzip zenoh.zip ``` ## Simulation examples @@ -114,7 +117,17 @@ ros2 launch nav2_bringup tb3_simulation_launch.py headless:=0 # ros2 launch nav2_bringup tb3_simulation_launch.py ``` -Start `zenoh-bridge-ros2dds` with the appropriate zenoh configuration, +Start `zenoh` router, + +```bash +zenohd + +# If using released standalaone binaries +# cd PATH_TO_EXTRACTED_ZENOH_ROUTER +# ./zenohd +``` + +Start `zenoh-bridge-ros2dds` with the appropriate zenoh client configuration, ```bash source /opt/ros/jazzy/setup.bash @@ -194,7 +207,17 @@ export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:~/turtlebot3_simulations/turtlebot3_ ros2 launch nav2_bringup unique_multi_tb3_simulation_launch.py ``` -Start `zenoh-bridge-ros2dds` with the appropriate zenoh configuration, +Start `zenoh` router, + +```bash +zenohd + +# If using released standalaone binaries +# cd PATH_TO_EXTRACTED_ZENOH_ROUTER +# ./zenohd +``` + +Start `zenoh-bridge-ros2dds` with the appropriate zenoh client configuration, ```bash source /opt/ros/jazzy/setup.bash @@ -265,6 +288,8 @@ ros2 run rmf_demos_tasks dispatch_patrol \ * For potential bandwidth issues, especially during multirobot sim example, spinning up a dedicated zenoh router and routing the `zenoh-bridge-ros2dds` manually to it, could help alleviate such issues. +* If `zenoh` messages are not received, make sure the versions between the `eclipse-zenoh` in `pip`, `zenoh-bridge-ros2dds` and `zenohd` are all the same. If the debian binary releases of `zenohd` have breaking changes, and the repo has not yet migrate to the newer version, please open an issue ticket and we will look into migrating as soon as possible. In the meantime, using an older standalone release of `zenohd` would be a temporary workaround. Our integration tests will attempt to catch these breaking changes too. + ## TODOs * hardware testing diff --git a/free_fleet_examples/zenoh_configs/turtlebot3_1_zenoh_config.json5 b/free_fleet_examples/zenoh_configs/turtlebot3_1_client_zenoh_config.json5 similarity index 100% rename from free_fleet_examples/zenoh_configs/turtlebot3_1_zenoh_config.json5 rename to free_fleet_examples/zenoh_configs/turtlebot3_1_client_zenoh_config.json5 From 4d651b115abddebc0507b91d422fe6850a28d0bf Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 11 Nov 2024 01:23:45 +0800 Subject: [PATCH 18/43] Re-usable workflow actions, split integration testing Signed-off-by: Aaron Chong --- .github/actions/build-and-test.yaml | 38 +++++++++++ .../integration-tests/docker-compose.yaml | 10 +-- .github/workflows/build.yaml | 40 ----------- .github/workflows/integration-tests.yaml | 32 +++++++++ .github/workflows/nightly.yaml | 67 ++++++++----------- .github/workflows/style.yaml | 20 +++--- .github/workflows/tests.yaml | 24 +++++++ 7 files changed, 135 insertions(+), 96 deletions(-) create mode 100644 .github/actions/build-and-test.yaml delete mode 100644 .github/workflows/build.yaml create mode 100644 .github/workflows/integration-tests.yaml create mode 100644 .github/workflows/tests.yaml diff --git a/.github/actions/build-and-test.yaml b/.github/actions/build-and-test.yaml new file mode 100644 index 0000000..a5138e1 --- /dev/null +++ b/.github/actions/build-and-test.yaml @@ -0,0 +1,38 @@ +name: build-and-test + +inputs: + ros-distribution: + description: string, ROS distribution to setup + required: true + zenoh-version: + description: string, version of eclipse-zenoh to install from pip + required: true + integration-testing: + description: string, supports ON or OFF only + required: false + default: "OFF" + +runs: + steps: + - name: install dependencies + run: | + sudo apt update && sudo apt install python3-pip -y + pip3 install eclipse-zenoh==${{ inputs.zenoh-version }} pycdr2 --break-system-packages + + - uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: ${{ inputs.ros-distribution }} + + - name: build and test + uses: ros-tooling/action-ros-ci@v0.3 + with: + package-name: free_fleet free_fleet_adapter free_fleet_examples + target-ros2-distro: ${{ inputs.ros-distribution }} + colcon-defaults: | + { + "build": { + "cmake-args": [ + "-DINTEGRATION_TESTING=${{ inputs.integration-testing }}" + ] + } + } diff --git a/.github/docker/integration-tests/docker-compose.yaml b/.github/docker/integration-tests/docker-compose.yaml index 566046d..78dc48f 100644 --- a/.github/docker/integration-tests/docker-compose.yaml +++ b/.github/docker/integration-tests/docker-compose.yaml @@ -2,10 +2,7 @@ version: "3" services: minimal-nav2-bringup: - image: minimal-nav2-bringup - build: - context: ../../../ - dockerfile: .github/docker/minimal-nav2-bringup/Dockerfile + image: ghcr.io/open-rmf/free_fleet/minimal-nav2-bringup:jazzy-latest stop_signal: SIGINT network_mode: host privileged: true @@ -15,10 +12,7 @@ services: - ROS_DOMAIN_ID=42 minimal-zenoh-bridge: - image: minimal-zenoh-bridge - build: - context: ../../../ - dockerfile: .github/docker/minimal-zenoh-bridge/Dockerfile + image: ghcr.io/open-rmf/free_fleet/minimal-zenoh-bridge:jazzy-latest network_mode: host stdin_open: true tty: true diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml deleted file mode 100644 index 6891b4b..0000000 --- a/.github/workflows/build.yaml +++ /dev/null @@ -1,40 +0,0 @@ -name: build -# on: -# pull_request: -# schedule: -# - cron: '0 0 * * *' -on: push - -jobs: - build_and_test: - runs-on: ubuntu-latest - container: - image: osrf/ros:${{ matrix.ros_distribution }}-desktop-noble - strategy: - matrix: - ros_distribution: - - jazzy - - rolling - steps: - - name: install dependencies - run: | - sudo apt update && sudo apt install python3-pip -y - pip3 install eclipse-zenoh==0.11.0 pycdr2 --break-system-packages - - - uses: ros-tooling/setup-ros@v0.7 - with: - required-ros-distributions: ${{ matrix.ros_distribution }} - - - name: build and test - uses: ros-tooling/action-ros-ci@v0.3 - with: - package-name: free_fleet free_fleet_adapter free_fleet_examples - target-ros2-distro: ${{ matrix.ros_distribution }} - colcon-defaults: | - { - "build": { - "cmake-args": [ - "-DINTEGRATION_TESTING=OFF" - ] - } - } diff --git a/.github/workflows/integration-tests.yaml b/.github/workflows/integration-tests.yaml new file mode 100644 index 0000000..c5f6695 --- /dev/null +++ b/.github/workflows/integration-tests.yaml @@ -0,0 +1,32 @@ +name: integration-tests + +on: push + +jobs: + integration-tests: + timeout-minutes: 10 + runs-on: ubuntu-latest + container: + image: osrf/ros:${{ matrix.ros_distribution }}-desktop-noble + strategy: + matrix: + ros_distribution: + - jazzy + + steps: + - name: Checkout + uses: actions/checkout@v1 + + - name: Start test fixture containers + run: docker-compose -f ".github/docker/integration-tests/docker-compose.yaml" up -d --build + + - name: build-and-test + uses: ./.github/actions/build-and-test + with: + ros-distribution: ${{ matrix.ros_distribution }} + zenoh-version: 1.0.1 + integration-testing: ON + + - name: Stop test fixture containers + if: always() + run: docker-compose -f ".github/docker/integration-tests/docker-compose.yaml" down diff --git a/.github/workflows/nightly.yaml b/.github/workflows/nightly.yaml index ad55db9..9d37ce1 100644 --- a/.github/workflows/nightly.yaml +++ b/.github/workflows/nightly.yaml @@ -11,14 +11,17 @@ jobs: ros_distribution: [jazzy] steps: - uses: actions/checkout@v4 + - name: Set up Docker Buildx uses: docker/setup-buildx-action@v3 + - name: Login to docker uses: docker/login-action@v3 with: registry: ghcr.io username: ${{ github.actor }} password: ${{ secrets.GITHUB_TOKEN }} + - name: Build and push minimal-nav2-bringup uses: docker/build-push-action@v5 with: @@ -27,6 +30,7 @@ jobs: ROS_DISTRO=${{ matrix.ros_distribution }} tags: ghcr.io/${{ github.repository }}/minimal-nav2-bringup:${{ matrix.ros_distribution }}-latest context: .github/docker/minimal-nav2-bringup + - name: Build and push minimal-zenoh-bridge uses: docker/build-push-action@v5 with: @@ -38,46 +42,31 @@ jobs: tags: ghcr.io/${{ github.repository }}/minimal-zenoh-bridge:${{ matrix.ros_distribution }}-latest context: .github/docker/minimal-zenoh-bridge - # integration-tests: - # timeout-minutes: 10 - # runs-on: ubuntu-latest - # container: - # image: osrf/ros:${{ matrix.ros_distribution }}-desktop-noble - # strategy: - # matrix: - # ros_distribution: - # - jazzy - - # steps: - # - name: Install dependencies - # run: | - # sudo apt update && sudo apt install python3-pip docker-compose -y - # pip3 install eclipse-zenoh==1.0.1 pycdr2 --break-system-packages - - # - name: Checkout - # uses: actions/checkout@v1 + integration-tests: + needs: build-minimal-docker-images + timeout-minutes: 10 + runs-on: ubuntu-latest + container: + image: osrf/ros:${{ matrix.ros_distribution }}-desktop-noble + strategy: + matrix: + ros_distribution: + - jazzy - # - name: Start test fixture containers - # run: docker-compose -f ".github/docker/integration-tests/docker-compose.yaml" up -d --build + steps: + - name: Checkout + uses: actions/checkout@v1 - # - uses: ros-tooling/setup-ros@v0.7 - # with: - # required-ros-distributions: ${{ matrix.ros_distribution }} + - name: Start test fixture containers + run: docker-compose -f ".github/docker/integration-tests/docker-compose.yaml" up -d --build - # - name: build and test - # uses: ros-tooling/action-ros-ci@v0.3 - # with: - # package-name: free_fleet free_fleet_adapter free_fleet_examples - # target-ros2-distro: ${{ matrix.ros_distribution }} - # colcon-defaults: | - # { - # "build": { - # "cmake-args": [ - # "-DINTEGRATION_TESTING=ON" - # ] - # } - # } + - name: build-and-test + uses: ./.github/actions/build-and-test + with: + ros-distribution: ${{ matrix.ros_distribution }} + zenoh-version: 1.0.1 + integration-testing: ON - # - name: Stop test fixture containers - # if: always() - # run: docker-compose -f ".github/docker/integration-tests/docker-compose.yaml" down + - name: Stop test fixture containers + if: always() + run: docker-compose -f ".github/docker/integration-tests/docker-compose.yaml" down diff --git a/.github/workflows/style.yaml b/.github/workflows/style.yaml index 3464871..3d8803c 100644 --- a/.github/workflows/style.yaml +++ b/.github/workflows/style.yaml @@ -9,12 +9,14 @@ jobs: container: image: ${{ matrix.docker_image }} steps: - - uses: actions/checkout@v3 - - name: deps - shell: bash - run: | - sudo apt update && sudo apt install pycodestyle - - name: pycodestyle - shell: bash - run: | - pycodestyle . + - uses: actions/checkout@v3 + + - name: deps + shell: bash + run: | + sudo apt update && sudo apt install pycodestyle + + - name: pycodestyle + shell: bash + run: | + pycodestyle . diff --git a/.github/workflows/tests.yaml b/.github/workflows/tests.yaml new file mode 100644 index 0000000..a22bb86 --- /dev/null +++ b/.github/workflows/tests.yaml @@ -0,0 +1,24 @@ +name: tests +# on: +# pull_request: +# schedule: +# - cron: '0 0 * * *' +on: push + +jobs: + build_and_test: + runs-on: ubuntu-latest + container: + image: osrf/ros:${{ matrix.ros_distribution }}-desktop-noble + strategy: + matrix: + ros_distribution: + - jazzy + - rolling + build-and-test: + - name: build-and-test + uses: ./.github/actions/build-and-test + with: + ros-distribution: ${{ matrix.ros_distribution }} + zenoh-version: 1.0.1 + integration-testing: OFF From 75458cb361715092ac81ee320d54eb1ac419acd7 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 11 Nov 2024 01:26:29 +0800 Subject: [PATCH 19/43] Missing docker-compose installation Signed-off-by: Aaron Chong --- .github/workflows/integration-tests.yaml | 4 ++++ .github/workflows/nightly.yaml | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/.github/workflows/integration-tests.yaml b/.github/workflows/integration-tests.yaml index c5f6695..ca1af35 100644 --- a/.github/workflows/integration-tests.yaml +++ b/.github/workflows/integration-tests.yaml @@ -17,6 +17,10 @@ jobs: - name: Checkout uses: actions/checkout@v1 + - name: Install docker-compose + run: | + sudo apt update && sudo apt install docker-compose -y + - name: Start test fixture containers run: docker-compose -f ".github/docker/integration-tests/docker-compose.yaml" up -d --build diff --git a/.github/workflows/nightly.yaml b/.github/workflows/nightly.yaml index 9d37ce1..6bd863c 100644 --- a/.github/workflows/nightly.yaml +++ b/.github/workflows/nightly.yaml @@ -57,6 +57,10 @@ jobs: - name: Checkout uses: actions/checkout@v1 + - name: Install docker-compose + run: | + sudo apt update && sudo apt install docker-compose -y + - name: Start test fixture containers run: docker-compose -f ".github/docker/integration-tests/docker-compose.yaml" up -d --build From 710abd8e9562a6c8132ee8c40359c11dd7b9a1c3 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 11 Nov 2024 01:32:16 +0800 Subject: [PATCH 20/43] Fix broken action, rename test job Signed-off-by: Aaron Chong --- .../actions/{build-and-test.yaml => build-and-test/action.yaml} | 0 .github/workflows/tests.yaml | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) rename .github/actions/{build-and-test.yaml => build-and-test/action.yaml} (100%) diff --git a/.github/actions/build-and-test.yaml b/.github/actions/build-and-test/action.yaml similarity index 100% rename from .github/actions/build-and-test.yaml rename to .github/actions/build-and-test/action.yaml diff --git a/.github/workflows/tests.yaml b/.github/workflows/tests.yaml index a22bb86..12d4d0d 100644 --- a/.github/workflows/tests.yaml +++ b/.github/workflows/tests.yaml @@ -6,7 +6,7 @@ name: tests on: push jobs: - build_and_test: + tests: runs-on: ubuntu-latest container: image: osrf/ros:${{ matrix.ros_distribution }}-desktop-noble From 74f2937951eb1cd59cce18e772f9cf3f6974d8f8 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 11 Nov 2024 01:35:43 +0800 Subject: [PATCH 21/43] Fix shell selection, use composite Signed-off-by: Aaron Chong --- .github/actions/build-and-test/action.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/actions/build-and-test/action.yaml b/.github/actions/build-and-test/action.yaml index a5138e1..a20f44b 100644 --- a/.github/actions/build-and-test/action.yaml +++ b/.github/actions/build-and-test/action.yaml @@ -13,11 +13,13 @@ inputs: default: "OFF" runs: + using: composite steps: - name: install dependencies run: | sudo apt update && sudo apt install python3-pip -y pip3 install eclipse-zenoh==${{ inputs.zenoh-version }} pycdr2 --break-system-packages + shell: bash - uses: ros-tooling/setup-ros@v0.7 with: From 60bb934b9466bc5b2b86103a7428eeab31f80ae7 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 11 Nov 2024 01:45:58 +0800 Subject: [PATCH 22/43] Set nightly schedule, fix steps in unit-tests Signed-off-by: Aaron Chong --- .github/workflows/nightly.yaml | 7 +++++-- .github/workflows/{tests.yaml => unit-tests.yaml} | 11 ++++------- 2 files changed, 9 insertions(+), 9 deletions(-) rename .github/workflows/{tests.yaml => unit-tests.yaml} (80%) diff --git a/.github/workflows/nightly.yaml b/.github/workflows/nightly.yaml index 6bd863c..2c095ab 100644 --- a/.github/workflows/nightly.yaml +++ b/.github/workflows/nightly.yaml @@ -1,6 +1,9 @@ -name: Nightly +name: nightly -on: push +on: + schedule: + # 2am SGT + - cron: '0 18 * * *' jobs: build-minimal-docker-images: diff --git a/.github/workflows/tests.yaml b/.github/workflows/unit-tests.yaml similarity index 80% rename from .github/workflows/tests.yaml rename to .github/workflows/unit-tests.yaml index 12d4d0d..8b83300 100644 --- a/.github/workflows/tests.yaml +++ b/.github/workflows/unit-tests.yaml @@ -1,12 +1,9 @@ -name: tests -# on: -# pull_request: -# schedule: -# - cron: '0 0 * * *' +name: unit-tests + on: push jobs: - tests: + unit-tests: runs-on: ubuntu-latest container: image: osrf/ros:${{ matrix.ros_distribution }}-desktop-noble @@ -15,7 +12,7 @@ jobs: ros_distribution: - jazzy - rolling - build-and-test: + steps: - name: build-and-test uses: ./.github/actions/build-and-test with: From 708000d489da5b1f008b0f3a2e124222e075a6a2 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 11 Nov 2024 01:49:15 +0800 Subject: [PATCH 23/43] Remove minimal zenoh router dockerfile, add checkout to unit-tests Signed-off-by: Aaron Chong --- .github/docker/minimal-zenoh-router/Dockerfile | 12 ------------ .github/workflows/unit-tests.yaml | 3 +++ 2 files changed, 3 insertions(+), 12 deletions(-) delete mode 100644 .github/docker/minimal-zenoh-router/Dockerfile diff --git a/.github/docker/minimal-zenoh-router/Dockerfile b/.github/docker/minimal-zenoh-router/Dockerfile deleted file mode 100644 index 472074b..0000000 --- a/.github/docker/minimal-zenoh-router/Dockerfile +++ /dev/null @@ -1,12 +0,0 @@ -ARG ROS_DISTRO=jazzy -FROM docker.io/ros:$ROS_DISTRO-ros-base -ARG ZENOH_VERSION=1.0.1 - -RUN apt update && apt install -y wget unzip - -RUN mkdir -p /zenoh && cd /zenoh \ - && wget -O zenoh.zip https://github.com/eclipse-zenoh/zenoh/releases/download/1.0.1/zenoh-1.0.1-x86_64-unknown-linux-gnu-standalone.zip \ - && unzip zenoh.zip \ - && rm zenoh.zip - -ENTRYPOINT ["bash", "-c", "/zenoh/zenohd"] diff --git a/.github/workflows/unit-tests.yaml b/.github/workflows/unit-tests.yaml index 8b83300..0234e56 100644 --- a/.github/workflows/unit-tests.yaml +++ b/.github/workflows/unit-tests.yaml @@ -13,6 +13,9 @@ jobs: - jazzy - rolling steps: + - name: Checkout + uses: actions/checkout@v1 + - name: build-and-test uses: ./.github/actions/build-and-test with: From c04cd9934ad0c9ce990ed4f60da4f70c9062540a Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 11 Nov 2024 02:12:29 +0800 Subject: [PATCH 24/43] Isolating tf listner components for testing Signed-off-by: Aaron Chong --- .../docker/minimal-zenoh-bridge/Dockerfile | 2 +- .../free_fleet_adapter/nav2_robot_adapter.py | 42 +++++++++++++++++++ .../tests/integration/test_tf.py | 34 ++++++++------- 3 files changed, 61 insertions(+), 17 deletions(-) diff --git a/.github/docker/minimal-zenoh-bridge/Dockerfile b/.github/docker/minimal-zenoh-bridge/Dockerfile index 5997e58..bbb95a1 100644 --- a/.github/docker/minimal-zenoh-bridge/Dockerfile +++ b/.github/docker/minimal-zenoh-bridge/Dockerfile @@ -1,7 +1,7 @@ ARG ROS_DISTRO=jazzy FROM docker.io/ros:$ROS_DISTRO-ros-base ARG ZENOH_VERSION=1.0.1 -ARG FREE_FLEET_BRANCH=efc/integration-testing +ARG FREE_FLEET_BRANCH=main RUN apt update && apt install -y wget unzip ros-jazzy-rmw-cyclonedds-cpp diff --git a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py index 916fe7b..6848015 100644 --- a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py @@ -43,6 +43,48 @@ import zenoh +class TfListener: + def __init__(self, robot_name, zenoh_session, tf_buffer): + self.robot_name = robot_name + self.zenoh_session = zenoh_session + self.tf_buffer = tf_buffer + + def _tf_callback(sample: zenoh.Sample): + try: + transform = TFMessage.deserialize(sample.payload.to_bytes()) + except Exception as e: + self.node.get_logger().debug( + f'Failed to deserialize TF payload: {type(e)}: {e}' + ) + return None + for zt in transform.transforms: + time = rclpy.time.Time( + seconds=zt.header.stamp.sec, + nanoseconds=zt.header.stamp.nanosec + ) + t = TransformStamped() + t.header.stamp = time.to_msg() + t.header.stamp + t.header.frame_id = namespacify(zt.header.frame_id, + self.robot_name) + t.child_frame_id = namespacify(zt.child_frame_id, + self.robot_name) + t.transform.translation.x = zt.transform.translation.x + t.transform.translation.y = zt.transform.translation.y + t.transform.translation.z = zt.transform.translation.z + t.transform.rotation.x = zt.transform.rotation.x + t.transform.rotation.y = zt.transform.rotation.y + t.transform.rotation.z = zt.transform.rotation.z + t.transform.rotation.w = zt.transform.rotation.w + self.tf_buffer.set_transform( + t, f'{self.robot_name}_TfListener') + + self.tf_sub = self.zenoh_session.declare_subscriber( + namespacify('tf', self.robot_name), + _tf_callback + ) + + class Nav2RobotAdapter: def __init__( self, diff --git a/free_fleet_examples/tests/integration/test_tf.py b/free_fleet_examples/tests/integration/test_tf.py index db4857b..9492ed3 100644 --- a/free_fleet_examples/tests/integration/test_tf.py +++ b/free_fleet_examples/tests/integration/test_tf.py @@ -16,9 +16,10 @@ import time -from free_fleet.convert import transform_stamped_to_ros2_msg -from free_fleet.types import TFMessage -from free_fleet.utils import namespacify +# from free_fleet.convert import transform_stamped_to_ros2_msg +# from free_fleet.types import TFMessage +# from free_fleet.utils import namespacify +from free_fleet_adapter.nav2_robot_adapter import TfListener from rclpy.time import Time from tf2_ros import Buffer @@ -30,17 +31,20 @@ def test_tf(): with zenoh.open(zenoh.Config()) as session: tf_buffer = Buffer() - def tf_callback(sample: zenoh.Sample): - transform = TFMessage.deserialize(sample.payload.to_bytes()) - for zt in transform.transforms: - t = transform_stamped_to_ros2_msg(zt) - tf_buffer.set_transform(t, 'free_fleet_examples_test_tf') + listener = TfListener('free_fleet_examples_test', session, tf_buffer) + listener - # Subscribe to TF - pub = session.declare_subscriber( - namespacify('tf', 'turtlebot3_1'), - tf_callback - ) + # def tf_callback(sample: zenoh.Sample): + # transform = TFMessage.deserialize(sample.payload.to_bytes()) + # for zt in transform.transforms: + # t = transform_stamped_to_ros2_msg(zt) + # tf_buffer.set_transform(t, 'free_fleet_examples_test_tf') + + # # Subscribe to TF + # pub = session.declare_subscriber( + # namespacify('tf', 'turtlebot3_1'), + # tf_callback + # ) transform_exists = False for i in range(10): @@ -51,13 +55,11 @@ def tf_callback(sample: zenoh.Sample): Time() ) transform_exists = True + break except Exception as err: print(f'Unable to get transform between base_footprint and ' f'map: {type(err)}: {err}') time.sleep(1) - pub.undeclare() - session.close() - assert transform_exists From 998241a3ec6551ba771b2cb3c2454c6682c1d908 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 11 Nov 2024 02:21:04 +0800 Subject: [PATCH 25/43] Use correct robot name, lint Signed-off-by: Aaron Chong --- free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py | 2 ++ free_fleet_examples/tests/integration/test_tf.py | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py index 6848015..999f386 100644 --- a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py @@ -44,6 +44,7 @@ class TfListener: + def __init__(self, robot_name, zenoh_session, tf_buffer): self.robot_name = robot_name self.zenoh_session = zenoh_session @@ -86,6 +87,7 @@ def _tf_callback(sample: zenoh.Sample): class Nav2RobotAdapter: + def __init__( self, name: str, diff --git a/free_fleet_examples/tests/integration/test_tf.py b/free_fleet_examples/tests/integration/test_tf.py index 9492ed3..28ddcf4 100644 --- a/free_fleet_examples/tests/integration/test_tf.py +++ b/free_fleet_examples/tests/integration/test_tf.py @@ -31,7 +31,7 @@ def test_tf(): with zenoh.open(zenoh.Config()) as session: tf_buffer = Buffer() - listener = TfListener('free_fleet_examples_test', session, tf_buffer) + listener = TfListener('turtlebot3_1', session, tf_buffer) listener # def tf_callback(sample: zenoh.Sample): From 40a4158cee27e3883f7d8f529aff21147e5966d1 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 11 Nov 2024 02:34:31 +0800 Subject: [PATCH 26/43] Clean up imports and fix namespacing Signed-off-by: Aaron Chong --- free_fleet/free_fleet/convert.py | 1 - .../free_fleet_adapter/nav2_robot_adapter.py | 37 +++---------------- .../tests/integration/test_tf.py | 19 +--------- 3 files changed, 7 insertions(+), 50 deletions(-) diff --git a/free_fleet/free_fleet/convert.py b/free_fleet/free_fleet/convert.py index 50cdcad..3a8d250 100644 --- a/free_fleet/free_fleet/convert.py +++ b/free_fleet/free_fleet/convert.py @@ -33,7 +33,6 @@ def transform_stamped_to_ros2_msg(msg: GeometryMsgs_TransformStamped ) -> TransformStamped: t = TransformStamped() t.header.stamp = transform_time_to_ros2_msg(msg.header.stamp) - t.header.stamp t.header.frame_id = msg.header.frame_id t.child_frame_id = msg.child_frame_id t.transform.translation.x = msg.transform.translation.x diff --git a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py index 999f386..0bc2956 100644 --- a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py @@ -14,6 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +from free_fleet.convert import transform_stamped_to_ros2_msg from free_fleet.types import ( ActionMsgs_CancelGoal_Response, GeometryMsgs_Point, @@ -34,10 +35,8 @@ make_cancel_all_goals_request, namespacify, ) -from geometry_msgs.msg import TransformStamped import numpy as np -import rclpy import rmf_adapter.easy_full_control as rmf_easy from tf_transformations import quaternion_from_euler import zenoh @@ -59,24 +58,11 @@ def _tf_callback(sample: zenoh.Sample): ) return None for zt in transform.transforms: - time = rclpy.time.Time( - seconds=zt.header.stamp.sec, - nanoseconds=zt.header.stamp.nanosec - ) - t = TransformStamped() - t.header.stamp = time.to_msg() - t.header.stamp + t = transform_stamped_to_ros2_msg(zt) t.header.frame_id = namespacify(zt.header.frame_id, - self.robot_name) + self.name) t.child_frame_id = namespacify(zt.child_frame_id, - self.robot_name) - t.transform.translation.x = zt.transform.translation.x - t.transform.translation.y = zt.transform.translation.y - t.transform.translation.z = zt.transform.translation.z - t.transform.rotation.x = zt.transform.rotation.x - t.transform.rotation.y = zt.transform.rotation.y - t.transform.rotation.z = zt.transform.rotation.z - t.transform.rotation.w = zt.transform.rotation.w + self.name) self.tf_buffer.set_transform( t, f'{self.robot_name}_TfListener') @@ -125,24 +111,11 @@ def _tf_callback(sample: zenoh.Sample): ) return None for zt in transform.transforms: - time = rclpy.time.Time( - seconds=zt.header.stamp.sec, - nanoseconds=zt.header.stamp.nanosec - ) - t = TransformStamped() - t.header.stamp = time.to_msg() - t.header.stamp + t = transform_stamped_to_ros2_msg(zt) t.header.frame_id = namespacify(zt.header.frame_id, self.name) t.child_frame_id = namespacify(zt.child_frame_id, self.name) - t.transform.translation.x = zt.transform.translation.x - t.transform.translation.y = zt.transform.translation.y - t.transform.translation.z = zt.transform.translation.z - t.transform.rotation.x = zt.transform.rotation.x - t.transform.rotation.y = zt.transform.rotation.y - t.transform.rotation.z = zt.transform.rotation.z - t.transform.rotation.w = zt.transform.rotation.w self.tf_buffer.set_transform(t, f'{self.name}_RobotAdapter') self.tf_sub = self.zenoh_session.declare_subscriber( diff --git a/free_fleet_examples/tests/integration/test_tf.py b/free_fleet_examples/tests/integration/test_tf.py index 28ddcf4..d9eb69b 100644 --- a/free_fleet_examples/tests/integration/test_tf.py +++ b/free_fleet_examples/tests/integration/test_tf.py @@ -16,9 +16,6 @@ import time -# from free_fleet.convert import transform_stamped_to_ros2_msg -# from free_fleet.types import TFMessage -# from free_fleet.utils import namespacify from free_fleet_adapter.nav2_robot_adapter import TfListener from rclpy.time import Time from tf2_ros import Buffer @@ -34,24 +31,12 @@ def test_tf(): listener = TfListener('turtlebot3_1', session, tf_buffer) listener - # def tf_callback(sample: zenoh.Sample): - # transform = TFMessage.deserialize(sample.payload.to_bytes()) - # for zt in transform.transforms: - # t = transform_stamped_to_ros2_msg(zt) - # tf_buffer.set_transform(t, 'free_fleet_examples_test_tf') - - # # Subscribe to TF - # pub = session.declare_subscriber( - # namespacify('tf', 'turtlebot3_1'), - # tf_callback - # ) - transform_exists = False for i in range(10): try: tf_buffer.lookup_transform( - 'base_footprint', - 'map', + 'turtlebot3_1/base_footprint', + 'turtlebot3_1/map', Time() ) transform_exists = True From 8f2a5079da09b114f2c0c9dafca87d28adb78301 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 11 Nov 2024 14:37:32 +0800 Subject: [PATCH 27/43] Refactor to TfHandler, added testing Signed-off-by: Aaron Chong --- README.md | 6 +- .../free_fleet_adapter/fleet_adapter.py | 28 ++++----- .../free_fleet_adapter/nav2_robot_adapter.py | 61 +++++++++++-------- free_fleet_examples/CMakeLists.txt | 6 +- ...ancel_all_goals.py => cancel_all_goals.py} | 0 .../{tests/test_tf.py => get_tf.py} | 41 +++---------- ...te_to_pose.py => send_navigate_to_pose.py} | 0 .../tests/integration/test_tf.py | 36 ++++++----- 8 files changed, 85 insertions(+), 93 deletions(-) rename free_fleet_examples/free_fleet_examples/{tests/test_cancel_all_goals.py => cancel_all_goals.py} (100%) rename free_fleet_examples/free_fleet_examples/{tests/test_tf.py => get_tf.py} (64%) rename free_fleet_examples/free_fleet_examples/{tests/test_navigate_to_pose.py => send_navigate_to_pose.py} (100%) diff --git a/README.md b/README.md index d6939c8..d3ce7f3 100644 --- a/README.md +++ b/README.md @@ -141,7 +141,7 @@ Listen to transforms over `zenoh`, ```bash source ~/ff_ws/install/setup.bash -ros2 run free_fleet_examples test_tf.py \ +ros2 run free_fleet_examples get_tf.py \ --namespace turtlebot3_1 ``` @@ -149,7 +149,7 @@ Start a `navigate_to_pose` action over `zenoh`, using example values, ```bash source ~/ff_ws/install/setup.bash -ros2 run free_fleet_examples test_navigate_to_pose.py \ +ros2 run free_fleet_examples send_navigate_to_pose.py \ --frame-id map \ --namespace turtlebot3_1 \ -x 1.808 \ @@ -294,7 +294,7 @@ ros2 run rmf_demos_tasks dispatch_patrol \ * hardware testing * attempt to optimize tf messages (not all are needed) -* robot adapter to be abstracted +* robot adapter and tf handler to be abstracted * ROS 1 nav support * custom actions to be abstracted * map switching support diff --git a/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py b/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py index 3ba602f..d96f066 100644 --- a/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py @@ -20,8 +20,6 @@ import threading import time -from free_fleet.utils import namespacify - import nudged import rclpy from rclpy.duration import Duration @@ -214,25 +212,21 @@ def run_in_parallel(*args, **kwargs): @parallel def update_robot(robot: Nav2RobotAdapter, tf_buffer: Buffer): - try: - # TODO(ac): parameterize the frames for lookup - transform = tf_buffer.lookup_transform( - namespacify('map', robot.name), - namespacify('base_footprint', robot.name), - rclpy.time.Time() - ) - orientation = euler_from_quaternion([ - transform.transform.rotation.x, - transform.transform.rotation.y, - transform.transform.rotation.z, - transform.transform.rotation.w - ]) - except Exception as err: + transform = robot.tf_handler.get_transform() + if transform is None: robot.node.get_logger().info( f'Failed to update robot [{robot.name}]: Unable to get transform ' - f'between base_footprint and map: {type(err)}: {err}' + f'between base_footprint and map' ) return None + + orientation = euler_from_quaternion([ + transform.transform.rotation.x, + transform.transform.rotation.y, + transform.transform.rotation.z, + transform.transform.rotation.w + ]) + robot_pose = [ transform.transform.translation.x, transform.transform.translation.y, diff --git a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py index 0bc2956..c85254b 100644 --- a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py @@ -36,33 +36,39 @@ namespacify, ) +from geometry_msgs.msg import TransformStamped import numpy as np +import rclpy import rmf_adapter.easy_full_control as rmf_easy from tf_transformations import quaternion_from_euler import zenoh -class TfListener: +class TfHandler: - def __init__(self, robot_name, zenoh_session, tf_buffer): + def __init__(self, robot_name, zenoh_session, tf_buffer, node=None): self.robot_name = robot_name self.zenoh_session = zenoh_session + self.node = node self.tf_buffer = tf_buffer def _tf_callback(sample: zenoh.Sample): try: transform = TFMessage.deserialize(sample.payload.to_bytes()) except Exception as e: - self.node.get_logger().debug( + error_message = \ f'Failed to deserialize TF payload: {type(e)}: {e}' - ) + if self.node is not None: + self.node.get_logger().debug(error_message) + else: + print(error_message) return None for zt in transform.transforms: t = transform_stamped_to_ros2_msg(zt) t.header.frame_id = namespacify(zt.header.frame_id, - self.name) + self.robot_name) t.child_frame_id = namespacify(zt.child_frame_id, - self.name) + self.robot_name) self.tf_buffer.set_transform( t, f'{self.robot_name}_TfListener') @@ -71,6 +77,25 @@ def _tf_callback(sample: zenoh.Sample): _tf_callback ) + def get_transform(self) -> TransformStamped | None: + try: + # TODO(ac): parameterize the frames for lookup + transform = self.tf_buffer.lookup_transform( + namespacify('map', self.robot_name), + namespacify('base_footprint', self.robot_name), + rclpy.time.Time() + ) + return transform + except Exception as err: + error_message = \ + 'Unable to get transform between base_footprint and map: ' \ + f'{type(err)}: {err}' + if self.node is not None: + self.node.get_logger().info(error_message) + else: + print(error_message) + return None + class Nav2RobotAdapter: @@ -102,25 +127,11 @@ def __init__( self.replan_counts = 0 - def _tf_callback(sample: zenoh.Sample): - try: - transform = TFMessage.deserialize(sample.payload.to_bytes()) - except Exception as e: - self.node.get_logger().debug( - f'Failed to deserialize TF payload: {type(e)}: {e}' - ) - return None - for zt in transform.transforms: - t = transform_stamped_to_ros2_msg(zt) - t.header.frame_id = namespacify(zt.header.frame_id, - self.name) - t.child_frame_id = namespacify(zt.child_frame_id, - self.name) - self.tf_buffer.set_transform(t, f'{self.name}_RobotAdapter') - - self.tf_sub = self.zenoh_session.declare_subscriber( - namespacify('tf', self.name), - _tf_callback + self.tf_handler = TfHandler( + self.name, + self.zenoh_session, + self.tf_buffer, + self.node ) def _battery_state_callback(sample: zenoh.Sample): diff --git a/free_fleet_examples/CMakeLists.txt b/free_fleet_examples/CMakeLists.txt index 3997135..44b75f6 100644 --- a/free_fleet_examples/CMakeLists.txt +++ b/free_fleet_examples/CMakeLists.txt @@ -47,9 +47,9 @@ foreach(path ${traffic_editor_paths}) endforeach() install(PROGRAMS - ${PROJECT_NAME}/tests/test_cancel_all_goals.py - ${PROJECT_NAME}/tests/test_navigate_to_pose.py - ${PROJECT_NAME}/tests/test_tf.py + ${PROJECT_NAME}/cancel_all_goals.py + ${PROJECT_NAME}/get_tf.py + ${PROJECT_NAME}/send_navigate_to_pose.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/free_fleet_examples/free_fleet_examples/tests/test_cancel_all_goals.py b/free_fleet_examples/free_fleet_examples/cancel_all_goals.py similarity index 100% rename from free_fleet_examples/free_fleet_examples/tests/test_cancel_all_goals.py rename to free_fleet_examples/free_fleet_examples/cancel_all_goals.py diff --git a/free_fleet_examples/free_fleet_examples/tests/test_tf.py b/free_fleet_examples/free_fleet_examples/get_tf.py similarity index 64% rename from free_fleet_examples/free_fleet_examples/tests/test_tf.py rename to free_fleet_examples/free_fleet_examples/get_tf.py index 696dafc..14f739b 100755 --- a/free_fleet_examples/free_fleet_examples/tests/test_tf.py +++ b/free_fleet_examples/free_fleet_examples/get_tf.py @@ -18,28 +18,15 @@ import sys import time -from free_fleet.convert import transform_stamped_to_ros2_msg -from free_fleet.types import TFMessage -from free_fleet.utils import namespacify -from rclpy.time import Time +from free_fleet_adapter.nav2_robot_adapter import TfHandler from tf2_ros import Buffer import zenoh -tf_buffer = Buffer() - - -def tf_callback(sample: zenoh.Sample): - transform = TFMessage.deserialize(sample.payload.to_bytes()) - for zt in transform.transforms: - t = transform_stamped_to_ros2_msg(zt) - tf_buffer.set_transform(t, 'free_fleet_examples_test_tf') - - def main(argv=sys.argv): parser = argparse.ArgumentParser( - prog='tf_listener', + prog='get_tf', description='Zenoh/ROS2 tf example') parser.add_argument('--zenoh-config', '-c', dest='config', metavar='FILE', type=str, help='A configuration file.') @@ -57,6 +44,8 @@ def main(argv=sys.argv): zenoh.try_init_log_from_env() + tf_buffer = Buffer() + # Open Zenoh Session with zenoh.open(conf) as session: info = session.info @@ -64,30 +53,20 @@ def main(argv=sys.argv): print(f'routers: {info.routers_zid()}') print(f'peers: {info.peers_zid()}') - # Subscribe to TF - pub = session.declare_subscriber( - namespacify('tf', args.namespace), - tf_callback - ) + tf_handler = TfHandler('turtlebot3_1', session, tf_buffer) try: while True: - try: - transform = tf_buffer.lookup_transform( - args.base_footprint_frame, - args.map_frame, - Time() - ) + transform = tf_handler.get_transform() + if transform is None: + print('Unable to get transform between base_footprint and' + ' map') + else: print(transform) - except Exception as err: - print(f'Unable to get transform between base_footprint and' - f' map: {type(err)}: {err}') - time.sleep(1) except (KeyboardInterrupt): pass finally: - pub.undeclare() session.close() diff --git a/free_fleet_examples/free_fleet_examples/tests/test_navigate_to_pose.py b/free_fleet_examples/free_fleet_examples/send_navigate_to_pose.py similarity index 100% rename from free_fleet_examples/free_fleet_examples/tests/test_navigate_to_pose.py rename to free_fleet_examples/free_fleet_examples/send_navigate_to_pose.py diff --git a/free_fleet_examples/tests/integration/test_tf.py b/free_fleet_examples/tests/integration/test_tf.py index d9eb69b..8bf93d3 100644 --- a/free_fleet_examples/tests/integration/test_tf.py +++ b/free_fleet_examples/tests/integration/test_tf.py @@ -16,35 +16,43 @@ import time -from free_fleet_adapter.nav2_robot_adapter import TfListener -from rclpy.time import Time +from free_fleet_adapter.nav2_robot_adapter import TfHandler from tf2_ros import Buffer import zenoh -def test_tf(): +def test_tf_does_not_exist(): zenoh.try_init_log_from_env() with zenoh.open(zenoh.Config()) as session: tf_buffer = Buffer() - listener = TfListener('turtlebot3_1', session, tf_buffer) - listener + tf_handler = TfHandler('missing_turtlebot3_1', session, tf_buffer) transform_exists = False for i in range(10): - try: - tf_buffer.lookup_transform( - 'turtlebot3_1/base_footprint', - 'turtlebot3_1/map', - Time() - ) + transform = tf_handler.get_transform() + if transform is not None: transform_exists = True break - except Exception as err: - print(f'Unable to get transform between base_footprint and ' - f'map: {type(err)}: {err}') + time.sleep(1) + + assert not transform_exists + + +def test_tf_exists(): + zenoh.try_init_log_from_env() + with zenoh.open(zenoh.Config()) as session: + tf_buffer = Buffer() + + tf_handler = TfHandler('turtlebot3_1', session, tf_buffer) + transform_exists = False + for i in range(10): + transform = tf_handler.get_transform() + if transform is not None: + transform_exists = True + break time.sleep(1) assert transform_exists From 2b66dacf5f86cf74ee0418187940f5a1b4bfafda Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 14 Nov 2024 11:52:56 +0800 Subject: [PATCH 28/43] Simplifying API Signed-off-by: Aaron Chong --- .../free_fleet_adapter/fleet_adapter.py | 4 +- .../free_fleet_adapter/robot_adapter.py | 80 +++++++++++++++++++ 2 files changed, 82 insertions(+), 2 deletions(-) create mode 100644 free_fleet_adapter/free_fleet_adapter/robot_adapter.py diff --git a/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py b/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py index d96f066..aa97a0f 100644 --- a/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py @@ -172,7 +172,7 @@ def update_loop(): # Update all the robots in parallel using a thread pool update_jobs = [] for robot in robots.values(): - update_jobs.append(update_robot(robot, tf_buffer)) + update_jobs.append(update_robot(robot)) asyncio.get_event_loop().run_until_complete( asyncio.wait(update_jobs) @@ -211,7 +211,7 @@ def run_in_parallel(*args, **kwargs): @parallel -def update_robot(robot: Nav2RobotAdapter, tf_buffer: Buffer): +def update_robot(robot: Nav2RobotAdapter): transform = robot.tf_handler.get_transform() if transform is None: robot.node.get_logger().info( diff --git a/free_fleet_adapter/free_fleet_adapter/robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/robot_adapter.py new file mode 100644 index 0000000..eb2a5dc --- /dev/null +++ b/free_fleet_adapter/free_fleet_adapter/robot_adapter.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from abc import ABC, abstractmethod +from typing import Annotated + +import rmf_adapter.easy_full_control as rmf_easy +from rmf_adapter.robot_update_handle import ActivityIdentifier + + +class RobotAdapter(ABC): + + ''' + This method returns the battery state of charge as a float, with value + between 0 and 1.0. + ''' + @abstractmethod + def battery_soc(self) -> float: + ... + + ''' + This method returns the last known 2D position in meters and orientation + (yaw) of the robot in radians as a list of 3 floats, in the form of + [x, y, yaw]. If the last known position of the robot is not available, + returns None. + ''' + @abstractmethod + def pose(self) -> Annotated[list[float], 3] | None: + ... + + ''' + This method is called to update RMF with the latest robot state. + ''' + @abstractmethod + def update(self, state: rmf_easy.RobotState): + ... + + ''' + This method is called to send a navigation command to the robot. + ''' + @abstractmethod + def navigate( + self, + destination: rmf_easy.Destination, + execution: rmf_easy.CommandExecution + ): + ... + + ''' + This method is called to stop the execution/continuation of the provided + activity. + ''' + @abstractmethod + def stop(self, activity): + ... + + ''' + This method is called to send a custom action command to the robot. + ''' + @abstractmethod + def execute_action( + self, + category: str, + description: dict, + execution: ActivityIdentifier + ): + ... From c29b497033930db8b07f8a44c5f8887e025316c9 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 14 Nov 2024 14:26:59 +0800 Subject: [PATCH 29/43] Moved integration testing to free fleet adapter, added abstract RobotAdapter Signed-off-by: Aaron Chong --- free_fleet_adapter/CMakeLists.txt | 45 +++++++++++++++++++ .../free_fleet_adapter/nav2_robot_adapter.py | 4 +- .../free_fleet_adapter/robot_adapter.py | 33 +++++++------- free_fleet_adapter/package.xml | 8 +++- free_fleet_adapter/setup.cfg | 4 -- free_fleet_adapter/setup.py | 37 --------------- .../tests/integration/__init__.py | 0 .../tests/integration/test_nav2_tf_handler.py | 6 +-- free_fleet_examples/CMakeLists.txt | 20 ++------- ..._all_goals.py => nav2_cancel_all_goals.py} | 0 .../{get_tf.py => nav2_get_tf.py} | 4 +- ..._pose.py => nav2_send_navigate_to_pose.py} | 0 12 files changed, 78 insertions(+), 83 deletions(-) create mode 100644 free_fleet_adapter/CMakeLists.txt delete mode 100644 free_fleet_adapter/setup.cfg delete mode 100644 free_fleet_adapter/setup.py rename {free_fleet_examples => free_fleet_adapter}/tests/integration/__init__.py (100%) rename free_fleet_examples/tests/integration/test_tf.py => free_fleet_adapter/tests/integration/test_nav2_tf_handler.py (87%) rename free_fleet_examples/free_fleet_examples/{cancel_all_goals.py => nav2_cancel_all_goals.py} (100%) rename free_fleet_examples/free_fleet_examples/{get_tf.py => nav2_get_tf.py} (94%) rename free_fleet_examples/free_fleet_examples/{send_navigate_to_pose.py => nav2_send_navigate_to_pose.py} (100%) diff --git a/free_fleet_adapter/CMakeLists.txt b/free_fleet_adapter/CMakeLists.txt new file mode 100644 index 0000000..2642131 --- /dev/null +++ b/free_fleet_adapter/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.5) + +project(free_fleet_adapter) + +find_package(ament_cmake REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +install(DIRECTORY + launch/ + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + set(_pytest_tests + tests/test_copyright.py + tests/test_flake8.py + tests/test_pep257.py + ) + foreach(_test_path ${_pytest_tests}) + get_filename_component(_test_name ${_test_path} NAME_WE) + ament_add_pytest_test(${_test_name} ${_test_path} + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + TIMEOUT 60 + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + endforeach() + + if (INTEGRATION_TESTING) + set(_pytest_tests + tests/integration/test_nav2_tf_handler.py + ) + foreach(_test_path ${_pytest_tests}) + get_filename_component(_test_name ${_test_path} NAME_WE) + ament_add_pytest_test(${_test_name} ${_test_path} + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + TIMEOUT 60 + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + endforeach() + endif() +endif() + +ament_package() diff --git a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py index c85254b..4d62ac2 100644 --- a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py @@ -44,7 +44,7 @@ import zenoh -class TfHandler: +class Nav2TfHandler: def __init__(self, robot_name, zenoh_session, tf_buffer, node=None): self.robot_name = robot_name @@ -127,7 +127,7 @@ def __init__( self.replan_counts = 0 - self.tf_handler = TfHandler( + self.tf_handler = Nav2TfHandler( self.name, self.zenoh_session, self.tf_buffer, diff --git a/free_fleet_adapter/free_fleet_adapter/robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/robot_adapter.py index eb2a5dc..ad1469f 100644 --- a/free_fleet_adapter/free_fleet_adapter/robot_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/robot_adapter.py @@ -22,35 +22,36 @@ class RobotAdapter(ABC): - - ''' + """Abstract Robot Adapter to be used by the free fleet adapter.""" + + """ This method returns the battery state of charge as a float, with value between 0 and 1.0. - ''' + """ @abstractmethod def battery_soc(self) -> float: ... - - ''' + + """ This method returns the last known 2D position in meters and orientation (yaw) of the robot in radians as a list of 3 floats, in the form of [x, y, yaw]. If the last known position of the robot is not available, - returns None. - ''' + returns None. + """ @abstractmethod def pose(self) -> Annotated[list[float], 3] | None: ... - - ''' + + """ This method is called to update RMF with the latest robot state. - ''' + """ @abstractmethod def update(self, state: rmf_easy.RobotState): ... - ''' + """ This method is called to send a navigation command to the robot. - ''' + """ @abstractmethod def navigate( self, @@ -59,17 +60,17 @@ def navigate( ): ... - ''' + """ This method is called to stop the execution/continuation of the provided activity. - ''' + """ @abstractmethod def stop(self, activity): ... - ''' + """ This method is called to send a custom action command to the robot. - ''' + """ @abstractmethod def execute_action( self, diff --git a/free_fleet_adapter/package.xml b/free_fleet_adapter/package.xml index 6b7ae1a..0d00c05 100644 --- a/free_fleet_adapter/package.xml +++ b/free_fleet_adapter/package.xml @@ -2,11 +2,13 @@ free_fleet_adapter - 2.4.0 + 0.1.0 Free fleet Open-RMF fleet adapter based on fleet_adapter_template Aaron Chong Apache License 2.0 + ament_cmake + launch_xml python3-numpy @@ -25,7 +27,9 @@ ament_flake8 ament_pep257 + ament_cmake_pytest + - ament_python + ament_cmake diff --git a/free_fleet_adapter/setup.cfg b/free_fleet_adapter/setup.cfg deleted file mode 100644 index 44aa8b8..0000000 --- a/free_fleet_adapter/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/free_fleet_adapter -[install] -install_scripts=$base/lib/free_fleet_adapter diff --git a/free_fleet_adapter/setup.py b/free_fleet_adapter/setup.py deleted file mode 100644 index 8ed825d..0000000 --- a/free_fleet_adapter/setup.py +++ /dev/null @@ -1,37 +0,0 @@ -from glob import glob -import os - -from setuptools import find_packages -from setuptools import setup - -package_name = 'free_fleet_adapter' - -setup( - name=package_name, - version='2.4.0', - packages=find_packages(), - data_files=[ - ( - 'share/ament_index/resource_index/packages', - ['resource/' + package_name], - ), - ('share/' + package_name, ['package.xml']), - ( - os.path.join('share', package_name, 'launch'), - glob('launch/*.launch.xml'), - ), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='Aaron Chong', - maintainer_email='aaronchong@intrinsic.ai', - description='Free Fleet adapter for interfacing with Open-RMF core ' - 'libraries', - license='Apache License 2.0', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'fleet_adapter=free_fleet_adapter.fleet_adapter:main', - ], - }, -) diff --git a/free_fleet_examples/tests/integration/__init__.py b/free_fleet_adapter/tests/integration/__init__.py similarity index 100% rename from free_fleet_examples/tests/integration/__init__.py rename to free_fleet_adapter/tests/integration/__init__.py diff --git a/free_fleet_examples/tests/integration/test_tf.py b/free_fleet_adapter/tests/integration/test_nav2_tf_handler.py similarity index 87% rename from free_fleet_examples/tests/integration/test_tf.py rename to free_fleet_adapter/tests/integration/test_nav2_tf_handler.py index 8bf93d3..2e3917f 100644 --- a/free_fleet_examples/tests/integration/test_tf.py +++ b/free_fleet_adapter/tests/integration/test_nav2_tf_handler.py @@ -16,7 +16,7 @@ import time -from free_fleet_adapter.nav2_robot_adapter import TfHandler +from free_fleet_adapter.nav2_robot_adapter import Nav2TfHandler from tf2_ros import Buffer import zenoh @@ -27,7 +27,7 @@ def test_tf_does_not_exist(): with zenoh.open(zenoh.Config()) as session: tf_buffer = Buffer() - tf_handler = TfHandler('missing_turtlebot3_1', session, tf_buffer) + tf_handler = Nav2TfHandler('missing_turtlebot3_1', session, tf_buffer) transform_exists = False for i in range(10): @@ -45,7 +45,7 @@ def test_tf_exists(): with zenoh.open(zenoh.Config()) as session: tf_buffer = Buffer() - tf_handler = TfHandler('turtlebot3_1', session, tf_buffer) + tf_handler = Nav2TfHandler('turtlebot3_1', session, tf_buffer) transform_exists = False for i in range(10): diff --git a/free_fleet_examples/CMakeLists.txt b/free_fleet_examples/CMakeLists.txt index 44b75f6..2a0ac95 100644 --- a/free_fleet_examples/CMakeLists.txt +++ b/free_fleet_examples/CMakeLists.txt @@ -47,9 +47,9 @@ foreach(path ${traffic_editor_paths}) endforeach() install(PROGRAMS - ${PROJECT_NAME}/cancel_all_goals.py - ${PROJECT_NAME}/get_tf.py - ${PROJECT_NAME}/send_navigate_to_pose.py + ${PROJECT_NAME}/nav2_cancel_all_goals.py + ${PROJECT_NAME}/nav2_get_tf.py + ${PROJECT_NAME}/nav2_send_navigate_to_pose.py DESTINATION lib/${PROJECT_NAME} ) @@ -83,20 +83,6 @@ if(BUILD_TESTING) WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} ) endforeach() - - if (INTEGRATION_TESTING) - set(_pytest_tests - tests/integration/test_tf.py - ) - foreach(_test_path ${_pytest_tests}) - get_filename_component(_test_name ${_test_path} NAME_WE) - ament_add_pytest_test(${_test_name} ${_test_path} - APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} - TIMEOUT 60 - WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} - ) - endforeach() - endif() endif() ament_package() diff --git a/free_fleet_examples/free_fleet_examples/cancel_all_goals.py b/free_fleet_examples/free_fleet_examples/nav2_cancel_all_goals.py similarity index 100% rename from free_fleet_examples/free_fleet_examples/cancel_all_goals.py rename to free_fleet_examples/free_fleet_examples/nav2_cancel_all_goals.py diff --git a/free_fleet_examples/free_fleet_examples/get_tf.py b/free_fleet_examples/free_fleet_examples/nav2_get_tf.py similarity index 94% rename from free_fleet_examples/free_fleet_examples/get_tf.py rename to free_fleet_examples/free_fleet_examples/nav2_get_tf.py index 14f739b..1480edf 100755 --- a/free_fleet_examples/free_fleet_examples/get_tf.py +++ b/free_fleet_examples/free_fleet_examples/nav2_get_tf.py @@ -18,7 +18,7 @@ import sys import time -from free_fleet_adapter.nav2_robot_adapter import TfHandler +from free_fleet_adapter.nav2_robot_adapter import Nav2TfHandler from tf2_ros import Buffer import zenoh @@ -53,7 +53,7 @@ def main(argv=sys.argv): print(f'routers: {info.routers_zid()}') print(f'peers: {info.peers_zid()}') - tf_handler = TfHandler('turtlebot3_1', session, tf_buffer) + tf_handler = Nav2TfHandler('turtlebot3_1', session, tf_buffer) try: while True: diff --git a/free_fleet_examples/free_fleet_examples/send_navigate_to_pose.py b/free_fleet_examples/free_fleet_examples/nav2_send_navigate_to_pose.py similarity index 100% rename from free_fleet_examples/free_fleet_examples/send_navigate_to_pose.py rename to free_fleet_examples/free_fleet_examples/nav2_send_navigate_to_pose.py From 1b4e8b9e7836dd7421e4ff730f13508074ca01d9 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 14 Nov 2024 15:30:54 +0800 Subject: [PATCH 30/43] Abstract out robot adapter, slight refactor Signed-off-by: Aaron Chong --- .../free_fleet_adapter/fleet_adapter.py | 33 ++++------ .../free_fleet_adapter/nav2_robot_adapter.py | 64 ++++++++++++++----- .../free_fleet_adapter/robot_adapter.py | 2 +- 3 files changed, 60 insertions(+), 39 deletions(-) diff --git a/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py b/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py index aa97a0f..c1131a3 100644 --- a/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py @@ -29,7 +29,6 @@ from rmf_adapter import Adapter, Transformation import rmf_adapter.easy_full_control as rmf_easy from tf2_ros import Buffer -from tf_transformations import euler_from_quaternion import yaml import zenoh @@ -212,27 +211,11 @@ def run_in_parallel(*args, **kwargs): @parallel def update_robot(robot: Nav2RobotAdapter): - transform = robot.tf_handler.get_transform() - if transform is None: - robot.node.get_logger().info( - f'Failed to update robot [{robot.name}]: Unable to get transform ' - f'between base_footprint and map' - ) + robot_pose = robot.pose() + if robot_pose is None: + robot.node.get_logger().info(f'Failed to pose of robot [{robot.name}]') return None - orientation = euler_from_quaternion([ - transform.transform.rotation.x, - transform.transform.rotation.y, - transform.transform.rotation.z, - transform.transform.rotation.w - ]) - - robot_pose = [ - transform.transform.translation.x, - transform.transform.translation.y, - orientation[2] - ] - state = rmf_easy.RobotState( robot.map, robot_pose, @@ -244,7 +227,15 @@ def update_robot(robot: Nav2RobotAdapter): robot.name, state, robot.configuration, - robot.make_callbacks() + rmf_easy.RobotCallbacks( + lambda destination, execution: robot.navigate( + destination, execution + ), + lambda activity: robot.stop(activity), + lambda category, description, execution: robot.execute_action( + category, description, execution + ) + ) ) return diff --git a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py index 4d62ac2..afa246d 100644 --- a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py @@ -14,6 +14,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +from typing import Annotated + from free_fleet.convert import transform_stamped_to_ros2_msg from free_fleet.types import ( ActionMsgs_CancelGoal_Response, @@ -35,12 +37,15 @@ make_cancel_all_goals_request, namespacify, ) +from free_fleet_adapter.robot_adapter import RobotAdapter from geometry_msgs.msg import TransformStamped import numpy as np import rclpy import rmf_adapter.easy_full_control as rmf_easy -from tf_transformations import quaternion_from_euler +from rmf_adapter.robot_update_handle import ActivityIdentifier +from tf_transformations import euler_from_quaternion, quaternion_from_euler + import zenoh @@ -97,7 +102,7 @@ def get_transform(self) -> TransformStamped | None: return None -class Nav2RobotAdapter: +class Nav2RobotAdapter(RobotAdapter): def __init__( self, @@ -109,6 +114,8 @@ def __init__( fleet_handle, tf_buffer ): + RobotAdapter.__init__(self) + self.name = name self.execution = None self.update_handle = None @@ -145,6 +152,31 @@ def _battery_state_callback(sample: zenoh.Sample): _battery_state_callback ) + def battery_soc(self) -> float: + return self.battery_soc + + def pose(self) -> Annotated[list[float], 3] | None: + transform = self.tf_handler.get_transform() + if transform is None: + self.node.get_logger().info( + f'Failed to update robot [{self.name}]: Unable to get ' + f'transform between base_footprint and map' + ) + return None + + orientation = euler_from_quaternion([ + transform.transform.rotation.x, + transform.transform.rotation.y, + transform.transform.rotation.z, + transform.transform.rotation.w + ]) + robot_pose = [ + transform.transform.translation.x, + transform.transform.translation.y, + orientation[2] + ] + return robot_pose + def _make_random_goal_id(self): return np.random.randint(0, 255, size=(16)).astype('uint8').tolist() @@ -188,7 +220,7 @@ def _is_navigation_done(self) -> bool: f'{type(e)}: {e}') continue - def update(self, state): + def update(self, state: rmf_easy.RobotState): activity_identifier = None if self.execution: # TODO(ac): use an enum to record what type of execution it is, @@ -203,18 +235,11 @@ def update(self, state): self.update_handle.update(state, activity_identifier) - def make_callbacks(self): - return rmf_easy.RobotCallbacks( - lambda destination, execution: self.navigate( - destination, execution - ), - lambda activity: self.stop(activity), - lambda category, description, execution: self.execute_action( - category, description, execution - ) - ) - - def navigate(self, destination, execution): + def navigate( + self, + destination: rmf_easy.Destination, + execution: rmf_easy.CommandExecution + ): self.execution = execution self.node.get_logger().info( f'Commanding [{self.name}] to navigate to {destination.position} ' @@ -289,7 +314,7 @@ def navigate(self, destination, execution): ) continue - def stop(self, activity): + def stop(self, activity: ActivityIdentifier): if self.execution is None: return @@ -317,7 +342,12 @@ def stop(self, activity): ) self.nav_goal_id = None - def execute_action(self, category: str, description: dict, execution): + def execute_action( + self, + category: str, + description: dict, + execution: ActivityIdentifier + ): # TODO(ac): change map using map_server load_map, and set initial # position again with /initialpose # TODO(ac): docking diff --git a/free_fleet_adapter/free_fleet_adapter/robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/robot_adapter.py index ad1469f..86b1060 100644 --- a/free_fleet_adapter/free_fleet_adapter/robot_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/robot_adapter.py @@ -65,7 +65,7 @@ def navigate( activity. """ @abstractmethod - def stop(self, activity): + def stop(self, activity: ActivityIdentifier): ... """ From c79371ab3c82726130f9c7e726df482b409f1260 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 14 Nov 2024 15:38:54 +0800 Subject: [PATCH 31/43] Robot existense test, with a planned failure to verify that it is running Signed-off-by: Aaron Chong --- free_fleet_adapter/CMakeLists.txt | 1 + .../integration/test_nav2_robot_adapter.py | 82 +++++++++++++++++++ 2 files changed, 83 insertions(+) create mode 100644 free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py diff --git a/free_fleet_adapter/CMakeLists.txt b/free_fleet_adapter/CMakeLists.txt index 2642131..b32028d 100644 --- a/free_fleet_adapter/CMakeLists.txt +++ b/free_fleet_adapter/CMakeLists.txt @@ -30,6 +30,7 @@ if(BUILD_TESTING) if (INTEGRATION_TESTING) set(_pytest_tests tests/integration/test_nav2_tf_handler.py + tests/integration/test_nav2_robot_adapter.py ) foreach(_test_path ${_pytest_tests}) get_filename_component(_test_name ${_test_path} NAME_WE) diff --git a/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py b/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py new file mode 100644 index 0000000..890be89 --- /dev/null +++ b/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import time + +from free_fleet_adapter.nav2_robot_adapter import Nav2RobotAdapter +from tf2_ros import Buffer + +import zenoh + + +def test_robot_does_not_exist(): + zenoh.try_init_log_from_env() + with zenoh.open(zenoh.Config()) as session: + tf_buffer = Buffer() + + robot_adapter = Nav2RobotAdapter( + name='missing_turtlebot3_1', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=None, + zenoh_session=session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + robot_exists = False + for i in range(10): + transform = robot_adapter.pose() + if transform is not None: + robot_exists = True + break + time.sleep(1) + + assert not robot_exists + + +def test_robot_exists(): + zenoh.try_init_log_from_env() + with zenoh.open(zenoh.Config()) as session: + tf_buffer = Buffer() + + robot_adapter = Nav2RobotAdapter( + name='turtlebot3_1', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=None, + zenoh_session=session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + robot_exists = False + for i in range(10): + transform = robot_adapter.pose() + if transform is not None: + robot_exists = True + break + time.sleep(1) + + # To check that it is running + assert not robot_exists + + +# def test_robot_navigate(): From f3f550dccddb7911fac9af3348f818cd211a387e Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 14 Nov 2024 16:20:51 +0800 Subject: [PATCH 32/43] setup rclpy node for testing too Signed-off-by: Aaron Chong --- .../free_fleet_adapter/nav2_robot_adapter.py | 24 +++++++------------ .../integration/test_nav2_robot_adapter.py | 10 ++++++-- .../tests/integration/test_nav2_tf_handler.py | 12 ++++++++-- 3 files changed, 27 insertions(+), 19 deletions(-) diff --git a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py index afa246d..c251263 100644 --- a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py @@ -51,7 +51,7 @@ class Nav2TfHandler: - def __init__(self, robot_name, zenoh_session, tf_buffer, node=None): + def __init__(self, robot_name, zenoh_session, tf_buffer, node): self.robot_name = robot_name self.zenoh_session = zenoh_session self.node = node @@ -61,12 +61,9 @@ def _tf_callback(sample: zenoh.Sample): try: transform = TFMessage.deserialize(sample.payload.to_bytes()) except Exception as e: - error_message = \ + self.node.get_logger().debug( f'Failed to deserialize TF payload: {type(e)}: {e}' - if self.node is not None: - self.node.get_logger().debug(error_message) - else: - print(error_message) + ) return None for zt in transform.transforms: t = transform_stamped_to_ros2_msg(zt) @@ -92,13 +89,10 @@ def get_transform(self) -> TransformStamped | None: ) return transform except Exception as err: - error_message = \ - 'Unable to get transform between base_footprint and map: ' \ + self.node.get_logger().info( + 'Unable to get transform between base_footprint and map: ' f'{type(err)}: {err}' - if self.node is not None: - self.node.get_logger().info(error_message) - else: - print(error_message) + ) return None @@ -158,10 +152,10 @@ def battery_soc(self) -> float: def pose(self) -> Annotated[list[float], 3] | None: transform = self.tf_handler.get_transform() if transform is None: - self.node.get_logger().info( - f'Failed to update robot [{self.name}]: Unable to get ' + error_message = \ + f'Failed to update robot [{self.name}]: Unable to get ' \ f'transform between base_footprint and map' - ) + self.node.get_logger().info(error_message) return None orientation = euler_from_quaternion([ diff --git a/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py b/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py index 890be89..c6c1a7d 100644 --- a/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py +++ b/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py @@ -18,11 +18,15 @@ from free_fleet_adapter.nav2_robot_adapter import Nav2RobotAdapter from tf2_ros import Buffer +import rclpy +from rclpy.node import Node import zenoh def test_robot_does_not_exist(): + rclpy.init() + node = Node('missing_turtlebot3_1_nav2_robot_adapter_node') zenoh.try_init_log_from_env() with zenoh.open(zenoh.Config()) as session: tf_buffer = Buffer() @@ -33,7 +37,7 @@ def test_robot_does_not_exist(): robot_config_yaml={ 'initial_map': 'L1', }, - node=None, + node=node, zenoh_session=session, fleet_handle=None, tf_buffer=tf_buffer @@ -51,6 +55,8 @@ def test_robot_does_not_exist(): def test_robot_exists(): + rclpy.init() + node = Node('turtlebot3_1_nav2_robot_adapter_node') zenoh.try_init_log_from_env() with zenoh.open(zenoh.Config()) as session: tf_buffer = Buffer() @@ -61,7 +67,7 @@ def test_robot_exists(): robot_config_yaml={ 'initial_map': 'L1', }, - node=None, + node=node, zenoh_session=session, fleet_handle=None, tf_buffer=tf_buffer diff --git a/free_fleet_adapter/tests/integration/test_nav2_tf_handler.py b/free_fleet_adapter/tests/integration/test_nav2_tf_handler.py index 2e3917f..ef16adf 100644 --- a/free_fleet_adapter/tests/integration/test_nav2_tf_handler.py +++ b/free_fleet_adapter/tests/integration/test_nav2_tf_handler.py @@ -18,16 +18,22 @@ from free_fleet_adapter.nav2_robot_adapter import Nav2TfHandler from tf2_ros import Buffer +import rclpy +from rclpy.node import Node import zenoh def test_tf_does_not_exist(): + rclpy.init() + node = Node('missing_turtlebot3_1_node') zenoh.try_init_log_from_env() with zenoh.open(zenoh.Config()) as session: tf_buffer = Buffer() - tf_handler = Nav2TfHandler('missing_turtlebot3_1', session, tf_buffer) + tf_handler = Nav2TfHandler( + 'missing_turtlebot3_1', session, tf_buffer, node + ) transform_exists = False for i in range(10): @@ -41,11 +47,13 @@ def test_tf_does_not_exist(): def test_tf_exists(): + rclpy.init() + node = Node('turtlebot3_1_node') zenoh.try_init_log_from_env() with zenoh.open(zenoh.Config()) as session: tf_buffer = Buffer() - tf_handler = Nav2TfHandler('turtlebot3_1', session, tf_buffer) + tf_handler = Nav2TfHandler('turtlebot3_1', session, tf_buffer, node) transform_exists = False for i in range(10): From 4661bb21e3fb97b2afd0bd149e43aa7c9a54e228 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 14 Nov 2024 16:48:41 +0800 Subject: [PATCH 33/43] Add coverage and fix lint Signed-off-by: Aaron Chong --- .github/actions/build-and-test/action.yaml | 7 ++++++- .github/workflows/integration-tests.yaml | 16 ++++++++++++++++ .github/workflows/unit-tests.yaml | 16 ++++++++++++++++ codecov.yaml | 16 ++++++++++++++++ .../tests/integration/test_nav2_robot_adapter.py | 2 +- .../tests/integration/test_nav2_tf_handler.py | 2 +- 6 files changed, 56 insertions(+), 3 deletions(-) create mode 100644 codecov.yaml diff --git a/.github/actions/build-and-test/action.yaml b/.github/actions/build-and-test/action.yaml index a20f44b..a2e8e1c 100644 --- a/.github/actions/build-and-test/action.yaml +++ b/.github/actions/build-and-test/action.yaml @@ -35,6 +35,11 @@ runs: "build": { "cmake-args": [ "-DINTEGRATION_TESTING=${{ inputs.integration-testing }}" - ] + ], + "mixin": ["coverage-pytest"] + }, + "test": { + "mixin": ["coverage-pytest"] } } + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml diff --git a/.github/workflows/integration-tests.yaml b/.github/workflows/integration-tests.yaml index ca1af35..6d978b5 100644 --- a/.github/workflows/integration-tests.yaml +++ b/.github/workflows/integration-tests.yaml @@ -34,3 +34,19 @@ jobs: - name: Stop test fixture containers if: always() run: docker-compose -f ".github/docker/integration-tests/docker-compose.yaml" down + + - name: Upload failed test results + uses: actions/upload-artifact@v4 + if: failure() + with: + name: test-results + path: ros_ws/build/*/test_results/*/*.catch2.xml + + - name: Upload coverage to Codecov + uses: codecov/codecov-action@v4 + with: + files: ros_ws/coveragepy/.coverage + flags: tests + fail_ci_if_error: true + env: + CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }} diff --git a/.github/workflows/unit-tests.yaml b/.github/workflows/unit-tests.yaml index 0234e56..526a43f 100644 --- a/.github/workflows/unit-tests.yaml +++ b/.github/workflows/unit-tests.yaml @@ -22,3 +22,19 @@ jobs: ros-distribution: ${{ matrix.ros_distribution }} zenoh-version: 1.0.1 integration-testing: OFF + + - name: Upload failed test results + uses: actions/upload-artifact@v4 + if: failure() + with: + name: test-results + path: ros_ws/build/*/test_results/*/*.catch2.xml + + - name: Upload coverage to Codecov + uses: codecov/codecov-action@v4 + with: + files: ros_ws/coveragepy/.coverage + flags: tests + fail_ci_if_error: true + env: + CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }} diff --git a/codecov.yaml b/codecov.yaml new file mode 100644 index 0000000..3b1bb71 --- /dev/null +++ b/codecov.yaml @@ -0,0 +1,16 @@ +# coverage: +# precision: 2 +# round: down +# range: "35...100" +# status: +# project: +# default: +# informational: true +# patch: off +fixes: + - "ros_ws/src/*/free_fleet/free_fleet/::" + - "ros_ws/src/*/free_fleet/free_fleet_adapter/::" + - "ros_ws/src/*/free_fleet/free_fleet_examples/::" +# comment: +# layout: "diff, flags, files" +# behavior: default diff --git a/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py b/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py index c6c1a7d..f8064b8 100644 --- a/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py +++ b/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py @@ -17,9 +17,9 @@ import time from free_fleet_adapter.nav2_robot_adapter import Nav2RobotAdapter -from tf2_ros import Buffer import rclpy from rclpy.node import Node +from tf2_ros import Buffer import zenoh diff --git a/free_fleet_adapter/tests/integration/test_nav2_tf_handler.py b/free_fleet_adapter/tests/integration/test_nav2_tf_handler.py index ef16adf..05b710e 100644 --- a/free_fleet_adapter/tests/integration/test_nav2_tf_handler.py +++ b/free_fleet_adapter/tests/integration/test_nav2_tf_handler.py @@ -17,9 +17,9 @@ import time from free_fleet_adapter.nav2_robot_adapter import Nav2TfHandler -from tf2_ros import Buffer import rclpy from rclpy.node import Node +from tf2_ros import Buffer import zenoh From df345937b6acd581363c04215934032a1aae7645 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Fri, 15 Nov 2024 11:23:52 +0800 Subject: [PATCH 34/43] Use unit test test cases Signed-off-by: Aaron Chong --- .github/workflows/integration-tests.yaml | 7 --- .github/workflows/unit-tests.yaml | 16 ------- .../integration/test_nav2_robot_adapter.py | 43 ++++++++++--------- .../tests/integration/test_nav2_tf_handler.py | 35 +++++++++------ 4 files changed, 44 insertions(+), 57 deletions(-) diff --git a/.github/workflows/integration-tests.yaml b/.github/workflows/integration-tests.yaml index 6d978b5..040b580 100644 --- a/.github/workflows/integration-tests.yaml +++ b/.github/workflows/integration-tests.yaml @@ -35,13 +35,6 @@ jobs: if: always() run: docker-compose -f ".github/docker/integration-tests/docker-compose.yaml" down - - name: Upload failed test results - uses: actions/upload-artifact@v4 - if: failure() - with: - name: test-results - path: ros_ws/build/*/test_results/*/*.catch2.xml - - name: Upload coverage to Codecov uses: codecov/codecov-action@v4 with: diff --git a/.github/workflows/unit-tests.yaml b/.github/workflows/unit-tests.yaml index 526a43f..0234e56 100644 --- a/.github/workflows/unit-tests.yaml +++ b/.github/workflows/unit-tests.yaml @@ -22,19 +22,3 @@ jobs: ros-distribution: ${{ matrix.ros_distribution }} zenoh-version: 1.0.1 integration-testing: OFF - - - name: Upload failed test results - uses: actions/upload-artifact@v4 - if: failure() - with: - name: test-results - path: ros_ws/build/*/test_results/*/*.catch2.xml - - - name: Upload coverage to Codecov - uses: codecov/codecov-action@v4 - with: - files: ros_ws/coveragepy/.coverage - flags: tests - fail_ci_if_error: true - env: - CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }} diff --git a/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py b/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py index f8064b8..c08ae67 100644 --- a/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py +++ b/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py @@ -15,20 +15,30 @@ # limitations under the License. import time +import unittest from free_fleet_adapter.nav2_robot_adapter import Nav2RobotAdapter import rclpy -from rclpy.node import Node from tf2_ros import Buffer import zenoh -def test_robot_does_not_exist(): - rclpy.init() - node = Node('missing_turtlebot3_1_nav2_robot_adapter_node') - zenoh.try_init_log_from_env() - with zenoh.open(zenoh.Config()) as session: +class TestNav2RobotAdapter(unittest.TestCase): + + @classmethod + def setUpClass(cls): + rclpy.init() + cls.node = rclpy.create_node('test_nav2_robot_adapter') + cls.zenoh_session = zenoh.open(zenoh.Config()) + + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + cls.zenoh_session.close() + rclpy.shutdown() + + def test_robot_does_not_exist(self): tf_buffer = Buffer() robot_adapter = Nav2RobotAdapter( @@ -37,8 +47,8 @@ def test_robot_does_not_exist(): robot_config_yaml={ 'initial_map': 'L1', }, - node=node, - zenoh_session=session, + node=self.node, + zenoh_session=self.zenoh_session, fleet_handle=None, tf_buffer=tf_buffer ) @@ -53,12 +63,7 @@ def test_robot_does_not_exist(): assert not robot_exists - -def test_robot_exists(): - rclpy.init() - node = Node('turtlebot3_1_nav2_robot_adapter_node') - zenoh.try_init_log_from_env() - with zenoh.open(zenoh.Config()) as session: + def test_robot_exists(self): tf_buffer = Buffer() robot_adapter = Nav2RobotAdapter( @@ -67,8 +72,8 @@ def test_robot_exists(): robot_config_yaml={ 'initial_map': 'L1', }, - node=node, - zenoh_session=session, + node=self.node, + zenoh_session=self.zenoh_session, fleet_handle=None, tf_buffer=tf_buffer ) @@ -81,8 +86,6 @@ def test_robot_exists(): break time.sleep(1) - # To check that it is running - assert not robot_exists - + assert robot_exists -# def test_robot_navigate(): + # def test_robot_navigate(): diff --git a/free_fleet_adapter/tests/integration/test_nav2_tf_handler.py b/free_fleet_adapter/tests/integration/test_nav2_tf_handler.py index 05b710e..71df251 100644 --- a/free_fleet_adapter/tests/integration/test_nav2_tf_handler.py +++ b/free_fleet_adapter/tests/integration/test_nav2_tf_handler.py @@ -15,24 +15,34 @@ # limitations under the License. import time +import unittest from free_fleet_adapter.nav2_robot_adapter import Nav2TfHandler import rclpy -from rclpy.node import Node from tf2_ros import Buffer import zenoh -def test_tf_does_not_exist(): - rclpy.init() - node = Node('missing_turtlebot3_1_node') - zenoh.try_init_log_from_env() - with zenoh.open(zenoh.Config()) as session: +class TestNav2TfHandler(unittest.TestCase): + + @classmethod + def setUpClass(cls): + rclpy.init() + cls.node = rclpy.create_node('test_nav2_tf_handler') + cls.zenoh_session = zenoh.open(zenoh.Config()) + + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + cls.zenoh_session.close() + rclpy.shutdown() + + def test_tf_does_not_exist(self): tf_buffer = Buffer() tf_handler = Nav2TfHandler( - 'missing_turtlebot3_1', session, tf_buffer, node + 'missing_turtlebot3_1', self.zenoh_session, tf_buffer, self.node ) transform_exists = False @@ -45,15 +55,12 @@ def test_tf_does_not_exist(): assert not transform_exists - -def test_tf_exists(): - rclpy.init() - node = Node('turtlebot3_1_node') - zenoh.try_init_log_from_env() - with zenoh.open(zenoh.Config()) as session: + def test_tf_exists(self): tf_buffer = Buffer() - tf_handler = Nav2TfHandler('turtlebot3_1', session, tf_buffer, node) + tf_handler = Nav2TfHandler( + 'turtlebot3_1', self.zenoh_session, tf_buffer, self.node + ) transform_exists = False for i in range(10): From 492b274e522cc46ebaf88d360594eb8c8bd7de6f Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Fri, 15 Nov 2024 15:27:21 +0800 Subject: [PATCH 35/43] Move tests Signed-off-by: Aaron Chong --- README.md | 8 +- codecov.yaml | 12 - .../free_fleet_adapter/nav2_robot_adapter.py | 69 +++-- .../integration/test_nav2_robot_adapter.py | 243 +++++++++++++++++- .../nav2_send_navigate_to_pose.py | 4 +- 5 files changed, 291 insertions(+), 45 deletions(-) diff --git a/README.md b/README.md index d3ce7f3..6498348 100644 --- a/README.md +++ b/README.md @@ -124,7 +124,7 @@ zenohd # If using released standalaone binaries # cd PATH_TO_EXTRACTED_ZENOH_ROUTER -# ./zenohd +# ./zenohd ``` Start `zenoh-bridge-ros2dds` with the appropriate zenoh client configuration, @@ -141,7 +141,7 @@ Listen to transforms over `zenoh`, ```bash source ~/ff_ws/install/setup.bash -ros2 run free_fleet_examples get_tf.py \ +ros2 run free_fleet_examples nav2_get_tf.py \ --namespace turtlebot3_1 ``` @@ -149,7 +149,7 @@ Start a `navigate_to_pose` action over `zenoh`, using example values, ```bash source ~/ff_ws/install/setup.bash -ros2 run free_fleet_examples send_navigate_to_pose.py \ +ros2 run free_fleet_examples nav2_send_navigate_to_pose.py \ --frame-id map \ --namespace turtlebot3_1 \ -x 1.808 \ @@ -214,7 +214,7 @@ zenohd # If using released standalaone binaries # cd PATH_TO_EXTRACTED_ZENOH_ROUTER -# ./zenohd +# ./zenohd ``` Start `zenoh-bridge-ros2dds` with the appropriate zenoh client configuration, diff --git a/codecov.yaml b/codecov.yaml index 3b1bb71..ddc5d27 100644 --- a/codecov.yaml +++ b/codecov.yaml @@ -1,16 +1,4 @@ -# coverage: -# precision: 2 -# round: down -# range: "35...100" -# status: -# project: -# default: -# informational: true -# patch: off fixes: - "ros_ws/src/*/free_fleet/free_fleet/::" - "ros_ws/src/*/free_fleet/free_fleet_adapter/::" - "ros_ws/src/*/free_fleet/free_fleet_examples/::" -# comment: -# layout: "diff, flags, files" -# behavior: default diff --git a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py index c251263..d4902da 100644 --- a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py @@ -171,11 +171,8 @@ def pose(self) -> Annotated[list[float], 3] | None: ] return robot_pose - def _make_random_goal_id(self): - return np.random.randint(0, 255, size=(16)).astype('uint8').tolist() - def _is_navigation_done(self) -> bool: - if self.nav_goal_id is None: + if self.execution is None or self.nav_goal_id is None: return True req = NavigateToPose_GetResult_Request(goal_id=self.nav_goal_id) @@ -214,7 +211,17 @@ def _is_navigation_done(self) -> bool: f'{type(e)}: {e}') continue + def _check_update_handle_initialization(self): + if self.update_handle is None: + error_message = \ + f'Failed to update robot {self.name}, robot adapter has not ' \ + 'yet been initialized with a fleet update handle.' + self.node.get_logger().error(error_message) + raise RuntimeError(error_message) + def update(self, state: rmf_easy.RobotState): + self._check_update_handle_initialization() + activity_identifier = None if self.execution: # TODO(ac): use an enum to record what type of execution it is, @@ -229,37 +236,31 @@ def update(self, state: rmf_easy.RobotState): self.update_handle.update(state, activity_identifier) - def navigate( + def _handle_navigate_to_pose( self, - destination: rmf_easy.Destination, - execution: rmf_easy.CommandExecution + map_name: str, + x: float, + y: float, + z: float, + yaw: float ): - self.execution = execution - self.node.get_logger().info( - f'Commanding [{self.name}] to navigate to {destination.position} ' - f'on map [{destination.map}]' - ) - - if destination.map != self.map: + if map_name != self.map: # TODO(ac): test this map related replanning behavior self.replan_counts += 1 self.node.get_logger().error( - f'Destination is on map [{destination.map}], while robot ' - f'[{self.name}] is on map [{self.map}], replan count ' - f'[{self.replan_counts}]' + f'Destination is on map [{map_name}], while robot [{self.name}] is ' + f'on map [{self.map}], replan count [{self.replan_counts}]' ) + + self._check_update_handle_initialization() self.update_handle.more().replan() return time_now = self.node.get_clock().now().seconds_nanoseconds() stamp = Time(sec=time_now[0], nanosec=time_now[1]) header = Header(stamp=stamp, frame_id='map') - position = GeometryMsgs_Point( - x=destination.position[0], - y=destination.position[1], - z=0 - ) - quat = quaternion_from_euler(0, 0, destination.position[2]) + position = GeometryMsgs_Point(x=x, y=y, z=z) + quat = quaternion_from_euler(0, 0, yaw) orientation = GeometryMsgs_Quaternion( x=quat[0], y=quat[1], @@ -269,7 +270,8 @@ def navigate( pose = GeometryMsgs_Pose(position=position, orientation=orientation) pose_stamped = GeometryMsgs_PoseStamped(header=header, pose=pose) - nav_goal_id = self._make_random_goal_id() + nav_goal_id = \ + np.random.randint(0, 255, size=(16)).astype('uint8').tolist() req = NavigateToPose_SendGoal_Request( goal_id=nav_goal_id, pose=pose_stamped, @@ -298,6 +300,7 @@ def navigate( f'Navigation goal {nav_goal_id} was rejected, replan ' f'count [{self.replan_counts}]' ) + self._check_update_handle_initialization() self.update_handle.more().replan() self.nav_goal_id = None return @@ -308,6 +311,24 @@ def navigate( ) continue + def navigate( + self, + destination: rmf_easy.Destination, + execution: rmf_easy.CommandExecution + ): + self.execution = execution + self.node.get_logger().info( + f'Commanding [{self.name}] to navigate to {destination.position} ' + f'on map [{destination.map}]' + ) + self._handle_navigate_to_pose( + destination.map, + destination.position[0], + destination.position[1], + 0.0, + destination.position[2] + ) + def stop(self, activity: ActivityIdentifier): if self.execution is None: return diff --git a/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py b/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py index c08ae67..1f5550e 100644 --- a/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py +++ b/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py @@ -14,11 +14,13 @@ # See the License for the specific language governing permissions and # limitations under the License. +import math import time import unittest from free_fleet_adapter.nav2_robot_adapter import Nav2RobotAdapter import rclpy +import rmf_adapter.easy_full_control as rmf_easy from tf2_ros import Buffer import zenoh @@ -38,7 +40,7 @@ def tearDownClass(cls): cls.zenoh_session.close() rclpy.shutdown() - def test_robot_does_not_exist(self): + def test_non_existent_robot_pose(self): tf_buffer = Buffer() robot_adapter = Nav2RobotAdapter( @@ -63,7 +65,7 @@ def test_robot_does_not_exist(self): assert not robot_exists - def test_robot_exists(self): + def test_robot_pose(self): tf_buffer = Buffer() robot_adapter = Nav2RobotAdapter( @@ -88,4 +90,239 @@ def test_robot_exists(self): assert robot_exists - # def test_robot_navigate(): + def test_robot_battery_soc(self): + tf_buffer = Buffer() + + robot_adapter = Nav2RobotAdapter( + name='turtlebot3_1', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + battery_soc = robot_adapter.battery_soc() + assert math.isclose(battery_soc, 1.0) + + def test_robot_unable_to_update(self): + tf_buffer = Buffer() + + robot_adapter = Nav2RobotAdapter( + name='turtlebot3_1', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + able_to_update = False + try: + robot_adapter.update( + rmf_easy.RobotState( + 'L1', + [0.0, 0.0, 0.0], + 1.0 + ) + ) + able_to_update = True + except RuntimeError: + able_to_update = False + assert not able_to_update + + def test_idle_robot_navigate_is_done(self): + tf_buffer = Buffer() + + robot_adapter = Nav2RobotAdapter( + name='turtlebot3_1', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + assert robot_adapter._is_navigation_done() + + def test_robot_stop_without_command(self): + tf_buffer = Buffer() + + robot_adapter = Nav2RobotAdapter( + name='turtlebot3_1', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + assert robot_adapter.execution is None + robot_adapter.stop() + assert robot_adapter.execution is None + assert robot_adapter._is_navigation_done() + + def test_robot_handle_navigate_to_invalid_map(self): + tf_buffer = Buffer() + + robot_adapter = Nav2RobotAdapter( + name='turtlebot3_1', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + able_to_handle_navigate = False + try: + robot_adapter._handle_navigate_to_pose( + 'invalid_map', + 0.0, + 1.0, + 2.0, + 0.0 + ) + able_to_handle_navigate = True + except RuntimeError: + able_to_handle_navigate = False + assert not able_to_handle_navigate + + def test_robot_handle_navigate_to_invalid_pose(self): + tf_buffer = Buffer() + + robot_adapter = Nav2RobotAdapter( + name='turtlebot3_1', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + able_to_handle_navigate = False + try: + robot_adapter._handle_navigate_to_pose( + 'L1', + 1000000, + 1000000, + 0.0, + 0.0 + ) + able_to_handle_navigate = True + except RuntimeError: + able_to_handle_navigate = False + assert not able_to_handle_navigate + + def test_robot_handle_navigate_to_pose(self): + tf_buffer = Buffer() + + robot_adapter = Nav2RobotAdapter( + name='turtlebot3_1', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + able_to_handle_navigate = False + try: + robot_adapter._handle_navigate_to_pose( + 'L1', + -0.395, + -0.606, + 0.0, + 0.0 + ) + able_to_handle_navigate = True + except RuntimeError: + able_to_handle_navigate = False + assert able_to_handle_navigate + + time.sleep(1) + assert not robot_adapter._is_navigation_done() + time.sleep(10) + assert robot_adapter._is_navigation_done() + + def test_robot_stop_navigate(self): + tf_buffer = Buffer() + + robot_adapter = Nav2RobotAdapter( + name='turtlebot3_1', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + able_to_handle_navigate = False + try: + robot_adapter._handle_navigate_to_pose( + 'L1', + 1.808, + 0.503, + 0.0, + 0.0 + ) + able_to_handle_navigate = True + except RuntimeError: + able_to_handle_navigate = False + assert able_to_handle_navigate + + time.sleep(1) + assert not robot_adapter._is_navigation_done() + time.sleep(1) + robot_adapter.stop(robot_adapter.execution.identifier) + time.sleep(1) + assert robot_adapter._is_navigation_done() + + def test_robot_execute_unknown_action(self): + tf_buffer = Buffer() + + robot_adapter = Nav2RobotAdapter( + name='turtlebot3_1', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + able_to_execute_action = False + try: + robot_adapter.execute_action( + 'unknown_category', + {}, + None + ) + able_to_execute_action = True + except RuntimeError: + able_to_execute_action = False + assert not able_to_execute_action diff --git a/free_fleet_examples/free_fleet_examples/nav2_send_navigate_to_pose.py b/free_fleet_examples/free_fleet_examples/nav2_send_navigate_to_pose.py index cbbd3b4..daec76f 100755 --- a/free_fleet_examples/free_fleet_examples/nav2_send_navigate_to_pose.py +++ b/free_fleet_examples/free_fleet_examples/nav2_send_navigate_to_pose.py @@ -132,13 +132,13 @@ def main(argv=sys.argv): ) # print("Result: {0}".format(rep.sequence)) print(f'Result: {rep.status}') - if rep.status == GoalStatus.STATUS_ABORTED: + if rep.status == GoalStatus.STATUS_ABORTED.value: print( 'Received (ERROR: "Plan aborted by ' 'planner_server")' ) break - if rep.status == GoalStatus.STATUS_SUCCEEDED: + if rep.status == GoalStatus.STATUS_SUCCEEDED.value: break except Exception as e: print(e) From 8140474b84670719d0c73dd7085b05d6dc1c466a Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Fri, 15 Nov 2024 15:47:05 +0800 Subject: [PATCH 36/43] Fix launch files Signed-off-by: Aaron Chong --- free_fleet_adapter/CMakeLists.txt | 7 +++++++ free_fleet_adapter/free_fleet_adapter/fleet_adapter.py | 4 +--- .../free_fleet_adapter/nav2_robot_adapter.py | 5 +++-- free_fleet_adapter/launch/fleet_adapter.launch.xml | 4 ++-- .../launch/tb3_simulation_fleet_adapter.launch.xml | 2 +- 5 files changed, 14 insertions(+), 8 deletions(-) diff --git a/free_fleet_adapter/CMakeLists.txt b/free_fleet_adapter/CMakeLists.txt index b32028d..a26f1c1 100644 --- a/free_fleet_adapter/CMakeLists.txt +++ b/free_fleet_adapter/CMakeLists.txt @@ -6,6 +6,13 @@ find_package(ament_cmake REQUIRED) ament_python_install_package(${PROJECT_NAME}) +install(PROGRAMS + ${PROJECT_NAME}/fleet_adapter.py + ${PROJECT_NAME}/robot_adapter.py + ${PROJECT_NAME}/nav2_robot_adapter.py + DESTINATION lib/${PROJECT_NAME} +) + install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME} diff --git a/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py b/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py index c1131a3..2cc7dbc 100644 --- a/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py @@ -20,6 +20,7 @@ import threading import time +from free_fleet_adapter.nav2_robot_adapter import Nav2RobotAdapter import nudged import rclpy from rclpy.duration import Duration @@ -33,9 +34,6 @@ import zenoh -from .nav2_robot_adapter import Nav2RobotAdapter - - # ------------------------------------------------------------------------------ # Helper functions # ------------------------------------------------------------------------------ diff --git a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py index d4902da..1af6677 100644 --- a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py @@ -248,8 +248,9 @@ def _handle_navigate_to_pose( # TODO(ac): test this map related replanning behavior self.replan_counts += 1 self.node.get_logger().error( - f'Destination is on map [{map_name}], while robot [{self.name}] is ' - f'on map [{self.map}], replan count [{self.replan_counts}]' + f'Destination is on map [{map_name}], while robot ' + f'[{self.name}] is on map [{self.map}], replan count ' + f'[{self.replan_counts}]' ) self._check_update_handle_initialization() diff --git a/free_fleet_adapter/launch/fleet_adapter.launch.xml b/free_fleet_adapter/launch/fleet_adapter.launch.xml index bb86984..f810b90 100644 --- a/free_fleet_adapter/launch/fleet_adapter.launch.xml +++ b/free_fleet_adapter/launch/fleet_adapter.launch.xml @@ -12,7 +12,7 @@ @@ -23,7 +23,7 @@ diff --git a/free_fleet_examples/launch/tb3_simulation_fleet_adapter.launch.xml b/free_fleet_examples/launch/tb3_simulation_fleet_adapter.launch.xml index 90d5a76..b2e3696 100644 --- a/free_fleet_examples/launch/tb3_simulation_fleet_adapter.launch.xml +++ b/free_fleet_examples/launch/tb3_simulation_fleet_adapter.launch.xml @@ -4,7 +4,7 @@ - + From ab6f94685377432ec704246d92e2a5605f40337d Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Fri, 15 Nov 2024 16:41:48 +0800 Subject: [PATCH 37/43] Rename functions Signed-off-by: Aaron Chong --- free_fleet_adapter/free_fleet_adapter/fleet_adapter.py | 4 ++-- .../free_fleet_adapter/nav2_robot_adapter.py | 6 ++++-- free_fleet_adapter/free_fleet_adapter/robot_adapter.py | 4 ++-- .../tests/integration/test_nav2_robot_adapter.py | 8 ++++---- 4 files changed, 12 insertions(+), 10 deletions(-) diff --git a/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py b/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py index 2cc7dbc..9f522ca 100644 --- a/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py @@ -209,7 +209,7 @@ def run_in_parallel(*args, **kwargs): @parallel def update_robot(robot: Nav2RobotAdapter): - robot_pose = robot.pose() + robot_pose = robot.get_pose() if robot_pose is None: robot.node.get_logger().info(f'Failed to pose of robot [{robot.name}]') return None @@ -217,7 +217,7 @@ def update_robot(robot: Nav2RobotAdapter): state = rmf_easy.RobotState( robot.map, robot_pose, - robot.battery_soc + robot.get_battery_soc() ) if robot.update_handle is None: diff --git a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py index 1af6677..434147c 100644 --- a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py @@ -146,10 +146,10 @@ def _battery_state_callback(sample: zenoh.Sample): _battery_state_callback ) - def battery_soc(self) -> float: + def get_battery_soc(self) -> float: return self.battery_soc - def pose(self) -> Annotated[list[float], 3] | None: + def get_pose(self) -> Annotated[list[float], 3] | None: transform = self.tf_handler.get_transform() if transform is None: error_message = \ @@ -305,6 +305,8 @@ def _handle_navigate_to_pose( self.update_handle.more().replan() self.nav_goal_id = None return + except RuntimeError as e: + raise e except Exception as e: payload = reply.err.payload.to_string() self.node.get_logger().error( diff --git a/free_fleet_adapter/free_fleet_adapter/robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/robot_adapter.py index 86b1060..943e7e3 100644 --- a/free_fleet_adapter/free_fleet_adapter/robot_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/robot_adapter.py @@ -29,7 +29,7 @@ class RobotAdapter(ABC): between 0 and 1.0. """ @abstractmethod - def battery_soc(self) -> float: + def get_battery_soc(self) -> float: ... """ @@ -39,7 +39,7 @@ def battery_soc(self) -> float: returns None. """ @abstractmethod - def pose(self) -> Annotated[list[float], 3] | None: + def get_pose(self) -> Annotated[list[float], 3] | None: ... """ diff --git a/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py b/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py index 1f5550e..d31722d 100644 --- a/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py +++ b/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py @@ -57,7 +57,7 @@ def test_non_existent_robot_pose(self): robot_exists = False for i in range(10): - transform = robot_adapter.pose() + transform = robot_adapter.get_pose() if transform is not None: robot_exists = True break @@ -82,7 +82,7 @@ def test_robot_pose(self): robot_exists = False for i in range(10): - transform = robot_adapter.pose() + transform = robot_adapter.get_pose() if transform is not None: robot_exists = True break @@ -105,7 +105,7 @@ def test_robot_battery_soc(self): tf_buffer=tf_buffer ) - battery_soc = robot_adapter.battery_soc() + battery_soc = robot_adapter.get_battery_soc() assert math.isclose(battery_soc, 1.0) def test_robot_unable_to_update(self): @@ -168,7 +168,7 @@ def test_robot_stop_without_command(self): tf_buffer=tf_buffer ) assert robot_adapter.execution is None - robot_adapter.stop() + robot_adapter.stop(None) assert robot_adapter.execution is None assert robot_adapter._is_navigation_done() From 511e2c79748f4b0470b02a56c7ac02710440a587 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Fri, 15 Nov 2024 17:09:20 +0800 Subject: [PATCH 38/43] Not to check execution, comment out stop command test Signed-off-by: Aaron Chong --- .../free_fleet_adapter/nav2_robot_adapter.py | 4 +- .../integration/test_nav2_robot_adapter.py | 103 ++++++------------ 2 files changed, 37 insertions(+), 70 deletions(-) diff --git a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py index 434147c..c73dc15 100644 --- a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py @@ -172,7 +172,7 @@ def get_pose(self) -> Annotated[list[float], 3] | None: return robot_pose def _is_navigation_done(self) -> bool: - if self.execution is None or self.nav_goal_id is None: + if self.nav_goal_id is None: return True req = NavigateToPose_GetResult_Request(goal_id=self.nav_goal_id) @@ -305,8 +305,6 @@ def _handle_navigate_to_pose( self.update_handle.more().replan() self.nav_goal_id = None return - except RuntimeError as e: - raise e except Exception as e: payload = reply.err.payload.to_string() self.node.get_logger().error( diff --git a/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py b/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py index d31722d..b87b75c 100644 --- a/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py +++ b/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py @@ -201,35 +201,6 @@ def test_robot_handle_navigate_to_invalid_map(self): able_to_handle_navigate = False assert not able_to_handle_navigate - def test_robot_handle_navigate_to_invalid_pose(self): - tf_buffer = Buffer() - - robot_adapter = Nav2RobotAdapter( - name='turtlebot3_1', - configuration=None, - robot_config_yaml={ - 'initial_map': 'L1', - }, - node=self.node, - zenoh_session=self.zenoh_session, - fleet_handle=None, - tf_buffer=tf_buffer - ) - - able_to_handle_navigate = False - try: - robot_adapter._handle_navigate_to_pose( - 'L1', - 1000000, - 1000000, - 0.0, - 0.0 - ) - able_to_handle_navigate = True - except RuntimeError: - able_to_handle_navigate = False - assert not able_to_handle_navigate - def test_robot_handle_navigate_to_pose(self): tf_buffer = Buffer() @@ -258,47 +229,45 @@ def test_robot_handle_navigate_to_pose(self): except RuntimeError: able_to_handle_navigate = False assert able_to_handle_navigate - - time.sleep(1) assert not robot_adapter._is_navigation_done() - time.sleep(10) + time.sleep(5) assert robot_adapter._is_navigation_done() - def test_robot_stop_navigate(self): - tf_buffer = Buffer() - - robot_adapter = Nav2RobotAdapter( - name='turtlebot3_1', - configuration=None, - robot_config_yaml={ - 'initial_map': 'L1', - }, - node=self.node, - zenoh_session=self.zenoh_session, - fleet_handle=None, - tf_buffer=tf_buffer - ) - - able_to_handle_navigate = False - try: - robot_adapter._handle_navigate_to_pose( - 'L1', - 1.808, - 0.503, - 0.0, - 0.0 - ) - able_to_handle_navigate = True - except RuntimeError: - able_to_handle_navigate = False - assert able_to_handle_navigate - - time.sleep(1) - assert not robot_adapter._is_navigation_done() - time.sleep(1) - robot_adapter.stop(robot_adapter.execution.identifier) - time.sleep(1) - assert robot_adapter._is_navigation_done() + # def test_robot_stop_navigate(self): + # tf_buffer = Buffer() + + # robot_adapter = Nav2RobotAdapter( + # name='turtlebot3_1', + # configuration=None, + # robot_config_yaml={ + # 'initial_map': 'L1', + # }, + # node=self.node, + # zenoh_session=self.zenoh_session, + # fleet_handle=None, + # tf_buffer=tf_buffer + # ) + + # able_to_handle_navigate = False + # try: + # robot_adapter._handle_navigate_to_pose( + # 'L1', + # 1.808, + # 0.503, + # 0.0, + # 0.0 + # ) + # able_to_handle_navigate = True + # except RuntimeError: + # able_to_handle_navigate = False + # assert able_to_handle_navigate + + # time.sleep(1) + # assert not robot_adapter._is_navigation_done() + # time.sleep(1) + # robot_adapter.stop(robot_adapter.execution.identifier) + # time.sleep(1) + # assert robot_adapter._is_navigation_done() def test_robot_execute_unknown_action(self): tf_buffer = Buffer() From 60d0f5dab1f5b9222b70658706cb2f606b56f6c9 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Fri, 15 Nov 2024 17:22:11 +0800 Subject: [PATCH 39/43] Use helper function for stop Signed-off-by: Aaron Chong --- .../free_fleet_adapter/nav2_robot_adapter.py | 37 +++++----- .../integration/test_nav2_robot_adapter.py | 68 +++++++++---------- 2 files changed, 54 insertions(+), 51 deletions(-) diff --git a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py index c73dc15..883fe5f 100644 --- a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py @@ -330,6 +330,24 @@ def navigate( destination.position[2] ) + def _handle_stop_navigation(self): + req = make_cancel_all_goals_request() + replies = self.zenoh_session.get( + namespacify( + 'navigate_to_pose/_action/cancel_goal', + self.name, + ), + payload=req.serialize(), + # timeout=0.5 + ) + for reply in replies: + rep = ActionMsgs_CancelGoal_Response.deserialize( + reply.ok.payload.to_bytes() + ) + self.node.get_logger().info( + 'Return code: %d' % rep.return_code + ) + def stop(self, activity: ActivityIdentifier): if self.execution is None: return @@ -340,22 +358,9 @@ def stop(self, activity: ActivityIdentifier): # supporting something other than navigation if self.nav_goal_id is not None: - req = make_cancel_all_goals_request() - replies = self.zenoh_session.get( - namespacify( - 'navigate_to_pose/_action/cancel_goal', - self.name, - ), - payload=req.serialize(), - # timeout=0.5 - ) - for reply in replies: - rep = ActionMsgs_CancelGoal_Response.deserialize( - reply.ok.payload.to_bytes() - ) - self.node.get_logger().info( - 'Return code: %d' % rep.return_code - ) + self._handle_stop_navigation() + # TODO(ac): check return code before setting nav_goal_id to + # None self.nav_goal_id = None def execute_action( diff --git a/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py b/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py index b87b75c..629295e 100644 --- a/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py +++ b/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py @@ -233,41 +233,39 @@ def test_robot_handle_navigate_to_pose(self): time.sleep(5) assert robot_adapter._is_navigation_done() - # def test_robot_stop_navigate(self): - # tf_buffer = Buffer() - - # robot_adapter = Nav2RobotAdapter( - # name='turtlebot3_1', - # configuration=None, - # robot_config_yaml={ - # 'initial_map': 'L1', - # }, - # node=self.node, - # zenoh_session=self.zenoh_session, - # fleet_handle=None, - # tf_buffer=tf_buffer - # ) - - # able_to_handle_navigate = False - # try: - # robot_adapter._handle_navigate_to_pose( - # 'L1', - # 1.808, - # 0.503, - # 0.0, - # 0.0 - # ) - # able_to_handle_navigate = True - # except RuntimeError: - # able_to_handle_navigate = False - # assert able_to_handle_navigate - - # time.sleep(1) - # assert not robot_adapter._is_navigation_done() - # time.sleep(1) - # robot_adapter.stop(robot_adapter.execution.identifier) - # time.sleep(1) - # assert robot_adapter._is_navigation_done() + def test_robot_stop_navigate(self): + tf_buffer = Buffer() + + robot_adapter = Nav2RobotAdapter( + name='turtlebot3_1', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + able_to_handle_navigate = False + try: + robot_adapter._handle_navigate_to_pose( + 'L1', + 1.808, + 0.503, + 0.0, + 0.0 + ) + able_to_handle_navigate = True + except RuntimeError: + able_to_handle_navigate = False + assert able_to_handle_navigate + assert not robot_adapter._is_navigation_done() + time.sleep(1) + robot_adapter._handle_stop_navigation() + time.sleep(1) + assert robot_adapter._is_navigation_done() def test_robot_execute_unknown_action(self): tf_buffer = Buffer() From fdff8bc9885cccbdaf39d01e2d3d411206e0ec4e Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Fri, 15 Nov 2024 17:38:58 +0800 Subject: [PATCH 40/43] Using all statuses Signed-off-by: Aaron Chong --- .../free_fleet_adapter/nav2_robot_adapter.py | 42 ++++++++++++------- .../integration/test_nav2_robot_adapter.py | 1 - 2 files changed, 26 insertions(+), 17 deletions(-) diff --git a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py index 883fe5f..731b1e7 100644 --- a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py @@ -189,22 +189,32 @@ def _is_navigation_done(self) -> bool: reply.ok.payload.to_bytes() ) self.node.get_logger().debug(f'Result: {rep.status}') - if rep.status == GoalStatus.STATUS_EXECUTING.value: - return False - elif rep.status == GoalStatus.STATUS_SUCCEEDED.value: - self.node.get_logger().info( - f'Navigation goal {self.nav_goal_id} reached' - ) - return True - else: - # TODO(ac): test replanning behavior if goal status is - # neither executing or succeeded - self.replan_counts += 1 - self.node.get_logger().error( - f'Navigation goal {self.nav_goal_id} status ' - f'{rep.status}, replan count [{self.replan_counts}]') - self.update_handle.more().replan() - return False + match rep.status: + case GoalStatus.STATUS_EXECUTING.value | \ + GoalStatus.STATUS_ACCEPTED.value | \ + GoalStatus.STATUS_CANCELING.value: + return False + case GoalStatus.STATUS_SUCCEEDED.value: + self.node.get_logger().info( + f'Navigation goal {self.nav_goal_id} reached' + ) + return True + case GoalStatus.STATUS_CANCELED.value: + self.node.get_logger().info( + f'Navigation goal {self.nav_goal_id} was cancelled' + ) + return True + case _: + # TODO(ac): test replanning behavior if goal status is + # neither executing or succeeded + self.replan_counts += 1 + self.node.get_logger().error( + f'Navigation goal {self.nav_goal_id} status ' + f'{rep.status}, replan count ' + f'[{self.replan_counts}]' + ) + self.update_handle.more().replan() + return False except Exception as e: self.node.get_logger().debug( f'Received (ERROR: "{reply.err.payload.to_string()}"): ' diff --git a/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py b/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py index 629295e..83d2dad 100644 --- a/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py +++ b/free_fleet_adapter/tests/integration/test_nav2_robot_adapter.py @@ -229,7 +229,6 @@ def test_robot_handle_navigate_to_pose(self): except RuntimeError: able_to_handle_navigate = False assert able_to_handle_navigate - assert not robot_adapter._is_navigation_done() time.sleep(5) assert robot_adapter._is_navigation_done() From 87830afeafa9835e8a24727bde8cff029a18afcf Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Fri, 15 Nov 2024 18:14:57 +0800 Subject: [PATCH 41/43] Badges Signed-off-by: Aaron Chong --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 6498348..69b511b 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,7 @@ # Free Fleet +![Nightly](https://github.com/open-rmf/free_fleet/actions/workflows/nightly.yaml/badge.svg)![Unit tests](https://github.com/open-rmf/free_fleet/actions/workflows/unit-tests.yaml/badge.svg)![Integration tests](https://github.com/open-rmf/free_fleet/actions/workflows/integration-tests.yaml/badge.svg)[![codecov](https://codecov.io/github/open-rmf/free_fleet/graph/badge.svg?token=JCOB9g3YTn)](https://codecov.io/github/open-rmf/free_fleet) + - **[Introduction](#introduction)** - **[Dependency installation, source build and setup](#dependency-installation-source-build-and-setup)** - **[Simulation examples](#simulation-examples)** From bac366020551118ecd175edb9e14372b893fb8a3 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Fri, 15 Nov 2024 18:15:50 +0800 Subject: [PATCH 42/43] Switch to easy-full-control branch before merging Signed-off-by: Aaron Chong --- .github/workflows/nightly.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/nightly.yaml b/.github/workflows/nightly.yaml index 2c095ab..5bcdea3 100644 --- a/.github/workflows/nightly.yaml +++ b/.github/workflows/nightly.yaml @@ -41,7 +41,7 @@ jobs: build-args: | ROS_DISTRO=${{ matrix.ros_distribution }} ZENOH_VERSION=1.0.1 - FREE_FLEET_BRANCH=efc/integration-testing + FREE_FLEET_BRANCH=easy-full-control tags: ghcr.io/${{ github.repository }}/minimal-zenoh-bridge:${{ matrix.ros_distribution }}-latest context: .github/docker/minimal-zenoh-bridge From d4e958938a2972840025960b5feaac48e6f9940a Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Fri, 15 Nov 2024 18:16:51 +0800 Subject: [PATCH 43/43] Update readme Signed-off-by: Aaron Chong --- README.md | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/README.md b/README.md index 69b511b..47feb54 100644 --- a/README.md +++ b/README.md @@ -296,12 +296,10 @@ ros2 run rmf_demos_tasks dispatch_patrol \ * hardware testing * attempt to optimize tf messages (not all are needed) -* robot adapter and tf handler to be abstracted * ROS 1 nav support * custom actions to be abstracted * map switching support -* static testing -* end-to-end testing +* end-to-end testing with Open-RMF * test replanning behavior * support for Rolling * docker images