diff --git a/.flake8 b/.flake8 index 91e7ae9..5389916 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,E203 +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 a0a6574..4ae8e81 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 0e43f9e..29bb70c 100644 --- a/.github/workflows/industrial_ci.yaml +++ b/.github/workflows/industrial_ci.yaml @@ -1,76 +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 - - spellcheck: + black: + name: Black + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: psf/black@stable + with: + options: --line-length=99 + + 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: 30 - steps: - - name: Checkout - uses: actions/checkout@v3 - - - name: Copy to src - run: | - mkdir -p src - find . -maxdepth 1 -not -name src -not -name . -exec mv {} src/ \; - - - name: Clone installation requirements - shell: bash - run: | - 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 - - - name: Copy only diff_drive_controller and imu_sensor_broadcaster, waits for features from ros2-control - shell: bash - run: | - cp -r src/ros2_controllers/diff_drive_controller src/ - cp -r src/ros2_controllers/imu_sensor_broadcaster src/ - rm -rf src/ros2_controllers - - - name: Copy only tf2_ros_py from geometry2, waits for https://github.com/ros2/geometry2/pull/641 - shell: bash - run: | - cp -r src/geometry2/tf2_ros_py src/ - rm -rf src/geometry2/ - - - name: Remove ign_ros2_control demo - shell: bash - run: | - rm -rf src/gazebosim/gz_ros2_control/ign_ros2_control_demos && - rm -rf src/gazebosim/gz_ros2_control/gz_ros2_control_tests - - 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/.pre-commit-config.yaml b/.pre-commit-config.yaml index 673c075..8b997ed 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", "--experimental-string-processing"] + # 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"] # 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 df8259c..ae49099 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 d0631c0..61bb2de 100644 --- a/.wordlist.txt +++ b/.wordlist.txt @@ -83,3 +83,4 @@ psutil src xml spawner +sp diff --git a/LICENSE_MIT.txt b/LICENSE_MIT.txt index a32436c..e54dae7 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/rosbot_xl/rosbot_xl_hardware.repos b/rosbot_xl/rosbot_xl_hardware.repos index 3f0cd08..7803e1a 100644 --- a/rosbot_xl/rosbot_xl_hardware.repos +++ b/rosbot_xl/rosbot_xl_hardware.repos @@ -6,7 +6,7 @@ repositories: ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git - version: gazebo-ignition-rosbot-xl-namespaces + version: ros2 rosbot_controllers: type: git url: https://github.com/husarion/rosbot_controllers @@ -15,7 +15,3 @@ repositories: type: git url: https://github.com/delihus/ros2_controllers version: humble - geometry2: - type: git - url: https://github.com/delihus/geometry2 - version: rolling diff --git a/rosbot_xl/rosbot_xl_simulation.repos b/rosbot_xl/rosbot_xl_simulation.repos index 40ba729..e4d80df 100644 --- a/rosbot_xl/rosbot_xl_simulation.repos +++ b/rosbot_xl/rosbot_xl_simulation.repos @@ -1,9 +1,4 @@ repositories: - gazebosim/gz_ros2_control: - type: git - url: https://github.com/ros-controls/gz_ros2_control - version: humble - husarion/husarion_office_gz: type: git url: https://github.com/husarion/husarion_office_gz diff --git a/rosbot_xl_bringup/config/ekf.yaml b/rosbot_xl_bringup/config/ekf.yaml index 55e8430..7681878 100644 --- a/rosbot_xl_bringup/config/ekf.yaml +++ b/rosbot_xl_bringup/config/ekf.yaml @@ -3,7 +3,7 @@ ## ekf config file ### /**/ekf_filter_node: ros__parameters: - frequency: 25.0 + frequency: 20.0 sensor_timeout: 0.05 two_d_mode: true diff --git a/rosbot_xl_bringup/config/laser_filter.yaml b/rosbot_xl_bringup/config/laser_filter.yaml index 02aac66..8ce842d 100644 --- a/rosbot_xl_bringup/config/laser_filter.yaml +++ b/rosbot_xl_bringup/config/laser_filter.yaml @@ -1,3 +1,4 @@ +--- /**/scan_to_scan_filter_chain: ros__parameters: filter1: diff --git a/rosbot_xl_bringup/launch/bringup.launch.py b/rosbot_xl_bringup/launch/bringup.launch.py index 15652a6..6ac3995 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. diff --git a/rosbot_xl_bringup/setup.py b/rosbot_xl_bringup/setup.py index b9d7233..1e6d8d3 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 051cd60..8507b9f 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. @@ -17,23 +17,18 @@ import launch_pytest import pytest import rclpy -import os -import random 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 +from test_utils import BringupTestNode, ekf_and_scan_test +from threading import Thread @launch_pytest.fixture def generate_test_description(): - proc_env = os.environ.copy() - proc_env["ROS_LOCALHOST_ONLY"] = "1" - proc_env["ROS_DOMAIN_ID"] = random.randint(0, 255) - rosbot_xl_bringup = get_package_share_directory("rosbot_xl_bringup") bringup_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -59,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 082713a..817be79 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. @@ -17,23 +17,18 @@ import launch_pytest import pytest import rclpy -import os -import random 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 +from test_utils import BringupTestNode, ekf_and_scan_test +from threading import Thread @launch_pytest.fixture def generate_test_description(): - proc_env = os.environ.copy() - proc_env["ROS_LOCALHOST_ONLY"] = "1" - proc_env["ROS_DOMAIN_ID"] = random.randint(0, 255) - rosbot_xl_bringup = get_package_share_directory("rosbot_xl_bringup") bringup_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -59,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 index 7e7c7bd..446c569 100644 --- a/rosbot_xl_bringup/test/test_multirobot_ekf_and_scan.py +++ b/rosbot_xl_bringup/test/test_multirobot_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. @@ -17,15 +17,15 @@ import launch_pytest import pytest import rclpy -import os -import random 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 +from rclpy.executors import SingleThreadedExecutor +from test_utils import BringupTestNode, ekf_and_scan_test +from threading import Thread robot_names = ["rosbot1", "rosbot2", "rosbot3"] @@ -33,10 +33,6 @@ @launch_pytest.fixture def generate_test_description(): - proc_env = os.environ.copy() - proc_env["ROS_LOCALHOST_ONLY"] = "1" - proc_env["ROS_DOMAIN_ID"] = random.randint(0, 255) - rosbot_xl_bringup = get_package_share_directory("rosbot_xl_bringup") actions = [] for i in range(len(robot_names)): @@ -63,37 +59,25 @@ def generate_test_description(): @pytest.mark.launch(fixture=generate_test_description) def test_namespaced_bringup_startup_success(): - for robot_name in robot_names: - rclpy.init() - try: - node = BringupTestNode("test_bringup", namespace=robot_name) - node.create_test_subscribers_and_publishers() - node.start_publishing_fake_hardware() - - node.start_node_thread() - msgs_received_flag = node.odom_tf_event.wait(timeout=30.0) - assert ( - msgs_received_flag - ), "Expected odom to base_link tf but it was not received. Check robot_localization!" - - finally: - rclpy.shutdown() + rclpy.init() + try: + nodes = {} + executor = SingleThreadedExecutor() - -@pytest.mark.launch(fixture=generate_test_description) -def test_namespaced_bringup_scan_filter(): - for robot_name in robot_names: - rclpy.init() - try: - node = BringupTestNode("test_bringup", namespace=robot_name) - node.create_test_subscribers_and_publishers() + 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() - node.start_node_thread() - msgs_received_flag = node.scan_filter_event.wait(timeout=30.0) - assert ( - msgs_received_flag - ), "Expected filtered scan but it is not filtered properly. Check laser_filter!" + 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: - rclpy.shutdown() + finally: + executor.shutdown() + rclpy.shutdown() diff --git a/rosbot_xl_bringup/test/test_namespaced_diff_drive_ekf_and_scan.py b/rosbot_xl_bringup/test/test_namespaced_diff_drive_ekf_and_scan.py index 1231518..34f6971 100644 --- a/rosbot_xl_bringup/test/test_namespaced_diff_drive_ekf_and_scan.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. @@ -17,23 +17,18 @@ import launch_pytest import pytest import rclpy -import os -import random 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 +from test_utils import BringupTestNode, ekf_and_scan_test +from threading import Thread @launch_pytest.fixture def generate_test_description(): - proc_env = os.environ.copy() - proc_env["ROS_LOCALHOST_ONLY"] = "1" - proc_env["ROS_DOMAIN_ID"] = random.randint(0, 255) - rosbot_xl_bringup = get_package_share_directory("rosbot_xl_bringup") bringup_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -48,7 +43,7 @@ def generate_test_description(): launch_arguments={ "use_sim": "False", "mecanum": "False", - "namespace": "rosbotxl", + "namespace": "rosbot_xl", }.items(), ) @@ -59,33 +54,12 @@ def generate_test_description(): def test_namespaced_bringup_startup_success(): rclpy.init() try: - node = BringupTestNode("test_bringup", namespace="rosbotxl") - 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_namespaced_bringup_scan_filter(): - rclpy.init() - try: - node = BringupTestNode("test_bringup", namespace="rosbotxl") - node.create_test_subscribers_and_publishers() + node = BringupTestNode("test_bringup", namespace="rosbot_xl") 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_namespaced_mecanum_ekf_and_scan.py b/rosbot_xl_bringup/test/test_namespaced_mecanum_ekf_and_scan.py index ffef5bc..4b4b17e 100644 --- a/rosbot_xl_bringup/test/test_namespaced_mecanum_ekf_and_scan.py +++ b/rosbot_xl_bringup/test/test_namespaced_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. @@ -17,23 +17,18 @@ import launch_pytest import pytest import rclpy -import os -import random 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 +from test_utils import BringupTestNode, ekf_and_scan_test +from threading import Thread @launch_pytest.fixture def generate_test_description(): - proc_env = os.environ.copy() - proc_env["ROS_LOCALHOST_ONLY"] = "1" - proc_env["ROS_DOMAIN_ID"] = random.randint(0, 255) - rosbot_xl_bringup = get_package_share_directory("rosbot_xl_bringup") bringup_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -48,7 +43,7 @@ def generate_test_description(): launch_arguments={ "use_sim": "False", "mecanum": "True", - "namespace": "rosbotxl", + "namespace": "rosbot_xl", }.items(), ) @@ -59,33 +54,12 @@ def generate_test_description(): def test_namespaced_bringup_startup_success(): rclpy.init() try: - node = BringupTestNode("test_bringup", namespace="rosbotxl") - 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_namespaced_bringup_scan_filter(): - rclpy.init() - try: - node = BringupTestNode("test_bringup", namespace="rosbotxl") - node.create_test_subscribers_and_publishers() + node = BringupTestNode("test_bringup", namespace="rosbot_xl") 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_utils.py b/rosbot_xl_bringup/test/test_utils.py index 926f772..527b725 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 @@ -41,18 +38,14 @@ def __init__(self, name="test_node", namespace=None): 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 ) @@ -63,13 +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( @@ -102,8 +93,8 @@ 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() @@ -117,7 +108,16 @@ def publish_scan(self): msg.range_max = 10.0 # 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) + msg.ranges = [random.random() for _ in range(int(msg.angle_max / msg.angle_increment))] + 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 6b75c97..90f70e3 100644 --- a/rosbot_xl_controller/config/diff_drive_controller.yaml +++ b/rosbot_xl_controller/config/diff_drive_controller.yaml @@ -1,3 +1,4 @@ +--- /**/controller_manager: ros__parameters: use_sim_time: false @@ -23,8 +24,8 @@ /**/rosbot_xl_base_controller: ros__parameters: tf_frame_prefix_enable: false - left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"] - right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"] + 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 diff --git a/rosbot_xl_controller/config/mecanum_drive_controller.yaml b/rosbot_xl_controller/config/mecanum_drive_controller.yaml index 9454762..265efb0 100644 --- a/rosbot_xl_controller/config/mecanum_drive_controller.yaml +++ b/rosbot_xl_controller/config/mecanum_drive_controller.yaml @@ -1,3 +1,4 @@ +--- /**/controller_manager: ros__parameters: use_sim_time: false diff --git a/rosbot_xl_controller/launch/controller.launch.py b/rosbot_xl_controller/launch/controller.launch.py index 67c4529..971d479 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. @@ -74,12 +74,10 @@ def launch_setup(context, *args, **kwargs): ) # 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], - ) + delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[robot_controller_spawner], ) ) diff --git a/rosbot_xl_controller/setup.py b/rosbot_xl_controller/setup.py index 197fbfd..1bb857b 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 4a6a3b9..c7d1ee8 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. @@ -17,23 +17,18 @@ import launch_pytest import pytest import rclpy -import os -import random 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 def generate_test_description(): - proc_env = os.environ.copy() - proc_env["ROS_LOCALHOST_ONLY"] = "1" - proc_env["ROS_DOMAIN_ID"] = random.randint(0, 255) - rosbot_xl_controller = get_package_share_directory("rosbot_xl_controller") bringup_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -59,20 +54,19 @@ def test_controllers_startup_fail(): rclpy.init() try: node = ControllersTestNode("test_controllers_startup_fail") - node.create_test_subscribers_and_publishers() - 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.!" ) - 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.!" ) - 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.!" @@ -82,25 +76,12 @@ def test_controllers_startup_fail(): @pytest.mark.launch(fixture=generate_test_description) -def test_controllers_startup_success(): +def test_controllers_startup(): rclpy.init() try: - node = ControllersTestNode("test_controllers_startup_success") - 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_xl_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 4830468..12530da 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. @@ -17,23 +17,19 @@ import launch_pytest import pytest import rclpy -import os -import random + 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 def generate_test_description(): - proc_env = os.environ.copy() - proc_env["ROS_LOCALHOST_ONLY"] = "1" - proc_env["ROS_DOMAIN_ID"] = random.randint(0, 255) - rosbot_xl_controller = get_package_share_directory("rosbot_xl_controller") bringup_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -59,20 +55,19 @@ def test_controllers_startup_fail(): rclpy.init() try: node = ControllersTestNode("test_controllers_startup_fail") - node.create_test_subscribers_and_publishers() - 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.!" ) - 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.!" ) - 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.!" @@ -82,25 +77,12 @@ def test_controllers_startup_fail(): @pytest.mark.launch(fixture=generate_test_description) -def test_controllers_startup_success(): +def test_controllers_startup(): rclpy.init() try: - node = ControllersTestNode("test_controllers_startup_success") - 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_xl_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 index ea43313..6c699f8 100644 --- a/rosbot_xl_controller/test/test_multirobot_controllers.py +++ b/rosbot_xl_controller/test/test_multirobot_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. @@ -17,25 +17,21 @@ import launch_pytest import pytest import rclpy -import os -import random + 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 +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(): - proc_env = os.environ.copy() - proc_env["ROS_LOCALHOST_ONLY"] = "1" - proc_env["ROS_DOMAIN_ID"] = random.randint(0, 255) - rosbot_controller = get_package_share_directory("rosbot_xl_controller") actions = [] for i in range(len(robot_names)): @@ -57,7 +53,7 @@ def generate_test_description(): ) # When there is no delay the controllers doesn't spawn correctly - delayed_controller_launch = TimerAction(period=i * 2.0, actions=[controller_launch]) + delayed_controller_launch = TimerAction(period=i * 5.0, actions=[controller_launch]) actions.append(delayed_controller_launch) return LaunchDescription(actions) @@ -69,23 +65,8 @@ def test_multirobot_controllers_startup_success(): rclpy.init() try: node = ControllersTestNode(f"test_{robot_name}_controllers", namespace=robot_name) - node.create_test_subscribers_and_publishers() 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, ( - f"Expected JointStates message but it was not received. Check {robot_name}/" - "joint_state_broadcaster!" - ) - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert msgs_received_flag, ( - f"Expected Odom message but it was not received. Check {robot_name}/" - "rosbot_xl_base_controller!" - ) - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), f"Expected Imu message but it was not received. Check {robot_name}/imu_broadcaster!" + 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 index bfe2dfb..a9b6b05 100644 --- a/rosbot_xl_controller/test/test_namespaced_diff_drive_controllers.py +++ b/rosbot_xl_controller/test/test_namespaced_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. @@ -17,23 +17,19 @@ import launch_pytest import pytest import rclpy -import os -import random + 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 def generate_test_description(): - proc_env = os.environ.copy() - proc_env["ROS_LOCALHOST_ONLY"] = "1" - proc_env["ROS_DOMAIN_ID"] = random.randint(0, 255) - rosbot_xl_controller = get_package_share_directory("rosbot_xl_controller") bringup_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -56,56 +52,12 @@ def generate_test_description(): @pytest.mark.launch(fixture=generate_test_description) -def test_namespaced_controllers_startup_fail(): +def test_namespaced_controllers_startup(): rclpy.init() try: - node = ControllersTestNode( - "test_namespaced_controllers_startup_fail", namespace="rosbot_xl" - ) - node.create_test_subscribers_and_publishers() - - node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received JointStates message that should not have appeared. Check whether other" - " robots are connected to your network.!" - ) - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received Odom message that should not have appeared. Check whether other robots are" - " connected to your network.!" - ) - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received Imu message that should not have appeared. Check whether other robots are" - " connected to your network.!" - ) - finally: - rclpy.shutdown() - - -@pytest.mark.launch(fixture=generate_test_description) -def test_namespaced_controllers_startup_success(): - rclpy.init() - try: - node = ControllersTestNode( - "test_namespaced_controllers_startup_success", namespace="rosbot_xl" - ) - node.create_test_subscribers_and_publishers() + node = ControllersTestNode("test_namespaced_controllers_startup", namespace="rosbot_xl") 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_xl_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_namespaced_mecanum_controllers.py b/rosbot_xl_controller/test/test_namespaced_mecanum_controllers.py index d303727..b3a2bee 100644 --- a/rosbot_xl_controller/test/test_namespaced_mecanum_controllers.py +++ b/rosbot_xl_controller/test/test_namespaced_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. @@ -17,23 +17,19 @@ import launch_pytest import pytest import rclpy -import os -import random + 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 def generate_test_description(): - proc_env = os.environ.copy() - proc_env["ROS_LOCALHOST_ONLY"] = "1" - proc_env["ROS_DOMAIN_ID"] = random.randint(0, 255) - rosbot_xl_controller = get_package_share_directory("rosbot_xl_controller") bringup_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -56,56 +52,12 @@ def generate_test_description(): @pytest.mark.launch(fixture=generate_test_description) -def test_namespaced_controllers_startup_fail(): +def test_namespaced_controllers_startup(): rclpy.init() try: - node = ControllersTestNode( - "test_namespaced_controllers_startup_fail", namespace="rosbot_xl" - ) - node.create_test_subscribers_and_publishers() - - node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received JointStates message that should not have appeared. Check whether other" - " robots are connected to your network.!" - ) - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received Odom message that should not have appeared. Check whether other robots are" - " connected to your network.!" - ) - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received Imu message that should not have appeared. Check whether other robots are" - " connected to your network.!" - ) - finally: - rclpy.shutdown() - - -@pytest.mark.launch(fixture=generate_test_description) -def test_namespaced_controllers_startup_success(): - rclpy.init() - try: - node = ControllersTestNode( - "test_namespaced_controllers_startup_success", namespace="rosbot_xl" - ) - node.create_test_subscribers_and_publishers() + node = ControllersTestNode("test_namespaced_controllers_startup", namespace="rosbot_xl") 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_xl_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_utils.py b/rosbot_xl_controller/test/test_utils.py index 1f9fbc7..a6a386c 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 @@ -35,7 +31,6 @@ def __init__(self, name="test_node", namespace=None): 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 ) @@ -47,16 +42,12 @@ def create_test_subscribers_and_publishers(self): self.imu_sub = self.create_subscription(Imu, "imu_broadcaster/imu", self.imu_callback, 10) # TODO: @delihus namespaces have not been implemented in microros yet - self.imu_publisher = self.create_publisher(Imu, "/_imu/data_raw", 10) + 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() @@ -89,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 0fd520d..0ef5dfa 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. diff --git a/rosbot_xl_gazebo/config/gz_intel_realsense_d435_remappings.yaml b/rosbot_xl_gazebo/config/gz_intel_realsense_d435_remappings.yaml index 86ee134..3b58a15 100644 --- a/rosbot_xl_gazebo/config/gz_intel_realsense_d435_remappings.yaml +++ b/rosbot_xl_gazebo/config/gz_intel_realsense_d435_remappings.yaml @@ -1,26 +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/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/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/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 + - 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 + - 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 index fe19ad5..c77c1e8 100644 --- a/rosbot_xl_gazebo/config/gz_orbbec_astra_remappings.yaml +++ b/rosbot_xl_gazebo/config/gz_orbbec_astra_remappings.yaml @@ -1,26 +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/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/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/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 + - 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 + - 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 index 81ffbbd..e313dcb 100644 --- a/rosbot_xl_gazebo/config/gz_slamtec_rplidar_remappings.yaml +++ b/rosbot_xl_gazebo/config/gz_slamtec_rplidar_remappings.yaml @@ -1,3 +1,4 @@ -- topic_name: /scan - ros_type_name: sensor_msgs/msg/LaserScan - gz_type_name: ignition.msgs.LaserScan +--- + - 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 index 6bea43a..dee8f67 100644 --- a/rosbot_xl_gazebo/config/gz_stereolabs_zed_remappings.yaml +++ b/rosbot_xl_gazebo/config/gz_stereolabs_zed_remappings.yaml @@ -1,22 +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/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 + - 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/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/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 + - 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 index e8ecc2c..0a7b2ca 100644 --- a/rosbot_xl_gazebo/config/gz_velodyne_puck_remappings.yaml +++ b/rosbot_xl_gazebo/config/gz_velodyne_puck_remappings.yaml @@ -1,5 +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 + - 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 d5d9185..bf48d40 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. @@ -49,9 +49,7 @@ def launch_setup(context, *args, **kwargs): pitch = LaunchConfiguration("pitch", default="0.0").perform(context) yaw = LaunchConfiguration("yaw", default="0.0").perform(context) - gz_args = ( - f"--headless-rendering -s -v 4 -r {world}" if eval(headless) else f"-r {world}" - ) + gz_args = f"--headless-rendering -s -v 4 -r {world}" if eval(headless) else f"-r {world}" gz_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( diff --git a/rosbot_xl_gazebo/launch/spawn.launch.py b/rosbot_xl_gazebo/launch/spawn.launch.py index 7659673..010633b 100644 --- a/rosbot_xl_gazebo/launch/spawn.launch.py +++ b/rosbot_xl_gazebo/launch/spawn.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. @@ -43,7 +43,7 @@ def launch_gz_bridge(context: LaunchContext, *args, **kwargs): ] namespace_ext = "" if namespace == "" else "/" + namespace - robot_name = 'rosbot_xl' + robot_name = "rosbot_xl" if namespace != "": robot_name = namespace diff --git a/rosbot_xl_gazebo/setup.py b/rosbot_xl_gazebo/setup.py index 895e904..9e5bccd 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. diff --git a/rosbot_xl_gazebo/test/test_diff_drive_simulation.py b/rosbot_xl_gazebo/test/test_diff_drive_simulation.py index 4623d04..2eac511 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 2024 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. @@ -17,8 +17,7 @@ import launch_pytest import pytest import rclpy -import os -import random + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription @@ -27,17 +26,13 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_testing.actions import ReadyToTest from launch_testing.util import KeepAliveProc - -from test_utils import SimulationTest, diff_test +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(): - proc_env = os.environ.copy() - proc_env["ROS_LOCALHOST_ONLY"] = "1" - proc_env["ROS_DOMAIN_ID"] = random.randint(0, 255) - rosbot_xl_gazebo = get_package_share_directory("rosbot_xl_gazebo") simulation_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -75,14 +70,11 @@ def generate_test_description(): def test_diff_drive_simulation(): rclpy.init() try: - node = SimulationTest("test_diff_drive_simulation") - node.create_test_subscribers_and_publishers() - node.start_node_thread() + node = SimulationTestNode("test_diff_drive_simulation") + Thread(target=lambda node: rclpy.spin(node), args=(node,)).start() diff_test(node) - node.shutdown() - finally: # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching # several tests in a row. diff --git a/rosbot_xl_gazebo/test/test_ign_kill_utils.py b/rosbot_xl_gazebo/test/test_ign_kill_utils.py index bb35c50..4f857a1 100644 --- a/rosbot_xl_gazebo/test/test_ign_kill_utils.py +++ b/rosbot_xl_gazebo/test/test_ign_kill_utils.py @@ -1,4 +1,4 @@ -# 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"); @@ -15,6 +15,7 @@ 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 diff --git a/rosbot_xl_gazebo/test/test_mecanum_simulation.py b/rosbot_xl_gazebo/test/test_mecanum_simulation.py index 622761c..c605d25 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 2024 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. @@ -17,8 +17,7 @@ import launch_pytest import pytest import rclpy -import os -import random + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription @@ -27,17 +26,13 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_testing.actions import ReadyToTest from launch_testing.util import KeepAliveProc - -from test_utils import SimulationTest, mecanum_test +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(): - proc_env = os.environ.copy() - proc_env["ROS_LOCALHOST_ONLY"] = "1" - proc_env["ROS_DOMAIN_ID"] = random.randint(0, 255) - rosbot_xl_gazebo = get_package_share_directory("rosbot_xl_gazebo") simulation_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -76,12 +71,10 @@ def generate_test_description(): def test_mecanum_simulation(): rclpy.init() try: - node = SimulationTest("test_mecanum_simulation") - node.create_test_subscribers_and_publishers() - node.start_node_thread() + node = SimulationTestNode("test_mecanum_simulation") + Thread(target=lambda node: rclpy.spin(node), args=(node,)).start() mecanum_test(node) - node.shutdown() 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 index 2fcf2ba..9d2a9f9 100644 --- a/rosbot_xl_gazebo/test/test_multirobot_diff_drive_simulation.py +++ b/rosbot_xl_gazebo/test/test_multirobot_diff_drive_simulation.py @@ -1,6 +1,6 @@ # Copyright 2021 Open Source Robotics Foundation, Inc. # Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2024 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. @@ -17,27 +17,20 @@ import launch_pytest import pytest import rclpy -import os -import random 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 MultiThreadedExecutor -from threading import Thread -from test_utils import SimulationTest, diff_test - +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(): - proc_env = os.environ.copy() - proc_env["ROS_LOCALHOST_ONLY"] = "1" - proc_env["ROS_DOMAIN_ID"] = random.randint(0, 255) - # IncludeLaunchDescription does not work with robots argument simulation_launch = ExecuteProcess( cmd=[ @@ -67,31 +60,30 @@ def generate_test_description(): @pytest.mark.launch(fixture=generate_test_description) def test_multirobot_diff_drive_simulation(): - robot_names = ["robot1", "robot2"] + robots = ["robot1", "robot2"] rclpy.init() try: - simulation_tests = {} - executor = MultiThreadedExecutor(num_threads=len(robot_names)) + nodes = {} + executor = SingleThreadedExecutor() - for robot_name in robot_names: - node = SimulationTest( - "test_multirobot_diff_drive_simulation", namespace=robot_name + for node_namespace in robots: + node = SimulationTestNode( + "test_multirobot_diff_drive_simulation", namespace=node_namespace ) - node.create_test_subscribers_and_publishers() - simulation_tests[robot_name] = node - executor.add_node(node.node) + nodes[node_namespace] = node + executor.add_node(node) - ros_spin_thread = Thread(target=lambda executor: executor.spin(), args=(executor,)) - ros_spin_thread.start() + Thread(target=lambda executor: executor.spin(), args=(executor,)).start() - for robot_name in robot_names: - node = simulation_tests[robot_name] - diff_test(node, robot_name) - node.shutdown() + for node_namespace in robots: + node = nodes[node_namespace] + diff_test(node, node_namespace) + executor.remove_node(node) + node.destroy_node() finally: - rclpy.shutdown() - # 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 index 73614f2..8f092d7 100644 --- a/rosbot_xl_gazebo/test/test_multirobot_mecanum_simulation.py +++ b/rosbot_xl_gazebo/test/test_multirobot_mecanum_simulation.py @@ -1,6 +1,6 @@ # Copyright 2021 Open Source Robotics Foundation, Inc. # Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2024 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. @@ -17,27 +17,20 @@ import launch_pytest import pytest import rclpy -import os -import random 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 MultiThreadedExecutor -from threading import Thread - -from test_utils import SimulationTest, mecanum_test +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(): - proc_env = os.environ.copy() - proc_env["ROS_LOCALHOST_ONLY"] = "1" - proc_env["ROS_DOMAIN_ID"] = random.randint(0, 255) - # IncludeLaunchDescription does not work with robots argument simulation_launch = ExecuteProcess( cmd=[ @@ -68,30 +61,30 @@ def generate_test_description(): @pytest.mark.launch(fixture=generate_test_description) def test_multirobot_mecanum_simulation(): - robot_names = ["robot1", "robot2"] + robots = ["robot1", "robot2"] rclpy.init() try: - simulation_tests = {} - executor = MultiThreadedExecutor(num_threads=len(robot_names)) + nodes = {} + executor = SingleThreadedExecutor() - for robot_name in robot_names: - node = SimulationTest("test_multirobot_mecanum_simulation", namespace=robot_name) - node.create_test_subscribers_and_publishers() - simulation_tests[robot_name] = node - executor.add_node(node.node) + for node_namespace in robots: + node = SimulationTestNode( + "test_multirobot_mecanum_simulation", namespace=node_namespace + ) + nodes[node_namespace] = node + executor.add_node(node) - ros_spin_thread = Thread(target=lambda executor: executor.spin(), args=(executor,)) - ros_spin_thread.start() + Thread(target=lambda executor: executor.spin(), args=(executor,)).start() - # Speed test - for robot_name in robot_names: - node = simulation_tests[robot_name] - mecanum_test(node, robot_name) - node.shutdown() + for node_namespace in robots: + node = nodes[node_namespace] + mecanum_test(node, node_namespace) + executor.remove_node(node) + node.destroy_node() finally: - rclpy.shutdown() - # 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_namespaced_diff_drive_simulation.py b/rosbot_xl_gazebo/test/test_namespaced_diff_drive_simulation.py index 708d523..f47cd76 100644 --- a/rosbot_xl_gazebo/test/test_namespaced_diff_drive_simulation.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 2024 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. @@ -17,8 +17,7 @@ import launch_pytest import pytest import rclpy -import os -import random + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription @@ -27,17 +26,13 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_testing.actions import ReadyToTest from launch_testing.util import KeepAliveProc - -from test_utils import SimulationTest, diff_test +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(): - proc_env = os.environ.copy() - proc_env["ROS_LOCALHOST_ONLY"] = "1" - proc_env["ROS_DOMAIN_ID"] = random.randint(0, 255) - rosbot_xl_gazebo = get_package_share_directory("rosbot_xl_gazebo") simulation_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -58,7 +53,7 @@ def generate_test_description(): "empty_with_plugins.sdf", ] ), - "namespace": "rosbot2r", + "namespace": "rosbot_xl", }.items(), ) @@ -76,12 +71,10 @@ def generate_test_description(): def test_namespaced_diff_drive_simulation(): rclpy.init() try: - node = SimulationTest("test_namespaced_diff_drive_simulation", namespace="rosbot2r") - 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() diff_test(node) - node.shutdown() finally: # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching diff --git a/rosbot_xl_gazebo/test/test_namespaced_mecanum_simulation.py b/rosbot_xl_gazebo/test/test_namespaced_mecanum_simulation.py index 49b5ca4..ef81010 100644 --- a/rosbot_xl_gazebo/test/test_namespaced_mecanum_simulation.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 2024 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. @@ -17,8 +17,6 @@ import launch_pytest import pytest import rclpy -import os -import random from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription @@ -27,17 +25,13 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_testing.actions import ReadyToTest from launch_testing.util import KeepAliveProc - -from test_utils import SimulationTest, mecanum_test +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(): - proc_env = os.environ.copy() - proc_env["ROS_LOCALHOST_ONLY"] = "1" - proc_env["ROS_DOMAIN_ID"] = random.randint(0, 255) - rosbot_xl_gazebo = get_package_share_directory("rosbot_xl_gazebo") simulation_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -59,7 +53,7 @@ def generate_test_description(): ] ), "headless": "True", - "namespace": "rosbot2r", + "namespace": "rosbot_xl", }.items(), ) @@ -77,14 +71,10 @@ def generate_test_description(): def test_namespaced_mecanum_simulation(): rclpy.init() try: - node = SimulationTest( - "test_namespaced_mecanum_simulation", namespace="rosbot2r" - ) - 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() mecanum_test(node) - node.shutdown() 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 0dfae2a..bda96de 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. @@ -15,95 +15,80 @@ import rclpy from rclpy.node import Node -from rclpy.executors import SingleThreadedExecutor from threading import Event -from threading import Thread from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry -from sensor_msgs.msg import LaserScan, Image, PointCloud2 +from sensor_msgs.msg import Imu, JointState -class SimulationTest: +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", namespace=None): - self.VELOCITY_STABILIZATION_DELAY = 3 - self.vel_stabilization_time_event = Event() + super().__init__(name, namespace=namespace) + self.cmd_vel_pub = self.create_publisher(Twist, "cmd_vel", 10) + # 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) + + # 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.twist = None - - self.controller_odom_flag = False - self.ekf_odom_flag = False - self.scan_event = Event() - self.camera_color_event = Event() - self.camera_points_event = Event() - - self.ros_context = rclpy.Context() - self.ros_executor = SingleThreadedExecutor() - self.node = Node(name, namespace=namespace) - self.ros_executor.add_node(self.node) + # 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() - # Use simulation time to correct run on slow machine + # 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.node.set_parameters([use_sim_time]) + self.set_parameters([use_sim_time]) - self.current_time = 1e-9 * self.node.get_clock().now().nanoseconds - self.goal_received_time = 1e-9 * self.node.get_clock().now().nanoseconds + # 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.node.get_logger().info(f"cmd_vel: x: {v_x}, y: {v_y}, yaw: {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.node.get_clock().now().nanoseconds - self.vel_stabilization_time_event.clear() - - def create_test_subscribers_and_publishers(self): - self.cmd_vel_publisher = self.node.create_publisher(Twist, "cmd_vel", 10) - - self.controller_odom_sub = self.node.create_subscription( - Odometry, "rosbot_xl_base_controller/odom", self.controller_callback, 10 - ) - - self.ekf_odom_sub = self.node.create_subscription( - Odometry, "odometry/filtered", self.ekf_callback, 10 - ) - - self.scan_sub = self.node.create_subscription(LaserScan, "scan", self.scan_callback, 10) - - self.camera_color_sub = self.node.create_subscription( - Image, "camera/image", self.camera_image_callback, 10 - ) - - self.camera_points_sub = self.node.create_subscription( - PointCloud2, "camera/points", self.camera_points_callback, 10 - ) - - self.timer = self.node.create_timer(1.0 / 10.0, self.timer_callback) - - def spin_handle_external_shutdown(self): - try: - self.ros_executor.spin() - except rclpy.executors.ExternalShutdownException: - pass - - def start_node_thread(self): - self.ros_spin_thread = Thread(target=self.spin_handle_external_shutdown) - self.ros_spin_thread.start() + self.goal_received_time = 1e-9 * self.get_clock().now().nanoseconds + 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): @@ -116,54 +101,56 @@ 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): - # Do not override the twist after reaching the command twist - if not self.controller_odom_flag: - self.node.get_logger().info(f"Received twist from controller: {data.twist.twist}") - self.controller_odom_flag = self.is_twist_ok(data.twist.twist) - self.twist = 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): - # Do not override the twist after reaching the command twist - if not self.ekf_odom_flag: - self.node.get_logger().debug(f"Received twist filtered: {data.twist.twist}") - self.ekf_odom_flag = self.is_twist_ok(data.twist.twist) + 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.current_time = 1e-9 * self.node.get_clock().now().nanoseconds - - if self.current_time > self.goal_received_time + self.VELOCITY_STABILIZATION_DELAY: - self.node.get_logger().info("Setting velocity stabilization time") - - self.vel_stabilization_time_event.set() + self.current_time = 1e-9 * self.get_clock().now().nanoseconds - def scan_callback(self, data: LaserScan): - self.node.get_logger().debug(f"Received scan length: {len(data.ranges)}") - if data.ranges: - self.scan_event.set() + if self.is_initialized() and self.goal_received_time: + self.publish_cmd_vel_msg() - def camera_image_callback(self, data: Image): - if data.data: - self.camera_color_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 camera_points_callback(self, data: PointCloud2): - if data.data: - self.camera_points_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.node.get_logger().info(f"Publishing twist: {twist_msg}") - self.cmd_vel_publisher.publish(twist_msg) - - def shutdown(self): - self.node.destroy_node() + self.cmd_vel_pub.publish(twist_msg) def __exit__(self, exep_type, exep_value, trace): if exep_type is not None: @@ -171,90 +158,80 @@ def __exit__(self, exep_type, exep_value, trace): self.shutdown() -def x_speed_test(node, 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) +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}" + ) + - assert node.vel_stabilization_time_event.wait(timeout=120.0), ( +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.controller_odom_flag, ( + assert node.is_controller_odom_correct, ( f"{robot_name}: does not move properly in x direction. Check" - f" rosbot_xl_base_controller! Twist: {node.twist}" + f" rosbot_xl_base_controller! Twist: {node.controller_twist}" f"\nCommand: x: {v_x}, y:{v_y}, yaw:{v_yaw}" ) - assert node.ekf_odom_flag, ( + assert node.is_ekf_odom_correct, ( f"{robot_name}: does not move properly in x direction. Check ekf_filter_node!" - f" Twist: {node.twist}" + f" Twist: {node.ekf_twist}" ) -def y_speed_test(node, v_x=0.0, v_y=0.0, v_yaw=0.0, robot_name="ROSbot"): +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(timeout=120.0), ( + 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.controller_odom_flag, ( + assert node.is_controller_odom_correct, ( f"{robot_name} does not move properly in y direction. Check" - f" rosbot_xl_base_controller! Twist: {node.twist}" + f" rosbot_xl_base_controller! Twist: {node.controller_twist}" f"\nCommand: x: {v_x}, y:{v_y}, yaw:{v_yaw}" ) - assert node.ekf_odom_flag, ( + assert node.is_ekf_odom_correct, ( f"{robot_name} does not move properly in y direction. Check ekf_filter_node!" - f" Twist: {node.twist}" + f" Twist: {node.ekf_twist}" ) -def yaw_speed_test(node, v_x=0.0, v_y=0.0, v_yaw=0.0, robot_name="ROSbot"): +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(timeout=120.0), ( + 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.controller_odom_flag, ( + assert node.is_controller_odom_correct, ( f"{robot_name} does not rotate properly. Check rosbot_xl_base_controller! Twist:" - f" {node.twist}" + f" {node.controller_twist}" f"\nCommand: x: {v_x}, y:{v_y}, yaw:{v_yaw}" ) assert ( - node.ekf_odom_flag - ), f"{robot_name} does not rotate properly. Check ekf_filter_node! Twist: {node.twist}" - - -def sensors_readings_test(node, robot_name="ROSbot"): - pass - # flag = node.scan_event.wait(timeout=20.0) - # assert flag, f"{robot_name}'s lidar does not work properly!" - - # for i in range(len(node.RANGE_SENSORS_TOPICS)): - # flag = node.ranges_events[i].wait(timeout=20.0) - # assert ( - # flag - # ), f"{robot_name}'s range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!" - - # flag = node.camera_color_event.wait(timeout=20.0) - # assert flag, f"{robot_name}'s camera color image does not work properly!" - - # flag = node.camera_points_event.wait(timeout=20.0) - # assert flag, f"{robot_name}'s camera point cloud does not work properly!" + node.is_ekf_odom_correct + ), f"{robot_name} does not rotate properly. Check ekf_filter_node! Twist: {node.ekf_twist}" -def diff_test(node, robot_name="ROSbot"): - sensors_readings_test(node, robot_name) +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, robot_name="ROSbot"): - sensors_readings_test(node, 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)