diff --git a/.flake8 b/.flake8 new file mode 100644 index 00000000..1b7fbcb2 --- /dev/null +++ b/.flake8 @@ -0,0 +1,6 @@ +[flake8] +extend-ignore = E203, B902,C816,D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404,I202 +import-order-style = google +max-line-length = 99 +show-source = true +statistics = true diff --git a/.github/workflows/bump_version.yaml b/.github/workflows/bump_version.yaml index 98253389..a0a6574f 100644 --- a/.github/workflows/bump_version.yaml +++ b/.github/workflows/bump_version.yaml @@ -1,60 +1,64 @@ +--- 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: - get-bump: - name: Get version bump - runs-on: ubuntu-latest - outputs: - bump: ${{ env.BUMP }} - steps: - - if: github.event_name == 'pull_request' && github.event.pull_request.merged == true + industrial_ci: + name: Industrial CI + uses: ./.github/workflows/industrial_ci.yaml + + get-bump: 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 + 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 + + 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-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 }}"}}' diff --git a/.github/workflows/industrial_ci.yaml b/.github/workflows/industrial_ci.yaml new file mode 100644 index 00000000..ed666a05 --- /dev/null +++ b/.github/workflows/industrial_ci.yaml @@ -0,0 +1,46 @@ +--- +name: Industrial CI +on: + 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: + name: Spellcheck + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: rojopolis/spellcheck-github-actions@0.33.1 + name: Spellcheck + + ros_industrial_ci: + name: ROS Industrial CI + needs: + - black + - spellcheck + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [humble] + runs-on: ubuntu-latest + timeout-minutes: 16 + steps: + - uses: actions/checkout@v3 + - name: Clone installation requirements + shell: bash + run: python3 -m pip install -U vcstool && vcs import . < ./rosbot_xl/rosbot_xl_hardware.repos && vcs import . < ./rosbot_xl/rosbot_xl_simulation.repos + - uses: ros-industrial/industrial_ci@master + env: + ROS_DISTRO: ${{matrix.ROS_DISTRO}} + HUSARION_ROS_BUILD: simulation diff --git a/.gitignore b/.gitignore index 9b35f9d0..c57e18a4 100644 --- a/.gitignore +++ b/.gitignore @@ -5,8 +5,18 @@ demo/test/.env demo/id dev_utils/ -# ROS2 +# ROS 2 build folders build/ install/ log/ -__pycache__/ \ No newline at end of file +__pycache__/ + +# ROSbots submodules +rosbot_hardware_interfaces/ +ros_components_description/ +rosbot_controllers/ +husarion/husarion_office_gz +gazebosim/gz_ros2_control + +# pyspelling +*.dic diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 00000000..5743d209 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,71 @@ +--- +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/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/psf/black + rev: 23.11.0 + hooks: + - id: black + args: ["--line-length=99", "--preview"] + + - repo: https://github.com/PyCQA/flake8 + rev: 6.1.0 + hooks: + - id: flake8 + args: ["--ignore=E501,W503,E203"] # ignore too long line and line break before binary operator, + # black checks it + + - repo: local + hooks: + - id: ament_lint_cmake + name: ament_lint_cmake + description: Check format of CMakeLists.txt files. + entry: ament_lint_cmake + language: system + files: CMakeLists\.txt$ + + - repo: local + hooks: + - id: ament_copyright + name: ament_copyright + description: Check if copyright notice is available in all files. + stages: [commit] + entry: ament_copyright + 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/.*$ diff --git a/.pyspelling.yaml b/.pyspelling.yaml new file mode 100644 index 00000000..df8259ca --- /dev/null +++ b/.pyspelling.yaml @@ -0,0 +1,53 @@ +matrix: +- 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 + + 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 + + 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 + + pipeline: + - pyspelling.filters.xml diff --git a/.wordlist.txt b/.wordlist.txt new file mode 100644 index 00000000..6b7d0f8d --- /dev/null +++ b/.wordlist.txt @@ -0,0 +1,72 @@ +wordlist +wordlists +pyspelling +metapackage +preconfigured +colcon +rosbot_xl_description +vcs +vcstool +rosdep +rosdistro +Dockerfiles +gamepad +teleoperation +laserscan +amcl +env +usr +ros +husarion +apache +http +www +urdf +xacro +imu +xl +rosbot +https +cmake +github +ament +Wojciechowski +Stepien +Nowak +Nikolas +ROSbot +Maciej +Krzysztof +extrinsics +Duc +Dominik +bringup +gz +ign +Jakub +Delicat +mecanum +fdir +webots +accelerometer +cmd +config +rl +rr +unstamped +vel +msg +lidar +gpu +gazebosim +msgs +nav +pytest +FIXME +TODO +nan +rclpy +utils +tf +yaml +odometry diff --git a/LICENSE.txt b/LICENSE.txt index 8b1f1a64..a835c7d1 100644 --- a/LICENSE.txt +++ b/LICENSE.txt @@ -1,5 +1,5 @@ ROSBOT XL ROS PACKAGE LICENSING -The default license for ROSbot XL ROS package is the Apache License, Version 2.0 -(see LICENSE_APACHE2.txt); you may elect at your option to use the ROSbot XL ROS -package under the MIT License (see LICENSE_MIT.txt). Contributions must be +The default license for ROSbot XL ROS package is the Apache License, Version 2.0 +(see LICENSE_APACHE2.txt); you may elect at your option to use the ROSbot XL ROS +package under the MIT License (see LICENSE_MIT.txt). Contributions must be made under both licenses. diff --git a/README.md b/README.md index a0261977..5e402d8b 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,7 @@ ROS2 packages for ROSbot XL ### `rosbot_xl` -Metapackeage that contains dependencies to other repositories. It is also used to define whether simulation dependencies should be used. +Metapackage that contains dependencies to other repositories. It is also used to define whether simulation dependencies should be used. ### `rosbot_xl_bringup` @@ -36,17 +36,21 @@ ROS2 hardware controller for ROSbot XL. Inputs and outputs data from ROS2 contro Available in [ROS_API.md](./ROS_API.md) -## Source build +## Usage on hardware -### Prerequisites +To run the software on real ROSbot XL, also communication with Digital Board will be necessary. +First update your firmware to make sure that you use the latest version, then run the `micro-ROS` agent. +For detailed instructions refer to the [rosbot_xl_firmware repository](https://github.com/husarion/rosbot_xl_firmware). -Install `colcon`, `vsc` and `rosdep`: +## Prepare environment + +1. **Install `colcon`, `vsc` and `rosdep`** ``` sudo apt-get update sudo apt-get install -y python3-colcon-common-extensions python3-vcstool python3-rosdep ``` -Create workspace folder and clone `rosbot_xl_ros` repository: +2. **Create workspace folder and clone `rosbot_xl_ros` repository:** ``` mkdir -p ros2_ws/src cd ros2_ws @@ -55,7 +59,7 @@ git clone https://github.com/husarion/rosbot_xl_ros src/ ### Build and run on hardware -Building: +1. **Building** ``` export HUSARION_ROS_BUILD=hardware @@ -71,7 +75,10 @@ rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y colcon build ``` -Running: +> [!NOTE] +> Before starting the software on the robot please make sure that you're using the latest firmware and run the `micro-ROS` agent as described in the [Usage on hardware](#usage-on-hardware) step. + +2. **Running** ``` source install/setup.bash ros2 launch rosbot_xl_bringup bringup.launch.py @@ -79,7 +86,7 @@ ros2 launch rosbot_xl_bringup bringup.launch.py ### Build and run Gazebo simulation -Building: +1. **Building** ``` export HUSARION_ROS_BUILD=simulation @@ -94,11 +101,72 @@ rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y colcon build ``` -Running: +2. **Running** ``` source install/setup.bash ros2 launch rosbot_xl_gazebo simulation.launch.py ``` +## Testing package + +### pre-commit +[pre-commit configuration](.pre-commit-config.yaml) prepares plenty of tests helping for developing and contributing. Usage: + +```bash +# install pre-commit +pip install pre-commit + +# initialize pre-commit workspace +pre-commit install + +# manually run tests +pre-commit run -a + +# update revision +pre-commit autoupdate +``` + +After initialization [pre-commit configuration](.pre-commit-config.yaml) will applied on every commit. + +### Industrial CI +``` +colcon test +``` + +> [!NOTE] +> Command `colcon test` does not build the code. Remember to build your code after changes. + +If tests finish with errors print logs: +``` +colcon test-result --verbose +``` + +### Format python code with [Black](https://github.com/psf/black) +``` +cd src/ +black rosbot* +``` + + +## Testing package + +### Industrial CI +``` +colcon test +``` + +> [!NOTE] +> Command `colcon test` does not build the code. Remember to build your code after changes. + +If tests finish with errors print logs: +``` +colcon test-result --verbose +``` + +### Format python code with [Black](https://github.com/psf/black) +``` +cd src/ +black rosbot* +``` ## Demos diff --git a/ROS_API.md b/ROS_API.md index 1908c250..3cddc39d 100644 --- a/ROS_API.md +++ b/ROS_API.md @@ -1,9 +1,9 @@ Use `bringup.launch.py` from `rosbot_xl_bringup` to start all base functionalities for ROSbot XL. It consists of the following parts: -- `scan_to_scan_filter_chain` from `laser_filters`, it subscribes to `/scan` topic and removes all points that are within the robot's footprint (defined by config `laser_filter.yaml` in `rosbot_xl_bringup` package). Filtered laserscan is then published on `/scan_filtered` topic - +- `scan_to_scan_filter_chain` from `laser_filters`, it subscribes to `/scan` topic and removes all points that are within the robot's footprint (defined by config `laser_filter.yaml` in `rosbot_xl_bringup` package). Filtered laser scan is then published on `/scan_filtered` topic + **Subscribes** - `/scan` (_sensor_msgs/LaserScan_) - + **Publishes** - `/scan_filtered` (_sensor_msgs/LaserScan_) @@ -12,13 +12,13 @@ Use `bringup.launch.py` from `rosbot_xl_bringup` to start all base functionaliti **Subscribes** - `/rosbot_xl_base_controller/odom` (_nav_msgs/Odometry_) - `/imu_broadcaster/imu` (_sensor_msgs/Imu_) - + **Publishes** - `/tf` (_tf2_msgs/TFMessage_) - `base_link`->`odom` transform - `/odometry/filtered` (_nav_msgs/Odometry_) -- `controller.launch.py` from `rosbot_xl_controller`, it loads robot model defined in `rosbot_xl_description` as well as ros2 control [rosbot_hardware_interfaces](https://github.com/husarion/rosbot_hardware_interfaces). It also starts controllers: +- `controller.launch.py` from `rosbot_xl_controller`, it loads robot model defined in `rosbot_xl_description` as well as ros2 control [rosbot_hardware_interfaces](https://github.com/husarion/rosbot_hardware_interfaces). It also starts controllers: * `joint_state_broadcaster` * `rosbot_xl_base_controller` - depending on the value of `mecanum` argument it can be `DiffDriveController` or `MecanumDriveController` * `imu_broadcaster` @@ -27,7 +27,7 @@ Use `bringup.launch.py` from `rosbot_xl_bringup` to start all base functionaliti - `/cmd_vel` (_geometry_msgs/Twist_) - `/_motors_responses` (_sensor_msgs/JointState_) - `/_imu/data_raw` (_sensor_msgs/Imu_) - + **Publishes** - `/tf` (_tf2_msgs/TFMessage_) - `/tf_static` (_tf2_msgs/TFMessage_) diff --git a/rosbot_xl/CHANGELOG.rst b/rosbot_xl/CHANGELOG.rst index b1285f1b..846670b6 100644 --- a/rosbot_xl/CHANGELOG.rst +++ b/rosbot_xl/CHANGELOG.rst @@ -2,6 +2,51 @@ Changelog for package rosbot_xl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.8.11 (2023-11-08) +------------------- +* Merge remote-tracking branch 'origin/master' into ekf-update +* Contributors: Jakub Delicat + +0.8.10 (2023-11-08) +------------------- +* Merge remote-tracking branch 'origin/master' into test-gazebo +* Contributors: Jakub Delicat + +0.8.9 (2023-11-07) +------------------ +* Merge remote-tracking branch 'origin/master' into test-bringup +* Merge remote-tracking branch 'origin/master' into test-bringup +* Contributors: Jakub Delicat + +0.8.8 (2023-10-25) +------------------ + +0.8.7 (2023-10-24) +------------------ + +0.8.6 (2023-10-24) +------------------ +* Merge remote-tracking branch 'origin/master' into test-controller +* Contributors: Jakub Delicat + +0.8.5 (2023-10-24) +------------------ +* Merge pull request `#45 `_ from husarion/add-pyspelling + Add pyspelling +* Merge remote-tracking branch 'origin/master' into add-pyspelling +* Contributors: Jakub Delicat, rafal-gorecki + +0.8.4 (2023-10-24) +------------------ +* Merge remote-tracking branch 'origin/master' into add-black +* Contributors: Jakub Delicat + +0.8.3 (2023-10-24) +------------------ +* Merge pull request `#50 `_ from husarion/add-pre-commit + added and applied precommit +* Contributors: Jakub Delicat + 0.8.2 (2023-05-23) ------------------ diff --git a/rosbot_xl/CMakeLists.txt b/rosbot_xl/CMakeLists.txt index 1949e356..e5ae9d51 100644 --- a/rosbot_xl/CMakeLists.txt +++ b/rosbot_xl/CMakeLists.txt @@ -3,4 +3,4 @@ project(rosbot_xl) find_package(ament_cmake REQUIRED) -ament_package() \ No newline at end of file +ament_package() diff --git a/rosbot_xl/package.xml b/rosbot_xl/package.xml index e03a1863..b17af212 100644 --- a/rosbot_xl/package.xml +++ b/rosbot_xl/package.xml @@ -2,9 +2,9 @@ rosbot_xl - 0.8.2 + 0.8.11 - Meta package that contains all packages of Rosbot XL + Meta package that contains all packages of ROSbot XL Apache License 2.0 Krzysztof Wojciechowski diff --git a/rosbot_xl/rosbot_xl_hardware.repos b/rosbot_xl/rosbot_xl_hardware.repos index 0bf1d2ec..84c92831 100644 --- a/rosbot_xl/rosbot_xl_hardware.repos +++ b/rosbot_xl/rosbot_xl_hardware.repos @@ -10,4 +10,4 @@ repositories: ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git - version: ros2 \ No newline at end of file + version: ros2 diff --git a/rosbot_xl/rosbot_xl_simulation.repos b/rosbot_xl/rosbot_xl_simulation.repos index 0d30339e..400406a9 100644 --- a/rosbot_xl/rosbot_xl_simulation.repos +++ b/rosbot_xl/rosbot_xl_simulation.repos @@ -3,10 +3,10 @@ repositories: type: git url: https://github.com/ros-controls/gz_ros2_control.git # on branch humble hardware isn't activated - # recently on master API breaking change was introduced, it is necessay to use commit before this change + # recently on master API breaking change was introduced, it is necessary to use commit before this change version: b296ff2f5c3758b637a70bd496fe6ed875ab03ce husarion/gazebo_worlds: type: git url: https://github.com/husarion/gazebo_worlds.git - version: main \ No newline at end of file + version: main diff --git a/rosbot_xl_bringup/CHANGELOG.rst b/rosbot_xl_bringup/CHANGELOG.rst index 1e9a6806..faa2f0b7 100644 --- a/rosbot_xl_bringup/CHANGELOG.rst +++ b/rosbot_xl_bringup/CHANGELOG.rst @@ -2,6 +2,56 @@ Changelog for package rosbot_xl_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.8.11 (2023-11-08) +------------------- +* Merge pull request `#53 `_ from husarion/ekf-update + Upgrade EKF +* Merge remote-tracking branch 'origin/master' into ekf-update +* Contributors: Jakub Delicat, rafal-gorecki + +0.8.10 (2023-11-08) +------------------- +* Merge remote-tracking branch 'origin/master' into test-gazebo +* Contributors: Jakub Delicat + +0.8.9 (2023-11-07) +------------------ +* Merge pull request `#47 `_ from husarion/test-bringup + Test bringup +* Merge remote-tracking branch 'origin/master' into test-bringup +* Merge remote-tracking branch 'origin/master' into test-bringup +* Contributors: Jakub Delicat, rafal-gorecki + +0.8.8 (2023-10-25) +------------------ + +0.8.7 (2023-10-24) +------------------ +* Merge pull request `#51 `_ from husarion/workflow-test + Workflow test +* Contributors: rafal-gorecki + +0.8.6 (2023-10-24) +------------------ +* Merge remote-tracking branch 'origin/master' into test-controller +* Contributors: Jakub Delicat + +0.8.5 (2023-10-24) +------------------ +* Merge remote-tracking branch 'origin/master' into add-pyspelling +* Contributors: Jakub Delicat + +0.8.4 (2023-10-24) +------------------ +* Merge remote-tracking branch 'origin/master' into add-black +* Contributors: Jakub Delicat + +0.8.3 (2023-10-24) +------------------ +* Merge pull request `#50 `_ from husarion/add-pre-commit + added and applied precommit +* Contributors: Jakub Delicat + 0.8.2 (2023-05-23) ------------------ diff --git a/rosbot_xl_bringup/CMakeLists.txt b/rosbot_xl_bringup/CMakeLists.txt deleted file mode 100644 index b78851c1..00000000 --- a/rosbot_xl_bringup/CMakeLists.txt +++ /dev/null @@ -1,12 +0,0 @@ -cmake_minimum_required(VERSION 3.10.2) -project(rosbot_xl_bringup) - -find_package(ament_cmake REQUIRED) - -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME} -) - -ament_package() \ No newline at end of file diff --git a/rosbot_xl_bringup/config/ekf.yaml b/rosbot_xl_bringup/config/ekf.yaml index 88da4f9b..19323308 100644 --- a/rosbot_xl_bringup/config/ekf.yaml +++ b/rosbot_xl_bringup/config/ekf.yaml @@ -1,83 +1,64 @@ -### ekf config file ### +# Ref: http://docs.ros.org/en/noetic/api/robot_localization/html/state_estimation_nodes.html + +## ekf config file ### ekf_filter_node: ros__parameters: - predict_to_current_time: false frequency: 25.0 - sensor_timeout: 0.04 + sensor_timeout: 0.05 two_d_mode: true - print_diagnostics: true - publish_tf: true + + transform_time_offset: 0.0 + transform_timeout: 0.05 map_frame: map odom_frame: odom base_link_frame: base_link world_frame: odom + publish_tf: true + publish_acceleration: false odom0: /rosbot_xl_base_controller/odom - # in diff drive dY will be always 0, but it should be taken into account, as covariances won't be calculated correctly otherwise (they blow up in y direction) odom0_config: [false, false, false, # X , Y , Z false, false, false, # roll , pitch ,yaw true, true, false, # dX , dY , dZ - false, false, false, # droll , dpitch ,dyaw + false, false, true, # droll , dpitch ,dyaw false, false, false] # ddX , ddY , ddZ - odom0_queue_size: 10 + odom0_queue_size: 2 odom0_nodelay: true odom0_differential: false - odom0_relative: false + odom0_relative: true imu0: /imu_broadcaster/imu imu0_config: [false, false, false, # X , Y , Z false, false, true, # roll , pitch ,yaw false, false, false, # dX , dY , dZ - false, false, false, # droll , dpitch ,dyaw + false, false, true, # droll , dpitch ,dyaw false, false, false] # ddX , ddY , ddZ - imu0_queue_size: 10 + imu0_queue_size: 5 imu0_nodelay: true imu0_differential: false imu0_relative: true imu0_remove_gravitational_acceleration: true -# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is -# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each -# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be. -# However, if users find that a given variable is slow to converge, one approach is to increase the -# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error -# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are -# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if -# unspecified. - process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] -# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal -# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in -# question. Users should take care not to use large values for variables that will not be measured directly. The values -# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below -#if unspecified. - initial_estimate_covariance: [1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9] + reset_on_time_jump: false + predict_to_current_time: false + print_diagnostics: false + + # Selected values ​​experimentally so as to ensure relatively fast convergence (values ​​should be about 10x higher than the sensor variance values) + dynamic_process_noise_covariance: true + process_noise_covariance: [5e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 5e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 5e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4e-2, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4e-2, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4e-2, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-5, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-5, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-5] diff --git a/rosbot_xl_bringup/launch/bringup.launch.py b/rosbot_xl_bringup/launch/bringup.launch.py index 90c46334..b3c4a006 100644 --- a/rosbot_xl_bringup/launch/bringup.launch.py +++ b/rosbot_xl_bringup/launch/bringup.launch.py @@ -1,4 +1,16 @@ -#!/usr/bin/env python3 +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. from launch import LaunchDescription from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument @@ -18,21 +30,41 @@ def generate_launch_description(): declare_mecanum_arg = DeclareLaunchArgument( "mecanum", default_value="False", - description="Whether to use mecanum drive controller (otherwise diff drive controller is used)", - ) - - lidar_model = LaunchConfiguration("lidar_model") - declare_lidar_model_arg = DeclareLaunchArgument( - "lidar_model", - default_value="slamtec_rplidar_s1", - description="Lidar model added to the URDF", + description=( + "Whether to use mecanum drive controller (otherwise diff drive controller is used)" + ), ) camera_model = LaunchConfiguration("camera_model") declare_camera_model_arg = DeclareLaunchArgument( "camera_model", default_value="None", - description="Camera model added to the URDF", + description="Add camera model to the robot URDF", + choices=[ + "None", + "intel_realsense_d435", + "stereolabs_zed", + "stereolabs_zedm", + "stereolabs_zed2", + "stereolabs_zed2i", + "stereolabs_zedx", + "stereolabs_zedxm", + ], + ) + + lidar_model = LaunchConfiguration("lidar_model") + declare_lidar_model_arg = DeclareLaunchArgument( + "lidar_model", + default_value="slamtec_rplidar_s1", + description="Add LiDAR model to the robot URDF", + choices=[ + "None", + "ouster_os1_32", + "slamtec_rplidar_a2", + "slamtec_rplidar_a3", + "slamtec_rplidar_s1", + "velodyne_puck", + ], ) include_camera_mount = LaunchConfiguration("include_camera_mount") @@ -58,13 +90,11 @@ def generate_launch_description(): controller_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - get_package_share_directory("rosbot_xl_controller"), - "launch", - "controller.launch.py", - ] - ) + PathJoinSubstitution([ + get_package_share_directory("rosbot_xl_controller"), + "launch", + "controller.launch.py", + ]) ), launch_arguments={ "mecanum": mecanum, @@ -82,9 +112,9 @@ def generate_launch_description(): name="ekf_filter_node", output="screen", parameters=[ - PathJoinSubstitution( - [get_package_share_directory("rosbot_xl_bringup"), "config", "ekf.yaml"] - ) + PathJoinSubstitution([ + get_package_share_directory("rosbot_xl_bringup"), "config", "ekf.yaml" + ]) ], ) @@ -92,13 +122,11 @@ def generate_launch_description(): package="laser_filters", executable="scan_to_scan_filter_chain", parameters=[ - PathJoinSubstitution( - [ - get_package_share_directory("rosbot_xl_bringup"), - "config", - "laser_filter.yaml", - ] - ) + PathJoinSubstitution([ + get_package_share_directory("rosbot_xl_bringup"), + "config", + "laser_filter.yaml", + ]) ], ) @@ -115,4 +143,4 @@ def generate_launch_description(): laser_filter_node, ] - return LaunchDescription(actions) \ No newline at end of file + return LaunchDescription(actions) diff --git a/rosbot_xl_bringup/package.xml b/rosbot_xl_bringup/package.xml index 82b5a572..3a88955a 100644 --- a/rosbot_xl_bringup/package.xml +++ b/rosbot_xl_bringup/package.xml @@ -2,20 +2,19 @@ rosbot_xl_bringup - 0.8.2 + 0.8.11 - The rosbot_xl_bringup package + ROSbot XL bringup package Apache License 2.0 Maciej Stepien + Jakub Delicat Husarion https://husarion.com/ https://github.com/husarion/rosbot_xl_ros https://github.com/husarion/rosbot_xl_ros/issues - ament_cmake - rosbot_xl_controller launch @@ -24,7 +23,16 @@ laser_filters robot_localization + python3-pytest + launch + launch_ros + launch_pytest + + laser_filters + robot_localization + rosbot_xl_controller + - ament_cmake + ament_python diff --git a/rosbot_xl_bringup/resource/rosbot_xl_bringup b/rosbot_xl_bringup/resource/rosbot_xl_bringup new file mode 100644 index 00000000..e69de29b diff --git a/rosbot_xl_bringup/setup.cfg b/rosbot_xl_bringup/setup.cfg new file mode 100644 index 00000000..08240e7c --- /dev/null +++ b/rosbot_xl_bringup/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rosbot_xl_bringup +[install] +install_scripts=$base/lib/rosbot_xl_bringup diff --git a/rosbot_xl_bringup/setup.py b/rosbot_xl_bringup/setup.py new file mode 100644 index 00000000..b9d7233d --- /dev/null +++ b/rosbot_xl_bringup/setup.py @@ -0,0 +1,41 @@ +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = "rosbot_xl_bringup" + +setup( + name=package_name, + version="0.8.2", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + (os.path.join("share", package_name, "launch"), glob("launch/*.launch.py")), + (os.path.join("share", package_name, "config"), glob("config/*.yaml")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Husarion", + maintainer_email="contact@husarion.com", + description="ROSbot XL bringup package", + license="Apache License 2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [], + }, +) diff --git a/rosbot_xl_bringup/test/test_copyright.py b/rosbot_xl_bringup/test/test_copyright.py new file mode 100644 index 00000000..f46f861d --- /dev/null +++ b/rosbot_xl_bringup/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" 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 new file mode 100644 index 00000000..05ae8fb9 --- /dev/null +++ b/rosbot_xl_bringup/test/test_diff_drive_ekf_and_scan.py @@ -0,0 +1,83 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import BringupTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_xl_bringup = get_package_share_directory("rosbot_xl_bringup") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + rosbot_xl_bringup, + "launch", + "bringup.launch.py", + ]) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "False", + "use_gpu": "False", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +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!" + + finally: + rclpy.shutdown() diff --git a/rosbot_xl_bringup/test/test_flake8.py b/rosbot_xl_bringup/test/test_flake8.py new file mode 100644 index 00000000..40602d8a --- /dev/null +++ b/rosbot_xl_bringup/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +from os.path import join, dirname +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + config_file = join(dirname(dirname(dirname(__file__))), ".flake8") + rc, errors = main_with_errors(argv=["--config", config_file]) + assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors) diff --git a/rosbot_xl_bringup/test/test_mecanum_ekf_and_scan.py b/rosbot_xl_bringup/test/test_mecanum_ekf_and_scan.py new file mode 100644 index 00000000..3424785e --- /dev/null +++ b/rosbot_xl_bringup/test/test_mecanum_ekf_and_scan.py @@ -0,0 +1,83 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import BringupTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_xl_bringup = get_package_share_directory("rosbot_xl_bringup") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + rosbot_xl_bringup, + "launch", + "bringup.launch.py", + ]) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "True", + "use_gpu": "False", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +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!" + + finally: + rclpy.shutdown() diff --git a/rosbot_xl_bringup/test/test_pep257.py b/rosbot_xl_bringup/test/test_pep257.py new file mode 100644 index 00000000..a2c3deb8 --- /dev/null +++ b/rosbot_xl_bringup/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/rosbot_xl_bringup/test/test_utils.py b/rosbot_xl_bringup/test/test_utils.py new file mode 100644 index 00000000..ac878e7f --- /dev/null +++ b/rosbot_xl_bringup/test/test_utils.py @@ -0,0 +1,113 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +import math +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 +from tf2_ros.transform_listener import TransformListener + + +class BringupTestNode(Node): + ROSBOT_HARDWARE_PUBLISHERS_RATE = 10.0 + __test__ = False + + def __init__(self, name="test_node"): + super().__init__(name) + 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.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( + LaserScan, "scan_filtered", self.filtered_scan_callback, 10 + ) + + self.timer = None + + def lookup_transform_odom(self): + try: + self.tf_buffer.lookup_transform("odom", "base_link", rclpy.time.Time()) + self.odom_tf_event.set() + except TransformException as ex: + self.get_logger().error(f"Could not transform odom to base_link: {ex}") + + def start_node_thread(self): + self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,)) + self.ros_spin_thread.start() + + def start_publishing_fake_hardware(self): + self.timer = self.create_timer( + 1.0 / self.ROSBOT_HARDWARE_PUBLISHERS_RATE, + self.timer_callback, + ) + + def filtered_scan_callback(self, msg: LaserScan): + if len(msg.ranges) > 0: + self.scan_filter_event.set() + + def timer_callback(self): + self.publish_fake_hardware_messages() + self.lookup_transform_odom() + self.publish_scan() + + def publish_fake_hardware_messages(self): + imu_msg = Imu() + imu_msg.header.stamp = self.get_clock().now().to_msg() + imu_msg.header.frame_id = "imu_link" + + joint_state_msg = JointState() + joint_state_msg.header.stamp = self.get_clock().now().to_msg() + joint_state_msg.name = [ + "fl_wheel_joint", + "fr_wheel_joint", + "rl_wheel_joint", + "rr_wheel_joint", + ] + 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) + + def publish_scan(self): + msg = LaserScan() + msg.header.frame_id = "base_link" + msg.angle_min = 0.0 + msg.angle_max = 2.0 * math.pi + msg.angle_increment = 0.05 + msg.time_increment = 0.1 + msg.scan_time = 0.1 + msg.range_min = 0.0 + 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) diff --git a/rosbot_xl_controller/CHANGELOG.rst b/rosbot_xl_controller/CHANGELOG.rst index 70c60386..7cf26f50 100644 --- a/rosbot_xl_controller/CHANGELOG.rst +++ b/rosbot_xl_controller/CHANGELOG.rst @@ -1,6 +1,58 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package rosbot_xl_controller -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.8.11 (2023-11-08) +------------------- +* Merge pull request `#53 `_ from husarion/ekf-update + Upgrade EKF +* Merge remote-tracking branch 'origin/master' into ekf-update +* Contributors: Jakub Delicat, rafal-gorecki + +0.8.10 (2023-11-08) +------------------- +* Merge remote-tracking branch 'origin/master' into test-gazebo +* Contributors: Jakub Delicat + +0.8.9 (2023-11-07) +------------------ +* Merge pull request `#47 `_ from husarion/test-bringup + Test bringup +* Merge remote-tracking branch 'origin/master' into test-bringup +* Merge remote-tracking branch 'origin/master' into test-bringup +* Contributors: Jakub Delicat, rafal-gorecki + +0.8.8 (2023-10-25) +------------------ + +0.8.7 (2023-10-24) +------------------ +* Merge pull request `#51 `_ from husarion/workflow-test + Workflow test +* Contributors: rafal-gorecki + +0.8.6 (2023-10-24) +------------------ +* Merge pull request `#46 `_ from husarion/test-controller + Test controller +* Merge remote-tracking branch 'origin/master' into test-controller +* Contributors: Jakub Delicat, rafal-gorecki + +0.8.5 (2023-10-24) +------------------ +* Merge remote-tracking branch 'origin/master' into add-pyspelling +* Contributors: Jakub Delicat + +0.8.4 (2023-10-24) +------------------ +* Merge remote-tracking branch 'origin/master' into add-black +* Contributors: Jakub Delicat + +0.8.3 (2023-10-24) +------------------ +* Merge pull request `#50 `_ from husarion/add-pre-commit + added and applied precommit +* Contributors: Jakub Delicat 0.8.2 (2023-05-23) ------------------ diff --git a/rosbot_xl_controller/CMakeLists.txt b/rosbot_xl_controller/CMakeLists.txt deleted file mode 100644 index 76fe7a70..00000000 --- a/rosbot_xl_controller/CMakeLists.txt +++ /dev/null @@ -1,12 +0,0 @@ -cmake_minimum_required(VERSION 3.10.2) -project(rosbot_xl_controller) - -find_package(ament_cmake REQUIRED) - -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME} -) - -ament_package() \ No newline at end of file diff --git a/rosbot_xl_controller/config/diff_drive_controller.yaml b/rosbot_xl_controller/config/diff_drive_controller.yaml index db7871ba..0dabb6bd 100644 --- a/rosbot_xl_controller/config/diff_drive_controller.yaml +++ b/rosbot_xl_controller/config/diff_drive_controller.yaml @@ -2,8 +2,8 @@ simulation_ignition_ros_control: ros__parameters: use_sim_time: true -# Separate controller manager used for simulation - only difference is -# the use_sim_time parameter (it is the easiest way to do it with ign ros2 control) +# Separate controller manager used for simulation - only difference is +# the use_sim_time parameter (it is the easiest way to do it with ign ros2 control) simulation_controller_manager: ros__parameters: use_sim_time: true @@ -32,6 +32,9 @@ imu_broadcaster: ros__parameters: sensor_name: "imu" frame_id: "imu_link" + static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet + static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally + static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally rosbot_xl_base_controller: ros__parameters: @@ -54,7 +57,7 @@ rosbot_xl_base_controller: odom_frame_id: odom base_frame_id: base_link pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values measured experimentally # Whether to use feedback or commands for odometry calculations open_loop: false diff --git a/rosbot_xl_controller/config/mecanum_drive_controller.yaml b/rosbot_xl_controller/config/mecanum_drive_controller.yaml index 05fb39cf..5d8f9868 100644 --- a/rosbot_xl_controller/config/mecanum_drive_controller.yaml +++ b/rosbot_xl_controller/config/mecanum_drive_controller.yaml @@ -2,8 +2,8 @@ simulation_ignition_ros_control: ros__parameters: use_sim_time: true -# Separate controller manager used for simulation - only difference is -# the use_sim_time parameter (it is the easiest way to do it with ign ros2 control) +# Separate controller manager used for simulation - only difference is +# the use_sim_time parameter (it is the easiest way to do it with ign ros2 control) simulation_controller_manager: ros__parameters: use_sim_time: true @@ -32,6 +32,9 @@ imu_broadcaster: ros__parameters: sensor_name: "imu" frame_id: "imu_link" + static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet + static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values based on diff_drive + static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values based on diff_drive rosbot_xl_base_controller: ros__parameters: @@ -54,7 +57,7 @@ rosbot_xl_base_controller: odom_frame_id: odom base_frame_id: base_link pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values based on diff_drive # Whether to use feedback or commands for odometry calculations open_loop: false diff --git a/rosbot_xl_controller/launch/controller.launch.py b/rosbot_xl_controller/launch/controller.launch.py index 80b87970..f06df1e5 100644 --- a/rosbot_xl_controller/launch/controller.launch.py +++ b/rosbot_xl_controller/launch/controller.launch.py @@ -36,21 +36,41 @@ def generate_launch_description(): declare_mecanum_arg = DeclareLaunchArgument( "mecanum", default_value="False", - description="Whether to use mecanum drive controller (otherwise diff drive controller is used)", - ) - - lidar_model = LaunchConfiguration("lidar_model") - declare_lidar_model_arg = DeclareLaunchArgument( - "lidar_model", - default_value="slamtec_rplidar_s1", - description="Lidar model added to the URDF", + description=( + "Whether to use mecanum drive controller (otherwise diff drive controller is used)", + ), ) camera_model = LaunchConfiguration("camera_model") declare_camera_model_arg = DeclareLaunchArgument( "camera_model", default_value="None", - description="Camera model added to the URDF", + description="Add camera model to the robot URDF", + choices=[ + "None", + "intel_realsense_d435", + "stereolabs_zed", + "stereolabs_zedm", + "stereolabs_zed2", + "stereolabs_zed2i", + "stereolabs_zedx", + "stereolabs_zedxm", + ], + ) + + lidar_model = LaunchConfiguration("lidar_model") + declare_lidar_model_arg = DeclareLaunchArgument( + "lidar_model", + default_value="None", + description="Add LiDAR model to the robot URDF", + choices=[ + "None", + "ouster_os1_32", + "slamtec_rplidar_a2", + "slamtec_rplidar_a3", + "slamtec_rplidar_s1", + "velodyne_puck", + ], ) include_camera_mount = LaunchConfiguration("include_camera_mount") @@ -74,58 +94,48 @@ def generate_launch_description(): description="Which simulation engine will be used", ) - controller_config_name = PythonExpression( - [ - "'mecanum_drive_controller.yaml' if ", - mecanum, - " else 'diff_drive_controller.yaml'", - ] - ) + controller_config_name = PythonExpression([ + "'mecanum_drive_controller.yaml' if ", + mecanum, + " else 'diff_drive_controller.yaml'", + ]) - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare("rosbot_xl_controller"), - "config", - controller_config_name, - ] - ) + robot_controllers = PathJoinSubstitution([ + FindPackageShare("rosbot_xl_controller"), + "config", + controller_config_name, + ]) - controller_manager_name = PythonExpression( - [ - "'/simulation_controller_manager' if ", - use_sim, - " else '/controller_manager'", - ] - ) + controller_manager_name = PythonExpression([ + "'/simulation_controller_manager' if ", + use_sim, + " else '/controller_manager'", + ]) # Get URDF via xacro - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("rosbot_xl_description"), - "urdf", - "rosbot_xl.urdf.xacro", - ] - ), - " mecanum:=", - mecanum, - " lidar_model:=", - lidar_model, - " camera_model:=", - camera_model, - " include_camera_mount:=", - include_camera_mount, - " use_sim:=", - use_sim, - " simulation_engine:=", - simulation_engine, - " simulation_controllers_config_file:=", - robot_controllers, - ] - ) + robot_description_content = Command([ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution([ + FindPackageShare("rosbot_xl_description"), + "urdf", + "rosbot_xl.urdf.xacro", + ]), + " mecanum:=", + mecanum, + " lidar_model:=", + lidar_model, + " camera_model:=", + camera_model, + " include_camera_mount:=", + include_camera_mount, + " use_sim:=", + use_sim, + " simulation_engine:=", + simulation_engine, + " simulation_controllers_config_file:=", + robot_controllers, + ]) robot_description = {"robot_description": robot_description_content} control_node = Node( @@ -173,12 +183,10 @@ def generate_launch_description(): ) # 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], ) ) @@ -218,4 +226,4 @@ def generate_launch_description(): delay_imu_broadcaster_spawner_after_robot_controller_spawner, ] - return LaunchDescription(actions) \ No newline at end of file + return LaunchDescription(actions) diff --git a/rosbot_xl_controller/package.xml b/rosbot_xl_controller/package.xml index 0a2e4cb1..aa51361c 100644 --- a/rosbot_xl_controller/package.xml +++ b/rosbot_xl_controller/package.xml @@ -2,21 +2,20 @@ rosbot_xl_controller - 0.8.2 + 0.8.11 Hardware configuration for ROSbot XL Apache License 2.0 Maciej Stepien Krzysztof Wojciechowski + Jakub Delicat Husarion https://husarion.com/ https://github.com/husarion/rosbot_xl_ros https://github.com/husarion/rosbot_xl_ros/issues - ament_cmake - rosbot_xl_description launch @@ -33,7 +32,25 @@ rosbot_hardware_interfaces + python3-pytest + launch + launch_ros + launch_pytest + + xacro + sensor_msgs + nav_msgs + + rosbot_xl_description + ros_components_description + + controller_manager + joint_state_broadcaster + imu_sensor_broadcaster + diff_drive_controller + mecanum_drive_controller + - ament_cmake + ament_python diff --git a/rosbot_xl_controller/resource/rosbot_xl_controller b/rosbot_xl_controller/resource/rosbot_xl_controller new file mode 100644 index 00000000..e69de29b diff --git a/rosbot_xl_controller/setup.cfg b/rosbot_xl_controller/setup.cfg new file mode 100644 index 00000000..8878b79a --- /dev/null +++ b/rosbot_xl_controller/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rosbot_xl_controller +[install] +install_scripts=$base/lib/rosbot_xl_controller diff --git a/rosbot_xl_controller/setup.py b/rosbot_xl_controller/setup.py new file mode 100644 index 00000000..197fbfd3 --- /dev/null +++ b/rosbot_xl_controller/setup.py @@ -0,0 +1,41 @@ +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = "rosbot_xl_controller" + +setup( + name=package_name, + version="0.8.2", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + (os.path.join("share", package_name, "launch"), glob("launch/*.launch.py")), + (os.path.join("share", package_name, "config"), glob("config/*.yaml")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Husarion", + maintainer_email="contact@husarion.com", + description="Hardware configuration for ROSbot XL", + license="Apache License 2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [], + }, +) diff --git a/rosbot_xl_controller/test/test_copyright.py b/rosbot_xl_controller/test/test_copyright.py new file mode 100644 index 00000000..f46f861d --- /dev/null +++ b/rosbot_xl_controller/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/rosbot_xl_controller/test/test_diff_drive_controllers.py b/rosbot_xl_controller/test/test_diff_drive_controllers.py new file mode 100644 index 00000000..86c9b745 --- /dev/null +++ b/rosbot_xl_controller/test/test_diff_drive_controllers.py @@ -0,0 +1,99 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import ControllersTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_xl_controller = get_package_share_directory("rosbot_xl_controller") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + rosbot_xl_controller, + "launch", + "controller.launch.py", + ]) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "False", + "use_gpu": "False", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_controllers_startup_fail(): + rclpy.init() + try: + node = ControllersTestNode("test_controllers_bringup") + 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_controllers_startup_success(): + rclpy.init() + try: + node = ControllersTestNode("test_controllers_bringup") + 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 + ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected Odom message but it was not received. Check rosbot_base_controller!" + msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected Imu message but it was not received. Check imu_broadcaster!" + finally: + rclpy.shutdown() diff --git a/rosbot_xl_controller/test/test_flake8.py b/rosbot_xl_controller/test/test_flake8.py new file mode 100644 index 00000000..40602d8a --- /dev/null +++ b/rosbot_xl_controller/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +from os.path import join, dirname +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + config_file = join(dirname(dirname(dirname(__file__))), ".flake8") + rc, errors = main_with_errors(argv=["--config", config_file]) + assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors) diff --git a/rosbot_xl_controller/test/test_mecanum_controllers.py b/rosbot_xl_controller/test/test_mecanum_controllers.py new file mode 100644 index 00000000..69df35d9 --- /dev/null +++ b/rosbot_xl_controller/test/test_mecanum_controllers.py @@ -0,0 +1,99 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import ControllersTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_xl_controller = get_package_share_directory("rosbot_xl_controller") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + rosbot_xl_controller, + "launch", + "controller.launch.py", + ]) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "True", + "use_gpu": "False", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_controllers_startup_fail(): + rclpy.init() + try: + node = ControllersTestNode("test_controllers_bringup") + 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_controllers_startup_success(): + rclpy.init() + try: + node = ControllersTestNode("test_controllers_bringup") + 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 + ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected Odom message but it was not received. Check rosbot_base_controller!" + msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected Imu message but it was not received. Check imu_broadcaster!" + finally: + rclpy.shutdown() diff --git a/rosbot_xl_controller/test/test_pep257.py b/rosbot_xl_controller/test/test_pep257.py new file mode 100644 index 00000000..a2c3deb8 --- /dev/null +++ b/rosbot_xl_controller/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/rosbot_xl_controller/test/test_utils.py b/rosbot_xl_controller/test/test_utils.py new file mode 100644 index 00000000..ecf984c8 --- /dev/null +++ b/rosbot_xl_controller/test/test_utils.py @@ -0,0 +1,91 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import 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 + + +class ControllersTestNode(Node): + ROSBOT_HARDWARE_PUBLISHERS_RATE = 10.0 + + __test__ = False + + def __init__(self, name="test_node"): + super().__init__(name) + self.joint_state_msg_event = Event() + self.odom_msg_event = Event() + self.imu_msg_event = Event() + + def create_test_subscribers_and_publishers(self): + self.joint_state_sub = self.create_subscription( + JointState, "/joint_states", self.joint_states_callback, 10 + ) + + self.odom_sub = self.create_subscription( + Odometry, "/rosbot_xl_base_controller/odom", self.odometry_callback, 10 + ) + + self.imu_sub = self.create_subscription(Imu, "/imu_broadcaster/imu", self.imu_callback, 10) + + self.imu_publisher = self.create_publisher(Imu, "_imu/data_raw", 10) + + self.joint_states_publisher = 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() + + def odometry_callback(self, data): + self.odom_msg_event.set() + + def imu_callback(self, data): + self.imu_msg_event.set() + + def start_publishing_fake_hardware(self): + self.timer = self.create_timer( + 1.0 / self.ROSBOT_HARDWARE_PUBLISHERS_RATE, + self.publish_fake_hardware_messages, + ) + + def publish_fake_hardware_messages(self): + imu_msg = Imu() + imu_msg.header.stamp = self.get_clock().now().to_msg() + imu_msg.header.frame_id = "imu_link" + + joint_state_msg = JointState() + joint_state_msg.header.stamp = self.get_clock().now().to_msg() + joint_state_msg.name = [ + "fl_wheel_joint", + "fr_wheel_joint", + "rl_wheel_joint", + "rr_wheel_joint", + ] + 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) diff --git a/rosbot_xl_controller/test/test_xacro.py b/rosbot_xl_controller/test/test_xacro.py new file mode 100644 index 00000000..a417a335 --- /dev/null +++ b/rosbot_xl_controller/test/test_xacro.py @@ -0,0 +1,81 @@ +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import xacro +import itertools +from ament_index_python.packages import get_package_share_directory + + +def test_rosbot_description_parsing(): + mecanum_values = ["true", "false"] + use_sim_values = ["true", "false"] + use_gpu_values = ["true", "false"] + simulation_engine_values = ["ignition-gazebo", "webots"] # 'gazebo-classic' + lidar_model = [ + "None", + "slamtec_rplidar_s1", + "slamtec_rplidar_a2", + "slamtec_rplidar_a3", + "velodyne_puck", + ] + camera_model = [ + "None", + "intel_realsense_d435", + "stereolabs_zed", + "stereolabs_zedm", + "stereolabs_zed2", + "stereolabs_zed2i", + "stereolabs_zedx", + "stereolabs_zedxm", + ] + + all_combinations = list( + itertools.product( + mecanum_values, + use_sim_values, + use_gpu_values, + simulation_engine_values, + lidar_model, + camera_model, + ) + ) + + for combination in all_combinations: + ( + mecanum, + use_sim, + use_gpu, + simulation_engine, + lidar_model, + camera_model, + ) = combination + mappings = { + "mecanum": mecanum, + "use_sim": use_sim, + "use_gpu": use_gpu, + "simulation_engine": simulation_engine, + "lidar_model": lidar_model, + "camera_model": camera_model, + } + rosbot_xl_description = get_package_share_directory("rosbot_xl_description") + xacro_path = os.path.join(rosbot_xl_description, "urdf/rosbot_xl.urdf.xacro") + try: + xacro.process_file(xacro_path, mappings=mappings) + except xacro.XacroException as e: + assert False, ( + f"xacro parsing failed: {str(e)} for mecanum: {mecanum}, " + f"use_sim: {use_sim}, use_gpu: {use_gpu}, simulation_engine: {simulation_engine}, " + f"lidar_model: {lidar_model}, camera_model: {camera_model}" + ) diff --git a/rosbot_xl_description/CHANGELOG.rst b/rosbot_xl_description/CHANGELOG.rst index bb105135..e17f4627 100644 --- a/rosbot_xl_description/CHANGELOG.rst +++ b/rosbot_xl_description/CHANGELOG.rst @@ -2,6 +2,51 @@ Changelog for package rosbot_xl_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.8.11 (2023-11-08) +------------------- +* Merge remote-tracking branch 'origin/master' into ekf-update +* Contributors: Jakub Delicat + +0.8.10 (2023-11-08) +------------------- +* Merge pull request `#48 `_ from husarion/test-gazebo + Test gazebo +* Merge remote-tracking branch 'origin/master' into test-gazebo +* Contributors: Jakub Delicat, rafal-gorecki + +0.8.9 (2023-11-07) +------------------ +* Merge remote-tracking branch 'origin/master' into test-bringup +* Merge remote-tracking branch 'origin/master' into test-bringup +* Contributors: Jakub Delicat + +0.8.8 (2023-10-25) +------------------ + +0.8.7 (2023-10-24) +------------------ + +0.8.6 (2023-10-24) +------------------ +* Merge remote-tracking branch 'origin/master' into test-controller +* Contributors: Jakub Delicat + +0.8.5 (2023-10-24) +------------------ +* Merge remote-tracking branch 'origin/master' into add-pyspelling +* Contributors: Jakub Delicat + +0.8.4 (2023-10-24) +------------------ +* Merge remote-tracking branch 'origin/master' into add-black +* Contributors: Jakub Delicat + +0.8.3 (2023-10-24) +------------------ +* Merge pull request `#50 `_ from husarion/add-pre-commit + added and applied precommit +* Contributors: Jakub Delicat + 0.8.2 (2023-05-23) ------------------ diff --git a/rosbot_xl_description/CMakeLists.txt b/rosbot_xl_description/CMakeLists.txt index e68c6019..0c62a80e 100644 --- a/rosbot_xl_description/CMakeLists.txt +++ b/rosbot_xl_description/CMakeLists.txt @@ -10,4 +10,4 @@ install(DIRECTORY ) ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.sh.in") -ament_package() \ No newline at end of file +ament_package() diff --git a/rosbot_xl_description/meshes/body.dae b/rosbot_xl_description/meshes/body.dae index ed3897a4..71ed7d7f 100644 --- a/rosbot_xl_description/meshes/body.dae +++ b/rosbot_xl_description/meshes/body.dae @@ -269,4 +269,4 @@ - \ No newline at end of file + diff --git a/rosbot_xl_description/meshes/components/antenna.dae b/rosbot_xl_description/meshes/components/antenna.dae index 4fbb7974..dab9742c 100644 --- a/rosbot_xl_description/meshes/components/antenna.dae +++ b/rosbot_xl_description/meshes/components/antenna.dae @@ -218,4 +218,4 @@ - \ No newline at end of file + diff --git a/rosbot_xl_description/meshes/components/antenna_connector.dae b/rosbot_xl_description/meshes/components/antenna_connector.dae index 68dbc8fa..2531e6b2 100644 --- a/rosbot_xl_description/meshes/components/antenna_connector.dae +++ b/rosbot_xl_description/meshes/components/antenna_connector.dae @@ -218,4 +218,4 @@ - \ No newline at end of file + diff --git a/rosbot_xl_description/meshes/components/camera_mount_bot.dae b/rosbot_xl_description/meshes/components/camera_mount_bot.dae index 5e54cfdf..013b82b0 100644 --- a/rosbot_xl_description/meshes/components/camera_mount_bot.dae +++ b/rosbot_xl_description/meshes/components/camera_mount_bot.dae @@ -118,4 +118,4 @@ - \ No newline at end of file + diff --git a/rosbot_xl_description/meshes/components/camera_mount_mid.dae b/rosbot_xl_description/meshes/components/camera_mount_mid.dae index 8f407bdf..5ed0604b 100644 --- a/rosbot_xl_description/meshes/components/camera_mount_mid.dae +++ b/rosbot_xl_description/meshes/components/camera_mount_mid.dae @@ -89,4 +89,4 @@ - \ No newline at end of file + diff --git a/rosbot_xl_description/meshes/components/camera_mount_top.dae b/rosbot_xl_description/meshes/components/camera_mount_top.dae index 417d8cde..44ea191b 100644 --- a/rosbot_xl_description/meshes/components/camera_mount_top.dae +++ b/rosbot_xl_description/meshes/components/camera_mount_top.dae @@ -118,4 +118,4 @@ - \ No newline at end of file + diff --git a/rosbot_xl_description/meshes/mecanum_a.dae b/rosbot_xl_description/meshes/mecanum_a.dae index a56f7826..84779c4e 100644 --- a/rosbot_xl_description/meshes/mecanum_a.dae +++ b/rosbot_xl_description/meshes/mecanum_a.dae @@ -115,4 +115,4 @@ - \ No newline at end of file + diff --git a/rosbot_xl_description/meshes/mecanum_b.dae b/rosbot_xl_description/meshes/mecanum_b.dae index cacc5c39..46f873bd 100644 --- a/rosbot_xl_description/meshes/mecanum_b.dae +++ b/rosbot_xl_description/meshes/mecanum_b.dae @@ -115,4 +115,4 @@ - \ No newline at end of file + diff --git a/rosbot_xl_description/meshes/wheel_a.dae b/rosbot_xl_description/meshes/wheel_a.dae index a2755e41..cb42dab7 100644 --- a/rosbot_xl_description/meshes/wheel_a.dae +++ b/rosbot_xl_description/meshes/wheel_a.dae @@ -115,4 +115,4 @@ - \ No newline at end of file + diff --git a/rosbot_xl_description/meshes/wheel_b.dae b/rosbot_xl_description/meshes/wheel_b.dae index 2375bb3d..62062e36 100644 --- a/rosbot_xl_description/meshes/wheel_b.dae +++ b/rosbot_xl_description/meshes/wheel_b.dae @@ -115,4 +115,4 @@ - \ No newline at end of file + diff --git a/rosbot_xl_description/package.xml b/rosbot_xl_description/package.xml index bcfe42f2..73702182 100644 --- a/rosbot_xl_description/package.xml +++ b/rosbot_xl_description/package.xml @@ -2,7 +2,7 @@ rosbot_xl_description - 0.8.2 + 0.8.11 The rosbot_xl_description package Apache License 2.0 @@ -20,7 +20,7 @@ xacro xacro - + ros_components_description diff --git a/rosbot_xl_description/urdf/body.urdf.xacro b/rosbot_xl_description/urdf/body.urdf.xacro index 01d4d8b0..3c015b06 100644 --- a/rosbot_xl_description/urdf/body.urdf.xacro +++ b/rosbot_xl_description/urdf/body.urdf.xacro @@ -51,4 +51,4 @@ - \ No newline at end of file + diff --git a/rosbot_xl_description/urdf/components/antenna.urdf.xacro b/rosbot_xl_description/urdf/components/antenna.urdf.xacro index e2149ecf..42cdb9be 100644 --- a/rosbot_xl_description/urdf/components/antenna.urdf.xacro +++ b/rosbot_xl_description/urdf/components/antenna.urdf.xacro @@ -54,4 +54,4 @@ - \ No newline at end of file + diff --git a/rosbot_xl_description/urdf/components/camera_mount.urdf.xacro b/rosbot_xl_description/urdf/components/camera_mount.urdf.xacro index c6fc42e6..068b602e 100644 --- a/rosbot_xl_description/urdf/components/camera_mount.urdf.xacro +++ b/rosbot_xl_description/urdf/components/camera_mount.urdf.xacro @@ -92,4 +92,4 @@ - \ No newline at end of file + diff --git a/rosbot_xl_description/urdf/rosbot_xl.urdf.xacro b/rosbot_xl_description/urdf/rosbot_xl.urdf.xacro index 8cb0789a..44a0dc5e 100644 --- a/rosbot_xl_description/urdf/rosbot_xl.urdf.xacro +++ b/rosbot_xl_description/urdf/rosbot_xl.urdf.xacro @@ -3,7 +3,7 @@ - + @@ -28,7 +28,7 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + - + - \ No newline at end of file + diff --git a/rosbot_xl_description/urdf/rosbot_xl_macro.urdf.xacro b/rosbot_xl_description/urdf/rosbot_xl_macro.urdf.xacro index 75e3a011..c152b269 100644 --- a/rosbot_xl_description/urdf/rosbot_xl_macro.urdf.xacro +++ b/rosbot_xl_description/urdf/rosbot_xl_macro.urdf.xacro @@ -156,4 +156,4 @@ - \ No newline at end of file + diff --git a/rosbot_xl_description/urdf/wheel.urdf.xacro b/rosbot_xl_description/urdf/wheel.urdf.xacro index 10416e8d..437ade74 100644 --- a/rosbot_xl_description/urdf/wheel.urdf.xacro +++ b/rosbot_xl_description/urdf/wheel.urdf.xacro @@ -62,7 +62,7 @@ - + @@ -106,4 +106,4 @@ - \ No newline at end of file + diff --git a/rosbot_xl_gazebo/CHANGELOG.rst b/rosbot_xl_gazebo/CHANGELOG.rst index f5491295..34280280 100644 --- a/rosbot_xl_gazebo/CHANGELOG.rst +++ b/rosbot_xl_gazebo/CHANGELOG.rst @@ -2,6 +2,56 @@ Changelog for package rosbot_xl_gazebo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.8.11 (2023-11-08) +------------------- +* Merge remote-tracking branch 'origin/master' into ekf-update +* Contributors: Jakub Delicat + +0.8.10 (2023-11-08) +------------------- +* Merge pull request `#48 `_ from husarion/test-gazebo + Test gazebo +* Merge pull request `#54 `_ from husarion/add-controller-odom-test + Add raw odom test +* Merge remote-tracking branch 'origin/master' into test-gazebo +* Contributors: Jakub Delicat, rafal-gorecki + +0.8.9 (2023-11-07) +------------------ +* Merge remote-tracking branch 'origin/master' into test-bringup +* Merge remote-tracking branch 'origin/master' into test-bringup +* Contributors: Jakub Delicat + +0.8.8 (2023-10-25) +------------------ + +0.8.7 (2023-10-24) +------------------ +* Merge pull request `#51 `_ from husarion/workflow-test + Workflow test +* Contributors: rafal-gorecki + +0.8.6 (2023-10-24) +------------------ +* Merge remote-tracking branch 'origin/master' into test-controller +* Contributors: Jakub Delicat + +0.8.5 (2023-10-24) +------------------ +* Merge remote-tracking branch 'origin/master' into add-pyspelling +* Contributors: Jakub Delicat + +0.8.4 (2023-10-24) +------------------ +* Merge remote-tracking branch 'origin/master' into add-black +* Contributors: Jakub Delicat + +0.8.3 (2023-10-24) +------------------ +* Merge pull request `#50 `_ from husarion/add-pre-commit + added and applied precommit +* Contributors: Jakub Delicat + 0.8.2 (2023-05-23) ------------------ diff --git a/rosbot_xl_gazebo/CMakeLists.txt b/rosbot_xl_gazebo/CMakeLists.txt deleted file mode 100644 index 04926f32..00000000 --- a/rosbot_xl_gazebo/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -cmake_minimum_required(VERSION 3.10.2) -project(rosbot_xl_gazebo) - -find_package(ament_cmake REQUIRED) - -install(DIRECTORY - launch - DESTINATION share/${PROJECT_NAME} -) - -ament_package() \ No newline at end of file diff --git a/rosbot_xl_gazebo/launch/simulation.launch.py b/rosbot_xl_gazebo/launch/simulation.launch.py index 597be558..a84812f4 100644 --- a/rosbot_xl_gazebo/launch/simulation.launch.py +++ b/rosbot_xl_gazebo/launch/simulation.launch.py @@ -1,12 +1,26 @@ -#!/usr/bin/env python3 +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. -from launch import LaunchDescription +from launch import LaunchDescription, LaunchContext from launch.actions import ( IncludeLaunchDescription, DeclareLaunchArgument, + OpaqueFunction, ) from launch.substitutions import ( PathJoinSubstitution, + PythonExpression, LaunchConfiguration, ) from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -16,26 +30,146 @@ from ament_index_python.packages import get_package_share_directory +def launch_gz_bridge(context: LaunchContext, *args, **kwargs): + camera_model = context.perform_substitution(LaunchConfiguration("camera_model")) + lidar_model = context.perform_substitution(LaunchConfiguration("lidar_model")) + + gz_args = ["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"] + gz_remapping = [] + depth_camera_parent_tf = None + + # Add camera topic + if camera_model.startswith("intel_realsense"): + gz_args.append( + "/camera/color/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo" + ) + gz_args.append("/camera/color/image_raw@sensor_msgs/msg/Image[ignition.msgs.Image") + gz_args.append("/camera/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo") + gz_args.append("/camera/depth@sensor_msgs/msg/Image[ignition.msgs.Image") + gz_args.append( + "/camera/depth/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked" + ) + + gz_remapping.append(("/camera/camera_info", "/camera/depth/camera_info")) + gz_remapping.append(("/camera/depth", "/camera/depth/image_raw")) + + depth_camera_parent_tf = "camera_depth_frame" + + elif camera_model.startswith("stereolabs_zed"): + zed = camera_model[len("stereolabs_") :] + + gz_args.append( + f"/{zed}/zed_node/rgb/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo" + ) + gz_args.append( + f"/{zed}/zed_node/rgb/image_rect_color@sensor_msgs/msg/Image[ignition.msgs.Image" + ) + gz_args.append( + f"/{zed}/zed_node/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo" + ) + gz_args.append(f"/{zed}/zed_node/depth@sensor_msgs/msg/Image[ignition.msgs.Image") + gz_args.append( + f"/{zed}/zed_node/depth/points@sensor_msgs/msg/PointCloud2" + "[ignition.msgs.PointCloudPacked" + ) + + gz_remapping.append((f"{zed}/zed_node/camera_info", f"/{zed}/zed_node/depth/camera_info")) + gz_remapping.append((f"{zed}/zed_node/depth", f"/{zed}/zed_node/depth/depth_registered")) + gz_remapping.append( + (f"{zed}/zed_node/depth/points", f"/{zed}/zed_node/point_cloud/cloud_registered") + ) + + depth_camera_parent_tf = "camera_center_optical_frame" + else: + pass + + # Add lidar topic + if lidar_model.startswith("slamtec_rplidar"): + gz_args.append("/scan@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan") + + elif lidar_model.startswith("velodyne"): + gz_args.append( + "/velodyne_points/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked" + ) + + gz_remapping.append(("/velodyne_points/points", "/velodyne_points")) + else: # FIXME: Checkout ouster + pass + + gz_bridge_node = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="ign_bridge", + arguments=gz_args, + remappings=gz_remapping, + output="screen", + ) + + # The frame of the point cloud from ignition gazebo 6 isn't provided by . + # See https://github.com/gazebosim/gz-sensors/issues/239 + point_cloud_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="point_cloud_tf", + output="log", + arguments=[ + "0.0", + "0.0", + "0.0", + "0.0", + "0.0", + "0.0", + depth_camera_parent_tf, + "rosbot_xl/base_link/camera_depth", + ], + ) + + if depth_camera_parent_tf: + return [gz_bridge_node, point_cloud_tf] + else: + return [gz_bridge_node] + + def generate_launch_description(): mecanum = LaunchConfiguration("mecanum") declare_mecanum_arg = DeclareLaunchArgument( "mecanum", default_value="False", - description="Whether to use mecanum drive controller (otherwise diff drive controller is used)", + description=( + "Whether to use mecanum drive controller (otherwise diff drive controller is used)" + ), + ) + + camera_model = LaunchConfiguration("camera_model") + declare_camera_model_arg = DeclareLaunchArgument( + "camera_model", + default_value="intel_realsense_d435", + description="Add camera model to the robot URDF", + choices=[ + "None", + "intel_realsense_d435", + "stereolabs_zed", + "stereolabs_zedm", + "stereolabs_zed2", + "stereolabs_zed2i", + "stereolabs_zedx", + "stereolabs_zedxm", + ], ) lidar_model = LaunchConfiguration("lidar_model") declare_lidar_model_arg = DeclareLaunchArgument( "lidar_model", default_value="slamtec_rplidar_s1", - description="Lidar model added to the URDF", - ) - - camera_model = LaunchConfiguration("camera_model") - declare_camera_model_arg = DeclareLaunchArgument( - "camera_model", - default_value="None", - description="Camera model added to the URDF", + description="Add LiDAR model to the robot URDF", + choices=[ + "None", + "ouster_os1_32", + "slamtec_rplidar_a2", + "slamtec_rplidar_a3", + "slamtec_rplidar_s1", + "velodyne_puck", + ], ) include_camera_mount = LaunchConfiguration("include_camera_mount") @@ -45,24 +179,39 @@ def generate_launch_description(): description="Whether to include camera mount to the robot URDF", ) - map_package = get_package_share_directory("husarion_office_gz") - world_file = PathJoinSubstitution([map_package, "worlds", "husarion_world.sdf"]) + world_package = get_package_share_directory("husarion_office_gz") + world_file = PathJoinSubstitution([world_package, "worlds", "husarion_world.sdf"]) world_cfg = LaunchConfiguration("world") declare_world_arg = DeclareLaunchArgument( - "world", default_value=["-r ", world_file], description="SDF world file" + "world", default_value=world_file, description="SDF world file" + ) + + headless = LaunchConfiguration("headless") + declare_headless_arg = DeclareLaunchArgument( + "headless", + default_value="False", + description="Run Gazebo Ignition in the headless mode", ) + headless_cfg = PythonExpression([ + "'--headless-rendering -s -r' if ", + headless, + " else '-r'", + ]) + gz_args = [headless_cfg, " ", world_cfg] + gz_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - get_package_share_directory("ros_gz_sim"), - "launch", - "gz_sim.launch.py", - ] - ) + PathJoinSubstitution([ + get_package_share_directory("ros_gz_sim"), + "launch", + "gz_sim.launch.py", + ]) ), - launch_arguments={"gz_args": world_cfg}.items(), + launch_arguments={ + "gz_args": gz_args, + "on_exit_shutdown": "True", + }.items(), ) gz_spawn_entity = Node( @@ -84,47 +233,14 @@ def generate_launch_description(): ], output="screen", ) - ign_bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="ign_bridge", - arguments=[ - "/clock" + "@rosgraph_msgs/msg/Clock" + "[ignition.msgs.Clock", - "/scan" + "@sensor_msgs/msg/LaserScan" + "[ignition.msgs.LaserScan", - "/velodyne_points/points" - + "@sensor_msgs/msg/PointCloud2" - + "[ignition.msgs.PointCloudPacked", - "/camera/color/camera_info" - + "@sensor_msgs/msg/CameraInfo" - + "[ignition.msgs.CameraInfo", - "/camera/color/image_raw" - + "@sensor_msgs/msg/Image" - + "[ignition.msgs.Image", - "/camera/camera_info" - + "@sensor_msgs/msg/CameraInfo" - + "[ignition.msgs.CameraInfo", - "/camera/depth" + "@sensor_msgs/msg/Image" + "[ignition.msgs.Image", - "/camera/depth/points" - + "@sensor_msgs/msg/PointCloud2" - + "[ignition.msgs.PointCloudPacked", - ], - remappings=[ - ("/velodyne_points/points", "/velodyne_points"), - ("/camera/camera_info", "/camera/depth/camera_info"), - ("/camera/depth", "/camera/depth/image_raw"), - ], - output="screen", - ) bringup_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - get_package_share_directory("rosbot_xl_bringup"), - "launch", - "bringup.launch.py", - ] - ) + PathJoinSubstitution([ + get_package_share_directory("rosbot_xl_bringup"), + "launch", + "bringup.launch.py", + ]) ), launch_arguments={ "mecanum": mecanum, @@ -136,18 +252,18 @@ def generate_launch_description(): }.items(), ) - return LaunchDescription( - [ - declare_mecanum_arg, - declare_lidar_model_arg, - declare_camera_model_arg, - declare_include_camera_mount_arg, - declare_world_arg, - # Sets use_sim_time for all nodes started below (doesn't work for nodes started from ignition gazebo) - SetParameter(name="use_sim_time", value=True), - gz_sim, - ign_bridge, - gz_spawn_entity, - bringup_launch, - ] - ) + return LaunchDescription([ + declare_mecanum_arg, + declare_lidar_model_arg, + declare_camera_model_arg, + declare_include_camera_mount_arg, + declare_world_arg, + declare_headless_arg, + # Sets use_sim_time for all nodes started below + # (doesn't work for nodes started from ignition gazebo) + SetParameter(name="use_sim_time", value=True), + gz_sim, + OpaqueFunction(function=launch_gz_bridge), + gz_spawn_entity, + bringup_launch, + ]) diff --git a/rosbot_xl_gazebo/package.xml b/rosbot_xl_gazebo/package.xml index c7269965..f807c6ab 100644 --- a/rosbot_xl_gazebo/package.xml +++ b/rosbot_xl_gazebo/package.xml @@ -2,21 +2,20 @@ rosbot_xl_gazebo - 0.8.2 + 0.8.11 - The rosbot_xl_gazebo package + Gazebo Ignition simulation for ROSbot XL Apache License 2.0 Maciej Stępień Krzysztof Wojciechowski + Jakub Delicat Husarion https://husarion.com/ https://github.com/husarion/rosbot_xl_ros https://github.com/husarion/rosbot_xl_ros/issues - ament_cmake - rosbot_xl_bringup launch @@ -30,7 +29,19 @@ ros_gz_bridge ign_ros2_control + python3-pytest + launch + launch_ros + launch_pytest + + tf_transformations + python-transforms3d-pip + nav_msgs + geometry_msgs + + rosbot_xl_bringup + - ament_cmake + ament_python - \ No newline at end of file + diff --git a/rosbot_xl_gazebo/resource/rosbot_xl_gazebo b/rosbot_xl_gazebo/resource/rosbot_xl_gazebo new file mode 100644 index 00000000..e69de29b diff --git a/rosbot_xl_gazebo/setup.cfg b/rosbot_xl_gazebo/setup.cfg new file mode 100644 index 00000000..ac5e4e70 --- /dev/null +++ b/rosbot_xl_gazebo/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rosbot_xl_gazebo +[install] +install_scripts=$base/lib/rosbot_xl_gazebo diff --git a/rosbot_xl_gazebo/setup.py b/rosbot_xl_gazebo/setup.py new file mode 100644 index 00000000..b6d806e6 --- /dev/null +++ b/rosbot_xl_gazebo/setup.py @@ -0,0 +1,40 @@ +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = "rosbot_xl_gazebo" + +setup( + name=package_name, + version="0.8.2", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + (os.path.join("share", package_name, "launch"), glob("launch/*.launch.py")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Husarion", + maintainer_email="contact@husarion.com", + description="Gazebo Ignition simulation for ROSbot XL", + license="Apache License 2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [], + }, +) diff --git a/rosbot_xl_gazebo/test/test_copyright.py b/rosbot_xl_gazebo/test/test_copyright.py new file mode 100644 index 00000000..f46f861d --- /dev/null +++ b/rosbot_xl_gazebo/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/rosbot_xl_gazebo/test/test_diff_drive_simulation.py b/rosbot_xl_gazebo/test/test_diff_drive_simulation.py new file mode 100644 index 00000000..4124afee --- /dev/null +++ b/rosbot_xl_gazebo/test/test_diff_drive_simulation.py @@ -0,0 +1,86 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from test_utils import SimulationTestNode +from test_ign_kill_utils import kill_ign_linux_processes + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_xl_gazebo = get_package_share_directory("rosbot_xl_gazebo") + simulation_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + rosbot_xl_gazebo, + "launch", + "simulation.launch.py", + ]) + ), + launch_arguments={ + "headless": "True", + }.items(), + ) + + return LaunchDescription([simulation_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_diff_drive_simulation(): + rclpy.init() + try: + node = SimulationTestNode("test_bringup") + node.create_test_subscribers_and_publishers() + node.start_node_thread() + + flag = node.odom_tf_event.wait(timeout=60.0) + assert ( + flag + ), "Expected odom to base_link tf but it was not received. Check robot_localization!" + + # 0.9 m/s and 3.0 rad/s are controller's limits defined in + # rosbot_controller/config/diff_drive_controller.yaml + node.set_destination_speed(0.9, 0.0, 0.0) + controller_flag = node.controller_odom_event.wait(timeout=20.0) + ekf_flag = node.ekf_odom_event.wait(timeout=20.0) + assert ( + controller_flag + ), "ROSbot does not move properly in x direction. Check rosbot_xl_base_controller!" + assert ekf_flag, "ROSbot does not move properly in x direction. Check ekf_filter_node!" + + node.set_destination_speed(0.0, 0.0, 3.0) + controller_flag = node.controller_odom_event.wait(timeout=20.0) + ekf_flag = node.ekf_odom_event.wait(timeout=20.0) + assert controller_flag, "ROSbot does not rotate properly. Check rosbot_xl_base_controller!" + assert ekf_flag, "ROSbot does not rotate properly. Check ekf_filter_node!" + + flag = node.scan_event.wait(timeout=20.0) + assert flag, "ROSbot's lidar does not work properly!" + + finally: + # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching + # several tests in a row. + kill_ign_linux_processes() + rclpy.shutdown() diff --git a/rosbot_xl_gazebo/test/test_flake8.py b/rosbot_xl_gazebo/test/test_flake8.py new file mode 100644 index 00000000..40602d8a --- /dev/null +++ b/rosbot_xl_gazebo/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +from os.path import join, dirname +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + config_file = join(dirname(dirname(dirname(__file__))), ".flake8") + rc, errors = main_with_errors(argv=["--config", config_file]) + assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors) diff --git a/rosbot_xl_gazebo/test/test_ign_kill_utils.py b/rosbot_xl_gazebo/test/test_ign_kill_utils.py new file mode 100644 index 00000000..7f62f936 --- /dev/null +++ b/rosbot_xl_gazebo/test/test_ign_kill_utils.py @@ -0,0 +1,38 @@ +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import subprocess + +# The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching +# several tests in a row. + + +def kill_ign_linux_processes(): + try: + result = subprocess.run( + ["pgrep", "-f", "ign gazebo"], + capture_output=True, + text=True, + check=True, + ) + + pids = result.stdout.strip().split("\n") + for pid in pids: + subprocess.run(["kill", pid], check=True) + print("Killed all Ignition Gazebo processes") + except subprocess.CalledProcessError as e: + print(e) + + +kill_ign_linux_processes() diff --git a/rosbot_xl_gazebo/test/test_mecanum_simulation.py b/rosbot_xl_gazebo/test/test_mecanum_simulation.py new file mode 100644 index 00000000..6113eea7 --- /dev/null +++ b/rosbot_xl_gazebo/test/test_mecanum_simulation.py @@ -0,0 +1,95 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from test_utils import SimulationTestNode +from test_ign_kill_utils import kill_ign_linux_processes + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_xl_gazebo = get_package_share_directory("rosbot_xl_gazebo") + simulation_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + rosbot_xl_gazebo, + "launch", + "simulation.launch.py", + ]) + ), + launch_arguments={ + "mecanum": "True", + "headless": "True", + }.items(), + ) + + return LaunchDescription([simulation_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_mecanum_simulation(): + rclpy.init() + try: + node = SimulationTestNode("test_bringup") + node.create_test_subscribers_and_publishers() + node.start_node_thread() + + flag = node.odom_tf_event.wait(timeout=60.0) + assert ( + flag + ), "Expected odom to base_link tf but it was not received. Check robot_localization!" + + # 0.9 m/s and 3.0 rad/s are controller's limits defined in + # rosbot_controller/config/mecanum_drive_controller.yaml + node.set_destination_speed(0.9, 0.0, 0.0) + controller_flag = node.controller_odom_event.wait(timeout=20.0) + ekf_flag = node.ekf_odom_event.wait(timeout=20.0) + assert ( + controller_flag + ), "ROSbot does not move properly in x direction. Check rosbot_xl_base_controller!" + assert ekf_flag, "ROSbot does not move properly in x direction. Check ekf_filter_node!" + + node.set_destination_speed(0.0, 0.9, 0.0) + controller_flag = node.controller_odom_event.wait(timeout=20.0) + ekf_flag = node.ekf_odom_event.wait(timeout=20.0) + assert ( + controller_flag + ), "ROSbot does not move properly in y direction. Check rosbot_xl_base_controller!" + assert ekf_flag, "ROSbot does not move properly in y direction. Check ekf_filter_node!" + + node.set_destination_speed(0.0, 0.0, 3.0) + controller_flag = node.controller_odom_event.wait(timeout=20.0) + ekf_flag = node.ekf_odom_event.wait(timeout=20.0) + assert controller_flag, "ROSbot does not rotate properly. Check rosbot_xl_base_controller!" + assert ekf_flag, "ROSbot does not rotate properly. Check ekf_filter_node!" + + flag = node.scan_event.wait(timeout=20.0) + assert flag, "ROSbot's lidar does not work properly!" + + finally: + # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching + # several tests in a row. + kill_ign_linux_processes() + rclpy.shutdown() diff --git a/rosbot_xl_gazebo/test/test_pep257.py b/rosbot_xl_gazebo/test/test_pep257.py new file mode 100644 index 00000000..a2c3deb8 --- /dev/null +++ b/rosbot_xl_gazebo/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/rosbot_xl_gazebo/test/test_slamtec_rplidar_a2.py b/rosbot_xl_gazebo/test/test_slamtec_rplidar_a2.py new file mode 100644 index 00000000..f1607a85 --- /dev/null +++ b/rosbot_xl_gazebo/test/test_slamtec_rplidar_a2.py @@ -0,0 +1,66 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from test_utils import SimulationTestNode +from test_ign_kill_utils import kill_ign_linux_processes + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_xl_gazebo = get_package_share_directory("rosbot_xl_gazebo") + simulation_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + rosbot_xl_gazebo, + "launch", + "simulation.launch.py", + ]) + ), + launch_arguments={ + "headless": "True", + "lidar_model": "slamtec_rplidar_a2", + }.items(), + ) + + return LaunchDescription([simulation_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_slamtec_rplidar_a2(): + rclpy.init() + try: + node = SimulationTestNode("test_bringup") + node.create_test_subscribers_and_publishers() + node.start_node_thread() + + msgs_received_flag = node.scan_event.wait(timeout=60.0) + assert msgs_received_flag, "ROSbot's lidar does not work properly!" + + finally: + # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching + # several tests in a row. + kill_ign_linux_processes() + rclpy.shutdown() diff --git a/rosbot_xl_gazebo/test/test_slamtec_rplidar_a3.py b/rosbot_xl_gazebo/test/test_slamtec_rplidar_a3.py new file mode 100644 index 00000000..ce68f668 --- /dev/null +++ b/rosbot_xl_gazebo/test/test_slamtec_rplidar_a3.py @@ -0,0 +1,66 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from test_utils import SimulationTestNode +from test_ign_kill_utils import kill_ign_linux_processes + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_xl_gazebo = get_package_share_directory("rosbot_xl_gazebo") + simulation_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + rosbot_xl_gazebo, + "launch", + "simulation.launch.py", + ]) + ), + launch_arguments={ + "headless": "True", + "lidar_model": "slamtec_rplidar_a3", + }.items(), + ) + + return LaunchDescription([simulation_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_slamtec_rplidar_a3(): + rclpy.init() + try: + node = SimulationTestNode("test_bringup") + node.create_test_subscribers_and_publishers() + node.start_node_thread() + + msgs_received_flag = node.scan_event.wait(timeout=60.0) + assert msgs_received_flag, "ROSbot's lidar does not work properly!" + + finally: + # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching + # several tests in a row. + kill_ign_linux_processes() + rclpy.shutdown() diff --git a/rosbot_xl_gazebo/test/test_slamtec_rplidar_s1.py b/rosbot_xl_gazebo/test/test_slamtec_rplidar_s1.py new file mode 100644 index 00000000..a209cc27 --- /dev/null +++ b/rosbot_xl_gazebo/test/test_slamtec_rplidar_s1.py @@ -0,0 +1,66 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from test_utils import SimulationTestNode +from test_ign_kill_utils import kill_ign_linux_processes + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_xl_gazebo = get_package_share_directory("rosbot_xl_gazebo") + simulation_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + rosbot_xl_gazebo, + "launch", + "simulation.launch.py", + ]) + ), + launch_arguments={ + "headless": "True", + "lidar_model": "slamtec_rplidar_s1", + }.items(), + ) + + return LaunchDescription([simulation_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_slamtec_rplidar_s1(): + rclpy.init() + try: + node = SimulationTestNode("test_bringup") + node.create_test_subscribers_and_publishers() + node.start_node_thread() + + msgs_received_flag = node.scan_event.wait(timeout=60.0) + assert msgs_received_flag, "ROSbot's lidar does not work properly!" + + finally: + # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching + # several tests in a row. + kill_ign_linux_processes() + rclpy.shutdown() diff --git a/rosbot_xl_gazebo/test/test_utils.py b/rosbot_xl_gazebo/test/test_utils.py new file mode 100644 index 00000000..b88fcf47 --- /dev/null +++ b/rosbot_xl_gazebo/test/test_utils.py @@ -0,0 +1,129 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +import math + +from threading import Event +from threading import Thread + +from rclpy.node import Node + +from geometry_msgs.msg import Twist +from nav_msgs.msg import Odometry +from sensor_msgs.msg import LaserScan + +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + + +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 + + def __init__(self, name="test_node"): + super().__init__(name) + self.v_x = 0.0 + self.v_y = 0.0 + self.v_yaw = 0.0 + + self.controller_odom_event = Event() + self.ekf_odom_event = Event() + self.odom_tf_event = Event() + self.scan_event = Event() + + def clear_odom_events(self): + self.controller_odom_event.clear() + self.ekf_odom_event.clear() + + def set_destination_speed(self, v_x, v_y, v_yaw): + self.clear_odom_events() + self.v_x = v_x + self.v_y = v_y + self.v_yaw = v_yaw + + def create_test_subscribers_and_publishers(self): + self.cmd_vel_publisher = self.create_publisher(Twist, "cmd_vel", 10) + + self.controller_odom_sub = self.create_subscription( + Odometry, "/rosbot_xl_base_controller/odom", self.controller_callback, 10 + ) + + self.ekf_odom_sub = self.create_subscription( + Odometry, "/odometry/filtered", self.ekf_callback, 10 + ) + + self.scan_sub = self.create_subscription( + LaserScan, "/scan_filtered", self.scan_callback, 10 + ) + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + self.timer = None + + def start_node_thread(self): + self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,)) + self.ros_spin_thread.start() + self.timer = self.create_timer(1.0 / 10.0, self.timer_callback) + + def is_twist_ok(self, twist: Twist): + def are_close_to_each_other(true_value, dest_value, tolerance=self.ACCURACY, eps=0.01): + acceptable_range = dest_value * tolerance + return abs(true_value - dest_value) <= acceptable_range + eps + + x_ok = are_close_to_each_other(twist.linear.x, self.v_x) + y_ok = are_close_to_each_other(twist.linear.y, self.v_y) + yaw_ok = are_close_to_each_other(twist.angular.z, self.v_yaw) + + return x_ok and y_ok and yaw_ok + + def controller_callback(self, data: Odometry): + if self.is_twist_ok(data.twist.twist): + self.controller_odom_event.set() + + def ekf_callback(self, data: Odometry): + if self.is_twist_ok(data.twist.twist): + self.ekf_odom_event.set() + + def lookup_transform_odom(self): + try: + self.tf_buffer.lookup_transform("odom", "base_link", rclpy.time.Time()) + self.odom_tf_event.set() + except TransformException as ex: + self.get_logger().error(f"Could not transform odom to base_link: {ex}") + + def timer_callback(self): + self.publish_cmd_vel_messages() + self.lookup_transform_odom() + + def scan_callback(self, data: LaserScan): + for range in data.ranges: + # minimal distance and nan configured in rosbot_xl_bringup/config_laser_filter.yaml + if range < 0.145 and not math.isnan(range): + return + + self.scan_event.set() + + def publish_cmd_vel_messages(self): + twist_msg = Twist() + + twist_msg.linear.x = self.v_x + twist_msg.linear.y = self.v_y + twist_msg.angular.z = self.v_yaw + + self.cmd_vel_publisher.publish(twist_msg) diff --git a/wordlist.dic b/wordlist.dic new file mode 100644 index 00000000..a4339c65 Binary files /dev/null and b/wordlist.dic differ