diff --git a/.flake8 b/.flake8 index 1b7fbcb2..5389916b 100644 --- a/.flake8 +++ b/.flake8 @@ -1,5 +1,5 @@ [flake8] -extend-ignore = E203, B902,C816,D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404,I202 +extend-ignore = E203,E501,B902,C816,D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404,I202,E203 import-order-style = google max-line-length = 99 show-source = true diff --git a/.github/workflows/bump_version.yaml b/.github/workflows/bump_version.yaml index a0a6574f..4ae8e81b 100644 --- a/.github/workflows/bump_version.yaml +++ b/.github/workflows/bump_version.yaml @@ -1,64 +1,65 @@ --- name: Bump version on: - workflow_dispatch: - inputs: - name: - description: Version to bump (major, minor, patch) - default: patch - required: true - pull_request: - branches: master - types: [closed] + workflow_dispatch: + inputs: + name: + description: Version to bump (major, minor, patch) + default: patch + required: true + pull_request: + branches: master + types: [closed] jobs: - industrial_ci: - name: Industrial CI - uses: ./.github/workflows/industrial_ci.yaml + industrial_ci: + name: Industrial CI + uses: ./.github/workflows/industrial_ci.yaml - get-bump: + get-bump: + name: Get version bump + runs-on: ubuntu-latest + needs: industrial_ci + outputs: + bump: ${{ env.BUMP }} + steps: + - if: github.event_name == 'pull_request' && github.event.pull_request.merged == true name: Get version bump - runs-on: ubuntu-latest - needs: industrial_ci - outputs: - bump: ${{ env.BUMP }} - steps: - - if: github.event_name == 'pull_request' && github.event.pull_request.merged == true - name: Get version bump - id: get-version-bump - uses: husarion-ci/action-get-version-bump@v0.3.0 - - if: github.event_name == 'pull_request' && github.event.pull_request.merged == true - run: echo "BUMP=${{ steps.get-version-bump.outputs.bump }}" >> $GITHUB_ENV - - if: github.event_name == 'workflow_dispatch' - run: echo "BUMP=${{ github.event.inputs.name }}" >> $GITHUB_ENV + id: get-version-bump + uses: husarion-ci/action-get-version-bump@v0.3.0 + - if: github.event_name == 'pull_request' && github.event.pull_request.merged == true + run: echo "BUMP=${{ steps.get-version-bump.outputs.bump }}" >> $GITHUB_ENV + - if: github.event_name == 'workflow_dispatch' + run: echo "BUMP=${{ github.event.inputs.name }}" >> $GITHUB_ENV - catkin-release: - name: Bump version - runs-on: ubuntu-latest - needs: get-bump - outputs: - new_version: ${{ steps.catkin-release.outputs.new_version }} - steps: - - name: Checkout - uses: actions/checkout@v2 - - name: Catkin release - id: catkin-release - uses: husarion-ci/action-catkin-release@v0.1.4 - with: - bump: ${{ needs.get-bump.outputs.bump }} - github_token: ${{ secrets.GITHUB_TOKEN }} - git_user: action-bot - git_email: action-bot@action-bot.com + catkin-release: + name: Bump version + runs-on: ubuntu-latest + needs: get-bump + outputs: + new_version: ${{ steps.catkin-release.outputs.new_version }} + steps: + - name: Checkout + uses: actions/checkout@v2 + - name: Catkin release + id: catkin-release + uses: husarion-ci/action-catkin-release@v0.1.4 + with: + bump: ${{ needs.get-bump.outputs.bump }} + github_token: ${{ secrets.GITHUB_TOKEN }} + git_user: action-bot + git_email: action-bot@action-bot.com - build-and-push-docker-image: - name: Create new docker image - runs-on: ubuntu-latest - needs: catkin-release - steps: - - name: trigger the endpoint - run: > - curl -X POST - -H "Accept: application/vnd.github+json" - -H "Authorization: Bearer ${{ secrets.GH_PAT }}" - https://api.github.com/repos/husarion/rosbot-xl-docker/dispatches - -d '{"event_type":"ros-package-update","client_payload":{"image_version":"${{ needs.catkin-release.outputs.new_version }}"}}' + build-docker-image: + name: Trigger rosbot-xl-docker to build image + runs-on: ubuntu-latest + needs: catkin-release + steps: + - name: trigger the endpoint + run: > + curl -X POST + -H "Accept: application/vnd.github+json" + -H "Authorization: Bearer ${{ secrets.GH_PAT }}" + https://api.github.com/repos/husarion/rosbot-xl-docker/dispatches + -d '{"event_type":"ros-package-update","client_payload":{"image_version":"${{ needs.catkin-release.outputs.new_version + }}"}}' diff --git a/.github/workflows/industrial_ci.yaml b/.github/workflows/industrial_ci.yaml index ed666a05..29bb70cf 100644 --- a/.github/workflows/industrial_ci.yaml +++ b/.github/workflows/industrial_ci.yaml @@ -1,46 +1,56 @@ --- name: Industrial CI on: - workflow_call: - workflow_dispatch: - push: + workflow_call: + workflow_dispatch: + push: jobs: - black: - name: Black - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v3 - - uses: psf/black@stable - with: - options: --line-length=99 + black: + name: Black + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: psf/black@stable + with: + options: --line-length=99 - spellcheck: + spellcheck: + name: Spellcheck + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: rojopolis/spellcheck-github-actions@0.33.1 name: Spellcheck - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v3 - - uses: rojopolis/spellcheck-github-actions@0.33.1 - name: Spellcheck - ros_industrial_ci: - name: ROS Industrial CI - needs: - - black - - spellcheck - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [humble] - runs-on: ubuntu-latest - timeout-minutes: 16 - steps: - - uses: actions/checkout@v3 - - name: Clone installation requirements - shell: bash - run: python3 -m pip install -U vcstool && vcs import . < ./rosbot_xl/rosbot_xl_hardware.repos && vcs import . < ./rosbot_xl/rosbot_xl_simulation.repos - - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: ${{matrix.ROS_DISTRO}} - HUSARION_ROS_BUILD: simulation + ros_industrial_ci: + name: ROS Industrial CI + needs: + - black + - spellcheck + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [humble] + runs-on: ubuntu-latest + timeout-minutes: 30 + steps: + - name: Checkout + uses: actions/checkout@v3 + + - name: Setup ROS2 Workspace and Clone Repositories + run: | + mkdir -p src + find . -maxdepth 1 -not -name src -not -name . -exec mv {} src/ \; + python3 -m pip install -U vcstool + vcs import src < src/rosbot_xl/rosbot_xl_hardware.repos + vcs import src < src/rosbot_xl/rosbot_xl_simulation.repos + cp -r src/ros2_controllers/diff_drive_controller src/ + cp -r src/ros2_controllers/imu_sensor_broadcaster src/ + rm -rf src/ros2_controllers + + - uses: ros-industrial/industrial_ci@master + env: + ROS_DISTRO: ${{matrix.ROS_DISTRO}} + HUSARION_ROS_BUILD: simulation diff --git a/.gitignore b/.gitignore index c57e18a4..e93e8e41 100644 --- a/.gitignore +++ b/.gitignore @@ -17,6 +17,10 @@ ros_components_description/ rosbot_controllers/ husarion/husarion_office_gz gazebosim/gz_ros2_control +ros2_controllers/ +diff_drive_controller/ +imu_sensor_broadcaster/ +tf2_ros_py/ # pyspelling *.dic diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 5743d209..8b997ed4 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,51 +1,53 @@ --- repos: - - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.5.0 - hooks: - - id: check-merge-conflict - - id: trailing-whitespace - - id: end-of-file-fixer - - id: check-yaml - - id: check-xml - - id: check-added-large-files - - id: check-ast - - id: check-json - - id: name-tests-test - files: ^.*\/test\/.*$ - args: [--pytest-test-first] + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.5.0 + hooks: + - id: check-merge-conflict + - id: trailing-whitespace + - id: end-of-file-fixer + - id: check-yaml + - id: check-xml + - id: check-added-large-files + - id: check-ast + - id: check-json + - id: name-tests-test + files: ^.*\/test\/.*$ + args: [--pytest-test-first] - - repo: https://github.com/codespell-project/codespell - rev: v2.2.6 - hooks: - - id: codespell - name: codespell - description: Checks for common misspellings in text files. - entry: codespell * - language: python - types: [text] + - repo: https://github.com/codespell-project/codespell + rev: v2.2.6 + hooks: + - id: codespell + name: codespell + description: Checks for common misspellings in text files. + entry: codespell * + language: python + types: [text] - - repo: https://github.com/jumanjihouse/pre-commit-hook-yamlfmt - rev: 0.2.3 - hooks: - - id: yamlfmt - files: ^.github|./\.yaml + - repo: https://github.com/jumanjihouse/pre-commit-hook-yamlfmt + rev: 0.2.3 + hooks: + - id: yamlfmt + files: ^(?!.*compose)(?!.*ekf\.yaml$).*$ + args: [--mapping, '2', --sequence, '4', --offset, '2', --width, '100'] - - repo: https://github.com/psf/black - rev: 23.11.0 - hooks: - - id: black - args: ["--line-length=99", "--preview"] + # Please keep this version until --experimental-string-processing come back or will be available by default + - repo: https://github.com/psf/black + rev: 23.11.0 + hooks: + - id: black + args: [--line-length=99, --experimental-string-processing] - - repo: https://github.com/PyCQA/flake8 - rev: 6.1.0 - hooks: - - id: flake8 - args: ["--ignore=E501,W503,E203"] # ignore too long line and line break before binary operator, + - repo: https://github.com/PyCQA/flake8 + rev: 7.0.0 + hooks: + - id: flake8 + args: ['--ignore=E501,W503'] # ignore too long line and line break before binary operator, # black checks it - - repo: local - hooks: + - repo: local + hooks: - id: ament_lint_cmake name: ament_lint_cmake description: Check format of CMakeLists.txt files. @@ -53,8 +55,8 @@ repos: language: system files: CMakeLists\.txt$ - - repo: local - hooks: + - repo: local + hooks: - id: ament_copyright name: ament_copyright description: Check if copyright notice is available in all files. @@ -63,9 +65,9 @@ repos: language: system # Docs - RestructuredText hooks - - repo: https://github.com/PyCQA/doc8 - rev: v1.1.1 - hooks: - - id: doc8 - args: ['--max-line-length=100', '--ignore=D001'] - exclude: ^.*\/CHANGELOG\.rst/.*$ + - repo: https://github.com/PyCQA/doc8 + rev: v1.1.1 + hooks: + - id: doc8 + args: [--max-line-length=100, --ignore=D001] + exclude: ^.*\/CHANGELOG\.rst/.*$ diff --git a/.pyspelling.yaml b/.pyspelling.yaml index df8259ca..ae490993 100644 --- a/.pyspelling.yaml +++ b/.pyspelling.yaml @@ -1,53 +1,54 @@ +--- matrix: -- name: Python Source - aspell: - lang: en - d: en_US - camel-case: true - sources: - - 'rosbot*/**/*.py' + - name: Python Source + aspell: + lang: en + d: en_US + camel-case: true + sources: + - rosbot*/**/*.py - dictionary: - encoding: utf-8 - output: wordlist.dic - wordlists: - - .wordlist.txt + dictionary: + encoding: utf-8 + output: wordlist.dic + wordlists: + - .wordlist.txt - pipeline: - - pyspelling.filters.python: - comments: true + pipeline: + - pyspelling.filters.python: + comments: true -- name: Markdown sources - aspell: - lang: en - d: en_US - camel-case: true - sources: - - 'rosbot*/**/*.md' - - 'rosbot*/**/*.txt' - dictionary: - encoding: utf-8 - output: wordlist.dic - wordlists: - - .wordlist.txt + - name: Markdown sources + aspell: + lang: en + d: en_US + camel-case: true + sources: + - rosbot*/**/*.md + - rosbot*/**/*.txt + dictionary: + encoding: utf-8 + output: wordlist.dic + wordlists: + - .wordlist.txt - pipeline: - - pyspelling.filters.text + pipeline: + - pyspelling.filters.text -- name: XML sources - aspell: - lang: en - d: en_US - camel-case: true - sources: - - 'rosbot*/**/*.xacro' - - 'rosbot*/**/*.urdf' - - 'rosbot*/**/*.xml' - dictionary: - encoding: utf-8 - output: wordlist.dic - wordlists: - - .wordlist.txt + - name: XML sources + aspell: + lang: en + d: en_US + camel-case: true + sources: + - rosbot*/**/*.xacro + - rosbot*/**/*.urdf + - rosbot*/**/*.xml + dictionary: + encoding: utf-8 + output: wordlist.dic + wordlists: + - .wordlist.txt - pipeline: - - pyspelling.filters.xml + pipeline: + - pyspelling.filters.xml diff --git a/.wordlist.txt b/.wordlist.txt index 6b7d0f8d..61bb2de6 100644 --- a/.wordlist.txt +++ b/.wordlist.txt @@ -70,3 +70,17 @@ utils tf yaml odometry +namespaces +namespace +delihus +microros +remappings +unbuffered +py +len +ns +psutil +src +xml +spawner +sp diff --git a/LICENSE_MIT.txt b/LICENSE_MIT.txt index a32436c6..e54dae75 100644 --- a/LICENSE_MIT.txt +++ b/LICENSE_MIT.txt @@ -1,6 +1,6 @@ MIT License -Copyright (c) 2023 Husarion sp. z o.o. +Copyright (c) 2024 Husarion sp. z o.o. Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/README.md b/README.md index 5e402d8b..440b64aa 100644 --- a/README.md +++ b/README.md @@ -69,6 +69,15 @@ vcs import src < src/rosbot_xl/rosbot_xl_hardware.repos rm -r src/rosbot_xl_gazebo +# Copy only diff_drive_controller and imu_sensor_broadcaster, waits for features from ros2-control +cp -r src/ros2_controllers/diff_drive_controller src/ +cp -r src/ros2_controllers/imu_sensor_broadcaster src/ +rm -rf src/ros2_controllers + +# Copy only tf2_ros_py from geometry2, waits for https://github.com/ros2/geometry2/pull/641 +cp -r src/geometry2/tf2_ros_py src/ +rm -rf src/geometry2/ + rosdep init rosdep update --rosdistro $ROS_DISTRO rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y @@ -95,6 +104,19 @@ source /opt/ros/$ROS_DISTRO/setup.bash vcs import src < src/rosbot_xl/rosbot_xl_hardware.repos vcs import src < src/rosbot_xl/rosbot_xl_simulation.repos +# Copy only diff_drive_controller and imu_sensor_broadcaster, waits for features from ros2-control +cp -r src/ros2_controllers/diff_drive_controller src/ +cp -r src/ros2_controllers/imu_sensor_broadcaster src/ +rm -rf src/ros2_controllers + +# Copy only tf2_ros_py from geometry2, waits for https://github.com/ros2/geometry2/pull/641 +cp -r src/geometry2/tf2_ros_py src/ +rm -rf src/geometry2/ + +# Remove ign_ros2_control demo +rm -rf src/gazebosim/gz_ros2_control/ign_ros2_control_demos && +rm -rf src/gazebosim/gz_ros2_control/gz_ros2_control_tests + rosdep init rosdep update --rosdistro $ROS_DISTRO rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y diff --git a/rosbot_xl/rosbot_xl_hardware.repos b/rosbot_xl/rosbot_xl_hardware.repos index 84c92831..7803e1a5 100644 --- a/rosbot_xl/rosbot_xl_hardware.repos +++ b/rosbot_xl/rosbot_xl_hardware.repos @@ -3,11 +3,15 @@ repositories: type: git url: https://github.com/husarion/rosbot_hardware_interfaces.git version: main + ros_components_description: + type: git + url: https://github.com/husarion/ros_components_description.git + version: ros2 rosbot_controllers: type: git url: https://github.com/husarion/rosbot_controllers version: main - ros_components_description: + ros2_controllers: type: git - url: https://github.com/husarion/ros_components_description.git - version: ros2 + url: https://github.com/delihus/ros2_controllers + version: humble diff --git a/rosbot_xl/rosbot_xl_simulation.repos b/rosbot_xl/rosbot_xl_simulation.repos index 400406a9..e4d80df2 100644 --- a/rosbot_xl/rosbot_xl_simulation.repos +++ b/rosbot_xl/rosbot_xl_simulation.repos @@ -1,12 +1,5 @@ repositories: - gazebosim/gz_ros2_control: + husarion/husarion_office_gz: type: git - url: https://github.com/ros-controls/gz_ros2_control.git - # on branch humble hardware isn't activated - # recently on master API breaking change was introduced, it is necessary to use commit before this change - version: b296ff2f5c3758b637a70bd496fe6ed875ab03ce - - husarion/gazebo_worlds: - type: git - url: https://github.com/husarion/gazebo_worlds.git + url: https://github.com/husarion/husarion_office_gz version: main diff --git a/rosbot_xl_bringup/config/ekf.yaml b/rosbot_xl_bringup/config/ekf.yaml index 19323308..76818786 100644 --- a/rosbot_xl_bringup/config/ekf.yaml +++ b/rosbot_xl_bringup/config/ekf.yaml @@ -1,9 +1,9 @@ # Ref: http://docs.ros.org/en/noetic/api/robot_localization/html/state_estimation_nodes.html ## ekf config file ### -ekf_filter_node: +/**/ekf_filter_node: ros__parameters: - frequency: 25.0 + frequency: 20.0 sensor_timeout: 0.05 two_d_mode: true @@ -17,7 +17,7 @@ ekf_filter_node: publish_tf: true publish_acceleration: false - odom0: /rosbot_xl_base_controller/odom + odom0: rosbot_xl_base_controller/odom odom0_config: [false, false, false, # X , Y , Z false, false, false, # roll , pitch ,yaw true, true, false, # dX , dY , dZ @@ -29,7 +29,7 @@ ekf_filter_node: odom0_differential: false odom0_relative: true - imu0: /imu_broadcaster/imu + imu0: imu_broadcaster/imu imu0_config: [false, false, false, # X , Y , Z false, false, true, # roll , pitch ,yaw false, false, false, # dX , dY , dZ diff --git a/rosbot_xl_bringup/config/laser_filter.yaml b/rosbot_xl_bringup/config/laser_filter.yaml index a9de47eb..8ce842da 100644 --- a/rosbot_xl_bringup/config/laser_filter.yaml +++ b/rosbot_xl_bringup/config/laser_filter.yaml @@ -1,4 +1,5 @@ -scan_to_scan_filter_chain: +--- +/**/scan_to_scan_filter_chain: ros__parameters: filter1: name: box_filter diff --git a/rosbot_xl_bringup/launch/bringup.launch.py b/rosbot_xl_bringup/launch/bringup.launch.py index 49cb4306..6ac39957 100644 --- a/rosbot_xl_bringup/launch/bringup.launch.py +++ b/rosbot_xl_bringup/launch/bringup.launch.py @@ -1,4 +1,4 @@ -# Copyright 2023 Husarion +# Copyright 2024 Husarion sp. z o.o. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -14,10 +14,7 @@ from launch import LaunchDescription from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument -from launch.substitutions import ( - PathJoinSubstitution, - LaunchConfiguration, -) +from launch.substitutions import PathJoinSubstitution, LaunchConfiguration from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node, SetParameter @@ -26,6 +23,13 @@ def generate_launch_description(): + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value="", + description="Namespace for all topics and tfs", + ) + mecanum = LaunchConfiguration("mecanum") declare_mecanum_arg = DeclareLaunchArgument( "mecanum", @@ -90,13 +94,18 @@ def generate_launch_description(): description="Which simulation engine will be used", ) + rosbot_xl_controller = get_package_share_directory("rosbot_xl_controller") + rosbot_xl_bringup = get_package_share_directory("rosbot_xl_bringup") + controller_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - PathJoinSubstitution([ - get_package_share_directory("rosbot_xl_controller"), - "launch", - "controller.launch.py", - ]) + PathJoinSubstitution( + [ + rosbot_xl_controller, + "launch", + "controller.launch.py", + ] + ) ), launch_arguments={ "mecanum": mecanum, @@ -105,44 +114,58 @@ def generate_launch_description(): "include_camera_mount": include_camera_mount, "use_sim": use_sim, "simulation_engine": simulation_engine, + "namespace": namespace, }.items(), ) + ekf_config = PathJoinSubstitution([rosbot_xl_bringup, "config", "ekf.yaml"]) + robot_localization_node = Node( package="robot_localization", executable="ekf_node", name="ekf_filter_node", output="screen", - parameters=[ - PathJoinSubstitution([ - get_package_share_directory("rosbot_xl_bringup"), "config", "ekf.yaml" - ]) + parameters=[ekf_config], + remappings=[ + ("/tf", "tf"), + ("/tf_static", "tf_static"), ], + namespace=namespace, + ) + + laser_filter_config = PathJoinSubstitution( + [ + rosbot_xl_bringup, + "config", + "laser_filter.yaml", + ] ) laser_filter_node = Node( package="laser_filters", executable="scan_to_scan_filter_chain", parameters=[ - PathJoinSubstitution([ - get_package_share_directory("rosbot_xl_bringup"), - "config", - "laser_filter.yaml", - ]) + laser_filter_config, ], + remappings=[ + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ], + namespace=namespace, ) - actions = [ - declare_mecanum_arg, - declare_lidar_model_arg, - declare_camera_model_arg, - declare_include_camera_mount_arg, - declare_use_sim_arg, - declare_simulation_engine_arg, - SetParameter(name="use_sim_time", value=use_sim), - controller_launch, - robot_localization_node, - laser_filter_node, - ] - - return LaunchDescription(actions) + return LaunchDescription( + [ + declare_namespace_arg, + declare_mecanum_arg, + declare_lidar_model_arg, + declare_camera_model_arg, + declare_include_camera_mount_arg, + declare_use_sim_arg, + declare_simulation_engine_arg, + SetParameter(name="use_sim_time", value=use_sim), + controller_launch, + robot_localization_node, + laser_filter_node, + ] + ) diff --git a/rosbot_xl_bringup/package.xml b/rosbot_xl_bringup/package.xml index b7cf84a2..726dbbe4 100644 --- a/rosbot_xl_bringup/package.xml +++ b/rosbot_xl_bringup/package.xml @@ -1,5 +1,6 @@ + rosbot_xl_bringup 0.8.12 @@ -15,22 +16,27 @@ https://github.com/husarion/rosbot_xl_ros https://github.com/husarion/rosbot_xl_ros/issues - rosbot_xl_controller - + + ros2launch launch launch_ros + ament_index_python + + + rosbot_xl_controller + laser_filters robot_localization + + rclpy python3-pytest - launch - launch_ros launch_pytest - laser_filters - robot_localization - rosbot_xl_controller + + tf2_ros_py + sensor_msgs ament_python diff --git a/rosbot_xl_bringup/setup.py b/rosbot_xl_bringup/setup.py index b9d7233d..1e6d8d34 100644 --- a/rosbot_xl_bringup/setup.py +++ b/rosbot_xl_bringup/setup.py @@ -1,4 +1,4 @@ -# Copyright 2023 Husarion +# Copyright 2024 Husarion sp. z o.o. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/rosbot_xl_bringup/test/test_diff_drive_ekf_and_scan.py b/rosbot_xl_bringup/test/test_diff_drive_ekf_and_scan.py index 05ae8fb9..8507b9fa 100644 --- a/rosbot_xl_bringup/test/test_diff_drive_ekf_and_scan.py +++ b/rosbot_xl_bringup/test/test_diff_drive_ekf_and_scan.py @@ -1,6 +1,6 @@ # Copyright 2021 Open Source Robotics Foundation, Inc. # Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2023 Husarion +# Copyright 2024 Husarion sp. z o.o. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -23,7 +23,8 @@ from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import BringupTestNode +from test_utils import BringupTestNode, ekf_and_scan_test +from threading import Thread @launch_pytest.fixture @@ -31,16 +32,17 @@ def generate_test_description(): rosbot_xl_bringup = get_package_share_directory("rosbot_xl_bringup") bringup_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - PathJoinSubstitution([ - rosbot_xl_bringup, - "launch", - "bringup.launch.py", - ]) + PathJoinSubstitution( + [ + rosbot_xl_bringup, + "launch", + "bringup.launch.py", + ] + ) ), launch_arguments={ "use_sim": "False", "mecanum": "False", - "use_gpu": "False", }.items(), ) @@ -52,32 +54,11 @@ def test_bringup_startup_success(): rclpy.init() try: node = BringupTestNode("test_bringup") - node.create_test_subscribers_and_publishers() node.start_publishing_fake_hardware() - node.start_node_thread() - msgs_received_flag = node.odom_tf_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected odom to base_link tf but it was not received. Check robot_localization!" - - finally: - rclpy.shutdown() - - -@pytest.mark.launch(fixture=generate_test_description) -def test_bringup_scan_filter(): - rclpy.init() - try: - node = BringupTestNode("test_bringup") - node.create_test_subscribers_and_publishers() - node.start_publishing_fake_hardware() - - node.start_node_thread() - msgs_received_flag = node.scan_filter_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected filtered scan but it is not filtered properly. Check laser_filter!" + Thread(target=lambda node: rclpy.spin(node), args=(node,)).start() + ekf_and_scan_test(node) + node.destroy_node() finally: rclpy.shutdown() diff --git a/rosbot_xl_bringup/test/test_mecanum_ekf_and_scan.py b/rosbot_xl_bringup/test/test_mecanum_ekf_and_scan.py index 3424785e..817be79d 100644 --- a/rosbot_xl_bringup/test/test_mecanum_ekf_and_scan.py +++ b/rosbot_xl_bringup/test/test_mecanum_ekf_and_scan.py @@ -1,6 +1,6 @@ # Copyright 2021 Open Source Robotics Foundation, Inc. # Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2023 Husarion +# Copyright 2024 Husarion sp. z o.o. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -23,7 +23,8 @@ from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import BringupTestNode +from test_utils import BringupTestNode, ekf_and_scan_test +from threading import Thread @launch_pytest.fixture @@ -31,16 +32,17 @@ def generate_test_description(): rosbot_xl_bringup = get_package_share_directory("rosbot_xl_bringup") bringup_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - PathJoinSubstitution([ - rosbot_xl_bringup, - "launch", - "bringup.launch.py", - ]) + PathJoinSubstitution( + [ + rosbot_xl_bringup, + "launch", + "bringup.launch.py", + ] + ) ), launch_arguments={ "use_sim": "False", "mecanum": "True", - "use_gpu": "False", }.items(), ) @@ -52,32 +54,11 @@ def test_bringup_startup_success(): rclpy.init() try: node = BringupTestNode("test_bringup") - node.create_test_subscribers_and_publishers() node.start_publishing_fake_hardware() - node.start_node_thread() - msgs_received_flag = node.odom_tf_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected odom to base_link tf but it was not received. Check robot_localization!" - - finally: - rclpy.shutdown() - - -@pytest.mark.launch(fixture=generate_test_description) -def test_bringup_scan_filter(): - rclpy.init() - try: - node = BringupTestNode("test_bringup") - node.create_test_subscribers_and_publishers() - node.start_publishing_fake_hardware() - - node.start_node_thread() - msgs_received_flag = node.scan_filter_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected filtered scan but it is not filtered properly. Check laser_filter!" + Thread(target=lambda node: rclpy.spin(node), args=(node,)).start() + ekf_and_scan_test(node) + node.destroy_node() finally: rclpy.shutdown() diff --git a/rosbot_xl_bringup/test/test_multirobot_ekf_and_scan.py b/rosbot_xl_bringup/test/test_multirobot_ekf_and_scan.py new file mode 100644 index 00000000..446c569e --- /dev/null +++ b/rosbot_xl_bringup/test/test_multirobot_ekf_and_scan.py @@ -0,0 +1,83 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2024 Husarion sp. z o.o. +# +# 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 launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from rclpy.executors import SingleThreadedExecutor +from test_utils import BringupTestNode, ekf_and_scan_test +from threading import Thread + + +robot_names = ["rosbot1", "rosbot2", "rosbot3"] + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_xl_bringup = get_package_share_directory("rosbot_xl_bringup") + actions = [] + for i in range(len(robot_names)): + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_xl_bringup, + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "False", + "namespace": robot_names[i], + }.items(), + ) + actions.append(bringup_launch) + + return LaunchDescription(actions) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_bringup_startup_success(): + rclpy.init() + try: + nodes = {} + executor = SingleThreadedExecutor() + + for node_namespace in robot_names: + node = BringupTestNode("test_bringup", namespace=node_namespace) + node.start_publishing_fake_hardware() + nodes[node_namespace] = node + executor.add_node(node) + + Thread(target=lambda executor: executor.spin(), args=(executor,)).start() + + for node_namespace in robot_names: + node = nodes[node_namespace] + ekf_and_scan_test(node, node_namespace) + executor.remove_node(node) + node.destroy_node() + + finally: + executor.shutdown() + rclpy.shutdown() diff --git a/rosbot_xl_gazebo/test/test_slamtec_rplidar_s1.py b/rosbot_xl_bringup/test/test_namespaced_diff_drive_ekf_and_scan.py similarity index 56% rename from rosbot_xl_gazebo/test/test_slamtec_rplidar_s1.py rename to rosbot_xl_bringup/test/test_namespaced_diff_drive_ekf_and_scan.py index a209cc27..34f69717 100644 --- a/rosbot_xl_gazebo/test/test_slamtec_rplidar_s1.py +++ b/rosbot_xl_bringup/test/test_namespaced_diff_drive_ekf_and_scan.py @@ -1,6 +1,6 @@ # Copyright 2021 Open Source Robotics Foundation, Inc. # Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2023 Husarion +# Copyright 2024 Husarion sp. z o.o. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -23,44 +23,43 @@ from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource - -from test_utils import SimulationTestNode -from test_ign_kill_utils import kill_ign_linux_processes +from test_utils import BringupTestNode, ekf_and_scan_test +from threading import Thread @launch_pytest.fixture def generate_test_description(): - rosbot_xl_gazebo = get_package_share_directory("rosbot_xl_gazebo") - simulation_launch = IncludeLaunchDescription( + rosbot_xl_bringup = get_package_share_directory("rosbot_xl_bringup") + bringup_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - PathJoinSubstitution([ - rosbot_xl_gazebo, - "launch", - "simulation.launch.py", - ]) + PathJoinSubstitution( + [ + rosbot_xl_bringup, + "launch", + "bringup.launch.py", + ] + ) ), launch_arguments={ - "headless": "True", - "lidar_model": "slamtec_rplidar_s1", + "use_sim": "False", + "mecanum": "False", + "namespace": "rosbot_xl", }.items(), ) - return LaunchDescription([simulation_launch]) + return LaunchDescription([bringup_launch]) @pytest.mark.launch(fixture=generate_test_description) -def test_slamtec_rplidar_s1(): +def test_namespaced_bringup_startup_success(): rclpy.init() try: - node = SimulationTestNode("test_bringup") - node.create_test_subscribers_and_publishers() - node.start_node_thread() + node = BringupTestNode("test_bringup", namespace="rosbot_xl") + node.start_publishing_fake_hardware() - msgs_received_flag = node.scan_event.wait(timeout=60.0) - assert msgs_received_flag, "ROSbot's lidar does not work properly!" + Thread(target=lambda node: rclpy.spin(node), args=(node,)).start() + ekf_and_scan_test(node) + node.destroy_node() finally: - # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching - # several tests in a row. - kill_ign_linux_processes() rclpy.shutdown() diff --git a/rosbot_xl_bringup/test/test_namespaced_mecanum_ekf_and_scan.py b/rosbot_xl_bringup/test/test_namespaced_mecanum_ekf_and_scan.py new file mode 100644 index 00000000..4b4b17e5 --- /dev/null +++ b/rosbot_xl_bringup/test/test_namespaced_mecanum_ekf_and_scan.py @@ -0,0 +1,65 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2024 Husarion sp. z o.o. +# +# 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 launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import BringupTestNode, ekf_and_scan_test +from threading import Thread + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_xl_bringup = get_package_share_directory("rosbot_xl_bringup") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_xl_bringup, + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "True", + "namespace": "rosbot_xl", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_bringup_startup_success(): + rclpy.init() + try: + node = BringupTestNode("test_bringup", namespace="rosbot_xl") + node.start_publishing_fake_hardware() + + Thread(target=lambda node: rclpy.spin(node), args=(node,)).start() + ekf_and_scan_test(node) + node.destroy_node() + + finally: + rclpy.shutdown() diff --git a/rosbot_xl_bringup/test/test_utils.py b/rosbot_xl_bringup/test/test_utils.py index ac878e7f..527b7257 100644 --- a/rosbot_xl_bringup/test/test_utils.py +++ b/rosbot_xl_bringup/test/test_utils.py @@ -1,5 +1,5 @@ # Copyright 2021 Open Source Robotics Foundation, Inc. -# Copyright 2023 Husarion +# Copyright 2024 Husarion sp. z o.o. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -18,10 +18,7 @@ import random from threading import Event -from threading import Thread - from rclpy.node import Node - from sensor_msgs.msg import JointState, Imu, LaserScan from tf2_ros import TransformException from tf2_ros.buffer import Buffer @@ -32,21 +29,23 @@ class BringupTestNode(Node): ROSBOT_HARDWARE_PUBLISHERS_RATE = 10.0 __test__ = False - def __init__(self, name="test_node"): - super().__init__(name) + def __init__(self, name="test_node", namespace=None): + super().__init__( + name, + namespace=namespace, + cli_args=["--ros-args", "-r", "/tf:=tf", "-r", "/tf_static:=tf_static"], + ) self.odom_tf_event = Event() self.scan_filter_event = Event() - def create_test_subscribers_and_publishers(self): - self.imu_publisher = self.create_publisher(Imu, "_imu/data_raw", 10) - - self.joint_states_publisher = self.create_publisher(JointState, "_motors_response", 10) + self.imu_pub = self.create_publisher(Imu, "/_imu/data_raw", 10) + self.joint_pub = self.create_publisher(JointState, "/_motors_response", 10) self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) - self.scan_publisher = self.create_publisher(LaserScan, "scan", 10) - self.filtered_scan_subscriber = self.create_subscription( + self.scan_pub = self.create_publisher(LaserScan, "scan", 10) + self.scan_filtered_sub = self.create_subscription( LaserScan, "scan_filtered", self.filtered_scan_callback, 10 ) @@ -57,11 +56,11 @@ def lookup_transform_odom(self): self.tf_buffer.lookup_transform("odom", "base_link", rclpy.time.Time()) self.odom_tf_event.set() except TransformException as ex: - self.get_logger().error(f"Could not transform odom to base_link: {ex}") - - def start_node_thread(self): - self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,)) - self.ros_spin_thread.start() + self.get_logger().error( + f"Could not transform odom to base_link: {ex}", + skip_first=True, + throttle_duration_sec=3.0, + ) def start_publishing_fake_hardware(self): self.timer = self.create_timer( @@ -94,12 +93,12 @@ def publish_fake_hardware_messages(self): joint_state_msg.position = [0.0, 0.0, 0.0, 0.0] joint_state_msg.velocity = [0.0, 0.0, 0.0, 0.0] - self.imu_publisher.publish(imu_msg) - self.joint_states_publisher.publish(joint_state_msg) + self.imu_pub.publish(imu_msg) + self.joint_pub.publish(joint_state_msg) def publish_scan(self): msg = LaserScan() - msg.header.frame_id = "base_link" + msg.header.frame_id = "laser" msg.angle_min = 0.0 msg.angle_max = 2.0 * math.pi msg.angle_increment = 0.05 @@ -110,4 +109,15 @@ def publish_scan(self): # fill ranges from 0.0m to 1.0m msg.ranges = [random.random() for _ in range(int(msg.angle_max / msg.angle_increment))] - self.scan_publisher.publish(msg) + self.scan_pub.publish(msg) + + +def ekf_and_scan_test(node: BringupTestNode, robot_name="ROSbot"): + assert node.odom_tf_event.wait(20.0), ( + f"{robot_name}: Expected odom to base_link tf but it was not received. Check" + " robot_localization!" + ) + + assert node.scan_filter_event.wait( + 20.0 + ), f"{robot_name}: Expected filtered scan but it is not filtered properly. Check laser_filter!" diff --git a/rosbot_xl_controller/config/diff_drive_controller.yaml b/rosbot_xl_controller/config/diff_drive_controller.yaml index 0dabb6bd..90f70e3e 100644 --- a/rosbot_xl_controller/config/diff_drive_controller.yaml +++ b/rosbot_xl_controller/config/diff_drive_controller.yaml @@ -1,22 +1,5 @@ -simulation_ignition_ros_control: - ros__parameters: - use_sim_time: true - -# Separate controller manager used for simulation - only difference is -# the use_sim_time parameter (it is the easiest way to do it with ign ros2 control) -simulation_controller_manager: - ros__parameters: - use_sim_time: true - update_rate: 25 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - rosbot_xl_base_controller: - type: diff_drive_controller/DiffDriveController - imu_broadcaster: - type: imu_sensor_broadcaster/IMUSensorBroadcaster - -controller_manager: +--- +/**/controller_manager: ros__parameters: use_sim_time: false update_rate: 25 # Hz @@ -28,18 +11,21 @@ controller_manager: imu_broadcaster: type: imu_sensor_broadcaster/IMUSensorBroadcaster -imu_broadcaster: +/**/imu_broadcaster: ros__parameters: - sensor_name: "imu" - frame_id: "imu_link" + tf_frame_prefix_enable: false + use_namespace_as_sensor_name_prefix: true + sensor_name: imu + frame_id: imu_link static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally -rosbot_xl_base_controller: +/**/rosbot_xl_base_controller: ros__parameters: - left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"] - right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"] + tf_frame_prefix_enable: false + left_wheel_names: [fl_wheel_joint, rl_wheel_joint] + right_wheel_names: [fr_wheel_joint, rr_wheel_joint] wheel_separation: 0.248 wheel_radius: 0.048 @@ -76,13 +62,14 @@ rosbot_xl_base_controller: # Velocity and acceleration limits # Whenever a min_* is unspecified, default to -max_* + # Limits from manual https://husarion.com/manuals/rosbot-xl/#hardware-guide linear: x: has_velocity_limits: true has_acceleration_limits: true has_jerk_limits: false # top speed is around 1.2m/s - max_velocity: 0.9 # m/s + max_velocity: 0.8 # m/s # min_velocity - When unspecified, -max_velocity is used max_acceleration: 1.0 # m/s^2 # min_acceleration - When unspecified, -max_acceleration is used. @@ -93,7 +80,7 @@ rosbot_xl_base_controller: has_velocity_limits: true has_acceleration_limits: true has_jerk_limits: false - max_velocity: 3.0 # rad/s + max_velocity: 3.14 # rad/s # min_velocity - When unspecified, -max_velocity is used max_acceleration: 4.0 # rad/s^2 # min_acceleration - When unspecified, -max_acceleration is used. diff --git a/rosbot_xl_controller/config/mecanum_drive_controller.yaml b/rosbot_xl_controller/config/mecanum_drive_controller.yaml index 5d8f9868..265efb07 100644 --- a/rosbot_xl_controller/config/mecanum_drive_controller.yaml +++ b/rosbot_xl_controller/config/mecanum_drive_controller.yaml @@ -1,22 +1,5 @@ -simulation_ignition_ros_control: - ros__parameters: - use_sim_time: true - -# Separate controller manager used for simulation - only difference is -# the use_sim_time parameter (it is the easiest way to do it with ign ros2 control) -simulation_controller_manager: - ros__parameters: - use_sim_time: true - update_rate: 25 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - rosbot_xl_base_controller: - type: mecanum_drive_controller/MecanumDriveController - imu_broadcaster: - type: imu_sensor_broadcaster/IMUSensorBroadcaster - -controller_manager: +--- +/**/controller_manager: ros__parameters: use_sim_time: false update_rate: 25 # Hz @@ -28,16 +11,20 @@ controller_manager: imu_broadcaster: type: imu_sensor_broadcaster/IMUSensorBroadcaster -imu_broadcaster: +/**/imu_broadcaster: ros__parameters: - sensor_name: "imu" - frame_id: "imu_link" + tf_frame_prefix_enable: false + use_namespace_as_sensor_name_prefix: true + + sensor_name: imu + frame_id: imu_link static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values based on diff_drive static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values based on diff_drive -rosbot_xl_base_controller: +/**/rosbot_xl_base_controller: ros__parameters: + tf_frame_prefix_enable: false front_left_wheel_name: fl_wheel_joint front_right_wheel_name: fr_wheel_joint rear_left_wheel_name: rl_wheel_joint @@ -76,13 +63,14 @@ rosbot_xl_base_controller: # Velocity and acceleration limits # Whenever a min_* is unspecified, default to -max_* + # Limits from manual https://husarion.com/manuals/rosbot-xl/#hardware-guide linear: x: has_velocity_limits: true has_acceleration_limits: true has_jerk_limits: false - # top speed is around 1.2m/s - max_velocity: 0.9 # m/s + # top speed is around 0.8m/s + max_velocity: 0.8 # m/s # min_velocity - When unspecified, -max_velocity is used max_acceleration: 1.0 # m/s^2 # min_acceleration - When unspecified, -max_acceleration is used. @@ -91,8 +79,8 @@ rosbot_xl_base_controller: has_velocity_limits: true has_acceleration_limits: true has_jerk_limits: false - # top speed is around 1.2m/s - max_velocity: 0.9 # m/s + # top speed is around 0.8m/s + max_velocity: 0.8 # m/s # min_velocity - When unspecified, -max_velocity is used max_acceleration: 1.0 # m/s^2 # min_acceleration - When unspecified, -max_acceleration is used. @@ -103,7 +91,7 @@ rosbot_xl_base_controller: has_velocity_limits: true has_acceleration_limits: true has_jerk_limits: false - max_velocity: 3.0 # rad/s + max_velocity: 3.14 # rad/s # min_velocity - When unspecified, -max_velocity is used max_acceleration: 4.0 # rad/s^2 # min_acceleration - When unspecified, -max_acceleration is used. diff --git a/rosbot_xl_controller/launch/controller.launch.py b/rosbot_xl_controller/launch/controller.launch.py index 54124b24..971d4798 100644 --- a/rosbot_xl_controller/launch/controller.launch.py +++ b/rosbot_xl_controller/launch/controller.launch.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # Copyright 2020 ros2_control Development Team -# Copyright 2023 Husarion +# Copyright 2024 Husarion sp. z o.o. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -15,10 +15,14 @@ # See the License for the specific language governing permissions and # limitations under the License. +from launch.event_handlers import OnProcessExit from launch import LaunchDescription -from launch.actions import RegisterEventHandler, DeclareLaunchArgument +from launch.actions import ( + DeclareLaunchArgument, + RegisterEventHandler, + OpaqueFunction, +) from launch.conditions import UnlessCondition -from launch.event_handlers import OnProcessExit from launch.substitutions import ( Command, PythonExpression, @@ -31,7 +35,91 @@ from launch_ros.substitutions import FindPackageShare +def launch_setup(context, *args, **kwargs): + namespace = LaunchConfiguration("namespace").perform(context) + controller_manager_name = "controller_manager" + if namespace != "": + controller_manager_name = namespace + "/" + controller_manager_name + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + controller_manager_name, + "--controller-manager-timeout", + "120", + "--namespace", + namespace, + ], + ) + + robot_controller_spawner = Node( + package="controller_manager", + name=LaunchConfiguration( + "robot_controller_spawner_name", + default=[namespace, "_robot_controller_spawner"], + ), + executable="spawner", + arguments=[ + "rosbot_xl_base_controller", + "--controller-manager", + controller_manager_name, + "--controller-manager-timeout", + "120", + "--namespace", + namespace, + ], + ) + + # Delay start of robot_controller after `joint_state_broadcaster` + delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[robot_controller_spawner], + ) + ) + + imu_broadcaster_spawner = Node( + package="controller_manager", + name=LaunchConfiguration( + "imu_spawner_name", default=[namespace, "_imu_broadcaster_spawner"] + ), + executable="spawner", + arguments=[ + "imu_broadcaster", + "--controller-manager", + controller_manager_name, + "--controller-manager-timeout", + "120", + "--namespace", + namespace, + ], + ) + + # Delay start of imu_broadcaster_spawner after `robot_controller_spawner` + delay_imu_broadcaster_spawner_after_robot_controller_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=robot_controller_spawner, + on_exit=[imu_broadcaster_spawner], + ) + ) + return [ + joint_state_broadcaster_spawner, + delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, + delay_imu_broadcaster_spawner_after_robot_controller_spawner, + ] + + def generate_launch_description(): + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value="", + description="Namespace for all topics and tfs", + ) + mecanum = LaunchConfiguration("mecanum") declare_mecanum_arg = DeclareLaunchArgument( "mecanum", @@ -96,61 +184,69 @@ def generate_launch_description(): description="Which simulation engine will be used", ) - controller_config_name = PythonExpression([ - "'mecanum_drive_controller.yaml' if ", - mecanum, - " else 'diff_drive_controller.yaml'", - ]) - - robot_controllers = PathJoinSubstitution([ - FindPackageShare("rosbot_xl_controller"), - "config", - controller_config_name, - ]) - - controller_manager_name = PythonExpression([ - "'/simulation_controller_manager' if ", - use_sim, - " else '/controller_manager'", - ]) + controller_config_name = PythonExpression( + [ + "'mecanum_drive_controller.yaml' if ", + mecanum, + " else 'diff_drive_controller.yaml'", + ] + ) # Get URDF via xacro - robot_description_content = Command([ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution([ - FindPackageShare("rosbot_xl_description"), - "urdf", - "rosbot_xl.urdf.xacro", - ]), - " mecanum:=", - mecanum, - " lidar_model:=", - lidar_model, - " camera_model:=", - camera_model, - " include_camera_mount:=", - include_camera_mount, - " use_sim:=", - use_sim, - " simulation_engine:=", - simulation_engine, - " simulation_controllers_config_file:=", - robot_controllers, - ]) + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("rosbot_xl_description"), + "urdf", + "rosbot_xl.urdf.xacro", + ] + ), + " mecanum:=", + mecanum, + " lidar_model:=", + lidar_model, + " camera_model:=", + camera_model, + " include_camera_mount:=", + include_camera_mount, + " use_sim:=", + use_sim, + " simulation_engine:=", + simulation_engine, + " namespace:=", + namespace, + ] + ) robot_description = {"robot_description": robot_description_content} + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("rosbot_xl_controller"), + "config", + controller_config_name, + ] + ) + control_node = Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_description, robot_controllers], + parameters=[ + robot_description, + robot_controllers, + ], remappings=[ - ("/imu_sensor_node/imu", "/_imu/data_raw"), + ("imu_sensor_node/imu", "/_imu/data_raw"), ("~/motors_cmd", "/_motors_cmd"), ("~/motors_response", "/_motors_response"), - ("/rosbot_xl_base_controller/cmd_vel_unstamped", "/cmd_vel"), + ("rosbot_xl_base_controller/cmd_vel_unstamped", "cmd_vel"), + ("/tf", "tf"), + ("/tf_static", "tf_static"), ], condition=UnlessCondition(use_sim), + namespace=namespace, ) robot_state_pub_node = Node( @@ -158,74 +254,22 @@ def generate_launch_description(): executable="robot_state_publisher", output="both", parameters=[robot_description], + remappings=[("/tf", "tf"), ("/tf_static", "tf_static")], + namespace=namespace, ) - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "joint_state_broadcaster", - "--controller-manager", - controller_manager_name, - "--controller-manager-timeout", - "120", - ], - ) - - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "rosbot_xl_base_controller", - "--controller-manager", - controller_manager_name, - "--controller-manager-timeout", - "120", - ], - ) - - # Delay start of robot_controller after joint_state_broadcaster - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], - ) + return LaunchDescription( + [ + declare_namespace_arg, + declare_mecanum_arg, + declare_lidar_model_arg, + declare_camera_model_arg, + declare_include_camera_mount_arg, + declare_use_sim_arg, + declare_simulation_engine_arg, + SetParameter(name="use_sim_time", value=use_sim), + control_node, + robot_state_pub_node, + OpaqueFunction(function=launch_setup), + ] ) - - imu_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "imu_broadcaster", - "--controller-manager", - controller_manager_name, - "--controller-manager-timeout", - "120", - ], - ) - - # Delay start of imu_broadcaster after robot_controller - # when spawning without delay ros2_control_node sometimes crashed - delay_imu_broadcaster_spawner_after_robot_controller_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=robot_controller_spawner, - on_exit=[imu_broadcaster_spawner], - ) - ) - - actions = [ - declare_mecanum_arg, - declare_lidar_model_arg, - declare_camera_model_arg, - declare_include_camera_mount_arg, - declare_use_sim_arg, - declare_simulation_engine_arg, - SetParameter(name="use_sim_time", value=use_sim), - control_node, - robot_state_pub_node, - joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, - delay_imu_broadcaster_spawner_after_robot_controller_spawner, - ] - - return LaunchDescription(actions) diff --git a/rosbot_xl_controller/package.xml b/rosbot_xl_controller/package.xml index d75af263..73ba3b76 100644 --- a/rosbot_xl_controller/package.xml +++ b/rosbot_xl_controller/package.xml @@ -1,5 +1,6 @@ + rosbot_xl_controller 0.8.12 @@ -16,40 +17,35 @@ https://github.com/husarion/rosbot_xl_ros https://github.com/husarion/rosbot_xl_ros/issues - rosbot_xl_description - + + ros2launch launch launch_ros + ament_index_python + + + rosbot_xl_description + ros_components_description + rosbot_hardware_interfaces + mecanum_drive_controller + xacro controller_manager robot_state_publisher - joint_state_broadcaster imu_sensor_broadcaster diff_drive_controller - mecanum_drive_controller - - rosbot_hardware_interfaces + + rclpy python3-pytest - launch - launch_ros launch_pytest - xacro + sensor_msgs nav_msgs - rosbot_xl_description - ros_components_description - - controller_manager - joint_state_broadcaster - imu_sensor_broadcaster - diff_drive_controller - mecanum_drive_controller - ament_python diff --git a/rosbot_xl_controller/setup.py b/rosbot_xl_controller/setup.py index 197fbfd3..1bb857ba 100644 --- a/rosbot_xl_controller/setup.py +++ b/rosbot_xl_controller/setup.py @@ -1,4 +1,4 @@ -# Copyright 2023 Husarion +# Copyright 2024 Husarion sp. z o.o. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/rosbot_xl_controller/test/test_diff_drive_controllers.py b/rosbot_xl_controller/test/test_diff_drive_controllers.py index 86c9b745..c7d1ee86 100644 --- a/rosbot_xl_controller/test/test_diff_drive_controllers.py +++ b/rosbot_xl_controller/test/test_diff_drive_controllers.py @@ -1,6 +1,6 @@ # Copyright 2021 Open Source Robotics Foundation, Inc. # Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2023 Husarion +# Copyright 2024 Husarion sp. z o.o. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -23,7 +23,8 @@ from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import ControllersTestNode +from test_utils import ControllersTestNode, controller_test +from threading import Thread @launch_pytest.fixture @@ -31,16 +32,17 @@ def generate_test_description(): rosbot_xl_controller = get_package_share_directory("rosbot_xl_controller") bringup_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - PathJoinSubstitution([ - rosbot_xl_controller, - "launch", - "controller.launch.py", - ]) + PathJoinSubstitution( + [ + rosbot_xl_controller, + "launch", + "controller.launch.py", + ] + ) ), launch_arguments={ "use_sim": "False", "mecanum": "False", - "use_gpu": "False", }.items(), ) @@ -51,49 +53,35 @@ def generate_test_description(): def test_controllers_startup_fail(): rclpy.init() try: - node = ControllersTestNode("test_controllers_bringup") - node.create_test_subscribers_and_publishers() + node = ControllersTestNode("test_controllers_startup_fail") - node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + Thread(target=lambda node: rclpy.spin(node), args=(node,)).start() + msgs_received_flag = node.joint_state_msg_event.wait(10.0) assert not msgs_received_flag, ( "Received JointStates message that should not have appeared. Check whether other" - " robots are connected to your network!" + " robots are connected to your network.!" ) - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + msgs_received_flag = node.odom_msg_event.wait(10.0) assert not msgs_received_flag, ( "Received Odom message that should not have appeared. Check whether other robots are" - " connected to your network!" + " connected to your network.!" ) - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + msgs_received_flag = node.imu_msg_event.wait(10.0) assert not msgs_received_flag, ( "Received Imu message that should not have appeared. Check whether other robots are" - " connected to your network!" + " connected to your network.!" ) finally: rclpy.shutdown() @pytest.mark.launch(fixture=generate_test_description) -def test_controllers_startup_success(): +def test_controllers_startup(): rclpy.init() try: - node = ControllersTestNode("test_controllers_bringup") - node.create_test_subscribers_and_publishers() + node = ControllersTestNode("test_controllers_startup") node.start_publishing_fake_hardware() - - node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Odom message but it was not received. Check rosbot_base_controller!" - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Imu message but it was not received. Check imu_broadcaster!" + Thread(target=lambda node: rclpy.spin(node), args=(node,)).start() + controller_test(node) finally: rclpy.shutdown() diff --git a/rosbot_xl_controller/test/test_mecanum_controllers.py b/rosbot_xl_controller/test/test_mecanum_controllers.py index 69df35d9..12530dab 100644 --- a/rosbot_xl_controller/test/test_mecanum_controllers.py +++ b/rosbot_xl_controller/test/test_mecanum_controllers.py @@ -1,6 +1,6 @@ # Copyright 2021 Open Source Robotics Foundation, Inc. # Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2023 Husarion +# Copyright 2024 Husarion sp. z o.o. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -18,12 +18,14 @@ import pytest import rclpy + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import ControllersTestNode +from test_utils import ControllersTestNode, controller_test +from threading import Thread @launch_pytest.fixture @@ -31,16 +33,17 @@ def generate_test_description(): rosbot_xl_controller = get_package_share_directory("rosbot_xl_controller") bringup_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - PathJoinSubstitution([ - rosbot_xl_controller, - "launch", - "controller.launch.py", - ]) + PathJoinSubstitution( + [ + rosbot_xl_controller, + "launch", + "controller.launch.py", + ] + ) ), launch_arguments={ "use_sim": "False", "mecanum": "True", - "use_gpu": "False", }.items(), ) @@ -51,49 +54,35 @@ def generate_test_description(): def test_controllers_startup_fail(): rclpy.init() try: - node = ControllersTestNode("test_controllers_bringup") - node.create_test_subscribers_and_publishers() + node = ControllersTestNode("test_controllers_startup_fail") - node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + Thread(target=lambda node: rclpy.spin(node), args=(node,)).start() + msgs_received_flag = node.joint_state_msg_event.wait(10.0) assert not msgs_received_flag, ( "Received JointStates message that should not have appeared. Check whether other" - " robots are connected to your network!" + " robots are connected to your network.!" ) - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + msgs_received_flag = node.odom_msg_event.wait(10.0) assert not msgs_received_flag, ( "Received Odom message that should not have appeared. Check whether other robots are" - " connected to your network!" + " connected to your network.!" ) - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + msgs_received_flag = node.imu_msg_event.wait(10.0) assert not msgs_received_flag, ( "Received Imu message that should not have appeared. Check whether other robots are" - " connected to your network!" + " connected to your network.!" ) finally: rclpy.shutdown() @pytest.mark.launch(fixture=generate_test_description) -def test_controllers_startup_success(): +def test_controllers_startup(): rclpy.init() try: - node = ControllersTestNode("test_controllers_bringup") - node.create_test_subscribers_and_publishers() + node = ControllersTestNode("test_controllers_startup") node.start_publishing_fake_hardware() - - node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Odom message but it was not received. Check rosbot_base_controller!" - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Imu message but it was not received. Check imu_broadcaster!" + Thread(target=lambda node: rclpy.spin(node), args=(node,)).start() + controller_test(node) finally: rclpy.shutdown() diff --git a/rosbot_xl_controller/test/test_multirobot_controllers.py b/rosbot_xl_controller/test/test_multirobot_controllers.py new file mode 100644 index 00000000..6c699f8a --- /dev/null +++ b/rosbot_xl_controller/test/test_multirobot_controllers.py @@ -0,0 +1,72 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2024 Husarion sp. z o.o. +# +# 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 launch_pytest +import pytest +import rclpy + + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, TimerAction +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import ControllersTestNode, controller_test +from threading import Thread + +robot_names = ["rosbot_xl1", "rosbot_xl2", "rosbot_xl3"] + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_controller = get_package_share_directory("rosbot_xl_controller") + actions = [] + for i in range(len(robot_names)): + controller_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_controller, + "launch", + "controller.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "True", + "namespace": robot_names[i], + }.items(), + ) + + # When there is no delay the controllers doesn't spawn correctly + delayed_controller_launch = TimerAction(period=i * 5.0, actions=[controller_launch]) + actions.append(delayed_controller_launch) + + return LaunchDescription(actions) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_multirobot_controllers_startup_success(): + for robot_name in robot_names: + rclpy.init() + try: + node = ControllersTestNode(f"test_{robot_name}_controllers", namespace=robot_name) + node.start_publishing_fake_hardware() + Thread(target=lambda node: rclpy.spin(node), args=(node,)).start() + controller_test(node, robot_name) + finally: + rclpy.shutdown() diff --git a/rosbot_xl_controller/test/test_namespaced_diff_drive_controllers.py b/rosbot_xl_controller/test/test_namespaced_diff_drive_controllers.py new file mode 100644 index 00000000..a9b6b056 --- /dev/null +++ b/rosbot_xl_controller/test/test_namespaced_diff_drive_controllers.py @@ -0,0 +1,63 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2024 Husarion sp. z o.o. +# +# 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 launch_pytest +import pytest +import rclpy + + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import ControllersTestNode, controller_test +from threading import Thread + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_xl_controller = get_package_share_directory("rosbot_xl_controller") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_xl_controller, + "launch", + "controller.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "False", + "namespace": "rosbot_xl", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_controllers_startup(): + rclpy.init() + try: + node = ControllersTestNode("test_namespaced_controllers_startup", namespace="rosbot_xl") + node.start_publishing_fake_hardware() + Thread(target=lambda node: rclpy.spin(node), args=(node,)).start() + controller_test(node) + finally: + rclpy.shutdown() diff --git a/rosbot_xl_controller/test/test_namespaced_mecanum_controllers.py b/rosbot_xl_controller/test/test_namespaced_mecanum_controllers.py new file mode 100644 index 00000000..b3a2beed --- /dev/null +++ b/rosbot_xl_controller/test/test_namespaced_mecanum_controllers.py @@ -0,0 +1,63 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2024 Husarion sp. z o.o. +# +# 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 launch_pytest +import pytest +import rclpy + + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import ControllersTestNode, controller_test +from threading import Thread + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_xl_controller = get_package_share_directory("rosbot_xl_controller") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_xl_controller, + "launch", + "controller.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "True", + "namespace": "rosbot_xl", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_controllers_startup(): + rclpy.init() + try: + node = ControllersTestNode("test_namespaced_controllers_startup", namespace="rosbot_xl") + node.start_publishing_fake_hardware() + Thread(target=lambda node: rclpy.spin(node), args=(node,)).start() + controller_test(node) + finally: + rclpy.shutdown() diff --git a/rosbot_xl_controller/test/test_utils.py b/rosbot_xl_controller/test/test_utils.py index ecf984c8..a6a386c9 100644 --- a/rosbot_xl_controller/test/test_utils.py +++ b/rosbot_xl_controller/test/test_utils.py @@ -1,5 +1,5 @@ # Copyright 2021 Open Source Robotics Foundation, Inc. -# Copyright 2023 Husarion +# Copyright 2024 Husarion sp. z o.o. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -13,13 +13,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -import rclpy from threading import Event -from threading import Thread - from rclpy.node import Node - from sensor_msgs.msg import JointState, Imu from nav_msgs.msg import Odometry @@ -29,33 +25,29 @@ class ControllersTestNode(Node): __test__ = False - def __init__(self, name="test_node"): - super().__init__(name) + def __init__(self, name="test_node", namespace=None): + super().__init__(name, namespace=namespace) self.joint_state_msg_event = Event() self.odom_msg_event = Event() self.imu_msg_event = Event() - def create_test_subscribers_and_publishers(self): self.joint_state_sub = self.create_subscription( - JointState, "/joint_states", self.joint_states_callback, 10 + JointState, "joint_states", self.joint_states_callback, 10 ) self.odom_sub = self.create_subscription( - Odometry, "/rosbot_xl_base_controller/odom", self.odometry_callback, 10 + Odometry, "rosbot_xl_base_controller/odom", self.odometry_callback, 10 ) - self.imu_sub = self.create_subscription(Imu, "/imu_broadcaster/imu", self.imu_callback, 10) + self.imu_sub = self.create_subscription(Imu, "imu_broadcaster/imu", self.imu_callback, 10) - self.imu_publisher = self.create_publisher(Imu, "_imu/data_raw", 10) + # TODO: @delihus namespaces have not been implemented in microros yet + self.imu_pub = self.create_publisher(Imu, "/_imu/data_raw", 10) - self.joint_states_publisher = self.create_publisher(JointState, "_motors_response", 10) + self.joint_pub = self.create_publisher(JointState, "/_motors_response", 10) self.timer = None - def start_node_thread(self): - self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,)) - self.ros_spin_thread.start() - def joint_states_callback(self, data): self.joint_state_msg_event.set() @@ -71,6 +63,7 @@ def start_publishing_fake_hardware(self): self.publish_fake_hardware_messages, ) + # TODO: @delihus namespaces have not been implemented in microros yet def publish_fake_hardware_messages(self): imu_msg = Imu() imu_msg.header.stamp = self.get_clock().now().to_msg() @@ -87,5 +80,22 @@ def publish_fake_hardware_messages(self): joint_state_msg.position = [0.0, 0.0, 0.0, 0.0] joint_state_msg.velocity = [0.0, 0.0, 0.0, 0.0] - self.imu_publisher.publish(imu_msg) - self.joint_states_publisher.publish(joint_state_msg) + self.imu_pub.publish(imu_msg) + self.joint_pub.publish(joint_state_msg) + + +def controller_test(node, robot_name="ROSbot"): + msgs_received_flag = node.joint_state_msg_event.wait(20.0) + assert msgs_received_flag, ( + f"{robot_name}: Expected JointStates message but it was not received. Check " + "joint_state_broadcaster!" + ) + msgs_received_flag = node.odom_msg_event.wait(20.0) + assert msgs_received_flag, ( + f"{robot_name}: Expected Odom message but it was not received. Check " + "rosbot_xl_base_controller!" + ) + msgs_received_flag = node.imu_msg_event.wait(20.0) + assert ( + msgs_received_flag + ), f"{robot_name}: Expected Imu message but it was not received. Check imu_broadcaster!" diff --git a/rosbot_xl_controller/test/test_xacro.py b/rosbot_xl_controller/test/test_xacro.py index f470b412..0ef5dfa7 100644 --- a/rosbot_xl_controller/test/test_xacro.py +++ b/rosbot_xl_controller/test/test_xacro.py @@ -1,4 +1,4 @@ -# Copyright 2023 Husarion +# Copyright 2024 Husarion sp. z o.o. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -21,9 +21,8 @@ def test_rosbot_description_parsing(): mecanum_values = ["true", "false"] use_sim_values = ["true", "false"] - use_gpu_values = ["true", "false"] simulation_engine_values = ["ignition-gazebo", "webots"] # 'gazebo-classic' - lidar_model = [ + lidar_models = [ "None", "slamtec_rplidar_a2", "slamtec_rplidar_a3", @@ -32,7 +31,7 @@ def test_rosbot_description_parsing(): "slamtec_rplidar_s3", "velodyne_puck", ] - camera_model = [ + camera_models = [ "None", "intel_realsense_d435", "orbbec_astra", @@ -48,10 +47,9 @@ def test_rosbot_description_parsing(): itertools.product( mecanum_values, use_sim_values, - use_gpu_values, simulation_engine_values, - lidar_model, - camera_model, + lidar_models, + camera_models, ) ) @@ -59,7 +57,6 @@ def test_rosbot_description_parsing(): ( mecanum, use_sim, - use_gpu, simulation_engine, lidar_model, camera_model, @@ -67,7 +64,6 @@ def test_rosbot_description_parsing(): mappings = { "mecanum": mecanum, "use_sim": use_sim, - "use_gpu": use_gpu, "simulation_engine": simulation_engine, "lidar_model": lidar_model, "camera_model": camera_model, @@ -79,6 +75,6 @@ def test_rosbot_description_parsing(): except xacro.XacroException as e: assert False, ( f"xacro parsing failed: {str(e)} for mecanum: {mecanum}, " - f"use_sim: {use_sim}, use_gpu: {use_gpu}, simulation_engine: {simulation_engine}, " - f"lidar_model: {lidar_model}, camera_model: {camera_model}" + f"use_sim: {use_sim}, simulation_engine: {simulation_engine}, " + f"lidar_model: {lidar_model}, camera_model: {camera_model}, " ) diff --git a/rosbot_xl_description/package.xml b/rosbot_xl_description/package.xml index bd4913ad..857edad7 100644 --- a/rosbot_xl_description/package.xml +++ b/rosbot_xl_description/package.xml @@ -1,5 +1,6 @@ + rosbot_xl_description 0.8.12 @@ -18,9 +19,7 @@ ament_cmake - xacro - xacro - + ros_components_description diff --git a/rosbot_xl_description/urdf/rosbot_xl.urdf.xacro b/rosbot_xl_description/urdf/rosbot_xl.urdf.xacro index e05cb2d9..a994bc39 100644 --- a/rosbot_xl_description/urdf/rosbot_xl.urdf.xacro +++ b/rosbot_xl_description/urdf/rosbot_xl.urdf.xacro @@ -7,14 +7,15 @@ - + - + namespace="$(arg namespace)" /> @@ -42,7 +43,8 @@ xyz="${lidar_xyz}" rpy="${lidar_rpy}" use_gpu="${lidar_use_gpu}" - simulation_engine="$(arg simulation_engine)" /> + simulation_engine="$(arg simulation_engine)" + namespace="$(arg namespace)" /> @@ -53,7 +55,8 @@ xyz="${lidar_xyz}" rpy="${lidar_rpy}" use_gpu="${lidar_use_gpu}" - simulation_engine="$(arg simulation_engine)" /> + simulation_engine="$(arg simulation_engine)" + namespace="$(arg namespace)" /> @@ -64,7 +67,8 @@ xyz="${lidar_xyz}" rpy="${lidar_rpy}" use_gpu="${lidar_use_gpu}" - simulation_engine="$(arg simulation_engine)" /> + simulation_engine="$(arg simulation_engine)" + namespace="$(arg namespace)" /> @@ -75,7 +79,8 @@ xyz="${lidar_xyz}" rpy="${lidar_rpy}" use_gpu="${lidar_use_gpu}" - simulation_engine="$(arg simulation_engine)" /> + simulation_engine="$(arg simulation_engine)" + namespace="$(arg namespace)" /> @@ -86,7 +91,8 @@ xyz="${lidar_xyz}" rpy="${lidar_rpy}" use_gpu="${lidar_use_gpu}" - simulation_engine="$(arg simulation_engine)" /> + simulation_engine="$(arg simulation_engine)" + namespace="$(arg namespace)" /> @@ -98,7 +104,8 @@ rpy="${lidar_rpy}" use_gpu="${lidar_use_gpu}" simulation_engine="$(arg simulation_engine)" - topic="velodyne_points" /> + topic="velodyne_points" + namespace="$(arg namespace)" /> @@ -119,7 +126,8 @@ xyz="${camera_xyz}" rpy="${camera_rpy}" use_nominal_extrinsics="$(arg use_sim)" - simulation_engine="$(arg simulation_engine)" /> + simulation_engine="$(arg simulation_engine)" + namespace="$(arg namespace)" /> @@ -130,7 +138,8 @@ parent_link="${camera_parent_link}" xyz="${camera_xyz}" rpy="${camera_rpy}" - simulation_engine="$(arg simulation_engine)" /> + simulation_engine="$(arg simulation_engine)" + namespace="$(arg namespace)" /> @@ -161,7 +170,8 @@ xyz="${camera_xyz}" rpy="${camera_rpy}" model="${camera_model_type}" - simulation_engine="$(arg simulation_engine)" /> + simulation_engine="$(arg simulation_engine)" + namespace="$(arg namespace)" /> diff --git a/rosbot_xl_description/urdf/rosbot_xl_macro.urdf.xacro b/rosbot_xl_description/urdf/rosbot_xl_macro.urdf.xacro index c152b269..84d01f6c 100644 --- a/rosbot_xl_description/urdf/rosbot_xl_macro.urdf.xacro +++ b/rosbot_xl_description/urdf/rosbot_xl_macro.urdf.xacro @@ -1,7 +1,16 @@ + params="use_sim mecanum simulation_engine namespace"> + + + + + + + + + @@ -18,24 +27,24 @@ - - - - - + rosbot_hardware_interfaces/RosbotImuSensor 120000 500 - + @@ -50,7 +59,7 @@ - + @@ -99,7 +108,7 @@ - + @@ -118,25 +127,34 @@ - - ${simulation_controllers_config_file} - simulation + + + + $(find rosbot_xl_controller)/config/mecanum_drive_controller.yaml + + + $(find rosbot_xl_controller)/config/diff_drive_controller.yaml + + - /rosbot_xl_base_controller/cmd_vel_unstamped:=/cmd_vel + ${gz_control_namespace} + rosbot_xl_base_controller/cmd_vel_unstamped:=cmd_vel + /tf:=tf - - + true 25 - imu/data_raw + ${ns}imu/data_raw false false imu_link imu_link + + ${ns} + diff --git a/rosbot_xl_description/urdf/wheel.urdf.xacro b/rosbot_xl_description/urdf/wheel.urdf.xacro index 437ade74..4df9b648 100644 --- a/rosbot_xl_description/urdf/wheel.urdf.xacro +++ b/rosbot_xl_description/urdf/wheel.urdf.xacro @@ -1,6 +1,6 @@ - + @@ -28,28 +28,28 @@ - + - + - + - + @@ -57,16 +57,16 @@ - + - + - + @@ -90,7 +90,7 @@ - + diff --git a/rosbot_xl_gazebo/config/gz_intel_realsense_d435_remappings.yaml b/rosbot_xl_gazebo/config/gz_intel_realsense_d435_remappings.yaml new file mode 100644 index 00000000..3b58a150 --- /dev/null +++ b/rosbot_xl_gazebo/config/gz_intel_realsense_d435_remappings.yaml @@ -0,0 +1,27 @@ +--- +# https://github.com/IntelRealSense/realsense-ros#published-topics + - topic_name: /camera/color/camera_info + ros_type_name: sensor_msgs/msg/CameraInfo + gz_type_name: ignition.msgs.CameraInfo + lazy: true + + - topic_name: /camera/color/image_raw + ros_type_name: sensor_msgs/msg/Image + gz_type_name: ignition.msgs.Image + lazy: true + + - topic_name: /camera/depth/camera_info + ros_type_name: sensor_msgs/msg/CameraInfo + gz_type_name: ignition.msgs.CameraInfo + lazy: true + + - topic_name: /camera/depth/image_rect_raw + ros_type_name: sensor_msgs/msg/Image + gz_type_name: ignition.msgs.Image + lazy: true + + - ros_topic_name: /camera/depth/color/points + gz_topic_name: /camera/depth/image_rect_raw/points + ros_type_name: sensor_msgs/msg/PointCloud2 + gz_type_name: ignition.msgs.PointCloudPacked + lazy: true diff --git a/rosbot_xl_gazebo/config/gz_orbbec_astra_remappings.yaml b/rosbot_xl_gazebo/config/gz_orbbec_astra_remappings.yaml new file mode 100644 index 00000000..c77c1e89 --- /dev/null +++ b/rosbot_xl_gazebo/config/gz_orbbec_astra_remappings.yaml @@ -0,0 +1,27 @@ +--- +# https://github.com/orbbec/OrbbecSDK_ROS2#all-available-topics + - topic_name: /camera/color/camera_info + ros_type_name: sensor_msgs/msg/CameraInfo + gz_type_name: ignition.msgs.CameraInfo + lazy: true + + - topic_name: /camera/color/image_raw + ros_type_name: sensor_msgs/msg/Image + gz_type_name: ignition.msgs.Image + lazy: true + + - topic_name: /camera/depth/camera_info + ros_type_name: sensor_msgs/msg/CameraInfo + gz_type_name: ignition.msgs.CameraInfo + lazy: true + + - topic_name: /camera/depth/image_raw + ros_type_name: sensor_msgs/msg/Image + gz_type_name: ignition.msgs.Image + lazy: true + + - ros_topic_name: /camera/depth/points + gz_topic_name: /camera/depth/image_raw/points + ros_type_name: sensor_msgs/msg/PointCloud2 + gz_type_name: ignition.msgs.PointCloudPacked + lazy: true diff --git a/rosbot_xl_gazebo/config/gz_slamtec_rplidar_remappings.yaml b/rosbot_xl_gazebo/config/gz_slamtec_rplidar_remappings.yaml new file mode 100644 index 00000000..e313dcb6 --- /dev/null +++ b/rosbot_xl_gazebo/config/gz_slamtec_rplidar_remappings.yaml @@ -0,0 +1,4 @@ +--- + - topic_name: /scan + ros_type_name: sensor_msgs/msg/LaserScan + gz_type_name: ignition.msgs.LaserScan diff --git a/rosbot_xl_gazebo/config/gz_stereolabs_zed_remappings.yaml b/rosbot_xl_gazebo/config/gz_stereolabs_zed_remappings.yaml new file mode 100644 index 00000000..dee8f678 --- /dev/null +++ b/rosbot_xl_gazebo/config/gz_stereolabs_zed_remappings.yaml @@ -0,0 +1,23 @@ +--- + - topic_name: /zed_node/rgb/camera_info + ros_type_name: sensor_msgs/msg/CameraInfo + gz_type_name: ignition.msgs.CameraInfo + + - topic_name: /zed_node/rgb/image_rect_color + ros_type_name: sensor_msgs/msg/Image + gz_type_name: ignition.msgs.Image + + - ros_topic_name: /zed_node/depth/camera_info + gz_topic_name: /zed_node/camera_info + ros_type_name: sensor_msgs/msg/CameraInfo + gz_type_name: ignition.msgs.CameraInfo + + - ros_topic_name: /zed_node/depth/depth_registered + gz_topic_name: /zed_node/depth + ros_type_name: sensor_msgs/msg/Image + gz_type_name: ignition.msgs.Image + + - ros_topic_name: /zed_node/point_cloud/cloud_registered + gz_topic_name: /zed_node/depth/points + ros_type_name: sensor_msgs/msg/PointCloud2 + gz_type_name: ignition.msgs.PointCloudPacked diff --git a/rosbot_xl_gazebo/config/gz_velodyne_puck_remappings.yaml b/rosbot_xl_gazebo/config/gz_velodyne_puck_remappings.yaml new file mode 100644 index 00000000..0a7b2caa --- /dev/null +++ b/rosbot_xl_gazebo/config/gz_velodyne_puck_remappings.yaml @@ -0,0 +1,6 @@ +--- +# https://github.com/ros-drivers/velodyne/tree/humble-devel/velodyne_pointcloud#published-topics + - ros_topic_name: velodyne_points + gz_topic_name: velodyne_points/points + ros_type_name: sensor_msgs/msg/PointCloud2 + gz_type_name: ignition.msgs.PointCloudPacked diff --git a/rosbot_xl_gazebo/launch/simulation.launch.py b/rosbot_xl_gazebo/launch/simulation.launch.py index 51420190..bf48d400 100644 --- a/rosbot_xl_gazebo/launch/simulation.launch.py +++ b/rosbot_xl_gazebo/launch/simulation.launch.py @@ -1,4 +1,4 @@ -# Copyright 2023 Husarion +# Copyright 2024 Husarion sp. z o.o. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,136 +12,128 @@ # See the License for the specific language governing permissions and # limitations under the License. -from launch import LaunchDescription, LaunchContext +from launch import LaunchDescription from launch.actions import ( IncludeLaunchDescription, DeclareLaunchArgument, OpaqueFunction, + LogInfo, + GroupAction, ) from launch.substitutions import ( + EnvironmentVariable, PathJoinSubstitution, - PythonExpression, LaunchConfiguration, + TextSubstitution, ) from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import Node, SetParameter +from launch_ros.actions import SetParameter from ament_index_python.packages import get_package_share_directory +from nav2_common.launch import ParseMultiRobotPose -def launch_gz_bridge(context: LaunchContext, *args, **kwargs): + +def launch_setup(context, *args, **kwargs): + namespace = LaunchConfiguration("namespace").perform(context) + mecanum = LaunchConfiguration("mecanum").perform(context) camera_model = context.perform_substitution(LaunchConfiguration("camera_model")) lidar_model = context.perform_substitution(LaunchConfiguration("lidar_model")) + world = LaunchConfiguration("world").perform(context) + headless = LaunchConfiguration("headless").perform(context) + x = LaunchConfiguration("x", default="0.0").perform(context) + y = LaunchConfiguration("y", default="2.0").perform(context) + z = LaunchConfiguration("z", default="0.0").perform(context) + roll = LaunchConfiguration("roll", default="0.0").perform(context) + pitch = LaunchConfiguration("pitch", default="0.0").perform(context) + yaw = LaunchConfiguration("yaw", default="0.0").perform(context) - gz_args = ["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"] - gz_remapping = [] - depth_camera_parent_tf = None - - # Add camera topic - if camera_model.startswith("intel_realsense"): - gz_args.append( - "/camera/color/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo" - ) - gz_args.append("/camera/color/image_raw@sensor_msgs/msg/Image[ignition.msgs.Image") - gz_args.append("/camera/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo") - gz_args.append("/camera/depth@sensor_msgs/msg/Image[ignition.msgs.Image") - gz_args.append( - "/camera/depth/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked" - ) - - gz_remapping.append(("/camera/camera_info", "/camera/depth/camera_info")) - gz_remapping.append(("/camera/depth", "/camera/depth/image_raw")) - - depth_camera_parent_tf = "camera_depth_frame" + gz_args = f"--headless-rendering -s -v 4 -r {world}" if eval(headless) else f"-r {world}" - if camera_model == "orbbec_astra": - gz_args.append("/camera/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo") - gz_args.append("/camera/image@sensor_msgs/msg/Image[ignition.msgs.Image") - gz_args.append("/camera/depth_image@sensor_msgs/msg/Image[ignition.msgs.Image") - gz_args.append("/camera/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked") - - gz_remapping.append(("/camera/camera_info", "/camera/depth/camera_info")) - gz_remapping.append(("/camera/depth", "/camera/depth/image_raw")) - gz_remapping.append(("/camera/depth_image", "/camera/depth/image_raw")) - gz_remapping.append(("/camera/points", "/camera/depth/points")) - - elif camera_model.startswith("stereolabs_zed"): - zed = camera_model[len("stereolabs_") :] - - gz_args.append( - f"/{zed}/zed_node/rgb/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo" - ) - gz_args.append( - f"/{zed}/zed_node/rgb/image_rect_color@sensor_msgs/msg/Image[ignition.msgs.Image" - ) - gz_args.append( - f"/{zed}/zed_node/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo" - ) - gz_args.append(f"/{zed}/zed_node/depth@sensor_msgs/msg/Image[ignition.msgs.Image") - gz_args.append( - f"/{zed}/zed_node/depth/points@sensor_msgs/msg/PointCloud2" - "[ignition.msgs.PointCloudPacked" - ) + gz_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + get_package_share_directory("ros_gz_sim"), + "launch", + "gz_sim.launch.py", + ] + ) + ), + launch_arguments={ + "gz_args": gz_args, + "on_exit_shutdown": "True", + }.items(), + ) - gz_remapping.append((f"{zed}/zed_node/camera_info", f"/{zed}/zed_node/depth/camera_info")) - gz_remapping.append((f"{zed}/zed_node/depth", f"/{zed}/zed_node/depth/depth_registered")) - gz_remapping.append( - (f"{zed}/zed_node/depth/points", f"/{zed}/zed_node/point_cloud/cloud_registered") + logger = LogInfo(msg=["Robots:", LaunchConfiguration("robots")]) + robots_list = ParseMultiRobotPose("robots").value() + if len(robots_list) == 0: + robots_list = { + namespace: { + "x": x, + "y": y, + "z": z, + "roll": roll, + "pitch": pitch, + "yaw": yaw, + } + } + + spawn_group = [] + for robot_name in robots_list: + init_pose = robots_list[robot_name] + spawn_robot = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + get_package_share_directory("rosbot_xl_gazebo"), + "launch", + "spawn.launch.py", + ] + ) + ), + launch_arguments={ + "mecanum": mecanum, + "use_sim": "True", + "lidar_model": lidar_model, + "camera_model": camera_model, + "simulation_engine": "ignition-gazebo", + "namespace": TextSubstitution(text=robot_name), + "x": TextSubstitution(text=str(init_pose["x"])), + "y": TextSubstitution(text=str(init_pose["y"])), + "z": TextSubstitution(text=str(init_pose["z"])), + "roll": TextSubstitution(text=str(init_pose["roll"])), + "pitch": TextSubstitution(text=str(init_pose["pitch"])), + "yaw": TextSubstitution(text=str(init_pose["yaw"])), + }.items(), ) - - depth_camera_parent_tf = "camera_center_optical_frame" - else: - pass - - # Add lidar topic - if lidar_model.startswith("slamtec_rplidar"): - gz_args.append("/scan@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan") - - elif lidar_model.startswith("velodyne"): - gz_args.append( - "/velodyne_points/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked" + group = GroupAction( + [ + LogInfo( + msg=[ + "Launching namespace=", + robot_name, + " init_pose=", + str(init_pose), + ] + ), + spawn_robot, + ] ) + spawn_group.append(group) - gz_remapping.append(("/velodyne_points/points", "/velodyne_points")) - elif lidar_model.startswith("ouster"): - gz_args.append( - "/velodyne_points/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked" - ) + return [logger, gz_sim, *spawn_group] - gz_remapping.append(("/velodyne_points/points", "/points")) - gz_bridge_node = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="ign_bridge", - arguments=gz_args, - remappings=gz_remapping, - output="screen", +def generate_launch_description(): + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), + description="Namespace for all topics and tfs", ) - if depth_camera_parent_tf: - # The frame of the point cloud from ignition gazebo 6 isn't provided by . - # See https://github.com/gazebosim/gz-sensors/issues/239 - point_cloud_tf = Node( - package="tf2_ros", - executable="static_transform_publisher", - name="point_cloud_tf", - output="log", - arguments=[ - "--child-frame-id", - "rosbot_xl/base_link/camera_depth", - "--frame-id", - depth_camera_parent_tf, - ], - ) - return [gz_bridge_node, point_cloud_tf] - else: - return [gz_bridge_node] - - -def generate_launch_description(): - mecanum = LaunchConfiguration("mecanum") declare_mecanum_arg = DeclareLaunchArgument( "mecanum", default_value="False", @@ -150,7 +142,6 @@ def generate_launch_description(): ), ) - camera_model = LaunchConfiguration("camera_model") declare_camera_model_arg = DeclareLaunchArgument( "camera_model", default_value="intel_realsense_d435", @@ -168,7 +159,6 @@ def generate_launch_description(): ], ) - lidar_model = LaunchConfiguration("lidar_model") declare_lidar_model_arg = DeclareLaunchArgument( "lidar_model", default_value="slamtec_rplidar_s3", @@ -184,7 +174,6 @@ def generate_launch_description(): ], ) - include_camera_mount = LaunchConfiguration("include_camera_mount") declare_include_camera_mount_arg = DeclareLaunchArgument( "include_camera_mount", default_value="False", @@ -193,89 +182,38 @@ def generate_launch_description(): world_package = get_package_share_directory("husarion_office_gz") world_file = PathJoinSubstitution([world_package, "worlds", "husarion_world.sdf"]) - world_cfg = LaunchConfiguration("world") declare_world_arg = DeclareLaunchArgument( "world", default_value=world_file, description="SDF world file" ) - headless = LaunchConfiguration("headless") declare_headless_arg = DeclareLaunchArgument( "headless", default_value="False", description="Run Gazebo Ignition in the headless mode", ) - headless_cfg = PythonExpression([ - "'--headless-rendering -s -r' if ", - headless, - " else '-r'", - ]) - gz_args = [headless_cfg, " ", world_cfg] - - gz_sim = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - get_package_share_directory("ros_gz_sim"), - "launch", - "gz_sim.launch.py", - ]) + declare_robots_arg = DeclareLaunchArgument( + "robots", + default_value=[], + description=( + "The list of the robots spawned in the simulation e. g. robots:='robot1={x: 0.0, y:" + " -1.0}; robot2={x: 1.0, y: -1.0}; robot3={x: 2.0, y: -1.0}; robot4={x: 3.0, y: -1.0}'" ), - launch_arguments={ - "gz_args": gz_args, - "on_exit_shutdown": "True", - }.items(), ) - gz_spawn_entity = Node( - package="ros_gz_sim", - executable="create", - arguments=[ - "-name", - "rosbot_xl", - "-allow_renaming", - "true", - "-topic", - "robot_description", - "-x", - "0", - "-y", - "2.0", - "-z", - "0.2", - ], - output="screen", + return LaunchDescription( + [ + declare_namespace_arg, + declare_mecanum_arg, + declare_lidar_model_arg, + declare_camera_model_arg, + declare_include_camera_mount_arg, + declare_world_arg, + declare_headless_arg, + declare_robots_arg, + # Sets use_sim_time for all nodes started below + # (doesn't work for nodes started from ignition gazebo) + SetParameter(name="use_sim_time", value=True), + OpaqueFunction(function=launch_setup), + ] ) - - bringup_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - get_package_share_directory("rosbot_xl_bringup"), - "launch", - "bringup.launch.py", - ]) - ), - launch_arguments={ - "mecanum": mecanum, - "lidar_model": lidar_model, - "camera_model": camera_model, - "include_camera_mount": include_camera_mount, - "use_sim": "True", - "simulation_engine": "ignition-gazebo", - }.items(), - ) - - return LaunchDescription([ - declare_mecanum_arg, - declare_lidar_model_arg, - declare_camera_model_arg, - declare_include_camera_mount_arg, - declare_world_arg, - declare_headless_arg, - # Sets use_sim_time for all nodes started below - # (doesn't work for nodes started from ignition gazebo) - SetParameter(name="use_sim_time", value=True), - gz_sim, - OpaqueFunction(function=launch_gz_bridge), - gz_spawn_entity, - bringup_launch, - ]) diff --git a/rosbot_xl_gazebo/launch/spawn.launch.py b/rosbot_xl_gazebo/launch/spawn.launch.py new file mode 100644 index 00000000..010633ba --- /dev/null +++ b/rosbot_xl_gazebo/launch/spawn.launch.py @@ -0,0 +1,298 @@ +# Copyright 2024 Husarion sp. z o.o. +# +# 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 launch import LaunchDescription, LaunchContext +from launch.actions import ( + IncludeLaunchDescription, + DeclareLaunchArgument, + OpaqueFunction, +) +from launch.substitutions import ( + PathJoinSubstitution, + LaunchConfiguration, + PythonExpression, +) +from launch.conditions import LaunchConfigurationNotEquals +from launch.launch_description_sources import PythonLaunchDescriptionSource +from nav2_common.launch import ReplaceString +from launch_ros.actions import Node, SetParameter + +from ament_index_python.packages import get_package_share_directory + + +def launch_gz_bridge(context: LaunchContext, *args, **kwargs): + lidar_model = context.perform_substitution(LaunchConfiguration("lidar_model")) + camera_model = context.perform_substitution(LaunchConfiguration("camera_model")) + namespace = context.perform_substitution(LaunchConfiguration("namespace")) + + pointcloud_rpy = [ + "1.57", + "-1.57", + "0", + ] + + namespace_ext = "" if namespace == "" else "/" + namespace + robot_name = "rosbot_xl" + if namespace != "": + robot_name = namespace + + depth_camera_child_tf = ( + robot_name + "/base_link" + namespace_ext + "/camera_" + camera_model + "_depth" + ) + depth_camera_parent_tf = "camera_depth_optical_frame" + + if lidar_model.startswith("slamtec_rplidar"): + lidar_model = "slamtec_rplidar" + + gz_lidar_remappings_file = PathJoinSubstitution( + [ + get_package_share_directory("rosbot_xl_gazebo"), + "config", + LaunchConfiguration( + "gz_lidar_remappings_file", + default=["gz_", lidar_model, "_remappings.yaml"], + ), + ] + ) + + namespaced_gz_lidar_remappings_file = ReplaceString( + source_file=gz_lidar_remappings_file, + replacements={"": namespace_ext}, + ) + + ign_lidar_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="ros_gz_lidar_bridge", + parameters=[{"config_file": namespaced_gz_lidar_remappings_file}], + remappings=[ + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ], + output="screen", + namespace=namespace, + condition=LaunchConfigurationNotEquals(lidar_model, "None"), + ) + + zed_model = None + if camera_model.startswith("stereolabs_zed"): + zed_model = camera_model.replace("stereolabs_", "") + camera_model = "stereolabs_zed" + depth_camera_child_tf = "rosbot_xl/base_link/camera_" + camera_model + "_depth" + depth_camera_parent_tf = "camera_center_optical_frame" + pointcloud_rpy = ["0", "0", "0"] + + gz_camera_remappings_file = PathJoinSubstitution( + [ + get_package_share_directory("rosbot_xl_gazebo"), + "config", + LaunchConfiguration( + "gz_camera_remappings_file", + default=["gz_", camera_model, "_remappings.yaml"], + ), + ] + ) + + namespaced_gz_camera_remappings_file = ReplaceString( + source_file=gz_camera_remappings_file, + replacements={"": namespace_ext}, + ) + + if zed_model is not None: + namespaced_gz_camera_remappings_file = ReplaceString( + source_file=namespaced_gz_camera_remappings_file, + replacements={"": zed_model}, + ) + + ign_camera_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="ros_gz_camera_bridge", + parameters=[{"config_file": namespaced_gz_camera_remappings_file}], + remappings=[ + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ], + output="screen", + namespace=namespace, + condition=LaunchConfigurationNotEquals(camera_model, "None"), + ) + + # The frame of the point cloud from ignition gazebo 6 isn't provided by . + # See https://github.com/gazebosim/gz-sensors/issues/239 + point_cloud_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="point_cloud_tf", + output="log", + arguments=[ + "0", + "0", + "0", + ] + + pointcloud_rpy + + [ + depth_camera_parent_tf, + depth_camera_child_tf, + ], + remappings=[ + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ], + namespace=namespace, + ) + + return [ign_lidar_bridge, ign_camera_bridge, point_cloud_tf] + + +def generate_launch_description(): + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value="", + description="Namespace for all topics and tfs", + ) + + mecanum = LaunchConfiguration("mecanum") + declare_mecanum_arg = DeclareLaunchArgument( + "mecanum", + default_value="False", + description=( + "Whether to use mecanum drive controller (otherwise diff drive controller is used)" + ), + ) + + camera_model = LaunchConfiguration("camera_model") + declare_camera_model_arg = DeclareLaunchArgument( + "camera_model", + default_value="intel_realsense_d435", + description="Add camera model to the robot URDF", + choices=[ + "None", + "intel_realsense_d435", + "orbbec_astra", + "stereolabs_zed", + "stereolabs_zedm", + "stereolabs_zed2", + "stereolabs_zed2i", + "stereolabs_zedx", + "stereolabs_zedxm", + ], + ) + + lidar_model = LaunchConfiguration("lidar_model") + declare_lidar_model_arg = DeclareLaunchArgument( + "lidar_model", + default_value="slamtec_rplidar_s3", + description="Add LiDAR model to the robot URDF", + choices=[ + "None", + "slamtec_rplidar_a2", + "slamtec_rplidar_a3", + "slamtec_rplidar_s1", + "slamtec_rplidar_s2", + "slamtec_rplidar_s3", + "velodyne_puck", + ], + ) + + include_camera_mount = LaunchConfiguration("include_camera_mount") + declare_include_camera_mount_arg = DeclareLaunchArgument( + "include_camera_mount", + default_value="False", + description="Whether to include camera mount to the robot URDF", + ) + + robot_name = PythonExpression( + ["'rosbot_xl'", " if '", namespace, "' == '' ", "else ", "'", namespace, "'"] + ) + + gz_spawn_entity = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-name", + robot_name, + "-allow_renaming", + "true", + "-topic", + "robot_description", + "-x", + LaunchConfiguration("x", default="0.00"), + "-y", + LaunchConfiguration("y", default="0.00"), + "-z", + LaunchConfiguration("z", default="0.00"), + "-R", + LaunchConfiguration("roll", default="0.00"), + "-P", + LaunchConfiguration("pitch", default="0.00"), + "-Y", + LaunchConfiguration("yaw", default="0.00"), + ], + output="screen", + namespace=namespace, + ) + + ign_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="ros_gz_bridge", + arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"], + remappings=[ + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ], + output="screen", + namespace=namespace, + ) + + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + get_package_share_directory("rosbot_xl_bringup"), + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "mecanum": mecanum, + "use_sim": "True", + "simulation_engine": "ignition-gazebo", + "camera_model": camera_model, + "lidar_model": lidar_model, + "include_camera_mount": include_camera_mount, + "namespace": namespace, + }.items(), + ) + + return LaunchDescription( + [ + declare_namespace_arg, + declare_mecanum_arg, + declare_camera_model_arg, + declare_lidar_model_arg, + declare_include_camera_mount_arg, + # Sets use_sim_time for all nodes started below + # (doesn't work for nodes started from ignition gazebo) + SetParameter(name="use_sim_time", value=True), + ign_bridge, + # ign_camera_bridge, + gz_spawn_entity, + bringup_launch, + OpaqueFunction(function=launch_gz_bridge), + ] + ) diff --git a/rosbot_xl_gazebo/package.xml b/rosbot_xl_gazebo/package.xml index 8a62d7e5..811b27ba 100644 --- a/rosbot_xl_gazebo/package.xml +++ b/rosbot_xl_gazebo/package.xml @@ -1,5 +1,6 @@ + rosbot_xl_gazebo 0.8.12 @@ -16,30 +17,37 @@ https://github.com/husarion/rosbot_xl_ros https://github.com/husarion/rosbot_xl_ros/issues - rosbot_xl_bringup - + + ros2launch launch launch_ros + ament_index_python + + rosbot_xl_bringup husarion_office_gz + + ros_gz_sim ros_gz_bridge ign_ros2_control + nav2_common + tf2_ros + + rclpy python3-pytest - launch - launch_ros launch_pytest + launch_testing - tf_transformations - python-transforms3d-pip + + python3-psutil nav_msgs geometry_msgs - - rosbot_xl_bringup + sensor_msgs ament_python diff --git a/rosbot_xl_gazebo/setup.py b/rosbot_xl_gazebo/setup.py index b6d806e6..9e5bccdd 100644 --- a/rosbot_xl_gazebo/setup.py +++ b/rosbot_xl_gazebo/setup.py @@ -1,4 +1,4 @@ -# Copyright 2023 Husarion +# Copyright 2024 Husarion sp. z o.o. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -26,6 +26,7 @@ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml"]), (os.path.join("share", package_name, "launch"), glob("launch/*.launch.py")), + (os.path.join("share", package_name, "config"), glob("config/*.yaml")), ], install_requires=["setuptools"], zip_safe=True, diff --git a/rosbot_xl_gazebo/test/test_diff_drive_simulation.py b/rosbot_xl_gazebo/test/test_diff_drive_simulation.py index e4677fc2..2eac511d 100644 --- a/rosbot_xl_gazebo/test/test_diff_drive_simulation.py +++ b/rosbot_xl_gazebo/test/test_diff_drive_simulation.py @@ -1,6 +1,6 @@ # Copyright 2021 Open Source Robotics Foundation, Inc. # Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2023 Husarion +# Copyright 2024 Husarion sp. z o.o. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -18,14 +18,17 @@ import pytest import rclpy + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource - -from test_utils import SimulationTestNode +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc +from test_utils import SimulationTestNode, diff_test from test_ign_kill_utils import kill_ign_linux_processes +from threading import Thread @launch_pytest.fixture @@ -33,57 +36,44 @@ def generate_test_description(): rosbot_xl_gazebo = get_package_share_directory("rosbot_xl_gazebo") simulation_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - PathJoinSubstitution([ - rosbot_xl_gazebo, - "launch", - "simulation.launch.py", - ]) + PathJoinSubstitution( + [ + rosbot_xl_gazebo, + "launch", + "simulation.launch.py", + ] + ) ), - launch_arguments={"headless": "True", "world": "empty.sdf"}.items(), + launch_arguments={ + "headless": "True", + "world": PathJoinSubstitution( + [ + get_package_share_directory("husarion_office_gz"), + "worlds", + "empty_with_plugins.sdf", + ] + ), + }.items(), ) - return LaunchDescription([simulation_launch]) + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) @pytest.mark.launch(fixture=generate_test_description) def test_diff_drive_simulation(): rclpy.init() try: - node = SimulationTestNode("test_bringup") - node.create_test_subscribers_and_publishers() - node.start_node_thread() - - flag = node.odom_tf_event.wait(timeout=60.0) - assert ( - flag - ), "Expected odom to base_link tf but it was not received. Check robot_localization!" - - # 0.9 m/s and 3.0 rad/s are controller's limits defined in - # rosbot_controller/config/diff_drive_controller.yaml - node.set_destination_speed(0.9, 0.0, 0.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting the" - f" target speed is: {(node.current_time - node.goal_received_time):.1f}." - ) - assert ( - node.controller_odom_flag - ), "ROSbot does not move properly in x direction. Check rosbot_xl_base_controller!" - assert ( - node.ekf_odom_flag - ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" - - node.set_destination_speed(0.0, 0.0, 3.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting the" - f" target speed is: {(node.current_time - node.goal_received_time):.1f}." - ) - assert ( - node.controller_odom_flag - ), "ROSbot does not rotate properly. Check rosbot_xl_base_controller!" - assert node.ekf_odom_flag, "ROSbot does not rotate properly. Check ekf_filter_node!" + node = SimulationTestNode("test_diff_drive_simulation") + Thread(target=lambda node: rclpy.spin(node), args=(node,)).start() - flag = node.scan_event.wait(timeout=20.0) - assert flag, "ROSbot's lidar does not work properly!" + diff_test(node) finally: # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching diff --git a/rosbot_xl_gazebo/test/test_flake8.py b/rosbot_xl_gazebo/test/test_flake8.py index 40602d8a..49c1644f 100644 --- a/rosbot_xl_gazebo/test/test_flake8.py +++ b/rosbot_xl_gazebo/test/test_flake8.py @@ -13,13 +13,11 @@ # limitations under the License. from ament_flake8.main import main_with_errors -from os.path import join, dirname import pytest @pytest.mark.flake8 @pytest.mark.linter def test_flake8(): - config_file = join(dirname(dirname(dirname(__file__))), ".flake8") - rc, errors = main_with_errors(argv=["--config", config_file]) + rc, errors = main_with_errors(argv=[]) assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors) diff --git a/rosbot_xl_gazebo/test/test_ign_kill_utils.py b/rosbot_xl_gazebo/test/test_ign_kill_utils.py index 7f62f936..4f857a1f 100644 --- a/rosbot_xl_gazebo/test/test_ign_kill_utils.py +++ b/rosbot_xl_gazebo/test/test_ign_kill_utils.py @@ -1,4 +1,5 @@ -# Copyright 2023 Husarion +# Copyright 2024 Husarion sp. z o.o. +# Copyright 2023 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. @@ -12,27 +13,18 @@ # See the License for the specific language governing permissions and # limitations under the License. -import subprocess +import psutil +from time import sleep # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching # several tests in a row. +# https://github.com/ros-controls/gz_ros2_control/blob/master/gz_ros2_control_tests/tests/position_test.py def kill_ign_linux_processes(): - try: - result = subprocess.run( - ["pgrep", "-f", "ign gazebo"], - capture_output=True, - text=True, - check=True, - ) - - pids = result.stdout.strip().split("\n") - for pid in pids: - subprocess.run(["kill", pid], check=True) - print("Killed all Ignition Gazebo processes") - except subprocess.CalledProcessError as e: - print(e) - - -kill_ign_linux_processes() + for proc in psutil.process_iter(): + # check whether the process name matches + if proc.name() == "ruby" or proc.name() == "parameter_bridge": + while proc.is_running(): + proc.kill() + sleep(1.0) diff --git a/rosbot_xl_gazebo/test/test_mecanum_simulation.py b/rosbot_xl_gazebo/test/test_mecanum_simulation.py index abe6fae0..c605d25f 100644 --- a/rosbot_xl_gazebo/test/test_mecanum_simulation.py +++ b/rosbot_xl_gazebo/test/test_mecanum_simulation.py @@ -1,6 +1,6 @@ # Copyright 2021 Open Source Robotics Foundation, Inc. # Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2023 Husarion +# Copyright 2024 Husarion sp. z o.o. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -18,14 +18,17 @@ import pytest import rclpy + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource - -from test_utils import SimulationTestNode +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc +from test_utils import SimulationTestNode, mecanum_test from test_ign_kill_utils import kill_ign_linux_processes +from threading import Thread @launch_pytest.fixture @@ -33,69 +36,45 @@ def generate_test_description(): rosbot_xl_gazebo = get_package_share_directory("rosbot_xl_gazebo") simulation_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - PathJoinSubstitution([ - rosbot_xl_gazebo, - "launch", - "simulation.launch.py", - ]) + PathJoinSubstitution( + [ + rosbot_xl_gazebo, + "launch", + "simulation.launch.py", + ] + ) ), - launch_arguments={"mecanum": "True", "headless": "True", "world": "empty.sdf"}.items(), + launch_arguments={ + "mecanum": "True", + "headless": "True", + "world": PathJoinSubstitution( + [ + get_package_share_directory("husarion_office_gz"), + "worlds", + "empty_with_plugins.sdf", + ] + ), + }.items(), ) - return LaunchDescription([simulation_launch]) + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) @pytest.mark.launch(fixture=generate_test_description) def test_mecanum_simulation(): rclpy.init() try: - node = SimulationTestNode("test_bringup") - node.create_test_subscribers_and_publishers() - node.start_node_thread() - - flag = node.odom_tf_event.wait(timeout=60.0) - assert ( - flag - ), "Expected odom to base_link tf but it was not received. Check robot_localization!" - - # 0.9 m/s and 3.0 rad/s are controller's limits defined in - # rosbot_controller/config/mecanum_drive_controller.yaml - node.set_destination_speed(0.9, 0.0, 0.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting the" - f" target speed is: {(node.current_time - node.goal_received_time):.1f}." - ) - assert ( - node.controller_odom_flag - ), "ROSbot does not move properly in x direction. Check rosbot_xl_base_controller!" - assert ( - node.ekf_odom_flag - ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" - - node.set_destination_speed(0.0, 0.9, 0.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting the" - f" target speed is: {(node.current_time - node.goal_received_time):.1f}." - ) - assert ( - node.controller_odom_flag - ), "ROSbot does not move properly in y direction. Check rosbot_xl_base_controller!" - assert ( - node.ekf_odom_flag - ), "ROSbot does not move properly in y direction. Check ekf_filter_node!" - - node.set_destination_speed(0.0, 0.0, 3.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting the" - f" target speed is: {(node.current_time - node.goal_received_time):.1f}." - ) - assert ( - node.controller_odom_flag - ), "ROSbot does not rotate properly. Check rosbot_xl_base_controller!" - assert node.ekf_odom_flag, "ROSbot does not rotate properly. Check ekf_filter_node!" + node = SimulationTestNode("test_mecanum_simulation") + Thread(target=lambda node: rclpy.spin(node), args=(node,)).start() - flag = node.scan_event.wait(timeout=20.0) - assert flag, "ROSbot's lidar does not work properly!" + mecanum_test(node) finally: # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching diff --git a/rosbot_xl_gazebo/test/test_multirobot_diff_drive_simulation.py b/rosbot_xl_gazebo/test/test_multirobot_diff_drive_simulation.py new file mode 100644 index 00000000..9d2a9f94 --- /dev/null +++ b/rosbot_xl_gazebo/test/test_multirobot_diff_drive_simulation.py @@ -0,0 +1,89 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2024 Husarion sp. z o.o. +# +# 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 launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc +from rclpy.executors import SingleThreadedExecutor +from test_utils import SimulationTestNode, diff_test +from test_ign_kill_utils import kill_ign_linux_processes +from threading import Thread + + +@launch_pytest.fixture +def generate_test_description(): + # IncludeLaunchDescription does not work with robots argument + simulation_launch = ExecuteProcess( + cmd=[ + "ros2", + "launch", + "rosbot_xl_gazebo", + "simulation.launch.py", + ( + f'world:={get_package_share_directory("husarion_office_gz")}' + "/worlds/empty_with_plugins.sdf" + ), + "robots:=robot1={y: -4.0}; robot2={y: 0.0};", + "headless:=True", + ], + output="screen", + ) + + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_multirobot_diff_drive_simulation(): + robots = ["robot1", "robot2"] + rclpy.init() + try: + nodes = {} + executor = SingleThreadedExecutor() + + for node_namespace in robots: + node = SimulationTestNode( + "test_multirobot_diff_drive_simulation", namespace=node_namespace + ) + nodes[node_namespace] = node + executor.add_node(node) + + Thread(target=lambda executor: executor.spin(), args=(executor,)).start() + + for node_namespace in robots: + node = nodes[node_namespace] + diff_test(node, node_namespace) + executor.remove_node(node) + node.destroy_node() + + finally: + # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching + # several tests in a row. + executor.shutdown() + kill_ign_linux_processes() + rclpy.shutdown() diff --git a/rosbot_xl_gazebo/test/test_multirobot_mecanum_simulation.py b/rosbot_xl_gazebo/test/test_multirobot_mecanum_simulation.py new file mode 100644 index 00000000..8f092d7a --- /dev/null +++ b/rosbot_xl_gazebo/test/test_multirobot_mecanum_simulation.py @@ -0,0 +1,90 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2024 Husarion sp. z o.o. +# +# 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 launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc +from rclpy.executors import SingleThreadedExecutor +from test_utils import SimulationTestNode, mecanum_test +from test_ign_kill_utils import kill_ign_linux_processes +from threading import Thread + + +@launch_pytest.fixture +def generate_test_description(): + # IncludeLaunchDescription does not work with robots argument + simulation_launch = ExecuteProcess( + cmd=[ + "ros2", + "launch", + "rosbot_xl_gazebo", + "simulation.launch.py", + ( + f'world:={get_package_share_directory("husarion_office_gz")}' + "/worlds/empty_with_plugins.sdf" + ), + "robots:=robot1={y: -4.0}; robot2={y: 0.0};", + "mecanum:=True", + "headless:=True", + ], + output="screen", + ) + + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_multirobot_mecanum_simulation(): + robots = ["robot1", "robot2"] + rclpy.init() + try: + nodes = {} + executor = SingleThreadedExecutor() + + for node_namespace in robots: + node = SimulationTestNode( + "test_multirobot_mecanum_simulation", namespace=node_namespace + ) + nodes[node_namespace] = node + executor.add_node(node) + + Thread(target=lambda executor: executor.spin(), args=(executor,)).start() + + for node_namespace in robots: + node = nodes[node_namespace] + mecanum_test(node, node_namespace) + executor.remove_node(node) + node.destroy_node() + + finally: + # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching + # several tests in a row. + executor.shutdown() + kill_ign_linux_processes() + rclpy.shutdown() diff --git a/rosbot_xl_gazebo/test/test_slamtec_rplidar_a2.py b/rosbot_xl_gazebo/test/test_namespaced_diff_drive_simulation.py similarity index 59% rename from rosbot_xl_gazebo/test/test_slamtec_rplidar_a2.py rename to rosbot_xl_gazebo/test/test_namespaced_diff_drive_simulation.py index f1607a85..f47cd768 100644 --- a/rosbot_xl_gazebo/test/test_slamtec_rplidar_a2.py +++ b/rosbot_xl_gazebo/test/test_namespaced_diff_drive_simulation.py @@ -1,6 +1,6 @@ # Copyright 2021 Open Source Robotics Foundation, Inc. # Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2023 Husarion +# Copyright 2024 Husarion sp. z o.o. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -18,14 +18,17 @@ import pytest import rclpy + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource - -from test_utils import SimulationTestNode +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc +from test_utils import SimulationTestNode, diff_test from test_ign_kill_utils import kill_ign_linux_processes +from threading import Thread @launch_pytest.fixture @@ -33,31 +36,45 @@ def generate_test_description(): rosbot_xl_gazebo = get_package_share_directory("rosbot_xl_gazebo") simulation_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - PathJoinSubstitution([ - rosbot_xl_gazebo, - "launch", - "simulation.launch.py", - ]) + PathJoinSubstitution( + [ + rosbot_xl_gazebo, + "launch", + "simulation.launch.py", + ] + ) ), launch_arguments={ "headless": "True", - "lidar_model": "slamtec_rplidar_a2", + "world": PathJoinSubstitution( + [ + get_package_share_directory("husarion_office_gz"), + "worlds", + "empty_with_plugins.sdf", + ] + ), + "namespace": "rosbot_xl", }.items(), ) - return LaunchDescription([simulation_launch]) + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) @pytest.mark.launch(fixture=generate_test_description) -def test_slamtec_rplidar_a2(): +def test_namespaced_diff_drive_simulation(): rclpy.init() try: - node = SimulationTestNode("test_bringup") - node.create_test_subscribers_and_publishers() - node.start_node_thread() + node = SimulationTestNode("test_namespaced_diff_drive_simulation", namespace="rosbot_xl") + Thread(target=lambda node: rclpy.spin(node), args=(node,)).start() - msgs_received_flag = node.scan_event.wait(timeout=60.0) - assert msgs_received_flag, "ROSbot's lidar does not work properly!" + diff_test(node) finally: # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching diff --git a/rosbot_xl_gazebo/test/test_slamtec_rplidar_a3.py b/rosbot_xl_gazebo/test/test_namespaced_mecanum_simulation.py similarity index 58% rename from rosbot_xl_gazebo/test/test_slamtec_rplidar_a3.py rename to rosbot_xl_gazebo/test/test_namespaced_mecanum_simulation.py index ce68f668..ef81010f 100644 --- a/rosbot_xl_gazebo/test/test_slamtec_rplidar_a3.py +++ b/rosbot_xl_gazebo/test/test_namespaced_mecanum_simulation.py @@ -1,6 +1,6 @@ # Copyright 2021 Open Source Robotics Foundation, Inc. # Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2023 Husarion +# Copyright 2024 Husarion sp. z o.o. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -23,9 +23,11 @@ from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource - -from test_utils import SimulationTestNode +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc +from test_utils import SimulationTestNode, mecanum_test from test_ign_kill_utils import kill_ign_linux_processes +from threading import Thread @launch_pytest.fixture @@ -33,31 +35,46 @@ def generate_test_description(): rosbot_xl_gazebo = get_package_share_directory("rosbot_xl_gazebo") simulation_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - PathJoinSubstitution([ - rosbot_xl_gazebo, - "launch", - "simulation.launch.py", - ]) + PathJoinSubstitution( + [ + rosbot_xl_gazebo, + "launch", + "simulation.launch.py", + ] + ) ), launch_arguments={ + "mecanum": "True", + "world": PathJoinSubstitution( + [ + get_package_share_directory("husarion_office_gz"), + "worlds", + "empty_with_plugins.sdf", + ] + ), "headless": "True", - "lidar_model": "slamtec_rplidar_a3", + "namespace": "rosbot_xl", }.items(), ) - return LaunchDescription([simulation_launch]) + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) @pytest.mark.launch(fixture=generate_test_description) -def test_slamtec_rplidar_a3(): +def test_namespaced_mecanum_simulation(): rclpy.init() try: - node = SimulationTestNode("test_bringup") - node.create_test_subscribers_and_publishers() - node.start_node_thread() + node = SimulationTestNode("test_namespaced_mecanum_simulation", namespace="rosbot_xl") + Thread(target=lambda node: rclpy.spin(node), args=(node,)).start() - msgs_received_flag = node.scan_event.wait(timeout=60.0) - assert msgs_received_flag, "ROSbot's lidar does not work properly!" + mecanum_test(node) finally: # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching diff --git a/rosbot_xl_gazebo/test/test_utils.py b/rosbot_xl_gazebo/test/test_utils.py index 50585d80..bda96ded 100644 --- a/rosbot_xl_gazebo/test/test_utils.py +++ b/rosbot_xl_gazebo/test/test_utils.py @@ -1,5 +1,5 @@ # Copyright 2021 Open Source Robotics Foundation, Inc. -# Copyright 2023 Husarion +# Copyright 2024 Husarion sp. z o.o. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -14,82 +14,81 @@ # limitations under the License. import rclpy +from rclpy.node import Node from threading import Event -from threading import Thread - -from rclpy.node import Node from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry -from sensor_msgs.msg import LaserScan - -from tf2_ros import TransformException -from tf2_ros.buffer import Buffer -from tf2_ros.transform_listener import TransformListener +from sensor_msgs.msg import Imu, JointState class SimulationTestNode(Node): __test__ = False + # The inaccuracies in measurement uncertainties and wheel slippage # cause the rosbot_xl_base_controller to determine inaccurate odometry. ACCURACY = 0.10 # 10% accuracy + VELOCITY_STABILIZATION_DELAY = 2 - def __init__(self, name="test_node"): - super().__init__(name) + def __init__(self, name="test_node", namespace=None): + super().__init__(name, namespace=namespace) + self.cmd_vel_pub = self.create_publisher(Twist, "cmd_vel", 10) - # Use simulation time to correct run on slow machine - use_sim_time = rclpy.parameter.Parameter("use_sim_time", rclpy.Parameter.Type.BOOL, True) - self.set_parameters([use_sim_time]) + # Robot callback + self.joint_sub = self.create_subscription( + JointState, "joint_states", self.joint_states_callback, 10 + ) + self.controller_sub = self.create_subscription( + Odometry, "rosbot_xl_base_controller/odom", self.controller_callback, 10 + ) + self.ekf_sub = self.create_subscription( + Odometry, "odometry/filtered", self.ekf_callback, 10 + ) + self.imu_sub = self.create_subscription(Imu, "imu_broadcaster/imu", self.imu_callback, 10) - self.VELOCITY_STABILIZATION_DELAY = 3 - self.goal_received_time = 1e-9 * self.get_clock().now().nanoseconds - self.vel_stabilization_time_event = Event() + # Timer - send cmd_vel and check if the time needed for speed stabilization has elapsed + self.timer = self.create_timer(0.1, self.timer_callback) + # Destination velocity for cmd_vel self.v_x = 0.0 self.v_y = 0.0 self.v_yaw = 0.0 - self.controller_odom_flag = False - self.ekf_odom_flag = False - self.odom_tf_event = Event() - self.scan_event = Event() + # Debug values + self.controller_twist = None + self.ekf_twist = None + + # Robot test flags and events + self.is_controller_msg = False + self.is_controller_odom_correct = False + self.is_ekf_msg = False + self.is_ekf_odom_correct = False + self.is_imu_msg = False + self.is_joint_msg = False + self.robot_initialized_event = Event() + self.vel_stabilization_time_event = Event() + + # Using /clock topic as time source (checking the simulation time) + use_sim_time = rclpy.parameter.Parameter("use_sim_time", rclpy.Parameter.Type.BOOL, True) + self.set_parameters([use_sim_time]) + + # Time values + self.current_time = 1e-9 * self.get_clock().now().nanoseconds + self.goal_received_time = None - def clear_odom_flag(self): - self.controller_odom_flag = False - self.ekf_odom_flag = False + def reset_flags(self): + self.is_controller_odom_correct = False + self.is_ekf_odom_correct = False + self.vel_stabilization_time_event.clear() def set_destination_speed(self, v_x, v_y, v_yaw): - self.clear_odom_flag() + self.get_logger().info(f"Setting cmd_vel: x: {v_x}, y: {v_y}, yaw: {v_yaw}") self.v_x = v_x self.v_y = v_y self.v_yaw = v_yaw self.goal_received_time = 1e-9 * self.get_clock().now().nanoseconds - self.vel_stabilization_time_event.clear() - - def create_test_subscribers_and_publishers(self): - self.cmd_vel_publisher = self.create_publisher(Twist, "cmd_vel", 10) - - self.controller_odom_sub = self.create_subscription( - Odometry, "/rosbot_xl_base_controller/odom", self.controller_callback, 10 - ) - - self.ekf_odom_sub = self.create_subscription( - Odometry, "/odometry/filtered", self.ekf_callback, 10 - ) - - self.scan_sub = self.create_subscription( - LaserScan, "/scan_filtered", self.scan_callback, 10 - ) - self.tf_buffer = Buffer() - self.tf_listener = TransformListener(self.tf_buffer, self) - - self.timer = None - - def start_node_thread(self): - self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,)) - self.ros_spin_thread.start() - self.timer = self.create_timer(1.0 / 10.0, self.timer_callback) + self.reset_flags() def is_twist_ok(self, twist: Twist): def are_close_to_each_other(current_value, dest_value, tolerance=self.ACCURACY, eps=0.01): @@ -102,40 +101,139 @@ def are_close_to_each_other(current_value, dest_value, tolerance=self.ACCURACY, return x_ok and y_ok and yaw_ok + def is_initialized(self): + if self.is_controller_msg and self.is_ekf_msg and self.is_imu_msg and self.is_joint_msg: + self.get_logger().info("Robot initialized!", once=True) + self.robot_initialized_event.set() + return self.robot_initialized_event.is_set() + def controller_callback(self, data: Odometry): - self.controller_odom_flag = self.is_twist_ok(data.twist.twist) + if not self.is_controller_msg: + self.get_logger().info("Controller message arrived") + self.is_controller_msg = True + self.controller_twist = data.twist.twist + self.is_controller_odom_correct = self.is_twist_ok(data.twist.twist) def ekf_callback(self, data: Odometry): - self.ekf_odom_flag = self.is_twist_ok(data.twist.twist) - - def lookup_transform_odom(self): - try: - self.tf_buffer.lookup_transform("odom", "base_link", rclpy.time.Time()) - self.odom_tf_event.set() - except TransformException as ex: - self.get_logger().error(f"Could not transform odom to base_link: {ex}") + if not self.is_ekf_msg: + self.get_logger().info("EKF message arrived") + self.is_ekf_msg = True + self.ekf_twist = data.twist.twist + self.is_ekf_odom_correct = self.is_twist_ok(data.twist.twist) + + def imu_callback(self, data): + if not self.is_imu_msg: + self.get_logger().info("IMU message arrived") + self.is_imu_msg = True + + def joint_states_callback(self, data): + if not self.is_joint_msg: + self.get_logger().info("Joint State message arrived") + self.is_joint_msg = True def timer_callback(self): - self.publish_cmd_vel_messages() - self.lookup_transform_odom() - self.current_time = 1e-9 * self.get_clock().now().nanoseconds - if self.current_time > self.goal_received_time + self.VELOCITY_STABILIZATION_DELAY: - self.vel_stabilization_time_event.set() - def scan_callback(self, data: LaserScan): - for range in data.ranges: - # minimal distance configured in rosbot_xl_bringup/config_laser_filter.yaml - if abs(range) < 0.145: - return + if self.is_initialized() and self.goal_received_time: + self.publish_cmd_vel_msg() - self.scan_event.set() + if self.current_time > self.goal_received_time + self.VELOCITY_STABILIZATION_DELAY: + self.get_logger().info( + "Speed stabilization time has expired", throttle_duration_sec=1 + ) + self.vel_stabilization_time_event.set() - def publish_cmd_vel_messages(self): + def publish_cmd_vel_msg(self): twist_msg = Twist() twist_msg.linear.x = self.v_x twist_msg.linear.y = self.v_y twist_msg.angular.z = self.v_yaw - self.cmd_vel_publisher.publish(twist_msg) + self.cmd_vel_pub.publish(twist_msg) + + def __exit__(self, exep_type, exep_value, trace): + if exep_type is not None: + raise Exception("Exception occurred, value: ", exep_value) + self.shutdown() + + +def wait_for_initialization(node: SimulationTestNode, robot_name="ROSbot"): + assert node.robot_initialized_event.wait(60), ( + f"{robot_name} does not initialized correctly!\n\tIs controller_msg:" + f" {node.is_controller_msg}\n\tIs ekf_msg: {node.is_ekf_msg}\n\tIs imu_msg:" + f" {node.is_imu_msg}\n\tIs joint_msg: {node.is_joint_msg}" + ) + + +def x_speed_test(node: SimulationTestNode, v_x=0.0, v_y=0.0, v_yaw=0.0, robot_name="ROSbot"): + node.set_destination_speed(v_x, v_y, v_yaw) + # node.rate.sleep() + assert node.vel_stabilization_time_event.wait(20.0), ( + f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" + " since setting the target speed is:" + f" {(node.current_time - node.goal_received_time):.1f}." + ) + assert node.is_controller_odom_correct, ( + f"{robot_name}: does not move properly in x direction. Check" + f" rosbot_xl_base_controller! Twist: {node.controller_twist}" + f"\nCommand: x: {v_x}, y:{v_y}, yaw:{v_yaw}" + ) + assert node.is_ekf_odom_correct, ( + f"{robot_name}: does not move properly in x direction. Check ekf_filter_node!" + f" Twist: {node.ekf_twist}" + ) + + +def y_speed_test(node: SimulationTestNode, v_x=0.0, v_y=0.0, v_yaw=0.0, robot_name="ROSbot"): + node.set_destination_speed(v_x, v_y, v_yaw) + + assert node.vel_stabilization_time_event.wait(20.0), ( + f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" + " since setting the target speed is:" + f" {(node.current_time - node.goal_received_time):.1f}." + ) + assert node.is_controller_odom_correct, ( + f"{robot_name} does not move properly in y direction. Check" + f" rosbot_xl_base_controller! Twist: {node.controller_twist}" + f"\nCommand: x: {v_x}, y:{v_y}, yaw:{v_yaw}" + ) + assert node.is_ekf_odom_correct, ( + f"{robot_name} does not move properly in y direction. Check ekf_filter_node!" + f" Twist: {node.ekf_twist}" + ) + + +def yaw_speed_test(node: SimulationTestNode, v_x=0.0, v_y=0.0, v_yaw=0.0, robot_name="ROSbot"): + node.set_destination_speed(v_x, v_y, v_yaw) + + assert node.vel_stabilization_time_event.wait(20.0), ( + f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" + " since setting the target speed is:" + f" {(node.current_time - node.goal_received_time):.1f}." + ) + assert node.is_controller_odom_correct, ( + f"{robot_name} does not rotate properly. Check rosbot_xl_base_controller! Twist:" + f" {node.controller_twist}" + f"\nCommand: x: {v_x}, y:{v_y}, yaw:{v_yaw}" + ) + assert ( + node.is_ekf_odom_correct + ), f"{robot_name} does not rotate properly. Check ekf_filter_node! Twist: {node.ekf_twist}" + + +def diff_test(node: SimulationTestNode, robot_name="ROSbot"): + wait_for_initialization(node, robot_name) + # 0.8 m/s and 3.14 rad/s are controller's limits defined in + # rosbot_xl_controller/config/mecanum_drive_controller.yaml + x_speed_test(node, v_x=0.8, robot_name=robot_name) + yaw_speed_test(node, v_yaw=3.14, robot_name=robot_name) + + +def mecanum_test(node: SimulationTestNode, robot_name="ROSbot"): + wait_for_initialization(node, robot_name) + # 0.8 m/s and 3.14 rad/s are controller's limits defined in + # rosbot_xl_controller/config/mecanum_drive_controller.yaml + x_speed_test(node, v_x=0.8, robot_name=robot_name) + y_speed_test(node, v_y=0.8, robot_name=robot_name) + yaw_speed_test(node, v_yaw=3.14, robot_name=robot_name)