diff --git a/.github/actions/build-and-test/action.yaml b/.github/actions/build-and-test/action.yaml index a2e8e1cd..74742c1a 100644 --- a/.github/actions/build-and-test/action.yaml +++ b/.github/actions/build-and-test/action.yaml @@ -7,7 +7,11 @@ inputs: zenoh-version: description: string, version of eclipse-zenoh to install from pip required: true - integration-testing: + nav1-integration-testing: + description: string, supports ON or OFF only + required: false + default: "OFF" + nav2-integration-testing: description: string, supports ON or OFF only required: false default: "OFF" @@ -18,7 +22,7 @@ runs: - name: install dependencies run: | sudo apt update && sudo apt install python3-pip -y - pip3 install eclipse-zenoh==${{ inputs.zenoh-version }} pycdr2 --break-system-packages + pip3 install eclipse-zenoh==${{ inputs.zenoh-version }} pycdr2 rosbags --break-system-packages shell: bash - uses: ros-tooling/setup-ros@v0.7 @@ -34,7 +38,8 @@ runs: { "build": { "cmake-args": [ - "-DINTEGRATION_TESTING=${{ inputs.integration-testing }}" + "-DNAV1_INTEGRATION_TESTING=${{ inputs.nav1-integration-testing }}", + "-DNAV2_INTEGRATION_TESTING=${{ inputs.nav2-integration-testing }}" ], "mixin": ["coverage-pytest"] }, diff --git a/.github/docker/integration-tests/nav1-docker-compose.yaml b/.github/docker/integration-tests/nav1-docker-compose.yaml new file mode 100644 index 00000000..b3dfbcdd --- /dev/null +++ b/.github/docker/integration-tests/nav1-docker-compose.yaml @@ -0,0 +1,31 @@ +services: + minimal-ros1-sim: + image: ghcr.io/open-rmf/free_fleet/minimal-ros1-sim:noetic-latest + stop_signal: SIGINT + network_mode: host + privileged: true + stdin_open: true + tty: true + + minimal-nav1-bringup: + image: ghcr.io/open-rmf/free_fleet/minimal-nav1-bringup:noetic-latest + stop_signal: SIGINT + network_mode: host + privileged: true + stdin_open: true + tty: true + + minimal-zenoh-bridge-ros1: + image: ghcr.io/open-rmf/free_fleet/minimal-zenoh-bridge-ros1:noetic-latest + network_mode: host + stdin_open: true + tty: true + + minimal-zenoh-router: + image: eclipse/zenoh + restart: unless-stopped + network_mode: host + stdin_open: true + tty: true + environment: + - RUST_LOG=debug diff --git a/.github/docker/integration-tests/docker-compose.yaml b/.github/docker/integration-tests/nav2-docker-compose.yaml similarity index 82% rename from .github/docker/integration-tests/docker-compose.yaml rename to .github/docker/integration-tests/nav2-docker-compose.yaml index 78dc48ff..47c22b5d 100644 --- a/.github/docker/integration-tests/docker-compose.yaml +++ b/.github/docker/integration-tests/nav2-docker-compose.yaml @@ -1,5 +1,3 @@ -version: "3" - services: minimal-nav2-bringup: image: ghcr.io/open-rmf/free_fleet/minimal-nav2-bringup:jazzy-latest @@ -11,8 +9,8 @@ services: environment: - ROS_DOMAIN_ID=42 - minimal-zenoh-bridge: - image: ghcr.io/open-rmf/free_fleet/minimal-zenoh-bridge:jazzy-latest + minimal-zenoh-bridge-ros2dds: + image: ghcr.io/open-rmf/free_fleet/minimal-zenoh-bridge-ros2dds:jazzy-latest network_mode: host stdin_open: true tty: true diff --git a/.github/docker/minimal-nav1-bringup/Dockerfile b/.github/docker/minimal-nav1-bringup/Dockerfile new file mode 100644 index 00000000..ac1bb930 --- /dev/null +++ b/.github/docker/minimal-nav1-bringup/Dockerfile @@ -0,0 +1,21 @@ +ARG ROS_DISTRO=noetic +FROM docker.io/ros:$ROS_DISTRO-ros-base + +RUN apt update && apt install -y curl ros-$ROS_DISTRO-turtlebot3-navigation ros-$ROS_DISTRO-dwa-local-planner + +RUN mkdir -p /tb3 && cd /tb3 \ + && curl -sL https://github.com/ros-navigation/navigation2/archive/refs/heads/master.tar.gz -o navigation2.tar.gz \ + && mkdir -p /tb3/navigation2 && tar zxf navigation2.tar.gz -C /tb3/navigation2 --strip-components=1 && rm navigation2.tar.gz + +ENV TURTLEBOT3_MODEL=burger + +# Modify existing launch file to add initial pose +RUN cd /tb3 \ + && curl --output turtlebot3_navigation.launch "https://raw.githubusercontent.com/ROBOTIS-GIT/turtlebot3/refs/heads/noetic-devel/turtlebot3_navigation/launch/turtlebot3_navigation.launch" \ + && sed -z 's|amcl.launch"/>|amcl.launch">|' turtlebot3_navigation.launch > turtlebot3_navigation_edited.launch + +RUN rm -rf \ + /var/lib/apt/lists \ + /dist + +ENTRYPOINT ["bash", "-c", ". /opt/ros/$ROS_DISTRO/setup.bash && roslaunch --wait /tb3/turtlebot3_navigation_edited.launch map_file:=/tb3/navigation2/nav2_bringup/maps/tb3_sandbox.yaml open_rviz:=false"] diff --git a/.github/docker/minimal-nav2-bringup/Dockerfile b/.github/docker/minimal-nav2-bringup/Dockerfile index cfc66b8e..b220bf6d 100644 --- a/.github/docker/minimal-nav2-bringup/Dockerfile +++ b/.github/docker/minimal-nav2-bringup/Dockerfile @@ -1,14 +1,14 @@ 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 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" +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 \ diff --git a/.github/docker/minimal-ros1-sim/Dockerfile b/.github/docker/minimal-ros1-sim/Dockerfile new file mode 100644 index 00000000..464f6137 --- /dev/null +++ b/.github/docker/minimal-ros1-sim/Dockerfile @@ -0,0 +1,12 @@ +ARG ROS_DISTRO=noetic +FROM docker.io/ros:$ROS_DISTRO-ros-base + +RUN apt update && apt install -y ros-$ROS_DISTRO-turtlebot3-gazebo ros-$ROS_DISTRO-gazebo-ros-pkgs + +ENV TURTLEBOT3_MODEL=burger + +RUN rm -rf \ + /var/lib/apt/lists \ + /dist + +ENTRYPOINT ["bash", "-c", ". /opt/ros/$ROS_DISTRO/setup.bash && roslaunch turtlebot3_gazebo turtlebot3_world.launch gui:=false"] diff --git a/.github/docker/minimal-zenoh-bridge-ros1/Dockerfile b/.github/docker/minimal-zenoh-bridge-ros1/Dockerfile new file mode 100644 index 00000000..0d46dff3 --- /dev/null +++ b/.github/docker/minimal-zenoh-bridge-ros1/Dockerfile @@ -0,0 +1,36 @@ +### Build bridge + +ARG ROS_DISTRO=noetic +FROM docker.io/ros:$ROS_DISTRO-ros-base +ARG ZENOH_BRIDGE_REPO=eclipse-zenoh/zenoh-plugin-ros1 +ARG ZENOH_BRIDGE_TAG=main +ARG FREE_FLEET_BRANCH=main + +RUN apt update && apt install -y build-essential curl git unzip wget + +# Get Rust +RUN curl https://sh.rustup.rs -sSf | bash -s -- -y + +ENV PATH="/root/.cargo/bin:${PATH}" + +RUN mkdir -p /zenoh-bridge && cd /zenoh-bridge \ + && git clone --recursive https://github.com/$ZENOH_BRIDGE_REPO -b $ZENOH_BRIDGE_TAG zenoh-plugin-ros1 \ + && cd /zenoh-bridge/zenoh-plugin-ros1 \ + && cargo build --package zenoh-bridge-ros1 --bin zenoh-bridge-ros1 --release + +RUN cd /zenoh-bridge \ + && wget -O turtlebot3_zenoh_bridge_ros1_client_config.json5 https://raw.githubusercontent.com/open-rmf/free_fleet/refs/heads/$FREE_FLEET_BRANCH/free_fleet_examples/config/zenoh/turtlebot3_zenoh_bridge_ros1_client_config.json5 + +RUN rm -rf \ + /var/lib/apt/lists \ + /dist + +### Set up bare minimum zenoh-bridge-ros1 image + +FROM docker.io/ros:$ROS_DISTRO-ros-base + +COPY --from=0 /zenoh-bridge/turtlebot3_zenoh_bridge_ros1_client_config.json5 /zenoh-bridge/turtlebot3_zenoh_bridge_ros1_client_config.json5 + +COPY --from=0 /zenoh-bridge/zenoh-plugin-ros1/target/release/zenoh-bridge-ros1 /zenoh-bridge/zenoh-bridge-ros1 + +ENTRYPOINT ["bash", "-c", ". /opt/ros/$ROS_DISTRO/setup.bash && /zenoh-bridge/zenoh-bridge-ros1 -c /zenoh-bridge/turtlebot3_zenoh_bridge_ros1_client_config.json5"] diff --git a/.github/docker/minimal-zenoh-bridge/Dockerfile b/.github/docker/minimal-zenoh-bridge-ros2dds/Dockerfile similarity index 58% rename from .github/docker/minimal-zenoh-bridge/Dockerfile rename to .github/docker/minimal-zenoh-bridge-ros2dds/Dockerfile index 62e31887..c3227c75 100644 --- a/.github/docker/minimal-zenoh-bridge/Dockerfile +++ b/.github/docker/minimal-zenoh-bridge-ros2dds/Dockerfile @@ -1,9 +1,9 @@ ARG ROS_DISTRO=jazzy FROM docker.io/ros:$ROS_DISTRO-ros-base -ARG ZENOH_VERSION=1.0.1 +ARG ZENOH_VERSION=1.1.0 ARG FREE_FLEET_BRANCH=main -RUN apt update && apt install -y wget unzip ros-jazzy-rmw-cyclonedds-cpp +RUN apt update && apt install -y wget unzip ros-$ROS_DISTRO-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 \ @@ -11,12 +11,12 @@ RUN mkdir -p /zenoh-bridge && cd /zenoh-bridge \ && rm zenoh-plugin-ros2dds.zip RUN cd /zenoh-bridge \ - && wget -O turtlebot3_1_client_zenoh_config.json5 https://raw.githubusercontent.com/open-rmf/free_fleet/refs/heads/$FREE_FLEET_BRANCH/free_fleet_examples/config/zenoh/turtlebot3_1_client_zenoh_config.json5 + && wget -O turtlebot3_1_zenoh_bridge_ros2dds_client_config.json5 https://raw.githubusercontent.com/open-rmf/free_fleet/refs/heads/$FREE_FLEET_BRANCH/free_fleet_examples/config/zenoh/turtlebot3_1_zenoh_bridge_ros2dds_client_config.json5 -ENV RMW_IMPLEMENTATION rmw_cyclonedds_cpp +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_client_zenoh_config.json5"] +ENTRYPOINT ["bash", "-c", ". /opt/ros/$ROS_DISTRO/setup.bash && /zenoh-bridge/zenoh-bridge-ros2dds -c /zenoh-bridge/turtlebot3_1_zenoh_bridge_ros2dds_client_config.json5"] diff --git a/.github/workflows/integration-tests.yaml b/.github/workflows/nav1-integration-tests.yaml similarity index 86% rename from .github/workflows/integration-tests.yaml rename to .github/workflows/nav1-integration-tests.yaml index 040b580c..8ba74931 100644 --- a/.github/workflows/integration-tests.yaml +++ b/.github/workflows/nav1-integration-tests.yaml @@ -1,4 +1,4 @@ -name: integration-tests +name: nav1-integration-tests on: push @@ -22,18 +22,18 @@ jobs: 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 + run: docker-compose -f ".github/docker/integration-tests/nav1-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 + zenoh-version: 1.1.0 + nav1-integration-testing: ON - name: Stop test fixture containers if: always() - run: docker-compose -f ".github/docker/integration-tests/docker-compose.yaml" down + run: docker-compose -f ".github/docker/integration-tests/nav1-docker-compose.yaml" down - name: Upload coverage to Codecov uses: codecov/codecov-action@v4 diff --git a/.github/workflows/nav2-integration-tests.yaml b/.github/workflows/nav2-integration-tests.yaml new file mode 100644 index 00000000..1d9cb3aa --- /dev/null +++ b/.github/workflows/nav2-integration-tests.yaml @@ -0,0 +1,45 @@ +name: nav2-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: 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/nav2-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.1.0 + nav2-integration-testing: ON + + - name: Stop test fixture containers + if: always() + run: docker-compose -f ".github/docker/integration-tests/nav2-docker-compose.yaml" down + + - 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/nightly.yaml b/.github/workflows/nightly.yaml index 5bcdea3d..417ab16e 100644 --- a/.github/workflows/nightly.yaml +++ b/.github/workflows/nightly.yaml @@ -6,8 +6,8 @@ on: - cron: '0 18 * * *' jobs: - build-minimal-docker-images: - name: Push minimal docker images to GitHub Packages + build-minimal-nav2-docker-images: + name: Push minimal nav2 docker images to GitHub Packages runs-on: ubuntu-latest strategy: matrix: @@ -34,19 +34,101 @@ jobs: 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 + - name: Build and push minimal-zenoh-bridge-ros2dds uses: docker/build-push-action@v5 with: push: true build-args: | ROS_DISTRO=${{ matrix.ros_distribution }} - ZENOH_VERSION=1.0.1 + ZENOH_VERSION=1.1.0 FREE_FLEET_BRANCH=easy-full-control - tags: ghcr.io/${{ github.repository }}/minimal-zenoh-bridge:${{ matrix.ros_distribution }}-latest - context: .github/docker/minimal-zenoh-bridge + tags: ghcr.io/${{ github.repository }}/minimal-zenoh-bridge-ros2dds:${{ matrix.ros_distribution }}-latest + context: .github/docker/minimal-zenoh-bridge-ros2dds - integration-tests: - needs: build-minimal-docker-images + build-minimal-nav1-docker-images: + name: Push minimal nav1 docker images to GitHub Packages + runs-on: ubuntu-latest + strategy: + matrix: + ros_distribution: [noetic] + 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-ros1-sim + uses: docker/build-push-action@v5 + with: + push: true + build-args: | + ROS_DISTRO=${{ matrix.ros_distribution }} + tags: ghcr.io/${{ github.repository }}/minimal-ros1-sim:${{ matrix.ros_distribution }}-latest + context: .github/docker/minimal-ros1-sim + + - name: Build and push minimal-nav1-bringup + uses: docker/build-push-action@v5 + with: + push: true + build-args: | + ROS_DISTRO=${{ matrix.ros_distribution }} + tags: ghcr.io/${{ github.repository }}/minimal-nav1-bringup:${{ matrix.ros_distribution }}-latest + context: .github/docker/minimal-nav1-bringup + + - name: Build and push minimal-zenoh-bridge-ros1 + uses: docker/build-push-action@v5 + with: + push: true + build-args: | + ROS_DISTRO=${{ matrix.ros_distribution }} + ZENOH_BRIDGE_REPO=aaronchongth/zenoh-plugin-ros1 + ZENOH_BRIDGE_TAG=namespace + FREE_FLEET_BRANCH=easy-full-control + tags: ghcr.io/${{ github.repository }}/minimal-zenoh-bridge-ros1:${{ matrix.ros_distribution }}-latest + context: .github/docker/minimal-zenoh-bridge-ros1 + + nav1-integration-tests: + needs: build-minimal-nav1-docker-images + 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: 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/nav1-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.1.0 + nav1-integration-testing: ON + + - name: Stop test fixture containers + if: always() + run: docker-compose -f ".github/docker/integration-tests/nav1-docker-compose.yaml" down + + nav2-integration-tests: + needs: build-minimal-nav2-docker-images timeout-minutes: 10 runs-on: ubuntu-latest container: @@ -65,15 +147,15 @@ jobs: 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 + run: docker-compose -f ".github/docker/integration-tests/nav2-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 + zenoh-version: 1.1.0 + nav2-integration-testing: ON - name: Stop test fixture containers if: always() - run: docker-compose -f ".github/docker/integration-tests/docker-compose.yaml" down + run: docker-compose -f ".github/docker/integration-tests/nav2-docker-compose.yaml" down diff --git a/README.md b/README.md index 3a384e00..e3bd309e 100644 --- a/README.md +++ b/README.md @@ -5,8 +5,9 @@ - **[Introduction](#introduction)** - **[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) + - [Nav2 Single turtlebot3 world](#nav2-single-turtlebot3-world) + - [Nav2 Multiple turtlebot3 world](#nav2-multiple-turtlebot3-world) + - [Nav1 Single turtlebot3 world](#nav1-single-turtlebot3-world) - **[Troubleshooting](#troubleshooting)** - **[TODOs](#todos)** @@ -25,7 +26,7 @@ Supports * [ROS 2 Jazzy](https://docs.ros.org/en/jazzy/index.html) * [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-bridge-ros2dds v1.1.0](https://github.com/eclipse-zenoh/zenoh-plugin-ros2dds/releases/tag/1.1.0) * [zenoh router](https://zenoh.io/docs/getting-started/installation/#ubuntu-or-any-debian) 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). @@ -40,10 +41,10 @@ System dependencies, sudo apt update && sudo apt install python3-pip ros-jazzy-rmw-cyclonedds-cpp ``` -The dependencies `eclipse-zenoh` and `pycdr2` are available through `pip`. Users can choose to set up a virtual environment, or `--break-system-packages` by performing the installation directly. +The dependencies `eclipse-zenoh`, `pycdr2`, `rosbags` are available through `pip`. Users can choose to set up a virtual environment, or `--break-system-packages` by performing the installation directly. ```bash -pip3 install pip install eclipse-zenoh==1.0.1 pycdr2 --break-system-packages +pip3 install pip install eclipse-zenoh==1.1.0 pycdr2 rosbags --break-system-packages ``` Install `zenohd` from the [official guide](https://zenoh.io/docs/getting-started/installation/#ubuntu-or-any-debian). @@ -73,7 +74,7 @@ Download and extract standalone binaries for `zenoh-bridge-ros2dds` (optionally ```bash # Change preferred zenoh version here -export ZENOH_VERSION=1.0.1 +export ZENOH_VERSION=1.1.0 # 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 @@ -96,7 +97,7 @@ sudo apt update && sudo apt install ros-jazzy-nav2-bringup git clone https://github.com/ROBOTIS-GIT/turtlebot3_simulations ~/turtlebot3_simulations ``` -### Single turtlebot3 world +### Nav2 Single turtlebot3 world ![](../media/ff_tb3_faster_smaller.gif) @@ -133,7 +134,7 @@ source /opt/ros/jazzy/setup.bash export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp cd PATH_TO_EXTRACTED_ZENOH_BRIDGE -./zenoh-bridge-ros2dds -c ~/ff_ws/src/free_fleet/free_fleet_examples/config/zenoh/turtlebot3_1_zenoh_config.json5 +./zenoh-bridge-ros2dds -c ~/ff_ws/src/free_fleet/free_fleet_examples/config/zenoh/turtlebot3_1_zenoh_bridge_ros2dds_client_config.json5 ``` Listen to transforms over `zenoh`, @@ -185,7 +186,7 @@ ros2 run rmf_demos_tasks dispatch_patrol \ -st 0 ``` -### Multiple turtlebot3 world +### Nav2 Multiple turtlebot3 world > [!NOTE] > This multi-robot simulation example is only for testing purposes, as it is a different setup than `free_fleet` is intended to be used. The simulation spawns 2 already namespaced robots, while the `free_fleet` architecture expects individual non-namespaced robots to be partnered with a namespaced `zenoh-bridge-ros2dds`. @@ -223,7 +224,7 @@ source /opt/ros/jazzy/setup.bash export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp cd PATH_TO_EXTRACTED_ZENOH_BRIDGE -./zenoh-bridge-ros2dds -c ~/ff_ws/src/free_fleet/free_fleet_examples/config/zenoh/unique_multi_tb3_zenoh_config.json5 +./zenoh-bridge-ros2dds -c ~/ff_ws/src/free_fleet/free_fleet_examples/config/zenoh/unique_multi_tb3_zenoh_bridge_ros2dds_client_config.json5 ``` Start the RMF core packages on a different `ROS_DOMAIN_ID` to simulate running on a different machine, @@ -236,7 +237,7 @@ export ROS_DOMAIN_ID=55 ros2 launch free_fleet_examples turtlebot3_world_rmf_common.launch.xml ``` -Launch the `free_fleet_adapter` with the current example's configurations, verify that `turtlebot3_1` has been added to fleet `turtletbot`. +Launch the `free_fleet_adapter` with the current example's configurations, verify that `turtlebot3_1` has been added to fleet `turtlebot3`. ```bash source ~/ff_ws/install/setup.bash @@ -273,6 +274,97 @@ ros2 run rmf_demos_tasks dispatch_patrol \ -R robot2 ``` +### Nav1 Single turtlebot3 world + +![](../media/nav1_sim_architecture.jpg) + +> [!WARNING] +> The Nav1 integration has only been tested in simulation and in ROS 1 Noetic, and is currently still using a fork of [zenoh-plugin-ros1](https://github.com/aaronchongth/zenoh-plugin-ros1), to support bridge namespacing. This will be updated after contributions to upstream has been made. + +Check out the [docker compose integration tests](.github/docker/integration-tests/nav1-docker-compose.yaml) for an overview of how the integration can be set up. + +On the machine where the free fleet adapter will run, start a `zenoh` router, + +```bash +zenohd + +# If using released standalaone binaries +# cd PATH_TO_EXTRACTED_ZENOH_ROUTER +# ./zenohd +``` + +In the ROS 1 Noetic environment, set up all the prerequisites as mentioned in the [official guide](https://emanual.robotis.com/docs/en/platform/turtlebot3/simulation/#gazebo-simulation), and start the turtlebot3 simulation. See the [relevant docker file](.github/docker/minimal-ros1-sim/Dockerfile) for reference. + +```bash +source /opt/ros/noetic/setup.bash +export TURTLEBOT3_MODEL=burger +roslaunch turtlebot3_gazebo turtlebot3_world.launch +``` + +In the ROS 1 Noetic environment, bringup the Nav1 stack with the prepared map in [navigation2](https://github.com/ros-navigation/navigation2/tree/main/nav2_bringup/maps). See the [relevant docker file](.github/docker/minimal-nav1-bringup/Dockerfile) for reference. + +```bash +# prepare the map +git clone https://github.com/ros-navigation/navigation2 + +source /opt/ros/noetic/setup.bash +export TURTLEBOT3_MODEL=burger +roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=/PATH_TO_navigation2/nav2_bringup/maps/tb3_sandbox.yaml +``` + +In the ROS 1 Noetic environment, set up prerequisites of [zenoh-plugin-ros1](https://github.com/aaronchongth/zenoh-plugin-ros1), build `zenoh-bridge-ros1` in release, and start it with the [provided config in examples](free_fleet_examples/config/zenoh/turtlebot3_zenoh_bridge_ros1_client_config.json5). See the [relevant docker file](.github/docker/minimal-zenoh-bridge-ros1/Dockerfile) for reference. + +```bash +# Get the config file +git clone https://github.com/open-rmf/free_fleet -b easy-full-control + +# Build the bridge +git clone --recursive https://github.com/aaronchongth/zenoh-plugin-ros1 +cd zenoh-plugin-ros1 +cargo build --package zenoh-bridge-ros1 --bin zenoh-bridge-ros1 --release + +# Use cargo run, or just run the executable directly +source /opt/ros/noetic/setup.bash +./target/release/zenoh-bridge-ros1 -c PATH_TO_free_fleet/free_fleet_examples/config/zenoh/turtlebot3_zenoh_bridge_ros1_client_config.json5 +``` + +On the machine where the free fleet adapter will run, start the common launch files and the free fleet adapter, + +```bash +source ~/ff_ws/install/setup.bash +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + +ros2 launch free_fleet_examples turtlebot3_world_rmf_common.launch.xml +``` + +Launch the `free_fleet_adapter` with the current example's configurations, verify that `tb3_0` has been added to fleet `turtlebot3`. + +```bash +source ~/ff_ws/install/setup.bash +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + +ros2 launch free_fleet_examples nav1_tb3_simulation_fleet_adapter.launch.xml + +# Or launch with the rmf-web API server address +# ros2 launch free_fleet_examples nav1_tb3_simulation_fleet_adapter.launch.xml server_uri:="ws://localhost:8000/_internal" +``` + +Dispatch example RMF patrol tasks using [`rmf-web`](https://github.com/open-rmf/rmf-web) on the same `ROS_DOMAIN_ID` as the RMF core packages, or use the `dispatch_patrol` scripts, which will cause the robot to negotiate as they perform their tasks. + +```bash +source ~/ff_ws/install/setup.bash +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +export ROS_DOMAIN_ID=55 + +# tb3_0 to run clockwise around the map +ros2 run rmf_demos_tasks dispatch_patrol \ + -p north_west north_east south_east south_west \ + -n 3 \ + -st 0 \ + -F turtlebot3 \ + -R tb3_0 +``` + ## Troubleshooting * Looking for the legacy implementation of `free_fleet`? Check out the [`legacy`](https://github.com/open-rmf/free_fleet/tree/legacy) branch. @@ -283,7 +375,9 @@ ros2 run rmf_demos_tasks dispatch_patrol \ * Simulations don't seem to work properly anymore? Try `ros2 deamon stop`, `ros2 daemon start`, or explicitly kill the `ros` and `gazebo` processes, or restart your machine. It's been noticed that if the ROS 2 or gazebo process are not terminated properly (happens rarely), the network traffic between the simulation robots and the fleet adapter get affected. -* Why is RMF not run with `use_sim_time:=true` in the examples? This is because it is on a different `ROS_DOMAIN_ID` than the simulation, therefore it will not have access to the simulation `clock` topic, the examples running RMF, `free_fleet_adapter` and the tasks will not be using sim time. +* ROS 1 Navigation stack simulation does not seem to work as expected? Check out the [integration tests docker compose](.github/docker/integration-tests/nav1-docker-compose.yaml), as well as the [simulation](.github/docker/minimal-ros1-sim/Dockerfile) and [bringup](.github/docker/minimal-nav1-bringup/Dockerfile) docker files, for any missing dependencies. + +* Why does RMF not run with `use_sim_time:=true` in the examples? This is because it is on a different `ROS_DOMAIN_ID` than the simulation, therefore it will not have access to the simulation `clock` topic, the examples running RMF, `free_fleet_adapter` and the tasks will not be using sim time. * 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. @@ -294,7 +388,6 @@ ros2 run rmf_demos_tasks dispatch_patrol \ ## TODOs * attempt to optimize tf messages (not all are needed) -* ROS 1 nav support * custom actions to be abstracted * map switching support * end-to-end testing with Open-RMF diff --git a/free_fleet/free_fleet/convert.py b/free_fleet/free_fleet/convert.py index 3a8d2500..61fa3906 100644 --- a/free_fleet/free_fleet/convert.py +++ b/free_fleet/free_fleet/convert.py @@ -16,12 +16,21 @@ from builtin_interfaces.msg import Time as RclTimeMsg -from free_fleet.types import GeometryMsgs_TransformStamped, Time +from free_fleet.ros1_types import ( + Time as Ros1Time, + TransformStamped as Ros1TransformStamped +) +from free_fleet.ros2_types import ( + GeometryMsgs_TransformStamped as Ros2TransformStamped, + Time as Ros2Time +) from geometry_msgs.msg import TransformStamped from rclpy import time as rclpyTime -def transform_time_to_ros2_msg(msg: Time) -> RclTimeMsg: +def transform_time_to_ros2_msg( + msg: Ros1Time.msg_type | Ros2Time +) -> RclTimeMsg: time = rclpyTime.Time( seconds=msg.sec, nanoseconds=msg.nanosec @@ -29,8 +38,9 @@ def transform_time_to_ros2_msg(msg: Time) -> RclTimeMsg: return time.to_msg() -def transform_stamped_to_ros2_msg(msg: GeometryMsgs_TransformStamped - ) -> TransformStamped: +def transform_stamped_to_ros2_msg( + msg: Ros1TransformStamped.msg_type | Ros2TransformStamped +) -> TransformStamped: t = TransformStamped() t.header.stamp = transform_time_to_ros2_msg(msg.header.stamp) t.header.frame_id = msg.header.frame_id diff --git a/free_fleet/free_fleet/ros1_types.py b/free_fleet/free_fleet/ros1_types.py new file mode 100644 index 00000000..166f0086 --- /dev/null +++ b/free_fleet/free_fleet/ros1_types.py @@ -0,0 +1,99 @@ +#!/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 rosbags.typesys import get_types_from_msg, get_typestore, Stores + + +ROS1_STORE = get_typestore(Stores.ROS1_NOETIC) + + +# https://github.com/ros/geometry2/blob/noetic-devel/tf2_msgs/msg/TFMessage.msg +TFMessage_Definition = """ +geometry_msgs/TransformStamped[] transforms +""" +ROS1_STORE.register( + get_types_from_msg(TFMessage_Definition, 'tf2_msgs/msg/TFMessage') +) + + +class TFMessage: + type_name = 'tf2_msgs/msg/TFMessage' + msg_type = ROS1_STORE.types[type_name] + + +class Transform: + type_name = 'geometry_msgs/msg/Transform' + msg_type = ROS1_STORE.types[type_name] + + +class TransformStamped: + type_name = 'geometry_msgs/msg/TransformStamped' + msg_type = ROS1_STORE.types[type_name] + + +class Vector3: + type_name = 'geometry_msgs/msg/Vector3' + msg_type = ROS1_STORE.types[type_name] + + +class Time: + type_name = 'builtin_interfaces/msg/Time' + msg_type = ROS1_STORE.types[type_name] + + +class Header: + type_name = 'std_msgs/msg/Header' + msg_type = ROS1_STORE.types[type_name] + + +class Pose: + type_name = 'geometry_msgs/msg/Pose' + msg_type = ROS1_STORE.types[type_name] + + +class Point: + type_name = 'geometry_msgs/msg/Point' + msg_type = ROS1_STORE.types[type_name] + + +class Quaternion: + type_name = 'geometry_msgs/msg/Quaternion' + msg_type = ROS1_STORE.types[type_name] + + +class PoseStamped: + type_name = 'geometry_msgs/msg/PoseStamped' + msg_type = ROS1_STORE.types[type_name] + + +class GoalID: + type_name = 'actionlib_msgs/msg/GoalID' + msg_type = ROS1_STORE.types[type_name] + + +class GoalStatus: + type_name = 'actionlib_msgs/msg/GoalStatus' + msg_type = ROS1_STORE.types[type_name] + + +class GoalStatusArray: + type_name = 'actionlib_msgs/msg/GoalStatusArray' + msg_type = ROS1_STORE.types[type_name] + + +class BatteryState: + type_name = 'sensor_msgs/msg/BatteryState' + msg_type = ROS1_STORE.types[type_name] diff --git a/free_fleet/free_fleet/types.py b/free_fleet/free_fleet/ros2_types.py similarity index 100% rename from free_fleet/free_fleet/types.py rename to free_fleet/free_fleet/ros2_types.py diff --git a/free_fleet/free_fleet/utils.py b/free_fleet/free_fleet/utils.py index 8538140a..29675f7c 100644 --- a/free_fleet/free_fleet/utils.py +++ b/free_fleet/free_fleet/utils.py @@ -14,7 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from free_fleet.types import ( +from free_fleet.ros2_types import ( ActionMsgs_CancelGoal_Request, ActionMsgs_GoalInfo, Time, @@ -33,7 +33,7 @@ def namespacify(base_name: str, namespace: str, delimiter: str = '/') -> str: else base_name -def make_cancel_all_goals_request() -> ActionMsgs_CancelGoal_Request: +def make_nav2_cancel_all_goals_request() -> ActionMsgs_CancelGoal_Request: """ Return a Nav2 CancelGoal request targeting all ongoing goals. @@ -46,3 +46,19 @@ def make_cancel_all_goals_request() -> ActionMsgs_CancelGoal_Request: Time(sec=0, nanosec=0) ) ) + + +# https://github.com/eclipse-zenoh/zenoh-plugin-ros1/issues/131 +def get_zenoh_name_of_ros1_topic( + ros1_store, + topic: str, + msg_type: str +) -> str: + # Get md5 and encode msg_type to construct zenoh topic + msg_type_split = msg_type.split('/') + msg_type_encoded = \ + '/'.join([msg_type_split[0], msg_type_split[2]]).encode('utf-8').hex() + md5 = ros1_store.generate_msgdef(msg_type)[1] + zenoh_topic = '/'.join([msg_type_encoded, md5, topic[1:]]) + + return zenoh_topic diff --git a/free_fleet/tests/test_convert.py b/free_fleet/tests/test_convert.py index 33b842a4..995d9d3e 100644 --- a/free_fleet/tests/test_convert.py +++ b/free_fleet/tests/test_convert.py @@ -18,36 +18,81 @@ transform_stamped_to_ros2_msg, transform_time_to_ros2_msg ) -from free_fleet.types import ( - GeometryMsgs_Quaternion, - GeometryMsgs_Transform, - GeometryMsgs_TransformStamped, - GeometryMsgs_Vector3, - Header, - Time +from free_fleet.ros1_types import ( + Header as Nav1Header, + Quaternion as Nav1Quaternion, + Time as Nav1Time, + Transform as Nav1Transform, + TransformStamped as Nav1TransformStamped, + Vector3 as Nav1Vector3 ) +from free_fleet.ros2_types import ( + GeometryMsgs_Quaternion as Nav2Quaternion, + GeometryMsgs_Transform as Nav2Transform, + GeometryMsgs_TransformStamped as Nav2TransformStamped, + GeometryMsgs_Vector3 as Nav2Vector3, + Header as Nav2Header, + Time as Nav2Time +) + + +def test_nav2_transform_time_to_ros2_msg(): + time = Nav2Time(sec=123, nanosec=456) + converted = transform_time_to_ros2_msg(time) + assert converted.sec == time.sec + assert converted.nanosec == time.nanosec + + +def test_nav2_transform_stamped_to_ros2_msg(): + frame_id = 'test_header_frame_id' + child_frame_id = 'test_child_frame_id' + + msg = Nav2TransformStamped( + header=Nav2Header( + stamp=Nav2Time(sec=123, nanosec=456), + frame_id=frame_id, + ), + child_frame_id=child_frame_id, + transform=Nav2Transform( + translation=Nav2Vector3(x=1, y=2, z=3), + rotation=Nav2Quaternion(), + ) + ) + converted = transform_stamped_to_ros2_msg(msg) + assert converted.header.stamp.sec == 123 + assert converted.header.stamp.nanosec == 456 + assert converted.header.frame_id == frame_id + assert converted.child_frame_id == child_frame_id + assert abs(converted.transform.translation.x - 1) <= 1e-6 + assert abs(converted.transform.translation.y - 2) <= 1e-6 + assert abs(converted.transform.translation.z - 3) <= 1e-6 + assert abs(converted.transform.rotation.x - 0) <= 1e-6 + assert abs(converted.transform.rotation.y - 0) <= 1e-6 + assert abs(converted.transform.rotation.z - 0) <= 1e-6 + assert abs(converted.transform.rotation.w - 1) <= 1e-6 -def test_transform_time_to_ros2_msg(): - time = Time(sec=123, nanosec=456) +def test_nav1_transform_time_to_ros2_msg(): + time = Nav1Time.msg_type(sec=123, nanosec=456) converted = transform_time_to_ros2_msg(time) assert converted.sec == time.sec assert converted.nanosec == time.nanosec -def test_transform_stamped_to_ros2_msg(): +def test_nav1_transform_stamped_to_ros2_msg(): frame_id = 'test_header_frame_id' child_frame_id = 'test_child_frame_id' - msg = GeometryMsgs_TransformStamped( - header=Header( - stamp=Time(sec=123, nanosec=456), + msg = Nav1TransformStamped.msg_type( + header=Nav1Header.msg_type( + seq=0, + stamp=Nav1Time.msg_type(sec=123, nanosec=456), frame_id=frame_id, ), child_frame_id=child_frame_id, - transform=GeometryMsgs_Transform( - translation=GeometryMsgs_Vector3(x=1, y=2, z=3), - rotation=GeometryMsgs_Quaternion(), + transform=Nav1Transform.msg_type( + translation=Nav1Vector3.msg_type(x=1, y=2, z=3), + rotation=Nav1Quaternion.msg_type(x=0, y=0, z=0, w=1), ) ) converted = transform_stamped_to_ros2_msg(msg) diff --git a/free_fleet/tests/test_utils.py b/free_fleet/tests/test_utils.py index 4cd170a0..e3b0f05c 100644 --- a/free_fleet/tests/test_utils.py +++ b/free_fleet/tests/test_utils.py @@ -14,7 +14,12 @@ # See the License for the specific language governing permissions and # limitations under the License. -from free_fleet.utils import make_cancel_all_goals_request, namespacify +from free_fleet.ros1_types import ROS1_STORE, TFMessage +from free_fleet.utils import ( + get_zenoh_name_of_ros1_topic, + make_nav2_cancel_all_goals_request, + namespacify +) def test_namespacify(): @@ -23,8 +28,21 @@ def test_namespacify(): assert namespacify('base_name', 'namespace', '!') == 'namespace!base_name' -def test_make_cancel_all_goals_request(): - request = make_cancel_all_goals_request() +def test_make_nav2_cancel_all_goals_request(): + request = make_nav2_cancel_all_goals_request() assert request.goal_info.goal_id.uuid == [0 for i in range(16)] assert request.goal_info.stamp.sec == 0 assert request.goal_info.stamp.nanosec == 0 + + +def test_get_zenoh_name_of_ros1_topic(): + test_topic = '/test/topic' + zenoh_topic = get_zenoh_name_of_ros1_topic( + ROS1_STORE, + test_topic, + TFMessage.type_name + ) + assert zenoh_topic.endswith(test_topic) + + topic_parts = zenoh_topic.split('/') + assert len(topic_parts) == 4 diff --git a/free_fleet_adapter/CMakeLists.txt b/free_fleet_adapter/CMakeLists.txt index 414011c5..314c8b3d 100644 --- a/free_fleet_adapter/CMakeLists.txt +++ b/free_fleet_adapter/CMakeLists.txt @@ -8,7 +8,9 @@ ament_python_install_package(${PROJECT_NAME}) install(PROGRAMS ${PROJECT_NAME}/fleet_adapter.py + ${PROJECT_NAME}/nav1_robot_adapter.py ${PROJECT_NAME}/nav2_robot_adapter.py + ${PROJECT_NAME}/robot_adapter.py DESTINATION lib/${PROJECT_NAME} ) @@ -33,7 +35,7 @@ if(BUILD_TESTING) ) endforeach() - if (INTEGRATION_TESTING) + if (NAV2_INTEGRATION_TESTING) set(_pytest_tests tests/integration/test_nav2_tf_handler.py tests/integration/test_nav2_robot_adapter.py @@ -46,6 +48,22 @@ if(BUILD_TESTING) WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} ) endforeach() + elseif (NAV1_INTEGRATION_TESTING) + # test_nav1_move_base_handler and test_nav1_robot_adapter will affect one + # another, therefore only one can be run during integration testing + set(_pytest_tests + # tests/integration/test_nav1_move_base_handler.py + tests/integration/test_nav1_tf_handler.py + tests/integration/test_nav1_robot_adapter.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() diff --git a/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py b/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py index ed178d73..b6766ed2 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.nav1_robot_adapter import Nav1RobotAdapter from free_fleet_adapter.nav2_robot_adapter import Nav2RobotAdapter import nudged import rclpy @@ -56,38 +57,20 @@ def compute_transforms(level, coords, node=None): # ------------------------------------------------------------------------------ -# Main +# Fleet adapter # ------------------------------------------------------------------------------ -def main(argv=sys.argv): - # Init rclpy and adapter - rclpy.init(args=argv) - rmf_adapter.init_rclcpp() - args_without_ros = rclpy.utilities.remove_ros_args(argv) - - parser = argparse.ArgumentParser( - prog='fleet_adapter', - description='Configure and spin up the fleet adapter') - parser.add_argument('-c', '--config_file', type=str, required=True, - help='Path to the config.yaml file') - parser.add_argument('-n', '--nav_graph', type=str, required=True, - help='Path to the nav_graph for this fleet adapter') - parser.add_argument('-s', '--server_uri', type=str, required=False, - default='', - help='URI of the api server to transmit state and ' - 'task information.') - parser.add_argument('-sim', '--use_sim_time', action='store_true', - help='Use sim time, default: false') - parser.add_argument( - '--zenoh-config', - type=str, - help='Path to custom zenoh configuration file to be used, if not ' - 'provided the default config will be used' - ) - args = parser.parse_args(args_without_ros[1:]) +# TODO(ac): End-to-end testing with fleet adapter. +def start_fleet_adapter( + config_path: str, + nav_graph_path: str, + zenoh_config_path: str | None, + server_uri: str | None, + use_sim_time: bool +): print('Starting fleet adapter...') - config_path = args.config_file - nav_graph_path = args.nav_graph + # Init adapter + rmf_adapter.init_rclcpp() fleet_config = rmf_easy.FleetConfiguration.from_config_files( config_path, nav_graph_path @@ -108,7 +91,7 @@ def main(argv=sys.argv): ) # Enable sim time for testing offline - if args.use_sim_time: + if use_sim_time: param = Parameter('use_sim_time', Parameter.Type.BOOL, True) node.set_parameters([param]) adapter.node.use_sim_time() @@ -116,11 +99,6 @@ def main(argv=sys.argv): adapter.start() time.sleep(1.0) - if args.server_uri == '': - server_uri = None - else: - server_uri = args.server_uri - fleet_config.server_uri = server_uri # Configure the transforms between robot and RMF frames @@ -134,8 +112,8 @@ def main(argv=sys.argv): please verify that the fleet config is valid.' # Initialize zenoh - zenoh_config = zenoh.Config.from_file(args.zenoh_config) \ - if args.zenoh_config is not None else zenoh.Config() + zenoh_config = zenoh.Config.from_file(zenoh_config_path) \ + if zenoh_config_path is not None else zenoh.Config() zenoh_session = zenoh.open(zenoh_config) # Set up tf2 buffer @@ -156,9 +134,39 @@ def main(argv=sys.argv): for robot_name in fleet_config.known_robots: robot_config_yaml = config_yaml['rmf_fleet']['robots'][robot_name] robot_config = fleet_config.get_known_robot_configuration(robot_name) - robots[robot_name] = Nav2RobotAdapter( - robot_name, robot_config, robot_config_yaml, node, zenoh_session, - fleet_handle, tf_buffer) + + if 'navigation_stack' not in robot_config_yaml: + error_message = \ + 'Navigation stack must be defined to set up RobotAdapter' + node.get_logger().error(error_message) + raise RuntimeError(error_message) + + nav_stack = robot_config_yaml['navigation_stack'] + if nav_stack == 2: + robots[robot_name] = Nav2RobotAdapter( + robot_name, + robot_config, + robot_config_yaml, + node, + zenoh_session, + fleet_handle, + tf_buffer + ) + elif nav_stack == 1: + robots[robot_name] = Nav1RobotAdapter( + robot_name, + robot_config, + robot_config_yaml, + node, + zenoh_session, + fleet_handle, + tf_buffer + ) + else: + error_message = \ + 'Navigation stack can only be 1 (experimental) or 2' + node.get_logger().error(error_message) + raise RuntimeError(error_message) update_period = 1.0/config_yaml['rmf_fleet'].get( 'robot_state_update_frequency', 10.0 @@ -195,7 +203,6 @@ def update_loop(): # Shutdown node.destroy_node() rclpy_executor.shutdown() - rclpy.shutdown() zenoh_session.close() @@ -211,7 +218,7 @@ def run_in_parallel(*args, **kwargs): @parallel -def update_robot(robot: Nav2RobotAdapter): +def update_robot(robot: Nav1RobotAdapter | Nav2RobotAdapter): robot_pose = robot.get_pose() if robot_pose is None: robot.node.get_logger().info(f'Failed to pose of robot [{robot.name}]') @@ -243,5 +250,46 @@ def update_robot(robot: Nav2RobotAdapter): robot.update(state) +# ------------------------------------------------------------------------------ +# Main +# ------------------------------------------------------------------------------ +def main(argv=sys.argv): + # Init rclpy + rclpy.init(args=argv) + args_without_ros = rclpy.utilities.remove_ros_args(argv) + + parser = argparse.ArgumentParser( + prog='fleet_adapter', + description='Configure and spin up the fleet adapter') + parser.add_argument('-c', '--config_file', type=str, required=True, + help='Path to the config.yaml file') + parser.add_argument('-n', '--nav_graph', type=str, required=True, + help='Path to the nav_graph for this fleet adapter') + parser.add_argument('-s', '--server_uri', type=str, required=False, + default='', + help='URI of the api server to transmit state and ' + 'task information.') + parser.add_argument('-sim', '--use_sim_time', action='store_true', + help='Use sim time, default: false') + parser.add_argument( + '--zenoh-config', + type=str, + help='Path to custom zenoh configuration file to be used, if not ' + 'provided the default config will be used' + ) + args = parser.parse_args(args_without_ros[1:]) + + start_fleet_adapter( + config_path=args.config_file, + nav_graph_path=args.nav_graph, + zenoh_config_path=args.zenoh_config + if args.zenoh_config != '' else None, + server_uri=args.server_uri if args.server_uri != '' else None, + use_sim_time=args.use_sim_time + ) + + rclpy.shutdown() + + if __name__ == '__main__': main(sys.argv) diff --git a/free_fleet_adapter/free_fleet_adapter/nav1_robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/nav1_robot_adapter.py new file mode 100644 index 00000000..3b04f06e --- /dev/null +++ b/free_fleet_adapter/free_fleet_adapter/nav1_robot_adapter.py @@ -0,0 +1,554 @@ +#!/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 typing import Annotated + +from free_fleet.convert import transform_stamped_to_ros2_msg +from free_fleet.ros1_types import ( + BatteryState, + GoalID, + GoalStatus, + GoalStatusArray, + Header, + Point, + Pose, + PoseStamped, + Quaternion, + ROS1_STORE, + TFMessage, + Time, +) +from free_fleet.utils import ( + get_zenoh_name_of_ros1_topic, + 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 rmf_adapter.robot_update_handle import ActivityIdentifier +from tf_transformations import euler_from_quaternion, quaternion_from_euler + +import zenoh + + +class Nav1TfHandler: + + def __init__(self, robot_name, zenoh_session, tf_buffer, node): + 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 = ROS1_STORE.deserialize_ros1( + sample.payload.to_bytes(), + TFMessage.type_name + ) + 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.robot_name) + t.child_frame_id = namespacify(zt.child_frame_id, + self.robot_name) + self.tf_buffer.set_transform( + t, f'{self.robot_name}_TfListener') + + zenoh_topic = get_zenoh_name_of_ros1_topic( + ROS1_STORE, + topic=f"/{namespacify('tf', self.robot_name)}", + msg_type=TFMessage.type_name + ) + self.tf_sub = self.zenoh_session.declare_subscriber( + zenoh_topic, + _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: + self.node.get_logger().info( + 'Unable to get transform between base_footprint and map: ' + f'{type(err)}: {err}' + ) + return None + + +class Nav1MoveBaseHandler: + + def __init__(self, robot_name, zenoh_session, node): + self.robot_name = robot_name + self.zenoh_session = zenoh_session + self.node = node + + self.goal_status_array = None + self.active_goal_status = None + self.previous_goal_status = None + self._tmp_active_goal_id = None + + def _move_base_status_callback(sample: zenoh.Sample): + try: + goal_status_array = ROS1_STORE.deserialize_ros1( + sample.payload.to_bytes(), + GoalStatusArray.type_name + ) + self.goal_status_array = goal_status_array + except Exception as e: + self.node.get_logger().debug( + f'Failed to deserialize GoalStatusArray payload: ' + f'{type(e)}: {e}' + ) + return + + active_goal_status = None + for goal_status in goal_status_array.status_list: + if goal_status.status == goal_status.ACTIVE: + active_goal_status = goal_status + else: + self.previous_goal_status = goal_status + self.active_goal_status = active_goal_status + + self.move_base_status_zenoh_topic = get_zenoh_name_of_ros1_topic( + ROS1_STORE, + topic=f"/{namespacify('move_base/status', self.robot_name)}", + msg_type=GoalStatusArray.type_name + ) + self.move_base_status_sub = self.zenoh_session.declare_subscriber( + self.move_base_status_zenoh_topic, + _move_base_status_callback + ) + + self.move_base_simple_goal_zenoh_topic = get_zenoh_name_of_ros1_topic( + ROS1_STORE, + topic=f"/{namespacify('move_base_simple/goal', self.robot_name)}", + msg_type=PoseStamped.type_name + ) + self.move_base_simple_goal_pub = self.zenoh_session.declare_publisher( + self.move_base_simple_goal_zenoh_topic + ) + + self.move_base_cancel_goal_zenoh_topic = get_zenoh_name_of_ros1_topic( + ROS1_STORE, + topic=f"/{namespacify('move_base/cancel', self.robot_name)}", + msg_type=GoalID.type_name + ) + self.move_base_cancel_goal_pub = self.zenoh_session.declare_publisher( + self.move_base_cancel_goal_zenoh_topic + ) + + def get_goal_status_array(self) -> GoalStatusArray.msg_type | None: + return self.goal_status_array + + def get_goal_status(self, goal_id: str) -> GoalStatus.msg_type | None: + if self.active_goal_status is not None and \ + self.active_goal_status.goal_id.id == goal_id: + return self.active_goal_status + + if self.previous_goal_status is not None and \ + self.previous_goal_status.goal_id.id == goal_id: + return self.previous_goal_status + + return None + + def get_active_goal_status(self) -> GoalStatus.msg_type | None: + return self.active_goal_status + + def navigate_to_pose( + self, + x: float, + y: float, + z: float, + yaw: float, + timeout_sec: float = 3 + ) -> str | None: + # Keep track of last status stamp to find the new goal status and ID + if self.goal_status_array is None: + self.node.get_logger().error( + 'move_base/status has not yet been received over zenoh topic ' + f'[{self.move_base_status_zenoh_topic}], unable to publish ' + 'navigate_to_pose.' + ) + return None + last_status_stamp = self.goal_status_array.header.stamp + + time_now = self.node.get_clock().now().seconds_nanoseconds() + stamp = Time.msg_type(sec=time_now[0], nanosec=time_now[1]) + header = Header.msg_type(seq=0, stamp=stamp, frame_id='map') + position = Point.msg_type(x=x, y=y, z=z) + quat = quaternion_from_euler(0, 0, yaw) + orientation = Quaternion.msg_type( + x=quat[0], + y=quat[1], + z=quat[2], + w=quat[3] + ) + pose = Pose.msg_type(position=position, orientation=orientation) + pose_stamped = PoseStamped.msg_type(header=header, pose=pose) + + serialized_msg = ROS1_STORE.serialize_ros1( + pose_stamped, + PoseStamped.msg_type.__msgtype__ + ) + self.move_base_simple_goal_pub.put(serialized_msg.tobytes()) + self.node.get_logger().info( + 'Sending move_base_simple/goal, over zenoh topic ' + f'[{self.move_base_simple_goal_zenoh_topic}], ' + f'msg: [{pose_stamped}]' + ) + + self._tmp_active_goal_id = None + + def _get_goal_id_callback(sample: zenoh.Sample): + try: + goal_status_array = ROS1_STORE.deserialize_ros1( + sample.payload.to_bytes(), + GoalStatusArray.type_name + ) + self.goal_status_array = goal_status_array + except Exception as e: + self.node.get_logger().debug( + f'Failed to deserialize GoalStatusArray payload: ' + f'{type(e)}: {e}' + ) + return None + + for goal_status in goal_status_array.status_list: + goal_status_stamp = goal_status.goal_id.stamp + if goal_status.status == goal_status.ACTIVE and \ + goal_status_stamp.sec >= last_status_stamp.sec and \ + goal_status_stamp.nanosec >= last_status_stamp.nanosec: + self._tmp_active_goal_id = goal_status.goal_id.id + + move_base_status_sub = self.zenoh_session.declare_subscriber( + self.move_base_status_zenoh_topic, + _get_goal_id_callback + ) + + # Timeout to get move_base goal ID + sleep_time = timeout_sec / 10 + for i in range(10): + if self._tmp_active_goal_id is not None: + break + time.sleep(sleep_time) + + move_base_status_sub.undeclare() + return self._tmp_active_goal_id + + def stop_current_navigation(self): + if self.active_goal_status is None: + return + + goal_id = self.active_goal_status.goal_id + serialized_msg = ROS1_STORE.serialize_ros1( + goal_id, + GoalID.msg_type.__msgtype__ + ) + self.move_base_cancel_goal_pub.put(serialized_msg.tobytes()) + self.node.get_logger().info( + 'Sending move_base/cancel, over zenoh topic ' + f'[{self.move_base_cancel_goal_zenoh_topic}], ' + f'msg: [{goal_id}]' + ) + + def cancel_navigation(self, goal_id_str: str): + time_now = self.node.get_clock().now().seconds_nanoseconds() + stamp = Time.msg_type(sec=time_now[0], nanosec=time_now[1]) + goal_id = GoalID.msg_type(stamp=stamp, id=goal_id_str) + serialized_msg = ROS1_STORE.serialize_ros1( + goal_id, + GoalID.msg_type.__msgtype__ + ) + self.move_base_cancel_goal_pub.put(serialized_msg.tobytes()) + self.node.get_logger().info( + 'Sending move_base/cancel, over zenoh topic ' + f'[{self.move_base_cancel_goal_zenoh_topic}], ' + f'msg: [{goal_id}]' + ) + + +class Nav1RobotAdapter(RobotAdapter): + + def __init__( + self, + name: str, + configuration, + robot_config_yaml, + node, + zenoh_session, + fleet_handle, + tf_buffer + ): + RobotAdapter.__init__(self, name, node, fleet_handle) + + self.execution = None + self.configuration = configuration + self.robot_config_yaml = robot_config_yaml + self.zenoh_session = zenoh_session + self.tf_buffer = tf_buffer + + self.map_name = self.robot_config_yaml['initial_map'] + + # TODO(ac): Only use full battery if sim is indicated + self.battery_soc = 1.0 + + self.replan_counts = 0 + + self.tf_handler = Nav1TfHandler( + self.name, + self.zenoh_session, + self.tf_buffer, + self.node + ) + self.move_base_handler = Nav1MoveBaseHandler( + self.name, + self.zenoh_session, + self.node + ) + self.nav_goal_id = None + + def _battery_state_callback(sample: zenoh.Sample): + try: + battery_state = ROS1_STORE.deserialize_ros1( + sample.payload.to_bytes(), + BatteryState.type_name + ) + except Exception as e: + self.node.get_logger().debug( + 'Failed to deserialize BatteryState payload: ' + f'{type(e)}: {e}' + ) + return + self.battery_soc = battery_state.percentage + + battery_state_zenoh_topic = get_zenoh_name_of_ros1_topic( + ROS1_STORE, + topic=f"/{namespacify('battery_state', self.name)}", + msg_type=BatteryState.type_name + ) + self.battery_state_sub = self.zenoh_session.declare_subscriber( + battery_state_zenoh_topic, + _battery_state_callback + ) + + def get_battery_soc(self) -> float: + return self.battery_soc + + def get_map_name(self) -> str: + return self.map_name + + def get_pose(self) -> Annotated[list[float], 3] | None: + transform = self.tf_handler.get_transform() + if transform is None: + 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([ + 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 _is_navigation_done(self) -> bool: + if self.nav_goal_id is None: + return True + + nav_goal_id = self.nav_goal_id + goal_status = \ + self.move_base_handler.get_goal_status(nav_goal_id) + if goal_status is None: + self.replan_counts += 1 + self.node.get_logger().error( + f'Navigation goal {nav_goal_id} is not found, replan count ' + f'[{self.replan_counts}]' + ) + self.update_handle.more().replan() + return False + + match goal_status.status: + case goal_status.PENDING | \ + goal_status.ACTIVE | \ + goal_status.PREEMPTING | \ + goal_status.RECALLING: + return False + case goal_status.SUCCEEDED: + self.node.get_logger().info( + f'Navigation goal {nav_goal_id} reached' + ) + return True + case goal_status.PREEMPTED: + self.node.get_logger().info( + f'Navigation goal {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 {nav_goal_id} status ' + f'{goal_status.status}, replan count ' + f'[{self.replan_counts}]' + ) + self.update_handle.more().replan() + return False + + 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: + if self.nav_goal_id is not None and self._is_navigation_done(): + if self.execution is not None: + self.execution.finished() + self.execution = None + self.nav_goal_id = None + self.replan_counts = 0 + else: + activity_identifier = self.execution.identifier + + self.update_handle.update(state, activity_identifier) + + def _handle_navigate_to_pose( + self, + map_name: str, + x: float, + y: float, + z: float, + yaw: float, + timeout_sec: float = 3.0 + ): + if map_name != self.map_name: + # 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 ' + f'[{self.name}] is on map [{self.map_name}], replan count ' + f'[{self.replan_counts}]' + ) + + self._check_update_handle_initialization() + self.update_handle.more().replan() + return + + nav_goal_id = self.move_base_handler.navigate_to_pose( + x, + y, + z, + yaw, + timeout_sec + ) + + if nav_goal_id is not None: + self.node.get_logger().info( + f'Navigation goal {nav_goal_id} accepted' + ) + self.nav_goal_id = nav_goal_id + return + + self.replan_counts += 1 + self.node.get_logger().error( + 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 + + 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 _handle_stop_navigation(self): + self.move_base_handler.stop_current_navigation() + + def stop(self, activity: ActivityIdentifier): + if self.execution is None: + return + + if self.execution.identifier.is_same(activity): + self.execution = None + # TODO(ac): check what is the type of execution when we start + # supporting something other than navigation + + if self.nav_goal_id is not None: + self._handle_stop_navigation() + # TODO(ac): check return code before setting nav_goal_id to + # None + self.nav_goal_id = None + + 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 + # We should never reach this point after initialization. + error_message = \ + f'Execute action [{category}] is unsupported, this might be a ' \ + 'configuration error.' + self.node.get_logger().error(error_message) + raise RuntimeError(error_message) 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 dd5602d4..9b097aae 100644 --- a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py @@ -17,7 +17,7 @@ from typing import Annotated from free_fleet.convert import transform_stamped_to_ros2_msg -from free_fleet.types import ( +from free_fleet.ros2_types import ( ActionMsgs_CancelGoal_Response, GeometryMsgs_Point, GeometryMsgs_Pose, @@ -34,9 +34,10 @@ Time, ) from free_fleet.utils import ( - make_cancel_all_goals_request, + make_nav2_cancel_all_goals_request, namespacify, ) +from free_fleet_adapter.robot_adapter import RobotAdapter from geometry_msgs.msg import TransformStamped import numpy as np @@ -95,7 +96,7 @@ def get_transform(self) -> TransformStamped | None: return None -class Nav2RobotAdapter: +class Nav2RobotAdapter(RobotAdapter): def __init__( self, @@ -107,10 +108,7 @@ def __init__( fleet_handle, tf_buffer ): - self.name = name - self.node = node - self.fleet_handle = fleet_handle - self.update_handle = None + RobotAdapter.__init__(self, name, node, fleet_handle) self.execution = None self.configuration = configuration @@ -346,7 +344,7 @@ def navigate( ) def _handle_stop_navigation(self): - req = make_cancel_all_goals_request() + req = make_nav2_cancel_all_goals_request() replies = self.zenoh_session.get( namespacify( 'navigate_to_pose/_action/cancel_goal', 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 00000000..91a34261 --- /dev/null +++ b/free_fleet_adapter/free_fleet_adapter/robot_adapter.py @@ -0,0 +1,100 @@ +#!/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): + """Abstract Robot Adapter to be used by the free fleet adapter.""" + + def __init__( + self, + name: str, + node, + fleet_handle + ): + self.name = name + self.node = node + self.fleet_handle = fleet_handle + self.update_handle = None + + """ + This method returns the battery state of charge as a float, with value + between 0 and 1.0. + """ + @abstractmethod + def get_battery_soc(self) -> float: + ... + + """ + This method returns the name of the current map that the robot is + localized on. + """ + @abstractmethod + def get_map_name(self) -> str: + ... + + """ + 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 get_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: ActivityIdentifier): + ... + + """ + This method is called to send a custom action command to the robot. + """ + @abstractmethod + def execute_action( + self, + category: str, + description: dict, + execution: ActivityIdentifier + ): + ... diff --git a/free_fleet_adapter/tests/integration/test_nav1_move_base_handler.py b/free_fleet_adapter/tests/integration/test_nav1_move_base_handler.py new file mode 100644 index 00000000..ad18616b --- /dev/null +++ b/free_fleet_adapter/tests/integration/test_nav1_move_base_handler.py @@ -0,0 +1,106 @@ +#!/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 +import unittest + +from free_fleet_adapter.nav1_robot_adapter import Nav1MoveBaseHandler +import rclpy + +import zenoh + + +class TestNav1MoveBaseHandler(unittest.TestCase): + + @classmethod + def setUpClass(cls): + rclpy.init() + cls.node = rclpy.create_node('test_nav1_move_base_handler') + cls.zenoh_session = zenoh.open(zenoh.Config()) + + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + cls.zenoh_session.close() + rclpy.shutdown() + + def test_move_base_statuses_do_not_exist(self): + move_base_handler = Nav1MoveBaseHandler( + 'missing_turtlebot3_1', self.zenoh_session, self.node + ) + + statuses_exists = False + for i in range(10): + goal_status_array = move_base_handler.get_goal_status_array() + if goal_status_array is not None: + statuses_exists = True + break + time.sleep(1) + + assert not statuses_exists + + def test_command_status_and_cancel(self): + move_base_handler = Nav1MoveBaseHandler( + 'tb3_0', self.zenoh_session, self.node + ) + + status_array = None + for i in range(10): + status_array = move_base_handler.get_goal_status_array() + if status_array is not None: + break + time.sleep(1) + assert status_array is not None + assert len(status_array.status_list) == 0 + + active_goal_status = move_base_handler.get_active_goal_status() + assert active_goal_status is None + + # Longer timeout during testing + goal_id = move_base_handler.navigate_to_pose( + x=-1.6, + y=0.5, + z=0.0, + yaw=0.0, + timeout_sec=5.0 + ) + assert goal_id is not None + + status_array = None + for i in range(10): + status_array = move_base_handler.get_goal_status_array() + if len(status_array.status_list) == 1 \ + and status_array.status_list[0].status == \ + status_array.status_list[0].ACTIVE: + break + time.sleep(1) + assert len(status_array.status_list) == 1 + + assert status_array.status_list[0].status == \ + status_array.status_list[0].ACTIVE + + active_goal_status = move_base_handler.get_active_goal_status() + assert active_goal_status is not None + assert status_array.status_list[0] == active_goal_status + assert active_goal_status.goal_id.id == goal_id + + move_base_handler.stop_navigation() + for i in range(10): + active_goal_status = move_base_handler.get_active_goal_status() + if active_goal_status is None: + break + time.sleep(1) + assert active_goal_status is None diff --git a/free_fleet_adapter/tests/integration/test_nav1_robot_adapter.py b/free_fleet_adapter/tests/integration/test_nav1_robot_adapter.py new file mode 100644 index 00000000..ea346de8 --- /dev/null +++ b/free_fleet_adapter/tests/integration/test_nav1_robot_adapter.py @@ -0,0 +1,356 @@ +#!/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 math +import time +import unittest + +from free_fleet_adapter.nav1_robot_adapter import Nav1RobotAdapter +import rclpy +import rmf_adapter.easy_full_control as rmf_easy +from tf2_ros import Buffer + +import zenoh + + +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_non_existent_robot_pose(self): + tf_buffer = Buffer() + + robot_adapter = Nav1RobotAdapter( + name='missing_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 + ) + + robot_exists = False + for i in range(10): + transform = robot_adapter.get_pose() + if transform is not None: + robot_exists = True + break + time.sleep(1) + + assert not robot_exists + + def test_robot_pose(self): + tf_buffer = Buffer() + + robot_adapter = Nav1RobotAdapter( + name='tb3_0', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + robot_exists = False + for i in range(10): + transform = robot_adapter.get_pose() + if transform is not None: + robot_exists = True + break + time.sleep(1) + + assert robot_exists + + def test_robot_battery_soc(self): + tf_buffer = Buffer() + + robot_adapter = Nav1RobotAdapter( + name='tb3_0', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + robot_exists = False + for i in range(10): + transform = robot_adapter.get_pose() + if transform is not None: + robot_exists = True + break + time.sleep(1) + + assert robot_exists + + battery_soc = robot_adapter.get_battery_soc() + assert math.isclose(battery_soc, 1.0) + + def test_robot_unable_to_update(self): + tf_buffer = Buffer() + + robot_adapter = Nav1RobotAdapter( + name='tb3_0', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + robot_exists = False + for i in range(10): + transform = robot_adapter.get_pose() + if transform is not None: + robot_exists = True + break + time.sleep(1) + + assert robot_exists + + 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 = Nav1RobotAdapter( + name='tb3_0', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + robot_exists = False + for i in range(10): + transform = robot_adapter.get_pose() + if transform is not None: + robot_exists = True + break + time.sleep(1) + + assert robot_exists + assert robot_adapter._is_navigation_done() + + def test_robot_stop_without_command(self): + tf_buffer = Buffer() + + robot_adapter = Nav1RobotAdapter( + name='tb3_0', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + robot_exists = False + for i in range(10): + transform = robot_adapter.get_pose() + if transform is not None: + robot_exists = True + break + time.sleep(1) + + assert robot_exists + assert robot_adapter.execution is None + robot_adapter.stop(None) + 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 = Nav1RobotAdapter( + name='tb3_0', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + robot_exists = False + for i in range(10): + transform = robot_adapter.get_pose() + if transform is not None: + robot_exists = True + break + time.sleep(1) + + assert robot_exists + + 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_pose(self): + tf_buffer = Buffer() + + robot_adapter = Nav1RobotAdapter( + name='tb3_0', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + robot_exists = False + for i in range(10): + transform = robot_adapter.get_pose() + if transform is not None: + robot_exists = True + break + time.sleep(1) + + assert robot_exists + + robot_adapter._handle_navigate_to_pose( + 'L1', + -1.8, + -0.5, + 0.0, + 0.0, + 5.0 + ) + assert not robot_adapter._is_navigation_done() + time.sleep(5) + assert robot_adapter._is_navigation_done() + + def test_robot_stop_navigate(self): + tf_buffer = Buffer() + + robot_adapter = Nav1RobotAdapter( + name='tb3_0', + configuration=None, + robot_config_yaml={ + 'initial_map': 'L1', + }, + node=self.node, + zenoh_session=self.zenoh_session, + fleet_handle=None, + tf_buffer=tf_buffer + ) + + robot_exists = False + for i in range(10): + transform = robot_adapter.get_pose() + if transform is not None: + robot_exists = True + break + time.sleep(1) + + assert robot_exists + + robot_adapter._handle_navigate_to_pose( + 'L1', + 1.808, + 0.503, + 0.0, + 0.0, + 5.0 + ) + assert robot_adapter.nav_goal_id is not None + 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() + + robot_adapter = Nav1RobotAdapter( + name='tb3_0', + 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_adapter/tests/integration/test_nav1_tf_handler.py b/free_fleet_adapter/tests/integration/test_nav1_tf_handler.py new file mode 100644 index 00000000..7f5b40c6 --- /dev/null +++ b/free_fleet_adapter/tests/integration/test_nav1_tf_handler.py @@ -0,0 +1,73 @@ +#!/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 +import unittest + +from free_fleet_adapter.nav1_robot_adapter import Nav1TfHandler +import rclpy +from tf2_ros import Buffer + +import zenoh + + +class TestNav1TfHandler(unittest.TestCase): + + @classmethod + def setUpClass(cls): + rclpy.init() + cls.node = rclpy.create_node('test_nav1_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 = Nav1TfHandler( + 'missing_turtlebot3_1', self.zenoh_session, tf_buffer, self.node + ) + + 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 not transform_exists + + def test_tf_exists(self): + tf_buffer = Buffer() + + tf_handler = Nav1TfHandler( + 'tb3_0', self.zenoh_session, tf_buffer, self.node + ) + + 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 diff --git a/free_fleet_examples/CMakeLists.txt b/free_fleet_examples/CMakeLists.txt index eb235f39..e2f7acc8 100644 --- a/free_fleet_examples/CMakeLists.txt +++ b/free_fleet_examples/CMakeLists.txt @@ -47,6 +47,9 @@ foreach(path ${traffic_editor_paths}) endforeach() install(PROGRAMS + ${PROJECT_NAME}/nav1_get_tf.py + ${PROJECT_NAME}/nav1_move_base_cancel.py + ${PROJECT_NAME}/nav1_move_base_simple_goal.py ${PROJECT_NAME}/nav2_cancel_all_goals.py ${PROJECT_NAME}/nav2_get_tf.py ${PROJECT_NAME}/nav2_send_navigate_to_pose.py diff --git a/free_fleet_examples/config/fleet/nav1_tb3_simulation_fleet_config.yaml b/free_fleet_examples/config/fleet/nav1_tb3_simulation_fleet_config.yaml new file mode 100644 index 00000000..c63a0050 --- /dev/null +++ b/free_fleet_examples/config/fleet/nav1_tb3_simulation_fleet_config.yaml @@ -0,0 +1,63 @@ +# FLEET CONFIG ================================================================= +# RMF Fleet parameters + +rmf_fleet: + name: "turtlebot3" + limits: + linear: [0.5, 0.75] # velocity, acceleration + angular: [0.6, 2.0] # velocity, acceleration + profile: # Robot profile is modelled as a circle + footprint: 0.3 # radius in m + vicinity: 0.5 # radius in m + reversible: True # whether robots in this fleet can reverse + battery_system: + voltage: 12.0 # V + capacity: 24.0 # Ahr + charging_current: 5.0 # A + mechanical_system: + mass: 20.0 # kg + moment_of_inertia: 10.0 #kgm^2 + friction_coefficient: 0.22 + ambient_system: + power: 20.0 # W + tool_system: + power: 0.0 # W + recharge_threshold: 0.10 # Battery level below which robots in this fleet will not operate + recharge_soc: 1.0 # Battery level to which robots in this fleet should be charged up to during recharging tasks + publish_fleet_state: 10.0 # Publish frequency for fleet state, ensure that it is same as robot_state_update_frequency + account_for_battery_drain: True + task_capabilities: # Specify the types of RMF Tasks that robots in this fleet are capable of performing + loop: True + delivery: True + # actions: ["some_action_here"] + finishing_request: "nothing" # [park, charge, nothing] + responsive_wait: True # Should responsive wait be on/off for the whole fleet by default? False if not specified. + robots: + tb3_0: + charger: "turtlebot3_1_charger" + responsive_wait: False # Should responsive wait be on/off for this specific robot? Overrides the fleet-wide setting. + # For Nav1RobotAdapter + navigation_stack: 1 + initial_map: "L1" + maps: + L1: + map_url: "/opt/ros/jazzy/share/nav2_bringup/maps/turtlebot3_world.yaml" + # initial_pose: [-1.6000019311904907, -0.5031192898750305, 0] + + robot_state_update_frequency: 10.0 # Hz + + +# TRANSFORM CONFIG ============================================================= +# For computing transforms between Robot and RMF coordinate systems + +# Optional +reference_coordinates: + L1: + rmf: [[8.9508, -6.6006], + [7.1006, -9.1508], + [12.3511, -9.2008], + [11.0510, -11.8010]] + robot: [[-1.04555, 2.5456], + [-2.90519, 0.00186], + [2.39611, -0.061773], + [1.08783, -2.59750]] diff --git a/free_fleet_examples/config/fleet/tb3_simulation_fleet_config.yaml b/free_fleet_examples/config/fleet/tb3_simulation_fleet_config.yaml index a0cec1b1..b8bb20ef 100644 --- a/free_fleet_examples/config/fleet/tb3_simulation_fleet_config.yaml +++ b/free_fleet_examples/config/fleet/tb3_simulation_fleet_config.yaml @@ -37,6 +37,7 @@ rmf_fleet: charger: "turtlebot3_1_charger" responsive_wait: False # Should responsive wait be on/off for this specific robot? Overrides the fleet-wide setting. # For Nav2RobotAdapter + navigation_stack: 2 initial_map: "L1" maps: L1: diff --git a/free_fleet_examples/config/zenoh/turtlebot3_1_client_zenoh_config.json5 b/free_fleet_examples/config/zenoh/turtlebot3_1_zenoh_bridge_ros2dds_client_config.json5 similarity index 80% rename from free_fleet_examples/config/zenoh/turtlebot3_1_client_zenoh_config.json5 rename to free_fleet_examples/config/zenoh/turtlebot3_1_zenoh_bridge_ros2dds_client_config.json5 index e07267b1..bad22011 100644 --- a/free_fleet_examples/config/zenoh/turtlebot3_1_client_zenoh_config.json5 +++ b/free_fleet_examples/config/zenoh/turtlebot3_1_zenoh_bridge_ros2dds_client_config.json5 @@ -13,15 +13,12 @@ pub_max_frequencies: [".*/navigate_to_pose/*=1", ".*/battery_state=1"], }, }, + // Zenoh related configurations mode: "client", connect: { - endpoints: [ - // "/:" - ] + endpoints: [] }, listen: { - endpoints: [ - // "/:" - ] + endpoints: [] }, } diff --git a/free_fleet_examples/config/zenoh/turtlebot3_zenoh_bridge_ros1_client_config.json5 b/free_fleet_examples/config/zenoh/turtlebot3_zenoh_bridge_ros1_client_config.json5 new file mode 100644 index 00000000..c36dd7d1 --- /dev/null +++ b/free_fleet_examples/config/zenoh/turtlebot3_zenoh_bridge_ros1_client_config.json5 @@ -0,0 +1,30 @@ +{ + plugins: { + ros1: { + bridge_namespace: "tb3_0", + subscriber_bridging_mode: "disabled", + publisher_bridging_mode: "disabled", + service_bridging_mode: "disabled", + client_bridging_mode: "disabled", + // Zenoh -> ROS 1 + subscriber_topic_custom_bridging_mode: { + "/move_base_simple/goal": "auto", + "/move_base/cancel": "auto", + }, + // ROS 1 -> Zenoh + publisher_topic_custom_bridging_mode: { + "/tf": "auto", + "/battery_state": "auto", + "/move_base/status": "auto" + }, + }, + }, + // Zenoh related configurations + mode: "client", + connect: { + endpoints: [] + }, + listen: { + endpoints: [] + }, +} diff --git a/free_fleet_examples/config/zenoh/unique_multi_tb3_zenoh_config.json5 b/free_fleet_examples/config/zenoh/unique_multi_tb3_zenoh_bridge_ros2dds_client_config.json5 similarity index 79% rename from free_fleet_examples/config/zenoh/unique_multi_tb3_zenoh_config.json5 rename to free_fleet_examples/config/zenoh/unique_multi_tb3_zenoh_bridge_ros2dds_client_config.json5 index 9ed8c87c..7296e427 100644 --- a/free_fleet_examples/config/zenoh/unique_multi_tb3_zenoh_config.json5 +++ b/free_fleet_examples/config/zenoh/unique_multi_tb3_zenoh_bridge_ros2dds_client_config.json5 @@ -12,15 +12,12 @@ pub_max_frequencies: [".*/navigate_to_pose/*=1", ".*/battery_state=1"], }, }, + // Zenoh related configurations mode: "client", connect: { - endpoints: [ - // "/:" - ] + endpoints: [] }, listen: { - endpoints: [ - // "/:" - ] + endpoints: [] }, } diff --git a/free_fleet_examples/free_fleet_examples/nav1_get_tf.py b/free_fleet_examples/free_fleet_examples/nav1_get_tf.py new file mode 100644 index 00000000..2191ff34 --- /dev/null +++ b/free_fleet_examples/free_fleet_examples/nav1_get_tf.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 argparse +import sys +import time + + +from free_fleet_adapter.nav1_robot_adapter import Nav1TfHandler +import rclpy +from tf2_ros import Buffer + +import zenoh + + +def main(argv=sys.argv): + # Init rclpy and adapter + rclpy.init(args=argv) + args_without_ros = rclpy.utilities.remove_ros_args(argv) + node = rclpy.node.Node('nav1_get_tf') + + parser = argparse.ArgumentParser( + prog='nav1_get_tf', + description='Zenoh/ROS1 tf example') + parser.add_argument('--zenoh-config', '-c', dest='config', metavar='FILE', + type=str, help='A configuration file.') + parser.add_argument('--namespace', '-n', type=str, default='') + parser.add_argument( + '-b', '--base-footprint-frame', default='base_footprint' + ) + parser.add_argument('-m', '--map-frame', default='map') + + args = parser.parse_args(args_without_ros[1:]) + + # Create Zenoh Config from file if provoded, or a default one otherwise + conf = zenoh.Config.from_file(args.config) \ + if args.config is not None else zenoh.Config() + + zenoh.try_init_log_from_env() + + tf_buffer = Buffer() + + # Open Zenoh Session + with zenoh.open(conf) as session: + info = session.info + print(f'zid: {info.zid()}') + print(f'routers: {info.routers_zid()}') + print(f'peers: {info.peers_zid()}') + + tf_handler = Nav1TfHandler(args.namespace, session, tf_buffer, node) + + try: + while True: + transform = tf_handler.get_transform() + if transform is None: + print('Unable to get transform between base_footprint and' + ' map') + else: + print(transform) + time.sleep(1) + except (KeyboardInterrupt): + pass + finally: + session.close() + + +if __name__ == '__main__': + main(sys.argv) diff --git a/free_fleet_examples/free_fleet_examples/nav1_move_base_cancel.py b/free_fleet_examples/free_fleet_examples/nav1_move_base_cancel.py new file mode 100644 index 00000000..321a846c --- /dev/null +++ b/free_fleet_examples/free_fleet_examples/nav1_move_base_cancel.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 argparse +import sys + +from free_fleet_adapter.nav1_robot_adapter import Nav1MoveBaseHandler +import rclpy + +import zenoh + + +def main(argv=sys.argv): + # Init rclpy and adapter + rclpy.init(args=argv) + args_without_ros = rclpy.utilities.remove_ros_args(argv) + node = rclpy.node.Node('nav1_move_base_cancel') + + parser = argparse.ArgumentParser( + prog='nav1_move_base_cancel', + description='Zenoh/ROS1 move_base cancel example') + parser.add_argument('--zenoh-config', '-c', dest='config', metavar='FILE', + type=str, help='A configuration file.') + parser.add_argument('--namespace', '-n', type=str, default='') + parser.add_argument('--goal-id', '-g', type=str) + + args = parser.parse_args(args_without_ros[1:]) + + # Create Zenoh Config from file if provoded, or a default one otherwise + conf = zenoh.Config.from_file(args.config) \ + if args.config is not None else zenoh.Config() + + zenoh.try_init_log_from_env() + + # Open Zenoh Session + with zenoh.open(conf) as session: + info = session.info + print(f'zid: {info.zid()}') + print(f'routers: {info.routers_zid()}') + print(f'peers: {info.peers_zid()}') + + move_base_handler = Nav1MoveBaseHandler(args.namespace, session, node) + move_base_handler.cancel_navigation( + args.goal_id + ) + + +if __name__ == '__main__': + main(sys.argv) diff --git a/free_fleet_examples/free_fleet_examples/nav1_move_base_simple_goal.py b/free_fleet_examples/free_fleet_examples/nav1_move_base_simple_goal.py new file mode 100644 index 00000000..f253d52f --- /dev/null +++ b/free_fleet_examples/free_fleet_examples/nav1_move_base_simple_goal.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. + + +import argparse +import sys +import time + + +from free_fleet_adapter.nav1_robot_adapter import Nav1MoveBaseHandler +import rclpy + +import zenoh + + +def main(argv=sys.argv): + # Init rclpy and adapter + rclpy.init(args=argv) + args_without_ros = rclpy.utilities.remove_ros_args(argv) + node = rclpy.node.Node('nav1_move_base_simple_goal') + + parser = argparse.ArgumentParser( + prog='nav1_move_base_simple_goal', + description='Zenoh/ROS1 move_base simple goal example') + parser.add_argument('--zenoh-config', '-c', dest='config', metavar='FILE', + type=str, help='A configuration file.') + parser.add_argument('--namespace', '-n', type=str, default='') + parser.add_argument('--frame-id', '-f', type=str, default='map') + parser.add_argument('-x', type=float) + parser.add_argument('-y', type=float) + parser.add_argument('--timeout-sec', '-t', type=float, default=3.0) + + args = parser.parse_args(args_without_ros[1:]) + + # Create Zenoh Config from file if provoded, or a default one otherwise + conf = zenoh.Config.from_file(args.config) \ + if args.config is not None else zenoh.Config() + + zenoh.try_init_log_from_env() + + # Open Zenoh Session + with zenoh.open(conf) as session: + info = session.info + print(f'zid: {info.zid()}') + print(f'routers: {info.routers_zid()}') + print(f'peers: {info.peers_zid()}') + + move_base_handler = Nav1MoveBaseHandler(args.namespace, session, node) + + # Wait for move_base/status to be received + time.sleep(3) + + goal_id = move_base_handler.navigate_to_pose( + args.x, + args.y, + 0.0, + 0.0, + args.timeout_sec + ) + if goal_id is not None: + print(f'goal_id: [{goal_id}]') + else: + print('Timed out getting goal_id') + + +if __name__ == '__main__': + main(sys.argv) diff --git a/free_fleet_examples/free_fleet_examples/nav2_cancel_all_goals.py b/free_fleet_examples/free_fleet_examples/nav2_cancel_all_goals.py index a1840602..5df8a835 100755 --- a/free_fleet_examples/free_fleet_examples/nav2_cancel_all_goals.py +++ b/free_fleet_examples/free_fleet_examples/nav2_cancel_all_goals.py @@ -17,8 +17,8 @@ import argparse import sys -from free_fleet.types import ActionMsgs_CancelGoal_Response -from free_fleet.utils import make_cancel_all_goals_request, namespacify +from free_fleet.ros2_types import ActionMsgs_CancelGoal_Response +from free_fleet.utils import make_nav2_cancel_all_goals_request, namespacify import zenoh @@ -39,7 +39,7 @@ def main(argv=sys.argv): # Open Zenoh Session session = zenoh.open(conf) - req = make_cancel_all_goals_request() + req = make_nav2_cancel_all_goals_request() # Send the query with the serialized request replies = session.get( 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 ab8e6f94..a9077ff5 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 @@ -18,7 +18,7 @@ import sys import time -from free_fleet.types import ( +from free_fleet.ros2_types import ( GeometryMsgs_Point, GeometryMsgs_Pose, GeometryMsgs_PoseStamped, diff --git a/free_fleet_examples/launch/nav1_tb3_simulation_fleet_adapter.launch.xml b/free_fleet_examples/launch/nav1_tb3_simulation_fleet_adapter.launch.xml new file mode 100644 index 00000000..6fad5a53 --- /dev/null +++ b/free_fleet_examples/launch/nav1_tb3_simulation_fleet_adapter.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + +