diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile new file mode 100644 index 00000000..88d19d69 --- /dev/null +++ b/.devcontainer/Dockerfile @@ -0,0 +1,37 @@ +FROM husarnet/ros:jazzy-ros-base + +ARG USERNAME=husarion +ARG UID=1001 +ARG GID=$UID + +# Install development tools +RUN apt update -q && \ + apt upgrade -q -y && \ + apt install -y --no-install-recommends \ + python3-pip \ + ros-dev-tools \ + ros-$ROS_DISTRO-plotjuggler \ + ros-$ROS_DISTRO-plotjuggler-ros \ + ros-$ROS_DISTRO-rqt* \ + ros-$ROS_DISTRO-rviz2 \ + ros-$ROS_DISTRO-teleop-twist-keyboard + +# Create and switch to user +RUN groupadd -g $GID $USERNAME && \ + useradd -lm -u $UID -g $USERNAME -s /bin/bash $USERNAME && \ + echo "$USERNAME ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers +USER $USERNAME + +# Setup workspace +WORKDIR /home/$USERNAME/ros2_ws +RUN mkdir -p /home/$USERNAME/ros2_ws/src && \ + echo 'source /opt/ros/'$ROS_DISTRO'/setup.bash' >> /home/$USERNAME/.bashrc && \ + echo 'source /home/'$USERNAME'/ros2_ws/install/setup.bash' >> /home/$USERNAME/.bashrc + +ENV TERM=xterm-256color +ENV RCUTILS_COLORIZED_OUTPUT=1 + +# Setup entrypoint +COPY ./ros_entrypoint.sh / +ENTRYPOINT ["/ros_entrypoint.sh"] +CMD ["bash"] diff --git a/.devcontainer/compose.yaml b/.devcontainer/compose.yaml new file mode 100644 index 00000000..2e45f75c --- /dev/null +++ b/.devcontainer/compose.yaml @@ -0,0 +1,25 @@ +services: + devcontainer: + build: . + ipc: host + network_mode: host + privileged: true + tty: true + runtime: nvidia + volumes: + - ros2_ws_volume:/home/husarion/ros2_ws + - ../:/home/husarion/ros2_ws/src/rosbot_ros:cached + - /tmp/.X11-unix:/tmp/.X11-unix + - /home/${USER}/.ssh:/home/husarion/.ssh + - /lib/modules:/lib/modules:ro + environment: + - DISPLAY + - RMW_IMPLEMENTATION + - QT_X11_NO_MITSHM=1 + - NVIDIA_VISIBLE_DEVICES=all + - NVIDIA_DRIVER_CAPABILITIES=all + - PIP_BREAK_SYSTEM_PACKAGES=1 + command: sleep infinity + +volumes: + ros2_ws_volume: diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 00000000..0c374c6c --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,35 @@ +{ + "dockerComposeFile": "compose.yaml", + "initializeCommand": "xhost +local:docker", + "service": "devcontainer", + "workspaceFolder": "/home/husarion/ros2_ws/src", + // "postCreateCommand": "~/ros2_ws/src/ros/.vscode/scripts/install_dependencies.sh && ~/ros2_ws/src/ros/.vscode/scripts/install_dependencies.sh", + "shutdownAction": "stopCompose", + "customizations": { + "vscode": { + "extensions": [ + "althack.ament-task-provider", + "charliermarsh.ruff", + "eamodio.gitlens", + "GitHub.copilot", + "ms-python.python", + "ms-iot.vscode-ros", + "ms-vscode.cpptools-extension-pack", + "smilerobotics.urdf", + "redhat.vscode-xml", + "tamasfe.even-better-toml", + "timonwong.shellcheck", + "streetsidesoftware.code-spell-checker", + "yzhang.markdown-all-in-one" + ], + "settings": { + "files.associations": { + "*.rviz": "yaml", + "*.srdf": "xml", + "*.urdf": "xml", + "*.xacro": "xml" + } + } + } + } +} diff --git a/.devcontainer/ros_entrypoint.sh b/.devcontainer/ros_entrypoint.sh new file mode 100755 index 00000000..24b358aa --- /dev/null +++ b/.devcontainer/ros_entrypoint.sh @@ -0,0 +1,5 @@ +#!/bin/bash +# shellcheck disable=SC1090,SC1091 +set -e + +exec "$@" diff --git a/.github/workflows/tests.yaml b/.github/workflows/tests.yaml index 9e565b73..6f3b4829 100644 --- a/.github/workflows/tests.yaml +++ b/.github/workflows/tests.yaml @@ -6,25 +6,11 @@ on: workflow_dispatch: jobs: - black: - name: Black - runs-on: ubuntu-24.04 - steps: - - name: Checkout - uses: actions/checkout@v3 - - name: Black - uses: psf/black@23.11.0 - with: - options: --line-length=99 - - spellcheck: - name: Spellcheck - runs-on: ubuntu-24.04 - steps: - - name: Checkout - uses: actions/checkout@v3 - - name: Spellcheck - uses: rojopolis/spellcheck-github-actions@0.33.1 + pre-commit: + name: Pre-commit + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: jazzy industrial_ci: name: Industrial CI @@ -36,19 +22,18 @@ jobs: ROS_DISTRO: [jazzy] steps: - name: Checkout - uses: actions/checkout@v3 - - - name: Act + Docker fix - run: | - sudo chown runner:docker /var/run/docker.sock + uses: actions/checkout@v4 - name: Setup ROS2 Workspace and Clone Repositories run: | - mkdir -p src - find . -maxdepth 1 -not -name src -not -name . -exec mv {} src/ \; + mkdir -p src/rosbot_ros + find . -maxdepth 1 -not -name src -not -name . -exec mv {} src/rosbot_ros \; python3 -m pip install -U vcstool - vcs import src < src/rosbot/rosbot_hardware.repos - vcs import src < src/rosbot/rosbot_simulation.repos + vcs import src < src/rosbot_ros/rosbot/rosbot_hardware.repos + vcs import src < src/rosbot_ros/rosbot/rosbot_simulation.repos + cp -r src/ros2_controllers/diff_drive_controller src/ + cp -r src/ros2_controllers/imu_sensor_broadcaster src/ + rm -rf src/ros2_controllers # Package micro_ros_msgs does not have industrial ci and tests does not pass. # For more information see https://github.com/micro-ROS/micro_ros_msgs/issues/7 @@ -56,15 +41,12 @@ jobs: shell: bash run: | sed '/if(BUILD_TESTING)/,/endif()/d' src/micro_ros_msgs/CMakeLists.txt -i - - - name: Fix pip - shell: bash - run: | - echo -e "[global]\nbreak-system-packages = true" >> ~/.config/pip/pip.conf + sed '/if(BUILD_TESTING)/,/endif()/d' src/ros_components_description/CMakeLists.txt -i - name: Running ROS Industrial CI uses: ros-industrial/industrial_ci@master env: + PIP_BREAK_SYSTEM_PACKAGES: 1 ROS_DISTRO: ${{matrix.ROS_DISTRO}} DOCKER_IMAGE: ros:${{matrix.ROS_DISTRO}}-ros-base IMMEDIATE_TEST_OUTPUT: true diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b238ea30..36d5e001 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,27 +1,51 @@ --- repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.5.0 + rev: v5.0.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 + # mesh files has to be taken into account + args: [--maxkb=3000] - id: check-ast - id: check-json + exclude: ^.vscode/|^.devcontainer/ + - id: check-merge-conflict + - id: check-symlinks + - id: check-xml + - id: check-yaml + - id: debug-statements + - id: destroyed-symlinks + - id: detect-private-key + - id: end-of-file-fixer + - id: fix-byte-order-marker - id: name-tests-test - files: ^.*\/test\/.*$ - args: [--pytest-test-first] + - id: mixed-line-ending + - id: trailing-whitespace + + - repo: https://github.com/PyCQA/isort + rev: 5.13.2 + hooks: + - id: isort + args: [--profile, black] + + - repo: https://github.com/cheshirekow/cmake-format-precommit + rev: v0.6.13 + hooks: + - id: cmake-format + + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v19.1.4 + hooks: + - id: clang-format - repo: https://github.com/codespell-project/codespell - rev: v2.2.6 + rev: v2.3.0 hooks: - id: codespell name: codespell description: Checks for common misspellings in text files. - entry: codespell * + entry: codespell + exclude_types: [rst, svg] language: python types: [text] @@ -33,23 +57,16 @@ repos: args: [--mapping, '2', --sequence, '4', --offset, '2', --width, '100'] - repo: https://github.com/psf/black - rev: 24.4.0 + rev: 24.10.0 hooks: - id: black args: [--line-length=99] - repo: https://github.com/PyCQA/flake8 - rev: 7.0.0 + rev: 7.1.1 hooks: - id: flake8 - args: ['--ignore=E501,W503'] # ignore too long line and line break before binary operator, - # black checks it - - - repo: https://github.com/PyCQA/isort - rev: 5.13.2 - hooks: - - id: isort - args: [--profile, black] + args: ['--ignore=E501,W503'] - repo: local hooks: @@ -65,14 +82,20 @@ repos: - id: ament_copyright name: ament_copyright description: Check if copyright notice is available in all files. - stages: [commit] + stages: [pre-commit] entry: ament_copyright language: system + exclude: ^.*\.md$ - # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: v1.1.1 + rev: v1.1.2 hooks: - id: doc8 args: [--max-line-length=100, --ignore=D001] exclude: ^.*\/CHANGELOG\.rst/.*$ + + - repo: https://github.com/tier4/pre-commit-hooks-ros + rev: v0.10.0 + hooks: + - id: prettier-package-xml + - id: sort-package-xml diff --git a/.pyspelling.yaml b/.pyspelling.yaml deleted file mode 100644 index ae490993..00000000 --- a/.pyspelling.yaml +++ /dev/null @@ -1,54 +0,0 @@ ---- -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 deleted file mode 100644 index e5989fd3..00000000 --- a/.wordlist.txt +++ /dev/null @@ -1,160 +0,0 @@ -Imu -IMU -JointState -LaserScan -MultiArray -Odometry -ROSbot -TFMessage -bringup -cmd -config -ekf -gazebosim -gz -https -husarion -imu -github -msg -msgs -nav -odom -odometry -py -ros -rosbot -tf -vel -yaml -DISTRO -LTS -Metapackage -ROSbots -SteveMacenski -URDF -cd -colcon -autoremove -cra -init -md -mkdir -preconfigured -repos -rosdep -rosdistro -src -sudo -teleop -ubuntu -vcs -vcstool -ws -ament -os -pytest -setuptools -xml -cmake -CMAKE -CXX -DLL -GNUCXX -PLUGINLIB -RUNTIME -Wextra -Wpedantic -ament -cmake -cpp -endif -lifecycle -pluginlib -rclcpp -realtime -urdf -xml -ROSbot's -ROSbots -xacro -MECANUM -dll -dllexport -dllimport -endforeach -foreach -mecanum -rcpputils -apache -http -unstamped -www -xl -SDF -astra -cfg -gpu -ign -orbbec -pointcloud -rl -rosgraph -rr -sdf -webots -accelerometer -vl -gaussian -fdir -Krzysztof -Maciej -StΔ™pieΕ„ -Stepien -Wojciechowski -gtest -Delicat -Jakub -Bence -Palacios -env -pids -pgrep -namespace -TODO -delihus -microros -namespaces -fastdds -localhost -SHM -UDPv -CustomUdpTransport -UDPv -Dominik -Nowak -pyftdi -usbutils -utils -usr -CBUS -RST -ftdi -DK -Ftdi -url -dev -stm -ttyUSB -subprocess -cbus -Dockerfile -unbuffered -libgpiod -REQ -gpiochip -gpiod -Rafal -Gorecki -gpiozero -sp diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 00000000..2ab32a70 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,58 @@ +# Developer info and tools + +## USB-A connection + +You can connect to the robot hardware on your own computer. To establish a connection, connect your computer to the robot using a USB-A cable. Then build the code locally and specify via the serial_port argument which processor should be used to establish the connection. + +```bash +ros2 launch rosbot_bringup bringup.launch.py serial_port:=/dev/ttyUSB0 +``` + +The hardware checks the connection via USB-A only during initialization and when btn1 or btn2 is pressed, so while executing the above command, hold down the reset button together with bnt1/bnt2 and release the reset button. After establishing a connection, you can release bnt1/bnt2. + +## 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 +``` + +After initialization [pre-commit configuration](.pre-commit-config.yaml) will applied on every commit. + +## Industrial CI + +```bash +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: + +``` bash +colcon test-result --verbose +``` + +### Testing `.github/workflows/industrial_ci.yaml` Locally + +At fist install [act](https://github.com/nektos/act): + +```bash +cd / +curl -s https://raw.githubusercontent.com/nektos/act/master/install.sh | sudo bash +``` + +And test the workflow with: + +```bash +act -W .github/workflows/industrial_ci.yaml +``` diff --git a/README.md b/README.md index 4ec883ba..4c8a4bad 100644 --- a/README.md +++ b/README.md @@ -1,204 +1,137 @@ # Rosbot ROS -ROS2 packages for ROSbot 2R and ROSbot 2 PRO. +ROS 2 packages for Husarion ROSbot series. -## ROS packages +![ROSbot](https://husarion.com/assets/images/rosbot3-preview2-f7dee8f0b4ea4de02e80d4dc9f2ca286.png) -### `rosbot` +## πŸ“š ROS API -Metapackage that contains dependencies to other repositories. +Documentation is available in ROS_API.md. -### `rosbot_bringup` +## πŸš€ Quick Start -Package that contains launch, which starts all base functionalities. Also configuration for [robot_localization](https://github.com/cra-ros-pkg/robot_localization) and [ros2_controllers](https://github.com/ros-controls/ros2_controllers) are defined there. +### βš™οΈ Prerequisites -### `rosbot_description` +1. Install all necessary tools: -URDF model used as a source of transforms on the physical robot. It was written to be compatible with ROS Industrial and preconfigured for ROS2 control. + ```bash + sudo apt-get update + sudo apt-get install -y python3-pip ros-dev-tools stm32flash + ``` -### `rosbot_gazebo` +2. Create a workspace folder and clone the rosbot_ros repository: -Launch files for Ignition Gazebo working with ROS2 control. + ```bash + mkdir -p ros2_ws + cd ros2_ws + git clone -b jazzy https://github.com/husarion/rosbot_ros src/rosbot_ros + ``` -### `rosbot_controller` +### πŸ€– Hardware -ROS2 hardware controllers configuration for ROSbots. - -## ROS API - -Available in [ROS_API.md](./ROS_API.md) - -## Usage on hardware - -To run the software on real ROSbot 2R, 2 PRO, also communication with the CORE2 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_ros2_firmware repository](https://github.com/husarion/rosbot_ros2_firmware). - -## Source build - -### Prerequisites - -Install all necessary tools: - -```bash -sudo apt-get update -sudo apt-get install -y python3-pip ros-dev-tools stm32flash -``` - -Create workspace folder and clone `rosbot_ros` repository: - -```bash -mkdir -p ros2_ws/src -cd ros2_ws -git clone https://github.com/husarion/rosbot_ros src/ -``` - -### Build and run on hardware - -Building: +#### Building ```bash -export HUSARION_ROS_BUILD=hardware +export HUSARION_ROS_BUILD_TYPE=hardware source /opt/ros/$ROS_DISTRO/setup.bash -vcs import src < src/rosbot/rosbot_hardware.repos - -# Build only diff_drive_controller and imu_sensor_broadcaster from ros2_controllers -cp -r src/ros2_controllers/diff_drive_controller src && cp -r src/ros2_controllers/imu_sensor_broadcaster src && rm -rf src/ros2_controllers - -rm -r src/rosbot_gazebo +vcs import src < src/rosbot_ros/rosbot/rosbot_hardware.repos +cp -r src/ros2_controllers/diff_drive_controller src/ +cp -r src/ros2_controllers/imu_sensor_broadcaster src/ +rm -rf src/ros2_controllers +rm -r src/rosbot_ros/rosbot_gazebo sudo rosdep init rosdep update --rosdistro $ROS_DISTRO rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y -colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -``` - -Flash firmware: - -```bash -source install/setup.bash -ros2 run rosbot_utils flash_firmware +colcon build --symlink-install --packages-up-to rosbot --cmake-args -DCMAKE_BUILD_TYPE=Release ``` -Running: +#### Run the Robot -```bash -source install/setup.bash -ros2 launch rosbot_bringup combined.launch.py -``` +1. Flash the firmware: -### Build and run Gazebo simulation + ```bash + sudo su + source install/setup.bash + ros2 run rosbot_utils flash_firmware + exit + ``` -Prerequisites: +> [!NOTE] +> To run the software on real ROSbots, communication with the CORE2 is required. Ensure the firmware is updated before running the micro-ROS agent. For detailed instructions, refer to the rosbot_ros2_firmware repository. -> [!TIP] -> The default version of Gazebo Ignition will be installed with the instructions below. If you want to install a different version of the simulator, it is necessary to: -> -> - Check compatible versions of ROS 2 and Gazebo in [this table](https://gazebosim.org/docs/garden/ros_installation#summary-of-compatible-ros-and-gazebo-combinations) -> - [Install the appropriate version](https://gazebosim.org/docs/fortress/install_ubuntu#binary-installation-on-ubuntu), -> - Add the `GZ_VERSION` environment variable appropriate to your version -> -> ```bash -> export GZ_VERSION=fortress -> ``` +2. Launch the robot: -If you have installed multiple versions of Gazebo use the global variable to select the correct one: + ```bash + source install/setup.bash + ros2 launch rosbot_bringup bringup.launch.py + ``` -```bash -export GZ_VERSION=fortress -``` +### πŸ–₯️ Simulation -Building: +#### Building ```bash -export HUSARION_ROS_BUILD=simulation +export HUSARION_ROS_BUILD_TYPE=simulation source /opt/ros/$ROS_DISTRO/setup.bash -vcs import src < src/rosbot/rosbot_hardware.repos -vcs import src < src/rosbot/rosbot_simulation.repos - -# Build only diff_drive_controller and imu_sensor_broadcaster from ros2_controllers -cp -r src/ros2_controllers/diff_drive_controller src && cp -r src/ros2_controllers/imu_sensor_broadcaster src && rm -rf src/ros2_controllers +vcs import src < src/rosbot_ros/rosbot/rosbot_simulation.repos +cp -r src/ros2_controllers/diff_drive_controller src/ +cp -r src/ros2_controllers/imu_sensor_broadcaster src/ +rm -rf src/ros2_controllers sudo rosdep init rosdep update --rosdistro $ROS_DISTRO rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y -colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release +colcon build --symlink-install --packages-up-to rosbot --cmake-args -DCMAKE_BUILD_TYPE=Release ``` -Running: +#### Run the Simulation ```bash source install/setup.bash ros2 launch rosbot_gazebo simulation.launch.py ``` -## Developer info - -### 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 -``` - -After initialization [pre-commit configuration](.pre-commit-config.yaml) will applied on every commit. - -### Industrial CI - -```bash -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: +### Launch Arguments + +| Symbol | Meaning | +| ------ | ---------------------------- | +| πŸ€– | Available for physical robot | +| πŸ–₯️ | Available in simulation | + +| πŸ€– | πŸ–₯️ | Argument | Description
***Type:*** `Default` | +| --- | --- | ------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| βœ… | βœ… | `namespace` | Namespace for all topics and tfs.
***string:*** `env(ROBOT_NAMESPACE)` | +| βœ… | ❌ | `mecanum` | Whether to use mecanum drive controller (otherwise diff drive controller is used).
***bool:*** `False` | +| βœ… | ❌ | `microros` | Automatically connect with hardware using microros.
***bool:*** `True` | +| βœ… | ❌ | `serial_baudrate` | Baud rate for serial communication .
***string:*** `576000` | +| βœ… | ❌ | `serial_port` | Automatically connect with hardware using microros.
***string:*** `/dev/ttySERIAL` | +| βœ… | ❌ | `fastrtps_profiles` | Path to the Fast RTPS default profiles file for Micro-ROS agent for localhost only setup.
***string:*** [`microros_localhost_only.xml`](./rosbot_bringup/config/microros_localhost_only.xml) | +| ❌ | βœ… | `gz_gui` | Run simulation with specific GUI layout.
***string:*** [`teleop.config`](https://github.com/husarion/husarion_gz_worlds/blob/main/config/teleop.config) | +| ❌ | βœ… | `gz_headless_mode` | Run the simulation in headless mode. Useful when a GUI is not needed or to reduce the number of calculations.
***bool:*** `False` | +| ❌ | βœ… | `gz_log_level` | Adjust the level of console output.
***int:*** `1` (choices: `0`, `1`, `2`, `3`, `4`) | +| ❌ | βœ… | `gz_world` | Absolute path to SDF world file.
***string:*** [`husarion_world.sdf`](https://github.com/husarion/husarion_gz_worlds/blob/main/worlds/husarion_world.sdf) | +| ❌ | βœ… | `robots` | Spawning multiple robots at positions with yaw orientations e.g.robots:='robot1={x: 0.0, y: -1.0}; robot2={x: 1.0, y: -1.0};'
***string:*** `''` | +| ❌ | βœ… | `x` | Initial robot position in the global 'x' axis.
***float:*** `0.0` | +| ❌ | βœ… | `y` | Initial robot position in the global 'y' axis.
***float:*** `2.0` | +| ❌ | βœ… | `z` | Initial robot position in the global 'z' axis.
***float:*** `0.0` | +| ❌ | βœ… | `roll` | Initial robot 'roll' orientation.
***float:*** `0.0` | +| ❌ | βœ… | `pitch` | Initial robot 'pitch' orientation.
***float:*** `0.0` | +| ❌ | βœ… | `yaw` | Initial robot 'yaw' orientation.
***float:*** `0.0` | -``` bash -colcon test-result --verbose -``` - -### Format python code with [Black](https://github.com/psf/black) - -```bash -cd src/ -black rosbot* -``` - -### Testing `.github/workflows/industrial_ci.yaml` Locally - -At fist install [act](https://github.com/nektos/act): - -```bash -cd / -curl -s https://raw.githubusercontent.com/nektos/act/master/install.sh | sudo bash -``` - -And test the workflow with: +> [!TIP] +> +> To read the arguments for individual packages, add the `-s` flag to the `ros2 launch` command (e.g. `ros2 launch rosbot_bringup bringup.launch.py ​​-s`) -```bash -act -W .github/workflows/industrial_ci.yaml -``` +## πŸ•ΉοΈ Demo -## Demo +Explore demos showcasing the capabilities of ROSbots: -Below you can find demos with ROSbots: -| link | description | -| - | - | -| [rosbot-docker](https://github.com/husarion/rosbot-docker/tree/ros2) | Simple example how to drive ROSbot with `teleop_twist_keyboard` using docker | -| [rosbot-sensors](https://github.com/husarion/rosbot-sensors) | Visualize all ROSbot sensors | -| [rosbot-gamepad](https://github.com/husarion/rosbot-gamepad) | Stream a live video from Orbbec Astra to a window on your PC. Control the robot using `teleop-twist-keyboard` | -| [rosbot-telepresence](https://github.com/husarion/rosbot-telepresence) | Stream a live video from Orbbec Astra to a window on your PC. Control the robot using `teleop-twist-keyboard` | -| [rosbot-autonomy](https://github.com/husarion/rosbot-autonomy) | A combination of `mapping` and `navigation` projects allowing simultaneous mapping and navigation in unknown environments. | +| πŸ“Ž Link | πŸ“– Description | +| ---------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------ | +| [rosbot-telepresence](https://github.com/husarion/rosbot-telepresence) | Stream live video from Orbbec Astra to a PC and control the robot using `teleop-twist-keyboard` | +| [rosbot-autonomy](https://github.com/husarion/rosbot-autonomy) | Enables simultaneous mapping and navigation, allowing the robot to move in unknown environments. | diff --git a/ROS_API.md b/ROS_API.md index 3889c04d..d6a9027c 100644 --- a/ROS_API.md +++ b/ROS_API.md @@ -1,44 +1,119 @@ -Use `bringup.launch.py` from `rosbot_bringup` to start all base functionalities for ROSbot 2, 2 PRO, 2R. It consists of following parts: - -- `ekf_node` from `robot_localization`, it is used to fuse wheel odometry and IMU data. Parameters are defined in `ekf.yaml` in `rosbot_bringup/config`. It subscribes to `/rosbot_base_controller/odom` and `/imu_broadcaster/imu` published by ros2 controllers and publishes fused odometry on `/odometry/filtered` topic - - **Subscribes** - - `/rosbot_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_) - - -Use `controller.launch.py` from `rosbot_controller`, it loads robot model defined in `rosbot_description` as well as ros2 control [rosbot_hardware_interfaces](https://github.com/husarion/rosbot_hardware_interfaces). It also starts controllers: - * `joint_state_broadcaster` - * `rosbot_base_controller` - * `imu_broadcaster` - - **Subscribes** - - `/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_) - - `/_motors_cmd` (_std_msgs/Float32MultiArray_) - - `/rosbot_base_controller/odom` (_nav_msgs/Odometry_) - - `/imu_broadcaster/imu` (_sensor_msgs/Imu_) - -Use `simulation.launch.py` from `rosbot_gazebo` to start all base functionalities for ROSbot 2, 2 PRO, 2R in the Gazebo simulator. -If you want to spawn multiple robots use `simulation.launch.py` with the `robots` argument e. g.: -```bash -ros2 launch rosbot_gazebo simulation.launch.py robots:='robot1={x: 0.0, y: -1.0}; robot2={x: 1.0, y: -1.0}; robot3={x: 2.0, y: -1.0}; robot4={x: 3.0, y: -1.0}' -``` - -If you want to use your own world add to the world's sdf file gazebo sensors plugins inside any `` tag: -```xml - - -``` - -> **Warning** -> The distance sensors' topics types from Gazebo simulation mismatch with the real ones. The range sensors are not implemented yet in the Gazebo Ignition (for more information look [here](https://github.com/gazebosim/gz-sensors/issues/19)). The real type is [sensor_msgs/msg/Range](https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/Range.msg) but simulated [sensor_msgs/msg/LaserScan](https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/LaserScan.msg). The first value of the `ranges` in [sensor_msgs/msg/LaserScan](https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/LaserScan.msg) is the `range` field of [sensor_msgs/msg/Range](https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/Range.msg). +# ROSbot - Software + +Detailed information about content of rosbot package for ROS2. + +## Package Description + +### `rosbot` + +Metapackage that contains dependencies to other repositories. It is also used to define whether simulation dependencies should be used. + +### `rosbot_bringup` + +Package that contains launch, which starts all base functionalities with the microros agent. Also configs for `robot_localization` and `laser_filters` are defined there. + +**Available Launch Files:** + +- `bringup.launch.py` - is responsible for communicating with firmware and activating all logic related to the robot's movement and processing of sensory data. +- `microros.launch.py` - establishes connection with the firmware. + +### `rosbot_controller` + +ROS2 hardware controller for ROSbot. It manages inputs and outputs data from ROS2 control, forwarding it via ROS topics to be read by microROS. The controller.launch.py file loads the robot model defined in rosbot_description along with ROS2 control dependencies from [rosbot_hardware_interfaces](https://github.com/husarion/rosbot_hardware_interfaces). + +### `rosbot_description` + +URDF model used for both simulation and as a source of transforms on physical robot. It was written to be compatible with ROS Industrial and preconfigured for ROS2 control. + +Available models: + +| MODEL | DESCRIPTION | +| ------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `rosbot` | Final configuration of rosbot with ability to attach external hardware. | +| `rosbot_base` | Base of rosbot prepared to be included into preexisting configuration. Meant to be compatible with concept of ROS Industrial ability for manipulators to have interchangeable end effectors. | + +### `rosbot_gazebo` + +Launch files for Ignition Gazebo working with ROS2 control. + +**Available Launch Files:** + +- `simulations.launch.py` - running a rosbot in Gazebo simulator and simulate all specified sensors. + +### `rosbot_utils` + +This package contains the stable firmware version with the flash script. + +## ROS API + +### Available Nodes + +[controller_manager/controller_manager]: https://github.com/ros-controls/ros2_control/blob/master/controller_manager +[diff_drive_controller/diff_drive_controller]: https://github.com/ros-controls/ros2_controllers/tree/master/diff_drive_controller +[imu_sensor_broadcaster/imu_sensor_broadcaster]: https://github.com/ros-controls/ros2_controllers/tree/master/imu_sensor_broadcaster +[joint_state_broadcaster/joint_state_broadcaster]: https://github.com/ros-controls/ros2_controllers/tree/master/joint_state_broadcaster +[laser_filters/scan_to_scan_filter_chain]: https://github.com/ros-perception/laser_filters/blob/ros2/src/scan_to_scan_filter_chain.cpp +[micro_ros_agent/micro_ros_agent]: https://github.com/micro-ROS/micro-ROS-Agent +[robot_localization/ekf_node]: https://github.com/cra-ros-pkg/robot_localization +[robot_state_publisher/robot_state_publisher]: https://github.com/ros/robot_state_publisher +[rosbot_hardware_interfaces/rosbot_imu_sensor]: https://github.com/husarion/rosbot_hardware_interfaces/blob/main/src/rosbot_imu_sensor.cpp +[rosbot_hardware_interfaces/rosbot_system]: https://github.com/husarion/rosbot_hardware_interfaces/blob/main/src/rosbot_system.cpp + +| NODE | DESCRIPTION | +| --------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| **`~/controller_manager`** | Controller Manager performs two main functions. First, it manages controllers and their required interfaces, handling tasks like loading, activating, deactivating, and unloading. Second, it interacts with hardware components, ensuring access to their interfaces.
_[controller_manager/controller_manager][]_ | +| **`~/ekf_filter_node`** | Used to fuse wheel odometry and IMU data. Parameters are defined in `rosbot_bringup/config/ekf.yaml`
_[robot_localization/ekf_node][]_ | +| **`~/imu_broadcaster`** | The broadcaster to publish readings of IMU sensors
_[imu_sensor_broadcaster/imu_sensor_broadcaster][]_ | +| **`~/imu_sensor_node`** | The node responsible for subscriptions to IMU data from the hardware
_[rosbot_hardware_interfaces/rosbot_imu_sensor][]_ | +| **`~/joint_state_broadcaster`** | The broadcaster reads all state interfaces and reports them on specific topics
_[joint_state_broadcaster/joint_state_broadcaster][]_ | +| **`~/laser_scan_box_filter`** | This is a filter that removes points in a laser scan inside of a cartesian box
_[laser_filters/scan_to_scan_filter_chain][]_ | +| **`~/robot_state_publisher`** | Uses the URDF specified by the parameter robot\*description and the joint positions from the topic joint\*states to calculate the forward kinematics of the robot and publish the results via tf
_[robot_state_publisher/robot_state_publisher][]_ | +| **`~/rosbot_system_node`** | The node communicating with the hardware responsible for receiving and sending data related to engine control
_[rosbot_hardware_interfaces/rosbot_system][]_ | +| **`~/rosbot_base_controller`** | The controller managing a mobile robot with a differential or omni drive (mecanum wheels). Converts speed commands for the robot body to wheel commands for the base. It also calculates odometry based on hardware feedback and shares it.`DiffDriveController` or `MecanumDriveController`
_[diff_drive_controller/diff_drive_controller][]_ | +| **`~/scan_to_scan_filter_chain`** | Node which subscribes to `/scan` topic and removes all points that are within the robot's footprint (defined by config `laser_filter.yaml` in `rosbot_bringup` package). Filtered laser scan is then published on `/scan_filtered` topic
_[laser_filters/scan_to_scan_filter_chain][]_ | +| **`/stm32_node`** | Node enabling communication with Digital Board, it provides the following interface
_[micro_ros_agent/micro_ros_agent][]_ | + +### Available Topics + +[control_msgs/DynamicJointState]: https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/DynamicJointState.msg +[diagnostic_msgs/DiagnosticArray]: https://docs.ros2.org/foxy/api/diagnostic_msgs/msg/DiagnosticArray.html +[geometry_msgs/PoseWithCovarianceStamped]: https://docs.ros2.org/foxy/api/geometry_msgs/msg/PoseWithCovarianceStamped.html +[geometry_msgs/Twist]: https://docs.ros2.org/foxy/api/geometry_msgs/msg/Twist.html +[lifecycle_msgs/TransitionEvent]: https://docs.ros2.org/foxy/api/lifecycle_msgs/msg/TransitionEvent.html +[nav_msgs/Odometry]: https://docs.ros2.org/foxy/api/nav_msgs/msg/Odometry.html +[sensor_msgs/BatteryState]: https://docs.ros2.org/foxy/api/sensor_msgs/msg/BatteryState.html +[sensor_msgs/Imu]: https://docs.ros2.org/foxy/api/sensor_msgs/msg/Imu.html +[sensor_msgs/JointState]: https://docs.ros2.org/foxy/api/sensor_msgs/msg/JointState.html +[sensor_msgs/LaserScan]: https://docs.ros2.org/foxy/api/sensor_msgs/msg/LaserScan.html +[std_msgs/Float32MultiArray]: https://docs.ros2.org/foxy/api/std_msgs/msg/Float32MultiArray.html +[std_msgs/String]: https://docs.ros2.org/foxy/api/std_msgs/msg/String.html +[tf2_msgs/TFMessage]: https://docs.ros2.org/foxy/api/tf2_msgs/msg/TFMessage.html + +| TOPIC | DESCRIPTION | +| ------------------------------------------------ | ------------------------------------------------------------------------------------------------------------------------------- | +| **`/battery_state`** | provides information about the state of the battery.
_[sensor_msgs/BatteryState][]_ | +| **`~/cmd_vel`** | sends velocity commands for controlling robot motion.
_[geometry_msgs/Twist][]_ | +| **`/diagnostics`** | contains diagnostic information about the robot's systems.
_[diagnostic_msgs/DiagnosticArray][]_ | +| **`~/dynamic_joint_states`** | publishes information about the dynamic state of joints.
_[control_msgs/DynamicJointState][]_ | +| **`~/imu_broadcaster/imu`** | broadcasts IMU (Inertial Measurement Unit) data.
_[sensor_msgs/Imu][]_ | +| **`~/imu_broadcaster/transition_event`** | signals transition events in the lifecycle of the IMU broadcaster node.
_[lifecycle_msgs/TransitionEvent][]_ | +| **`~/joint_state_broadcaster/transition_event`** | indicates transition events in the lifecycle of the joint state broadcaster node.
_[lifecycle_msgs/TransitionEvent][]_ | +| **`~/joint_states`** | publishes information about the state of robot joints.
_[sensor_msgs/JointState][]_ | +| **`~/laser_scan_box_filter/transition_event`** | signals transition events in the lifecycle of the laser scan box filter node.
_[lifecycle_msgs/TransitionEvent][]_ | +| **`~/odometry/filtered`** | publishes filtered odometry data.
_[nav_msgs/Odometry][]_ | +| **`~/robot_description`** | publishes the robot's description.
_[std_msgs/String][]_ | +| **`~/rosbot_base_controller/odom`** | provides odometry data from the base controller of the ROSbot.
_[nav_msgs/Odometry][]_ | +| **`~/rosbot_base_controller/transition_event`** | indicates transition events in the lifecycle of the ROSbot base controller node.
_[lifecycle_msgs/TransitionEvent][]_ | +| **`~/scan`** | publishes raw laser scan data.
_[sensor_msgs/LaserScan][]_ | +| **`~/scan_filtered`** | publishes filtered laser scan data.
_[sensor_msgs/LaserScan][]_ | +| **`~/set_pose`** | sets the robot's pose with covariance.
_[geometry_msgs/PoseWithCovarianceStamped][]_ | +| **`~/tf`** | publishes transformations between coordinate frames over time.
_[tf2_msgs/TFMessage][]_ | +| **`~/tf_static`** | publishes static transformations between coordinate frames.
_[tf2_msgs/TFMessage][]_ | + +**Hidden topic:** + +| TOPIC | DESCRIPTION | +| ------------------------ | --------------------------------------------------------------------- | +| **`/_imu/data_raw`** | raw data image from imu sensor
_[sensor_msgs/Imu][]_ | +| **`/_motors_cmd`** | desired speed on each wheel
_[std_msgs/Float32MultiArray][]_ | +| **`/_motors_responses`** | raw data readings from each wheel
_[sensor_msgs/JointState][]_ | diff --git a/docker/.env b/docker/.env new file mode 100644 index 00000000..05b532ca --- /dev/null +++ b/docker/.env @@ -0,0 +1 @@ +ROBOT_NAMESPACE=rosbot diff --git a/docker/Dockerfile.hardware b/docker/Dockerfile.hardware new file mode 100644 index 00000000..d8282034 --- /dev/null +++ b/docker/Dockerfile.hardware @@ -0,0 +1,38 @@ +ARG ROS_DISTRO=jazzy + +FROM husarnet/ros:${ROS_DISTRO}-ros-core + +WORKDIR /ros2_ws + +ENV HUSARION_ROS_BUILD_TYPE=hardware +ENV PIP_BREAK_SYSTEM_PACKAGES=1 + +COPY .. src/rosbot_ros + +RUN apt-get update && apt-get install -y \ + python3-pip \ + ros-dev-tools \ + stm32flash \ + ros-${ROS_DISTRO}-teleop-twist-keyboard && \ + # Setup workspace + vcs import src < src/rosbot_ros/rosbot/rosbot_$HUSARION_ROS_BUILD_TYPE.repos && \ + cp -r src/ros2_controllers/diff_drive_controller src/ && \ + cp -r src/ros2_controllers/imu_sensor_broadcaster src/ && \ + rm -rf src/ros2_controllers && \ + rm -r src/rosbot_ros/rosbot_gazebo && \ + # Install dependencies + rosdep init && \ + rosdep update --rosdistro $ROS_DISTRO && \ + rosdep install --from-paths src -y -i && \ + # Build + source /opt/ros/$ROS_DISTRO/setup.bash && \ + colcon build --symlink-install --packages-up-to rosbot --cmake-args -DCMAKE_BUILD_TYPE=Release && \ + # Get version + echo $(cat /ros2_ws/src/rosbot/package.xml | grep '' | sed -r 's/.*([0-9]+.[0-9]+.[0-9]+)<\/version>/\1/g') >> /version.txt && \ + # Size optimization + export SUDO_FORCE_REMOVE=yes && \ + apt-get remove -y \ + ros-dev-tools && \ + apt-get autoremove -y && \ + apt-get clean && \ + rm -rf /var/lib/apt/lists/* diff --git a/docker/Dockerfile.simulation b/docker/Dockerfile.simulation new file mode 100644 index 00000000..87138105 --- /dev/null +++ b/docker/Dockerfile.simulation @@ -0,0 +1,37 @@ +ARG ROS_DISTRO=jazzy + +FROM husarnet/ros:${ROS_DISTRO}-ros-core + +WORKDIR /ros2_ws + +ENV HUSARION_ROS_BUILD_TYPE=simulation +ENV PIP_BREAK_SYSTEM_PACKAGES=1 + +COPY .. src/rosbot_ros + +RUN apt-get update && apt-get install -y \ + python3-pip \ + ros-dev-tools \ + stm32flash \ + ros-${ROS_DISTRO}-teleop-twist-keyboard && \ + # Setup workspace + vcs import src < src/rosbot_ros/rosbot/rosbot_$HUSARION_ROS_BUILD_TYPE.repos && \ + cp -r src/ros2_controllers/diff_drive_controller src/ && \ + cp -r src/ros2_controllers/imu_sensor_broadcaster src/ && \ + rm -rf src/ros2_controllers && \ + # Install dependencies + rosdep init && \ + rosdep update --rosdistro $ROS_DISTRO && \ + rosdep install --from-paths src -y -i && \ + # Build + source /opt/ros/$ROS_DISTRO/setup.bash && \ + colcon build --symlink-install --packages-up-to rosbot --cmake-args -DCMAKE_BUILD_TYPE=Release && \ + # Get version + echo $(cat /ros2_ws/src/rosbot/package.xml | grep '' | sed -r 's/.*([0-9]+.[0-9]+.[0-9]+)<\/version>/\1/g') >> /version.txt && \ + # Size optimization + export SUDO_FORCE_REMOVE=yes && \ + apt-get remove -y \ + ros-dev-tools && \ + apt-get autoremove -y && \ + apt-get clean && \ + rm -rf /var/lib/apt/lists/* diff --git a/docker/compose.hardware.yaml b/docker/compose.hardware.yaml new file mode 100644 index 00000000..ff4a6cdc --- /dev/null +++ b/docker/compose.hardware.yaml @@ -0,0 +1,24 @@ +# Quick Start +# +# 1. Run `docker compose -f compose.hardware.yaml up` on the ROSbot +# 2. Open a shell inside a docker container `docker compose -f compose.hardware.yaml exec -it rosbot bash` +# 3. Run `ros2 run teleop_twist_keyboard teleop_twist_keyboard` inside the container + +services: + rosbot: + build: + context: .. + dockerfile: docker/Dockerfile.hardware + network_mode: host + ipc: host + restart: unless-stopped + devices: + - ${SERIAL_PORT:-/dev/ttyUSB0} + - /dev/bus/usb/ # FTDI + environment: + - USER + command: > + ros2 launch rosbot_bringup bringup.launch.py + mecanum:=${MECANUM:-False} + namespace:=${ROBOT_NAMESPACE:-rosbot} + serial_port:=${SERIAL_PORT:-/dev/ttyUSB0} diff --git a/docker/compose.simulation.yaml b/docker/compose.simulation.yaml new file mode 100644 index 00000000..535181b5 --- /dev/null +++ b/docker/compose.simulation.yaml @@ -0,0 +1,34 @@ +# Quick Start +# +# 1. Run `xhost +local:docker && docker compose -f compose.simulation.yaml up` on the laptop +# (optionally you can chang `gpu-config` -> `cpu config`). +# 2. Open a shell inside a docker container `docker compose -f compose.simulation.yaml exec -it rosbot bash` +# 3. Run `ros2 run teleop_twist_keyboard teleop_twist_keyboard` inside the container + +x-gpu-config: + &gpu-config + runtime: nvidia + environment: + - USER + - DISPLAY=${DISPLAY:?err} + - NVIDIA_VISIBLE_DEVICES=all + - NVIDIA_DRIVER_CAPABILITIES=all + +x-cpu-config: + &cpu-config + environment: + - USER + - DISPLAY=${DISPLAY:?err} + +services: + rosbot: + build: + context: .. + dockerfile: docker/Dockerfile.simulation + network_mode: host + ipc: host + <<: [ *gpu-config] + command: > + ros2 launch rosbot_gazebo simulation.launch.py + mecanum:=${MECANUM:-False} + namespace:=${ROBOT_NAMESPACE:-rosbot} diff --git a/docker/justfile b/docker/justfile new file mode 100644 index 00000000..0e6f5ef4 --- /dev/null +++ b/docker/justfile @@ -0,0 +1,89 @@ +set dotenv-load # to read ROBOT_NAMESPACE from .env file + +[private] +default: + @just --list --unsorted + +[private] +alias flash := flash-firmware + +[private] +alias teleop := run-teleop-docker + +# run teleop_twist_keybaord (inside rviz2 container) +run-teleop-docker: _run-as-user + #!/bin/bash + docker compose exec rosbot /bin/bash -c "/ros_entrypoint.sh ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r __ns:=/${ROBOT_NAMESPACE}" + +flash-firmware: _install-yq _run-as-user + #!/bin/bash + echo "Stopping all running containers" + docker ps -q | xargs -r docker stop + + if grep -q "Raspberry Pi" /proc/cpuinfo; then + echo "Setting up Raspberry Pi GPIO Port" + gpio_chip=/dev/gpiochip0 + serial_port=/dev/ttyAMA0 + elif grep -q "Intel(R) Atom(TM) x5-Z8350" /proc/cpuinfo; then + echo "Setting up UpBoard GPIO Port" + gpio_chip=/dev/gpiochip4 + serial_port=/dev/ttyS4 + else + echo "Probably user computer" + gpio_chip=/dev/bus/usb + serial_port=/dev/ttyUSB0 + enable_usb="--usb" + fi + + echo "Flashing the firmware for STM32 microcontroller in ROSbot through $serial_port" + docker run --rm -it \ + --device $gpio_chip \ + --device $serial_port \ + docker-rosbot \ + ros2 run rosbot_utils flash_firmware ${enable_usb} + +run_hardware: + #/bin/bash + docker compose -f compose.hardware.yaml up + +build_hardware: + #/bin/bash + docker compose -f compose.hardware.yaml build + +_run-as-root: + #!/bin/bash + if [ "$EUID" -ne 0 ]; then + echo -e "\e[1;33mPlease re-run as root user to install dependencies\e[0m" + exit 0 + fi + +_run-as-user: + #!/bin/bash + if [ "$EUID" -eq 0 ]; then + echo -e "\e[1;33mPlease re-run as non-root user\e[0m" + exit 0 + fi + +_install-yq: + #!/bin/bash + if ! command -v /usr/bin/yq &> /dev/null; then + if [ "$EUID" -ne 0 ]; then + echo -e "\e[1;33mPlease run as root to install dependencies\e[0m" + exit 0 + fi + + YQ_VERSION=v4.35.1 + ARCH=$(arch) + + if [ "$ARCH" = "x86_64" ]; then + YQ_ARCH="amd64" + elif [ "$ARCH" = "aarch64" ]; then + YQ_ARCH="arm64" + else + YQ_ARCH="$ARCH" + fi + + curl -L https://github.com/mikefarah/yq/releases/download/${YQ_VERSION}/yq_linux_${YQ_ARCH} -o /usr/bin/yq + chmod +x /usr/bin/yq + echo "yq installed successfully!" + fi diff --git a/rosbot/package.xml b/rosbot/package.xml index 2a0ee6ef..b39ffd53 100644 --- a/rosbot/package.xml +++ b/rosbot/package.xml @@ -4,7 +4,9 @@ rosbot 0.13.2 Meta package that contains all packages of Rosbot 2 2R PRO + Husarion + Apache License 2.0 https://husarion.com/ @@ -18,10 +20,8 @@ rosbot_bringup rosbot_controller rosbot_description - - rosbot_utils - - rosbot_gazebo + rosbot_gazebo + rosbot_utils ament_cmake diff --git a/rosbot/rosbot_hardware.repos b/rosbot/rosbot_hardware.repos index 1033c438..6d08ab98 100644 --- a/rosbot/rosbot_hardware.repos +++ b/rosbot/rosbot_hardware.repos @@ -1,22 +1,25 @@ repositories: - # jazzy-devel - rosbot_hardware_interfaces: - type: git - url: https://github.com/husarion/rosbot_hardware_interfaces.git - version: jazzy-devel - ros_components_description: - type: git - url: https://github.com/husarion/ros_components_description.git - version: 1cb25600afa5941d21d48c0af8e63ad2eb3afaa0 husarion_controllers: type: git url: https://github.com/husarion/husarion_controllers - version: jazzy + version: 053c5b1320352c496ac2d02c8bc44c695d8dd4ad micro_ros_msgs: type: git url: https://github.com/micro-ROS/micro_ros_msgs.git - version: jazzy + version: 4594d9db17db735b1e655141fb4afb4cdcfc5789 micro-ROS-Agent: type: git url: https://github.com/micro-ROS/micro-ROS-Agent.git - version: jazzy + version: af007872b034d1ed31de4815377031350ab0034b + ros_components_description: + type: git + url: https://github.com/husarion/ros_components_description.git + version: d34d27edb3abc12ee6d197d0d513e00fe9a5604e + ros2_controllers: # Bug1: https://github.com/ros-controls/ros2_controllers/pull/1420 Bug2: https://github.com/ros-controls/gz_ros2_control/issues/428 + type: git + url: https://github.com/husarion/ros2_controllers/ + version: 0784c2c7c9bad6566c994fb1cb7efe8a06db1df5 + rosbot_hardware_interfaces: + type: git + url: https://github.com/husarion/rosbot_hardware_interfaces.git + version: b699e375a677ef53fa31c5fc27766c2df35e39c3 diff --git a/rosbot/rosbot_simulation.repos b/rosbot/rosbot_simulation.repos index bded3899..47bbeab0 100644 --- a/rosbot/rosbot_simulation.repos +++ b/rosbot/rosbot_simulation.repos @@ -1,5 +1,17 @@ repositories: + husarion_controllers: + type: git + url: https://github.com/husarion/husarion_controllers + version: 053c5b1320352c496ac2d02c8bc44c695d8dd4ad husarion_gz_worlds: type: git url: https://github.com/husarion/husarion_gz_worlds - version: main + version: ce6069b167682f3e2528975a3f6968ec666a639d + ros_components_description: + type: git + url: https://github.com/husarion/ros_components_description.git + version: d34d27edb3abc12ee6d197d0d513e00fe9a5604e + ros2_controllers: # Bug1: https://github.com/ros-controls/ros2_controllers/pull/1420 Bug2: https://github.com/ros-controls/gz_ros2_control/issues/428 + type: git + url: https://github.com/husarion/ros2_controllers/ + version: 0784c2c7c9bad6566c994fb1cb7efe8a06db1df5 diff --git a/rosbot_bringup/config/laser_filter.yaml b/rosbot_bringup/config/laser_filter.yaml new file mode 100644 index 00000000..4b6db116 --- /dev/null +++ b/rosbot_bringup/config/laser_filter.yaml @@ -0,0 +1,19 @@ +--- +/**/scan_to_scan_filter_chain: + ros__parameters: + filter1: + name: box_filter + type: laser_filters/LaserScanBoxFilter + params: + box_frame: /base_link + + max_x: 0.1 + min_x: -0.12 + + max_y: 0.12 + min_y: -0.12 + + max_z: 0.2 + min_z: 0.0 + + invert: false # activate to remove all points outside of the box diff --git a/rosbot_bringup/config/microros_localhost_only.xml b/rosbot_bringup/config/microros_localhost_only.xml index b6969925..8a092766 100644 --- a/rosbot_bringup/config/microros_localhost_only.xml +++ b/rosbot_bringup/config/microros_localhost_only.xml @@ -1,8 +1,8 @@ - - - + + + + CustomUdpTransport diff --git a/rosbot_bringup/launch/bringup.launch.py b/rosbot_bringup/launch/bringup.launch.py index 06ab2112..5cfb9334 100644 --- a/rosbot_bringup/launch/bringup.launch.py +++ b/rosbot_bringup/launch/bringup.launch.py @@ -13,102 +13,104 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + LogInfo, + TimerAction, +) +from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import ( EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution, + PythonExpression, ) -from launch_ros.actions import Node, SetParameter +from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +from nav2_common.launch import ReplaceString def generate_launch_description(): + microros = LaunchConfiguration("microros") namespace = LaunchConfiguration("namespace") + use_sim = LaunchConfiguration("use_sim", default="False") + + declare_microros_arg = DeclareLaunchArgument( + "microros", + default_value="True", + description="Automatically connect with hardware using microros.", + choices=["True", "true", "False", "false"], + ) + declare_namespace_arg = DeclareLaunchArgument( "namespace", default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), description="Namespace for all topics and tfs", ) - use_sim = LaunchConfiguration("use_sim") - declare_use_sim_arg = DeclareLaunchArgument( - "use_sim", - default_value="False", - description="Whether simulation is used", - ) - - use_gpu = LaunchConfiguration("use_gpu") - declare_use_gpu_arg = DeclareLaunchArgument( - "use_gpu", - default_value="False", - description="Whether GPU acceleration is used", - ) - - simulation_engine = LaunchConfiguration("simulation_engine") - declare_simulation_engine_arg = DeclareLaunchArgument( - "simulation_engine", - default_value="webots", - description="Which simulation engine to be used", - choices=["ignition-gazebo", "gazebo-classic", "webots"], - ) - - rosbot_controller = FindPackageShare("rosbot_controller") rosbot_bringup = FindPackageShare("rosbot_bringup") - - mecanum = LaunchConfiguration("mecanum") - declare_mecanum_arg = DeclareLaunchArgument( - "mecanum", - default_value="False", - description=( - "Whether to use mecanum drive controller (otherwise diff drive controller is used)" - ), - ) + rosbot_controller = FindPackageShare("rosbot_controller") controller_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - rosbot_controller, - "launch", - "controller.launch.py", - ] - ) + PathJoinSubstitution([rosbot_controller, "launch", "controller.launch.py"]) ), launch_arguments={ - "use_sim": use_sim, - "mecanum": mecanum, - "use_gpu": use_gpu, - "simulation_engine": simulation_engine, "namespace": namespace, }.items(), ) + microros_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([rosbot_bringup, "launch", "microros.launch.py"]) + ), + condition=IfCondition(PythonExpression([microros, " and not ", use_sim])), + ) + ekf_config = PathJoinSubstitution([rosbot_bringup, "config", "ekf.yaml"]) robot_localization_node = Node( package="robot_localization", executable="ekf_node", output="screen", - parameters=[ekf_config], - remappings=[ - ("/diagnostics", "diagnostics"), - ("/tf", "tf"), - ("/tf_static", "tf_static"), - ], + parameters=[ekf_config, {"tf_prefix": namespace}], + remappings=[("/diagnostics", "diagnostics")], + namespace=namespace, + ) + + laser_filter_config = PathJoinSubstitution([rosbot_bringup, "config", "laser_filter.yaml"]) + + namespace_ext = PythonExpression(["'", namespace, "' + '/' if '", namespace, "' else ''"]) + + laser_filter_config = ReplaceString( + source_file=laser_filter_config, replacements={"/": namespace_ext} + ) + + laser_filter_node = Node( + package="laser_filters", + executable="scan_to_scan_filter_chain", + parameters=[laser_filter_config], namespace=namespace, ) + green_color = "\033[92m" + reset_color = "\033[0m" + + status_info = TimerAction( + period=15.0, + actions=[LogInfo(msg=f"{green_color}All systems are up and running!{reset_color}")], + ) + actions = [ + declare_microros_arg, declare_namespace_arg, - declare_mecanum_arg, - declare_use_sim_arg, - declare_use_gpu_arg, - declare_simulation_engine_arg, - SetParameter(name="use_sim_time", value=use_sim), controller_launch, + microros_launch, + laser_filter_node, robot_localization_node, + status_info, ] return LaunchDescription(actions) diff --git a/rosbot_bringup/launch/combined.launch.py b/rosbot_bringup/launch/microros.launch.py similarity index 68% rename from rosbot_bringup/launch/combined.launch.py rename to rosbot_bringup/launch/microros.launch.py index a3eeae66..2510a88e 100644 --- a/rosbot_bringup/launch/combined.launch.py +++ b/rosbot_bringup/launch/microros.launch.py @@ -17,21 +17,18 @@ from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, - IncludeLaunchDescription, + LogInfo, OpaqueFunction, SetEnvironmentVariable, ) -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_microros_agent_node(context, *args, **kwargs): - # Additional environment variable setup actions env_setup_actions = [] - # Check if ROS_DOMAIN_ID is set and not empty ros_domain_id = os.environ.get("ROS_DOMAIN_ID") if ros_domain_id: env_setup_actions.append( @@ -40,20 +37,22 @@ def generate_microros_agent_node(context, *args, **kwargs): serial_port = LaunchConfiguration("serial_port").perform(context) serial_baudrate = LaunchConfiguration("serial_baudrate").perform(context) - localhost_only_fastrtps_profiles_file = LaunchConfiguration( - "localhost_only_fastrtps_profiles_file" - ).perform(context) + fastrtps_profiles = LaunchConfiguration("fastrtps_profiles").perform(context) if os.environ.get("ROS_LOCALHOST_ONLY") == "1": - # with localhost only setup fastdds is required with a custom config - rmw_implementation = "rmw_fastrtps_cpp" - env_setup_actions.extend( [ - SetEnvironmentVariable(name="RMW_IMPLEMENTATION", value=rmw_implementation), + LogInfo( + msg=[ + "ROS_LOCALHOST_ONLY set to 1. Using FASTRTPS_DEFAULT_PROFILES_FILE=", + fastrtps_profiles, + ".", + ] + ), + SetEnvironmentVariable(name="RMW_IMPLEMENTATION", value="rmw_fastrtps_cpp"), SetEnvironmentVariable( name="FASTRTPS_DEFAULT_PROFILES_FILE", - value=localhost_only_fastrtps_profiles_file, + value=fastrtps_profiles, ), ] ) @@ -88,31 +87,22 @@ def generate_launch_description(): "serial_baudrate", default_value="576000", description="Baud rate for serial communication" ) - # Locate the rosbot_bringup package - package_dir = FindPackageShare("rosbot_bringup").find("rosbot_bringup") - - # Construct the path to the XML file within the package - fastrtps_profiles_file = os.path.join(package_dir, "config", "microros_localhost_only.xml") - - declare_localhost_only_fastrtps_profiles_file_arg = DeclareLaunchArgument( - "localhost_only_fastrtps_profiles_file", + fastrtps_profiles_file = PathJoinSubstitution( + [FindPackageShare("rosbot_bringup"), "config", "microros_localhost_only.xml"] + ) + declare_fastrtps_profiles_arg = DeclareLaunchArgument( + "fastrtps_profiles", default_value=fastrtps_profiles_file, description=( - "Path to the Fast RTPS default profiles file for Micro-ROS agent for localhost only" - " setup" + "Path to the Fast RTPS default profiles file for Micro-ROS agent for localhost only setup" ), ) - bringup_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/bringup.launch.py"]) - ) - return LaunchDescription( [ declare_serial_port_arg, declare_serial_baudrate_arg, - declare_localhost_only_fastrtps_profiles_file_arg, + declare_fastrtps_profiles_arg, OpaqueFunction(function=generate_microros_agent_node), - bringup_launch, ] ) diff --git a/rosbot_bringup/package.xml b/rosbot_bringup/package.xml index d88edc1b..2be81752 100644 --- a/rosbot_bringup/package.xml +++ b/rosbot_bringup/package.xml @@ -4,7 +4,9 @@ rosbot_bringup 0.13.2 ROSbot 2, 2R, PRO bringup package + Husarion + Apache License 2.0 https://husarion.com/ @@ -14,21 +16,21 @@ Jakub Delicat Rafal Gorecki + laser_filters launch launch_ros - rosbot_controller - robot_localization micro_ros_agent micro_ros_msgs + robot_localization + rosbot_controller - python3-pytest launch - launch_ros launch_pytest - + launch_ros + nav_msgs + python3-pytest robot_localization rosbot_controller - nav_msgs ament_python diff --git a/rosbot_bringup/test/test_diff_drive_ekf.py b/rosbot_bringup/test/test_diff_drive.py similarity index 84% rename from rosbot_bringup/test/test_diff_drive_ekf.py rename to rosbot_bringup/test/test_diff_drive.py index 5764616a..8c25a50c 100644 --- a/rosbot_bringup/test/test_diff_drive_ekf.py +++ b/rosbot_bringup/test/test_diff_drive.py @@ -22,7 +22,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import PathJoinSubstitution from launch_ros.substitutions import FindPackageShare -from test_utils import BringupTestNode +from test_utils import BringupTestNode, readings_data_test @launch_pytest.fixture @@ -39,9 +39,7 @@ def generate_test_description(): ) ), launch_arguments={ - "use_sim": "False", - "mecanum": "False", - "use_gpu": "False", + "microros": "False", }.items(), ) @@ -57,10 +55,7 @@ def test_bringup_startup_success(): node.start_publishing_fake_hardware() node.start_node_thread() - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected odometry/filtered message but it was not received. Check robot_localization!" + readings_data_test(node) finally: rclpy.shutdown() diff --git a/rosbot_bringup/test/test_flake8.py b/rosbot_bringup/test/test_flake8.py deleted file mode 100644 index 22fffcb8..00000000 --- a/rosbot_bringup/test/test_flake8.py +++ /dev/null @@ -1,23 +0,0 @@ -# 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. - -import pytest -from ament_flake8.main import main_with_errors - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors) diff --git a/rosbot_bringup/test/test_mecanum_ekf.py b/rosbot_bringup/test/test_mecanum.py similarity index 85% rename from rosbot_bringup/test/test_mecanum_ekf.py rename to rosbot_bringup/test/test_mecanum.py index 066641da..3871dd6f 100644 --- a/rosbot_bringup/test/test_mecanum_ekf.py +++ b/rosbot_bringup/test/test_mecanum.py @@ -22,7 +22,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import PathJoinSubstitution from launch_ros.substitutions import FindPackageShare -from test_utils import BringupTestNode +from test_utils import BringupTestNode, readings_data_test @launch_pytest.fixture @@ -39,9 +39,8 @@ def generate_test_description(): ) ), launch_arguments={ - "use_sim": "False", "mecanum": "True", - "use_gpu": "False", + "microros": "False", }.items(), ) @@ -57,10 +56,7 @@ def test_bringup_startup_success(): node.start_publishing_fake_hardware() node.start_node_thread() - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected odometry/filtered message but it was not received. Check robot_localization!" + readings_data_test(node) finally: rclpy.shutdown() diff --git a/rosbot_bringup/test/test_multirobot_ekf.py b/rosbot_bringup/test/test_multirobot.py similarity index 78% rename from rosbot_bringup/test/test_multirobot_ekf.py rename to rosbot_bringup/test/test_multirobot.py index 8086fb3a..e400c1b5 100644 --- a/rosbot_bringup/test/test_multirobot_ekf.py +++ b/rosbot_bringup/test/test_multirobot.py @@ -18,13 +18,13 @@ import pytest import rclpy from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription +from launch.actions import IncludeLaunchDescription, TimerAction from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import PathJoinSubstitution from launch_ros.substitutions import FindPackageShare -from test_utils import BringupTestNode +from test_utils import BringupTestNode, readings_data_test -robot_names = ["robot1", "robot2", "robot3"] +robot_names = ["robot1", "robot2"] @launch_pytest.fixture @@ -43,20 +43,20 @@ def generate_test_description(): ) ), launch_arguments={ - "use_sim": "False", - "mecanum": "False", - "use_gpu": "False", "namespace": robot_names[i], + "microros": "False", }.items(), ) - actions.append(bringup_launch) + delayed_bringup = TimerAction(period=5.0 * i, actions=[bringup_launch]) + actions.append(delayed_bringup) return LaunchDescription(actions) @pytest.mark.launch(fixture=generate_test_description) def test_multirobot_bringup_startup_success(): + for robot_name in robot_names: rclpy.init() try: @@ -65,11 +65,7 @@ def test_multirobot_bringup_startup_success(): node.start_publishing_fake_hardware() node.start_node_thread() - msgs_received_flag = node.odom_msg_event.wait(timeout=20.0) - assert msgs_received_flag, ( - f"Expected {robot_name}/odometry/filtered message but it was not received. " - "Check robot_localization!" - ) + readings_data_test(node, robot_name) finally: rclpy.shutdown() diff --git a/rosbot_bringup/test/test_namespaced_diff_drive_ekf.py b/rosbot_bringup/test/test_namespaced_diff_drive.py similarity index 82% rename from rosbot_bringup/test/test_namespaced_diff_drive_ekf.py rename to rosbot_bringup/test/test_namespaced_diff_drive.py index 3bb6ff72..4c5f9786 100644 --- a/rosbot_bringup/test/test_namespaced_diff_drive_ekf.py +++ b/rosbot_bringup/test/test_namespaced_diff_drive.py @@ -22,7 +22,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import PathJoinSubstitution from launch_ros.substitutions import FindPackageShare -from test_utils import BringupTestNode +from test_utils import BringupTestNode, readings_data_test @launch_pytest.fixture @@ -39,10 +39,8 @@ def generate_test_description(): ) ), launch_arguments={ - "use_sim": "False", - "mecanum": "False", - "use_gpu": "False", - "namespace": "rosbot2r", + "namespace": "rosbot", + "microros": "False", }.items(), ) @@ -53,15 +51,12 @@ def generate_test_description(): def test_namespaced_bringup_startup_success(): rclpy.init() try: - node = BringupTestNode("test_bringup", namespace="rosbot2r") + node = BringupTestNode("test_bringup", namespace="rosbot") node.create_test_subscribers_and_publishers() node.start_publishing_fake_hardware() node.start_node_thread() - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected odometry/filtered message but it was not received. Check robot_localization!" + readings_data_test(node) finally: rclpy.shutdown() diff --git a/rosbot_bringup/test/test_namespaced_mecanum_ekf.py b/rosbot_bringup/test/test_namespaced_mecanum.py similarity index 84% rename from rosbot_bringup/test/test_namespaced_mecanum_ekf.py rename to rosbot_bringup/test/test_namespaced_mecanum.py index 0beb2aa1..32d6cd00 100644 --- a/rosbot_bringup/test/test_namespaced_mecanum_ekf.py +++ b/rosbot_bringup/test/test_namespaced_mecanum.py @@ -22,7 +22,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import PathJoinSubstitution from launch_ros.substitutions import FindPackageShare -from test_utils import BringupTestNode +from test_utils import BringupTestNode, readings_data_test @launch_pytest.fixture @@ -39,10 +39,9 @@ def generate_test_description(): ) ), launch_arguments={ - "use_sim": "False", "mecanum": "True", - "use_gpu": "False", - "namespace": "rosbot2r", + "namespace": "rosbot", + "microros": "False", }.items(), ) @@ -53,15 +52,12 @@ def generate_test_description(): def test_namespaced_bringup_startup_success(): rclpy.init() try: - node = BringupTestNode("test_bringup", namespace="rosbot2r") + node = BringupTestNode("test_bringup", namespace="rosbot") node.create_test_subscribers_and_publishers() node.start_publishing_fake_hardware() node.start_node_thread() - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected odometry/filtered message but it was not received. Check robot_localization!" + readings_data_test(node) finally: rclpy.shutdown() diff --git a/rosbot_bringup/test/test_utils.py b/rosbot_bringup/test/test_utils.py index 3d46fdff..90ae455b 100644 --- a/rosbot_bringup/test/test_utils.py +++ b/rosbot_bringup/test/test_utils.py @@ -13,12 +13,15 @@ # See the License for the specific language governing permissions and # limitations under the License. +import math +import random +import time from threading import Event, Thread import rclpy from nav_msgs.msg import Odometry from rclpy.node import Node -from sensor_msgs.msg import Imu, JointState +from sensor_msgs.msg import Imu, JointState, LaserScan class BringupTestNode(Node): @@ -28,33 +31,65 @@ class BringupTestNode(Node): def __init__(self, name="test_node", namespace=None): super().__init__(name, namespace=namespace) - self.odom_msg_event = Event() + self.joint_state_msg_event = Event() + self.controller_odom_msg_event = Event() + self.imu_msg_event = Event() + self.ekf_odom_msg_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.ros_spin_thread = None + self.timer = None - self.joint_states_publisher = self.create_publisher(JointState, "/_motors_response", 10) + def create_test_subscribers_and_publishers(self): + self.imu_pub = self.create_publisher(Imu, "/_imu/data_raw", 10) + self.joint_pub = self.create_publisher(JointState, "/_motors_response", 10) + self.scan_pub = self.create_publisher(LaserScan, "scan", 10) - self.odom_sub = self.create_subscription( - Odometry, "odometry/filtered", self.odometry_callback, 10 + self.joint_state_sub = self.create_subscription( + JointState, "joint_states", self.joint_states_callback, 10 + ) + self.controller_odom_sub = self.create_subscription( + Odometry, "rosbot_base_controller/odom", self.controller_odometry_callback, 10 + ) + self.imu_sub = self.create_subscription(Imu, "imu_broadcaster/imu", self.imu_callback, 10) + self.ekf_odom_sub = self.create_subscription( + Odometry, "odometry/filtered", self.ekf_odometry_callback, 10 + ) + self.scan_filtered_sub = self.create_subscription( + LaserScan, "scan_filtered", self.filtered_scan_callback, 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() + if not self.ros_spin_thread: + self.ros_spin_thread = Thread(target=rclpy.spin, args=(self,), daemon=True) + 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, - ) + if not self.timer: + self.timer = self.create_timer( + 1.0 / self.ROSBOT_HARDWARE_PUBLISHERS_RATE, + self.timer_callback, + ) def timer_callback(self): self.publish_fake_hardware_messages() + self.publish_scan() + + def joint_states_callback(self, msg: JointState): + self.joint_state_msg_event.set() + + def controller_odometry_callback(self, msg: Odometry): + self.controller_odom_msg_event.set() + + def imu_callback(self, msg: Imu): + self.imu_msg_event.set() - def odometry_callback(self, data): - self.odom_msg_event.set() + def ekf_odometry_callback(self, msg: Odometry): + self.ekf_odom_msg_event.set() + + def filtered_scan_callback(self, msg: LaserScan): + if len(msg.ranges) > 0: + self.scan_filter_event.set() def publish_fake_hardware_messages(self): imu_msg = Imu() @@ -72,5 +107,58 @@ def publish_fake_hardware_messages(self): joint_state_msg.position = [0.0, 0.0, 0.0, 0.0] joint_state_msg.velocity = [0.0, 0.0, 0.0, 0.0] - self.imu_publisher.publish(imu_msg) - self.joint_states_publisher.publish(joint_state_msg) + self.imu_pub.publish(imu_msg) + self.joint_pub.publish(joint_state_msg) + + def publish_scan(self): + msg = LaserScan() + msg.header.frame_id = "laser" + 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_pub.publish(msg) + + +def wait_for_all_events(events, timeout): + start_time = time.time() + while time.time() - start_time < timeout: + if all(event.is_set() for event in events): + return True, [] + time.sleep(0.1) + + not_set_events = [i for i, event in enumerate(events) if not event.is_set()] + return False, not_set_events + + +def readings_data_test(node, robot_name="ROSbot"): + events = [ + node.joint_state_msg_event, + node.controller_odom_msg_event, + node.imu_msg_event, + node.ekf_odom_msg_event, + ] + + event_names = [ + "JointStates", + "Controller Odometry", + "IMU", + "EKF Odometry", + ] + + msgs_received_flag, not_set_indices = wait_for_all_events(events, timeout=20.0) + + if not msgs_received_flag: + not_set_event_names = [event_names[i] for i in not_set_indices] + missing_events = ", ".join(not_set_event_names) + raise AssertionError( + f"{robot_name}: Not all expected messages were received. Missing: {missing_events}." + ) + + print(f"{robot_name}: All messages received successfully.") diff --git a/rosbot_controller/config/diff_drive_controller.yaml b/rosbot_controller/config/diff_drive_controller.yaml index 304a0d77..ea7f4b7a 100644 --- a/rosbot_controller/config/diff_drive_controller.yaml +++ b/rosbot_controller/config/diff_drive_controller.yaml @@ -12,9 +12,7 @@ /**/imu_broadcaster: ros__parameters: - tf_frame_prefix_enable: false - use_namespace_as_sensor_name_prefix: true - + tf_frame_prefix_enable: true sensor_name: imu frame_id: imu_link static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet @@ -23,7 +21,7 @@ /**/rosbot_base_controller: ros__parameters: - tf_frame_prefix_enable: false + tf_frame_prefix_enable: true left_wheel_names: [fl_wheel_joint, rl_wheel_joint] right_wheel_names: [fr_wheel_joint, rr_wheel_joint] @@ -34,7 +32,7 @@ # For skid drive kinematics it will act as ICR coefficient # Kinematic model with ICR coefficient isn't totally accurate and this coefficient can differ # for various ground types, but checking on our office floor 1.3 looked approximately alright - wheel_separation_multiplier: 1.45 + wheel_separation_multiplier: 1.36 left_wheel_radius_multiplier: 1.0 right_wheel_radius_multiplier: 1.0 diff --git a/rosbot_controller/config/mecanum_drive_controller.yaml b/rosbot_controller/config/mecanum_drive_controller.yaml index e951cc04..218925f2 100644 --- a/rosbot_controller/config/mecanum_drive_controller.yaml +++ b/rosbot_controller/config/mecanum_drive_controller.yaml @@ -12,8 +12,7 @@ /**/imu_broadcaster: ros__parameters: - tf_frame_prefix_enable: false - use_namespace_as_sensor_name_prefix: true + tf_frame_prefix_enable: true sensor_name: imu frame_id: imu_link @@ -23,7 +22,7 @@ /**/rosbot_base_controller: ros__parameters: - tf_frame_prefix_enable: false + tf_frame_prefix_enable: true front_left_wheel_name: fl_wheel_joint front_right_wheel_name: fr_wheel_joint diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index 31eae850..1afe7919 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -13,10 +13,16 @@ # See the License for the specific language governing permissions and # limitations under the License. -# Import necessary modules from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, TimerAction +from launch.actions import ( + DeclareLaunchArgument, + EmitEvent, + RegisterEventHandler, + TimerAction, +) from launch.conditions import UnlessCondition +from launch.event_handlers import OnProcessIO +from launch.events import Shutdown from launch.substitutions import ( Command, FindExecutable, @@ -24,68 +30,27 @@ PathJoinSubstitution, PythonExpression, ) -from launch_ros.actions import Node, SetParameter +from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -# from nav2_common.launch import ReplaceString + def generate_launch_description(): - # Declare launch arguments namespace = LaunchConfiguration("namespace") + mecanum = LaunchConfiguration("mecanum") + use_sim = LaunchConfiguration("use_sim", default="False") + declare_namespace_arg = DeclareLaunchArgument( "namespace", default_value="", - description="Namespace for all topics and TFs", + description="Adds a namespace to all running nodes.", ) - mecanum = LaunchConfiguration("mecanum") declare_mecanum_arg = DeclareLaunchArgument( "mecanum", default_value="False", - description=( - "Whether to use mecanum drive controller (otherwise diff drive controller is used)" - ), - ) - - use_sim = LaunchConfiguration("use_sim") - declare_use_sim_arg = DeclareLaunchArgument( - "use_sim", - default_value="False", - description="Whether simulation is used", - ) - - use_gpu = LaunchConfiguration("use_gpu") - declare_use_gpu_arg = DeclareLaunchArgument( - "use_gpu", - default_value="False", - description="Whether GPU acceleration is used", - ) - - simulation_engine = LaunchConfiguration("simulation_engine") - declare_simulation_engine_arg = DeclareLaunchArgument( - "simulation_engine", - default_value="webots", - description="Which simulation engine to be used", - choices=["ignition-gazebo", "gazebo-classic", "webots"], - ) - - controller_config_name = PythonExpression( - [ - "'mecanum_drive_controller.yaml' if ", - mecanum, - " else 'diff_drive_controller.yaml'", - ] - ) - - namespace_ext = PythonExpression( - ["''", " if '", namespace, "' == '' ", "else ", "'", namespace, "/'"] - ) - - controller_manager_name = LaunchConfiguration( - "controller_manager_name", - default=[namespace_ext, "controller_manager"], + description="Whether to use mecanum drive controller (otherwise diff drive controller is used)", ) - # Get URDF via xacro robot_description_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), @@ -99,23 +64,19 @@ def generate_launch_description(): ), " mecanum:=", mecanum, - " use_sim:=", - use_sim, - " use_gpu:=", - use_gpu, - " simulation_engine:=", - simulation_engine, " namespace:=", namespace, - # Uncomment the line below if you need to include the 'use_ros2_control' parameter - # " use_ros2_control:=True", + " use_sim:=", + use_sim, ] ) robot_description = {"robot_description": robot_description_content} - # Controller configurations - # robot_controllers_config = PathJoinSubstitution( - robot_controllers = PathJoinSubstitution( + controller_config_name = PythonExpression( + ["'mecanum_drive_controller.yaml' if ", mecanum, " else 'diff_drive_controller.yaml'"] + ) + + controllers_config_file = PathJoinSubstitution( [ FindPackageShare("rosbot_controller"), "config", @@ -123,55 +84,43 @@ def generate_launch_description(): ] ) - # namespaced_robot_controllers_config = ReplaceString( - # source_file=robot_controllers_config, - # replacements={"": namespace, "//": "/"}, - # ) - - # Define nodes control_node = Node( package="controller_manager", executable="ros2_control_node", - parameters=[ - robot_description, - # namespaced_robot_controllers_config, - robot_controllers, - ], + parameters=[robot_description, controllers_config_file], remappings=[ ("imu_sensor_node/imu", "/_imu/data_raw"), ("~/motors_cmd", "/_motors_cmd"), ("~/motors_response", "/_motors_response"), ("rosbot_base_controller/cmd_vel", "cmd_vel"), - ("/tf", "tf"), - ("/tf_static", "tf_static"), ], condition=UnlessCondition(use_sim), namespace=namespace, - respawn=True, - respawn_delay=2.0, ) + namespace_ext = PythonExpression(["'", namespace, "' + '/' if '", namespace, "' else ''"]) + robot_state_pub_node = Node( package="robot_state_publisher", executable="robot_state_publisher", - parameters=[robot_description], - remappings=[("/tf", "tf"), ("/tf_static", "tf_static")], + parameters=[ + {"robot_description": robot_description_content}, + {"frame_prefix": namespace_ext}, + ], namespace=namespace, ) - # Create spawner nodes joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", arguments=[ "joint_state_broadcaster", "--controller-manager", - controller_manager_name, + "controller_manager", "--controller-manager-timeout", "10", - # "--namespace", - # namespace, ], + namespace=namespace, ) robot_controller_spawner = Node( @@ -180,12 +129,11 @@ def generate_launch_description(): arguments=[ "rosbot_base_controller", "--controller-manager", - controller_manager_name, + "controller_manager", "--controller-manager-timeout", "10", - # "--namespace", - # namespace, ], + namespace=namespace, ) imu_broadcaster_spawner = Node( @@ -194,38 +142,58 @@ def generate_launch_description(): arguments=[ "imu_broadcaster", "--controller-manager", - controller_manager_name, + "controller_manager", "--controller-manager-timeout", "10", - # "--namespace", - # namespace, ], + namespace=namespace, ) - # Wrap the spawner nodes in a TimerAction to delay execution by 2 seconds + # spawners expect ros2_control_node to be running delayed_spawner_nodes = TimerAction( - period=1.0, + period=3.0, actions=[ - control_node, joint_state_broadcaster_spawner, robot_controller_spawner, imu_broadcaster_spawner, ], ) - # Set 'use_sim_time' parameter - # set_use_sim_time = SetParameter('use_sim_time', value=use_sim)fr + def check_if_log_is_fatal(event): + red_color = "\033[91m" + reset_color = "\033[0m" + if "fatal" in event.text.decode().lower() or "failed" in event.text.decode().lower(): + print(f"{red_color}Fatal error: {event.text}. Emitting shutdown...{reset_color}") + return EmitEvent(event=Shutdown(reason="Spawner failed")) + + joint_state_monitor = RegisterEventHandler( + OnProcessIO( + target_action=joint_state_broadcaster_spawner, + on_stderr=lambda event: check_if_log_is_fatal(event), + ) + ) + robot_controller_monitor = RegisterEventHandler( + OnProcessIO( + target_action=robot_controller_spawner, + on_stderr=lambda event: check_if_log_is_fatal(event), + ) + ) + imu_broadcaster_monitor = RegisterEventHandler( + OnProcessIO( + target_action=imu_broadcaster_spawner, + on_stderr=lambda event: check_if_log_is_fatal(event), + ) + ) - # Assemble the LaunchDescription return LaunchDescription( [ declare_namespace_arg, declare_mecanum_arg, - declare_use_sim_arg, - declare_use_gpu_arg, - declare_simulation_engine_arg, - SetParameter("use_sim_time", value=use_sim), + control_node, robot_state_pub_node, - delayed_spawner_nodes, # Add the delayed spawner nodes here + delayed_spawner_nodes, + joint_state_monitor, + robot_controller_monitor, + imu_broadcaster_monitor, ] ) diff --git a/rosbot_controller/package.xml b/rosbot_controller/package.xml index d8f8cd81..798d310a 100644 --- a/rosbot_controller/package.xml +++ b/rosbot_controller/package.xml @@ -4,7 +4,9 @@ rosbot_controller 0.13.2 Hardware configuration for ROSbot 2, 2R, PRO + Husarion + Apache License 2.0 https://husarion.com/ @@ -15,40 +17,24 @@ ament_cmake + controller_manager + diff_drive_controller + imu_sensor_broadcaster + joint_state_broadcaster launch launch_ros + mecanum_drive_controller nav2_common - - xacro - controller_manager robot_state_publisher - - rosbot_description ros_components_description + rosbot_description rosbot_hardware_interfaces - - joint_state_broadcaster - imu_sensor_broadcaster - diff_drive_controller - mecanum_drive_controller + xacro python3-pytest - launch - launch_ros - launch_pytest - - xacro - sensor_msgs - nav_msgs - - rosbot_description ros_components_description - - controller_manager - joint_state_broadcaster - imu_sensor_broadcaster - diff_drive_controller - mecanum_drive_controller + rosbot_description + xacro ament_python diff --git a/rosbot_controller/test/test_diff_drive_controllers.py b/rosbot_controller/test/test_diff_drive_controllers.py deleted file mode 100644 index 5ca62c6a..00000000 --- a/rosbot_controller/test/test_diff_drive_controllers.py +++ /dev/null @@ -1,89 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2024 Husarion sp. z o.o. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import launch_pytest -import pytest -import rclpy -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare -from test_utils import ControllersTestNode, controller_readings_test - - -@launch_pytest.fixture -def generate_test_description(): - rosbot_controller = FindPackageShare("rosbot_controller") - bringup_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - rosbot_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() - controller_readings_test(node) - finally: - rclpy.shutdown() diff --git a/rosbot_controller/test/test_flake8.py b/rosbot_controller/test/test_flake8.py deleted file mode 100644 index 22fffcb8..00000000 --- a/rosbot_controller/test/test_flake8.py +++ /dev/null @@ -1,23 +0,0 @@ -# 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. - -import pytest -from ament_flake8.main import main_with_errors - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors) diff --git a/rosbot_controller/test/test_mecanum_controllers.py b/rosbot_controller/test/test_mecanum_controllers.py deleted file mode 100644 index 8e92ac6f..00000000 --- a/rosbot_controller/test/test_mecanum_controllers.py +++ /dev/null @@ -1,89 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2024 Husarion sp. z o.o. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import launch_pytest -import pytest -import rclpy -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare -from test_utils import ControllersTestNode, controller_readings_test - - -@launch_pytest.fixture -def generate_test_description(): - rosbot_controller = FindPackageShare("rosbot_controller") - bringup_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - rosbot_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() - controller_readings_test(node) - finally: - rclpy.shutdown() diff --git a/rosbot_controller/test/test_multirobot_controllers.py b/rosbot_controller/test/test_multirobot_controllers.py deleted file mode 100644 index 438804e8..00000000 --- a/rosbot_controller/test/test_multirobot_controllers.py +++ /dev/null @@ -1,71 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2024 Husarion sp. z o.o. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import launch_pytest -import pytest -import rclpy -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, TimerAction -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare -from test_utils import ControllersTestNode, controller_readings_test - -robot_names = ["robot1", "robot2", "robot3"] - - -@launch_pytest.fixture -def generate_test_description(): - rosbot_controller = FindPackageShare("rosbot_controller") - actions = [] - for i in range(len(robot_names)): - controller_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - rosbot_controller, - "launch", - "controller.launch.py", - ] - ) - ), - launch_arguments={ - "use_sim": "False", - "mecanum": "True", - "namespace": robot_names[i], - }.items(), - ) - - # When there is no delay the controllers doesn't spawn correctly - delayed_controller_launch = TimerAction(period=i * 2.0, actions=[controller_launch]) - actions.append(delayed_controller_launch) - - return LaunchDescription(actions) - - -@pytest.mark.launch(fixture=generate_test_description) -def test_multirobot_controllers_startup_success(): - for robot_name in robot_names: - rclpy.init() - try: - node = ControllersTestNode(f"test_{robot_name}_controllers", namespace=robot_name) - node.create_test_subscribers_and_publishers() - node.start_publishing_fake_hardware() - - node.start_node_thread() - controller_readings_test(node, robot_name) - finally: - rclpy.shutdown() diff --git a/rosbot_controller/test/test_namespaced_diff_drive_controllers.py b/rosbot_controller/test/test_namespaced_diff_drive_controllers.py deleted file mode 100644 index 97d87bfe..00000000 --- a/rosbot_controller/test/test_namespaced_diff_drive_controllers.py +++ /dev/null @@ -1,90 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2024 Husarion sp. z o.o. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import launch_pytest -import pytest -import rclpy -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare -from test_utils import ControllersTestNode, controller_readings_test - - -@launch_pytest.fixture -def generate_test_description(): - rosbot_controller = FindPackageShare("rosbot_controller") - bringup_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - rosbot_controller, - "launch", - "controller.launch.py", - ] - ) - ), - launch_arguments={ - "use_sim": "False", - "mecanum": "False", - "use_gpu": "False", - "namespace": "rosbot2r", - }.items(), - ) - - return LaunchDescription([bringup_launch]) - - -@pytest.mark.launch(fixture=generate_test_description) -def test_namespaced_controllers_startup_fail(): - rclpy.init() - try: - node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") - node.create_test_subscribers_and_publishers() - - node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received JointStates message that should not have appeared. Check whether other" - " robots are connected to your network.!" - ) - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received Odom message that should not have appeared. Check whether other robots are" - " connected to your network.!" - ) - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received Imu message that should not have appeared. Check whether other robots are" - " connected to your network.!" - ) - finally: - rclpy.shutdown() - - -@pytest.mark.launch(fixture=generate_test_description) -def test_namespaced_controllers_startup_success(): - rclpy.init() - try: - node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") - node.create_test_subscribers_and_publishers() - node.start_publishing_fake_hardware() - - node.start_node_thread() - controller_readings_test(node) - finally: - rclpy.shutdown() diff --git a/rosbot_controller/test/test_namespaced_mecanum_controllers.py b/rosbot_controller/test/test_namespaced_mecanum_controllers.py deleted file mode 100644 index 1d38cce4..00000000 --- a/rosbot_controller/test/test_namespaced_mecanum_controllers.py +++ /dev/null @@ -1,90 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2024 Husarion sp. z o.o. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import launch_pytest -import pytest -import rclpy -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare -from test_utils import ControllersTestNode, controller_readings_test - - -@launch_pytest.fixture -def generate_test_description(): - rosbot_controller = FindPackageShare("rosbot_controller") - bringup_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - rosbot_controller, - "launch", - "controller.launch.py", - ] - ) - ), - launch_arguments={ - "use_sim": "False", - "mecanum": "True", - "use_gpu": "False", - "namespace": "rosbot2r", - }.items(), - ) - - return LaunchDescription([bringup_launch]) - - -@pytest.mark.launch(fixture=generate_test_description) -def test_namespaced_controllers_startup_fail(): - rclpy.init() - try: - node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") - node.create_test_subscribers_and_publishers() - - node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received JointStates message that should not have appeared. Check whether other" - " robots are connected to your network.!" - ) - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received Odom message that should not have appeared. Check whether other robots are" - " connected to your network.!" - ) - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received Imu message that should not have appeared. Check whether other robots are" - " connected to your network.!" - ) - finally: - rclpy.shutdown() - - -@pytest.mark.launch(fixture=generate_test_description) -def test_namespaced_controllers_startup_success(): - rclpy.init() - try: - node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") - node.create_test_subscribers_and_publishers() - node.start_publishing_fake_hardware() - - node.start_node_thread() - controller_readings_test(node) - finally: - rclpy.shutdown() diff --git a/rosbot_controller/test/test_utils.py b/rosbot_controller/test/test_utils.py deleted file mode 100644 index e20cdff6..00000000 --- a/rosbot_controller/test/test_utils.py +++ /dev/null @@ -1,107 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# Copyright 2024 Husarion sp. z o.o. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from threading import Event, Thread - -import rclpy -from nav_msgs.msg import Odometry -from rclpy.node import Node -from sensor_msgs.msg import Imu, JointState - - -class ControllersTestNode(Node): - ROSBOT_HARDWARE_PUBLISHERS_RATE = 10.0 - - __test__ = False - - def __init__(self, name="test_node", namespace=None): - super().__init__(name, namespace=namespace) - self.joint_state_msg_event = Event() - self.odom_msg_event = Event() - self.imu_msg_event = Event() - - def create_test_subscribers_and_publishers(self): - self.joint_state_sub = self.create_subscription( - JointState, "joint_states", self.joint_states_callback, 10 - ) - - self.odom_sub = self.create_subscription( - Odometry, "rosbot_base_controller/odom", self.odometry_callback, 10 - ) - - self.imu_sub = self.create_subscription(Imu, "imu_broadcaster/imu", self.imu_callback, 10) - - # TODO: @delihus namespaces have not been implemented in microros yet - self.imu_pub = self.create_publisher(Imu, "/_imu/data_raw", 10) - - self.joint_pub = self.create_publisher(JointState, "/_motors_response", 10) - - self.timer = None - - def start_node_thread(self): - self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,)) - self.ros_spin_thread.start() - - def joint_states_callback(self, data): - self.joint_state_msg_event.set() - - 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, - ) - - # TODO: @delihus namespaces have not been implemented in microros yet - def publish_fake_hardware_messages(self): - imu_msg = Imu() - imu_msg.header.stamp = self.get_clock().now().to_msg() - 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_pub.publish(imu_msg) - self.joint_pub.publish(joint_state_msg) - - -def controller_readings_test(node, robot_name="ROSbot"): - msgs_received_flag = node.joint_state_msg_event.wait(10.0) - assert msgs_received_flag, ( - f"{robot_name}: Expected JointStates message but it was not received. Check " - "joint_state_broadcaster!" - ) - msgs_received_flag = node.odom_msg_event.wait(10.0) - assert msgs_received_flag, ( - f"{robot_name}: Expected Odom message but it was not received. Check " - "rosbot_base_controller!" - ) - msgs_received_flag = node.imu_msg_event.wait(10.0) - assert ( - msgs_received_flag - ), f"{robot_name}: Expected Imu message but it was not received. Check imu_broadcaster!" diff --git a/rosbot_controller/test/test_xacro.py b/rosbot_controller/test/test_xacro.py index 933acb13..7e06d41e 100644 --- a/rosbot_controller/test/test_xacro.py +++ b/rosbot_controller/test/test_xacro.py @@ -22,27 +22,21 @@ 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' use_multirobot_system_values = ["true", "false"] all_combinations = list( itertools.product( mecanum_values, use_sim_values, - use_gpu_values, - simulation_engine_values, use_multirobot_system_values, ) ) for combination in all_combinations: - mecanum, use_sim, use_gpu, simulation_engine, use_multirobot_system = combination + mecanum, use_sim, use_multirobot_system = combination mappings = { "mecanum": mecanum, "use_sim": use_sim, - "use_gpu": use_gpu, - "simulation_engine": simulation_engine, "use_multirobot_system": use_multirobot_system, } rosbot_description = get_package_share_directory("rosbot_description") @@ -51,6 +45,5 @@ def test_rosbot_description_parsing(): xacro.process_file(xacro_path, mappings=mappings) except xacro.XacroException as e: assert False, ( - f"xacro parsing failed: {str(e)} for mecanum: {mecanum}, use_sim:" - f" {use_sim}, use_gpu: {use_gpu}, simulation_engine: {simulation_engine}" + f"xacro parsing failed: {str(e)} for mecanum: {mecanum}, use_sim:" f" {use_sim}" ) diff --git a/rosbot_description/CMakeLists.txt b/rosbot_description/CMakeLists.txt index 0a3c3a12..52883316 100644 --- a/rosbot_description/CMakeLists.txt +++ b/rosbot_description/CMakeLists.txt @@ -3,11 +3,7 @@ project(rosbot_description) find_package(ament_cmake REQUIRED) -install(DIRECTORY - meshes - urdf - DESTINATION share/${PROJECT_NAME} -) +install(DIRECTORY meshes urdf DESTINATION share/${PROJECT_NAME}) -ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.sh.in") +ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/setup_envs.sh.in") ament_package() diff --git a/rosbot_description/env-hooks/rosbot_description.sh.in b/rosbot_description/hooks/setup_envs.sh.in similarity index 65% rename from rosbot_description/env-hooks/rosbot_description.sh.in rename to rosbot_description/hooks/setup_envs.sh.in index 45d23a05..9b552599 100755 --- a/rosbot_description/env-hooks/rosbot_description.sh.in +++ b/rosbot_description/hooks/setup_envs.sh.in @@ -1,3 +1,2 @@ ament_prepend_unique_value GAZEBO_MODEL_PATH "@CMAKE_INSTALL_PREFIX@/share" ament_prepend_unique_value GZ_SIM_RESOURCE_PATH "@CMAKE_INSTALL_PREFIX@/share" -ament_prepend_unique_value IGN_GAZEBO_RESOURCE_PATH "@CMAKE_INSTALL_PREFIX@/share" diff --git a/rosbot_description/package.xml b/rosbot_description/package.xml index 64f8f4f3..b79d3f47 100644 --- a/rosbot_description/package.xml +++ b/rosbot_description/package.xml @@ -4,7 +4,9 @@ rosbot_description 0.13.2 ROSbot 2, 2R, PRO description package + Husarion + Apache License 2.0 https://husarion.com/ @@ -16,6 +18,8 @@ ament_cmake + depthai_descriptions + ament_cmake diff --git a/rosbot_description/urdf/body.urdf.xacro b/rosbot_description/urdf/body.urdf.xacro index 647087b0..d6c4631a 100644 --- a/rosbot_description/urdf/body.urdf.xacro +++ b/rosbot_description/urdf/body.urdf.xacro @@ -1,7 +1,7 @@ - + @@ -18,8 +18,8 @@ - - + + @@ -39,7 +39,7 @@ - Gazebo/White + Gazebo/Black @@ -74,10 +74,10 @@ - + - + diff --git a/rosbot_description/urdf/components/vl53lox.urdf.xacro b/rosbot_description/urdf/components/vl53lox.urdf.xacro index 6518a943..ccb382e6 100644 --- a/rosbot_description/urdf/components/vl53lox.urdf.xacro +++ b/rosbot_description/urdf/components/vl53lox.urdf.xacro @@ -20,43 +20,36 @@ - - - + + - ${namespace_ext}range/${prefix} - ${prefix}_range - ${prefix}_range + ${namespace_ext}range/${prefix} + ${prefix}_range - 5.0 - - - - 1 - 1 - -0.01 - 0.01 - - - - 0.01 - 0.90 - 0.02 - - - gaussian - 0.1 - 0.005 - - - true - false - - - ${namespace_ext} - - - - + 5.0 + + + + 1 + 1 + -0.01 + 0.01 + + + + 0.01 + 0.90 + 0.02 + + + gaussian + 0.1 + 0.005 + + + true + false + + diff --git a/rosbot_description/urdf/rosbot.urdf.xacro b/rosbot_description/urdf/rosbot.urdf.xacro index eccac3db..d0bae083 100644 --- a/rosbot_description/urdf/rosbot.urdf.xacro +++ b/rosbot_description/urdf/rosbot.urdf.xacro @@ -2,34 +2,28 @@ - - - - - + + model="s2" + namespace="$(arg namespace)" /> - - + + diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index cc891462..122b03c6 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -1,27 +1,16 @@ - + - - - - - - - - - - - - - + + - + @@ -31,13 +20,13 @@ - + rosbot_hardware_interfaces/RosbotImuSensor - 120000 - 500 + 30000 + 1000 - + @@ -52,12 +41,12 @@ - + rosbot_hardware_interfaces/RosbotSystem - 120000 - 500 + 30000 + 1000 @@ -69,12 +58,7 @@ - - gz_ros2_control/GazeboSimSystem - - - webots_ros2_control::Ros2ControlSystem - + gz_ros2_control/GazeboSimSystem @@ -100,124 +84,54 @@ - - - - - - - - - - - - - - + + + + + + + + + + + + - - - - - - $(find rosbot_controller)/config/mecanum_drive_controller.yaml - - - $(find rosbot_controller)/config/diff_drive_controller.yaml - - - - - ${namespace} - - rosbot_base_controller/cmd_vel:=cmd_vel - /tf:=tf - - - - - - true - 25 - ${namespace_ext}imu/data_raw - false - false - imu_link - imu_link - - ${namespace_ext} - - - - + + - - - - - - - 10.0 - 2 - fl_wheel_joint - fr_wheel_joint - rl_wheel_joint - rr_wheel_joint - 0.192 - 0.192 - ${wheel_radius*2.0} - ${wheel_radius*2.0} - 1.5 - 1.0 - true - true - true - odom - base_link - - - - - - - fl_wheel_joint - fr_wheel_joint - rl_wheel_joint - rr_wheel_joint - - - - - - true - 25 - - - ~/out:=imu/data/raw - - - - - + + $(find rosbot_controller)/config/mecanum_drive_controller.yaml + + + $(find rosbot_controller)/config/diff_drive_controller.yaml + - - - - - true - /imu_broadcaster/imu - true - imu_link - imu gyro - imu accelerometer - imu inertial_unit - - - + + + ${namespace} + + rosbot_base_controller/cmd_vel:=cmd_vel + + + + + + true + 25 + imu/data_raw + false + false + imu_link + + ${namespace} + + + diff --git a/rosbot_description/urdf/wheel.urdf.xacro b/rosbot_description/urdf/wheel.urdf.xacro index 6f88e481..ba1e8a60 100644 --- a/rosbot_description/urdf/wheel.urdf.xacro +++ b/rosbot_description/urdf/wheel.urdf.xacro @@ -44,15 +44,15 @@ - - + + - - + + @@ -90,21 +90,39 @@ - - + + - 1.0 - 0.0 - ${fdir} + 1 + 1 + 0.035 + 0 + 0 0 1 - - + + + + + + + + 0.8 + 0.2 + 0.035 + 0 + ${fdir} + + + + + + diff --git a/rosbot_gazebo/config/gz_remappings.yaml b/rosbot_gazebo/config/gz_remappings.yaml index 550cc139..ab6feff0 100644 --- a/rosbot_gazebo/config/gz_remappings.yaml +++ b/rosbot_gazebo/config/gz_remappings.yaml @@ -1,51 +1,61 @@ --- - topic_name: /clock ros_type_name: rosgraph_msgs/msg/Clock - gz_type_name: ignition.msgs.Clock + gz_type_name: gz.msgs.Clock + direction: GZ_TO_ROS + + - topic_name: /cmd_vel + ros_type_name: geometry_msgs/msg/TwistStamped + gz_type_name: gz.msgs.Twist direction: GZ_TO_ROS - topic_name: /scan ros_type_name: sensor_msgs/msg/LaserScan - gz_type_name: ignition.msgs.LaserScan + gz_type_name: gz.msgs.LaserScan + direction: GZ_TO_ROS - topic_name: /camera/color/camera_info ros_type_name: sensor_msgs/msg/CameraInfo - gz_type_name: ignition.msgs.CameraInfo - lazy: true + gz_type_name: gz.msgs.CameraInfo + direction: GZ_TO_ROS - topic_name: /camera/color/image_raw ros_type_name: sensor_msgs/msg/Image - gz_type_name: ignition.msgs.Image - lazy: true + gz_type_name: gz.msgs.Image + direction: GZ_TO_ROS - topic_name: /camera/depth/camera_info ros_type_name: sensor_msgs/msg/CameraInfo - gz_type_name: ignition.msgs.CameraInfo - lazy: true + gz_type_name: gz.msgs.CameraInfo + direction: GZ_TO_ROS - topic_name: /camera/depth/image_raw ros_type_name: sensor_msgs/msg/Image - gz_type_name: ignition.msgs.Image - lazy: true + gz_type_name: gz.msgs.Image + direction: GZ_TO_ROS - ros_topic_name: /camera/depth/points gz_topic_name: /camera/depth/image_raw/points ros_type_name: sensor_msgs/msg/PointCloud2 - gz_type_name: ignition.msgs.PointCloudPacked - lazy: true + gz_type_name: gz.msgs.PointCloudPacked + direction: GZ_TO_ROS - topic_name: /range/fl ros_type_name: sensor_msgs/msg/LaserScan - gz_type_name: ignition.msgs.LaserScan + gz_type_name: gz.msgs.LaserScan + direction: GZ_TO_ROS - topic_name: /range/fr ros_type_name: sensor_msgs/msg/LaserScan - gz_type_name: ignition.msgs.LaserScan + gz_type_name: gz.msgs.LaserScan + direction: GZ_TO_ROS - topic_name: /range/rl ros_type_name: sensor_msgs/msg/LaserScan - gz_type_name: ignition.msgs.LaserScan + gz_type_name: gz.msgs.LaserScan + direction: GZ_TO_ROS - topic_name: /range/rr ros_type_name: sensor_msgs/msg/LaserScan - gz_type_name: ignition.msgs.LaserScan + gz_type_name: gz.msgs.LaserScan + direction: GZ_TO_ROS diff --git a/rosbot_gazebo/launch/simulation.launch.py b/rosbot_gazebo/launch/simulation.launch.py index ab6afe7e..50f01d4f 100644 --- a/rosbot_gazebo/launch/simulation.launch.py +++ b/rosbot_gazebo/launch/simulation.launch.py @@ -13,13 +13,7 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - GroupAction, - IncludeLaunchDescription, - LogInfo, - OpaqueFunction, -) +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import ( EnvironmentVariable, @@ -32,35 +26,37 @@ from nav2_common.launch import ParseMultiRobotPose -def launch_setup(context, *args, **kwargs): - namespace = LaunchConfiguration("namespace").perform(context) - mecanum = LaunchConfiguration("mecanum").perform(context) - world = LaunchConfiguration("world").perform(context) - headless = LaunchConfiguration("headless").perform(context) - use_gpu = LaunchConfiguration("use_gpu").perform(context) - x = LaunchConfiguration("x", default="0.0").perform(context) - y = LaunchConfiguration("y", default="2.0").perform(context) - z = LaunchConfiguration("z", default="0.0").perform(context) - roll = LaunchConfiguration("roll", default="0.0").perform(context) - pitch = LaunchConfiguration("pitch", default="0.0").perform(context) - yaw = LaunchConfiguration("yaw", default="0.0").perform(context) +def generate_launch_description(): + namespace = LaunchConfiguration("namespace") + x = LaunchConfiguration("x", default="-1.0") + y = LaunchConfiguration("y", default="-2.0") + z = LaunchConfiguration("z", default="0.0") + roll = LaunchConfiguration("roll", default="0.0") + pitch = LaunchConfiguration("pitch", default="0.0") + yaw = LaunchConfiguration("yaw", default="0.0") + + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), + description="Namespace for all topics and tfs", + ) - gz_args = f"--headless-rendering -s -v 4 -r {world}" if eval(headless) else f"-r {world}" + declare_robots_arg = DeclareLaunchArgument( + "robots", + default_value="", + description=( + "Spawning multiple robots at positions with yaw orientations e.g." + "robots:='robot1={x: 0.0, y: -1.0}; robot2={x: 1.0, y: -1.0}; robot3={x: 2.0, y: -1.0};'" + ), + ) gz_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( - [ - FindPackageShare("ros_gz_sim"), - "launch", - "gz_sim.launch.py", - ] + [FindPackageShare("husarion_gz_worlds"), "launch", "gz_sim.launch.py"] ) ), - launch_arguments={ - "gz_args": gz_args, - "on_exit_shutdown": "True", - }.items(), + launch_arguments={"gz_log_level": "1"}.items(), ) robots_list = ParseMultiRobotPose("robots").value() @@ -75,106 +71,45 @@ def launch_setup(context, *args, **kwargs): "yaw": yaw, } } + else: + for robot_name in robots_list: + init_pose = robots_list[robot_name] + for key, value in init_pose.items(): + init_pose[key] = TextSubstitution(text=str(value)) spawn_group = [] - for robot_name in robots_list: + for idx, robot_name in enumerate(robots_list): init_pose = robots_list[robot_name] - group = GroupAction( - [ - LogInfo( - msg=[ - "Launching namespace=", - robot_name, - " init_pose=", - str(init_pose), + spawn_robot = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("rosbot_gazebo"), + "launch", + "spawn.launch.py", ] - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("rosbot_gazebo"), - "launch", - "spawn.launch.py", - ] - ) - ), - launch_arguments={ - "mecanum": mecanum, - "use_sim": "True", - "use_gpu": use_gpu, - "simulation_engine": "ignition-gazebo", - "namespace": TextSubstitution(text=robot_name), - "x": TextSubstitution(text=str(init_pose["x"])), - "y": TextSubstitution(text=str(init_pose["y"])), - "z": TextSubstitution(text=str(init_pose["z"])), - "roll": TextSubstitution(text=str(init_pose["roll"])), - "pitch": TextSubstitution(text=str(init_pose["pitch"])), - "yaw": TextSubstitution(text=str(init_pose["yaw"])), - }.items(), - ), - ] + ) + ), + launch_arguments={ + "use_sim": "True", + "namespace": robot_name, + "x": init_pose["x"], + "y": init_pose["y"], + "z": init_pose["z"], + "roll": init_pose["roll"], + "pitch": init_pose["pitch"], + "yaw": init_pose["yaw"], + }.items(), ) - spawn_group.append(group) - - return [gz_sim, *spawn_group] - - -def generate_launch_description(): - declare_namespace_arg = DeclareLaunchArgument( - "namespace", - default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), - description="Namespace for all topics and tfs", - ) - - declare_mecanum_arg = DeclareLaunchArgument( - "mecanum", - default_value="False", - description=( - "Whether to use mecanum drive controller (otherwise diff drive controller is used)" - ), - ) - - world_package = FindPackageShare("husarion_gz_worlds") - world_file = PathJoinSubstitution([world_package, "worlds", "husarion_world.sdf"]) - declare_world_arg = DeclareLaunchArgument( - "world", default_value=world_file, description="SDF world file" - ) - - declare_headless_arg = DeclareLaunchArgument( - "headless", - default_value="False", - description="Run Gazebo Ignition in the headless mode", - choices=["True", "False"], - ) - - declare_use_gpu_arg = DeclareLaunchArgument( - "use_gpu", - default_value="True", - description="Whether GPU acceleration is used", - ) - - declare_robots_arg = DeclareLaunchArgument( - "robots", - default_value="", - description=( - "Spawning multiple robots at positions with yaw orientations e. g. robots:='robot1={x:" - " 0.0, y: -1.0}; robot2={x: 1.0, y: -1.0}; robot3={x: 2.0, y: -1.0}; robot4={x: 3.0," - " y: -1.0}'" - ), - ) + spawn_robot_delay = TimerAction(period=5.0 * idx, actions=[spawn_robot]) + spawn_group.append(spawn_robot_delay) return LaunchDescription( [ declare_namespace_arg, - declare_mecanum_arg, - declare_world_arg, - declare_headless_arg, - declare_use_gpu_arg, declare_robots_arg, - # Sets use_sim_time for all nodes started below - # (doesn't work for nodes started from ignition gazebo) SetParameter(name="use_sim_time", value=True), - OpaqueFunction(function=launch_setup), + gz_sim, + *spawn_group, ] ) diff --git a/rosbot_gazebo/launch/spawn.launch.py b/rosbot_gazebo/launch/spawn.launch.py index 7d231a7e..160bd9ef 100644 --- a/rosbot_gazebo/launch/spawn.launch.py +++ b/rosbot_gazebo/launch/spawn.launch.py @@ -13,7 +13,7 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import ( LaunchConfiguration, @@ -27,34 +27,45 @@ def generate_launch_description(): namespace = LaunchConfiguration("namespace") + x = LaunchConfiguration("x") + y = LaunchConfiguration("y") + z = LaunchConfiguration("z") + roll = LaunchConfiguration("roll") + pitch = LaunchConfiguration("pitch") + yaw = LaunchConfiguration("yaw") + declare_namespace_arg = DeclareLaunchArgument( "namespace", default_value="", description="Namespace for all topics and tfs", ) - namespace_ext = PythonExpression( - ["''", " if '", namespace, "' == '' ", "else ", "'/", namespace, "'"] + declare_x_arg = DeclareLaunchArgument( + "x", default_value="-1.0", description="Initial robot position in the global 'x' axis." ) - mecanum = LaunchConfiguration("mecanum") - declare_mecanum_arg = DeclareLaunchArgument( - "mecanum", - default_value="False", - description=( - "Whether to use mecanum drive controller (otherwise diff drive controller is used)" - ), + declare_y_arg = DeclareLaunchArgument( + "y", default_value="-2.0", description="Initial robot position in the global 'y' axis." + ) + + declare_z_arg = DeclareLaunchArgument( + "z", default_value="0.0", description="Initial robot position in the global 'z' axis." + ) + + declare_roll_arg = DeclareLaunchArgument( + "roll", default_value="0.0", description="Initial robot 'roll' orientation." ) - use_gpu = LaunchConfiguration("use_gpu") - declare_use_gpu_arg = DeclareLaunchArgument( - "use_gpu", - default_value="True", - description="Whether GPU acceleration is used", + declare_pitch_arg = DeclareLaunchArgument( + "pitch", default_value="0.0", description="Initial robot 'pitch' orientation." ) - robot_name = PythonExpression( - ["'rosbot'", " if '", namespace, "' == '' ", "else ", "'", namespace, "'"] + declare_yaw_arg = DeclareLaunchArgument( + "yaw", default_value="0.0", description="Initial robot 'yaw' orientation." + ) + + namespace_ext = PythonExpression( + ["''", " if '", namespace, "' == '' ", "else ", "'/", namespace, "'"] ) gz_remappings_file = PathJoinSubstitution( @@ -71,38 +82,52 @@ def generate_launch_description(): executable="create", arguments=[ "-name", - robot_name, + namespace, "-allow_renaming", "true", "-topic", "robot_description", "-x", - LaunchConfiguration("x", default="0.00"), + x, "-y", - LaunchConfiguration("y", default="0.00"), + y, "-z", - LaunchConfiguration("z", default="0.00"), + z, "-R", - LaunchConfiguration("roll", default="0.00"), + roll, "-P", - LaunchConfiguration("pitch", default="0.00"), + pitch, "-Y", - LaunchConfiguration("yaw", default="0.00"), + yaw, ], - output="screen", namespace=namespace, ) + welcome_msg = LogInfo( + msg=[ + "Spawning ROSbot\n\tNamespace: '", + namespace, + "'\n\tInitial pose: (", + x, + ", ", + y, + ", ", + z, + ", ", + roll, + ", ", + pitch, + ", ", + yaw, + ")", + ] + ) + ign_bridge = Node( package="ros_gz_bridge", executable="parameter_bridge", name="ros_gz_bridge", parameters=[{"config_file": namespaced_gz_remappings_file}], - remappings=[ - ("/tf", "tf"), - ("/tf_static", "tf_static"), - ], - output="screen", namespace=namespace, ) @@ -117,10 +142,7 @@ def generate_launch_description(): ) ), launch_arguments={ - "mecanum": mecanum, "use_sim": "True", - "use_gpu": use_gpu, - "simulation_engine": "ignition-gazebo", "namespace": namespace, }.items(), ) @@ -128,11 +150,14 @@ def generate_launch_description(): return LaunchDescription( [ declare_namespace_arg, - declare_mecanum_arg, - declare_use_gpu_arg, - # Sets use_sim_time for all nodes started below - # (doesn't work for nodes started from ignition gazebo) + declare_x_arg, + declare_y_arg, + declare_z_arg, + declare_roll_arg, + declare_pitch_arg, + declare_yaw_arg, SetParameter(name="use_sim_time", value=True), + welcome_msg, ign_bridge, gz_spawn_entity, bringup_launch, diff --git a/rosbot_gazebo/package.xml b/rosbot_gazebo/package.xml index aa9952ed..9a1de041 100644 --- a/rosbot_gazebo/package.xml +++ b/rosbot_gazebo/package.xml @@ -4,7 +4,9 @@ rosbot_gazebo 0.13.2 Gazebo Ignition simulation for ROSbot 2, 2R, PRO + Husarion + Apache License 2.0 https://husarion.com/ @@ -12,32 +14,25 @@ https://github.com/husarion/rosbot_ros/issues Jakub Delicat - Rafal Gorecki Krzysztof Wojciechowski + Rafal Gorecki - rosbot_bringup - + gz_ros2_control + husarion_gz_worlds launch launch_ros - - husarion_gz_worlds - ros_gz_sim - - ros_gz_bridge - gz_ros2_control nav2_common + ros_gz_bridge + ros_gz_sim + rosbot_bringup - python3-pytest + geometry_msgs launch - launch_ros launch_pytest - - tf_transformations - + launch_ros nav_msgs - geometry_msgs + python3-pytest + tf_transformations ament_python diff --git a/rosbot_gazebo/test/test_diff_drive_simulation.py b/rosbot_gazebo/test/test_diff_drive.py similarity index 90% rename from rosbot_gazebo/test/test_diff_drive_simulation.py rename to rosbot_gazebo/test/test_diff_drive.py index 981c7cb4..ccdeec23 100644 --- a/rosbot_gazebo/test/test_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_diff_drive.py @@ -44,14 +44,11 @@ def generate_test_description(): ) ), launch_arguments={ - "headless": "True", - "world": PathJoinSubstitution( - [ - FindPackageShare("husarion_gz_worlds"), - "worlds", - "empty_with_plugins.sdf", - ] + "gz_headless_mode": "True", + "gz_world": PathJoinSubstitution( + [FindPackageShare("husarion_gz_worlds"), "worlds", "empty_with_plugins.sdf"] ), + "microros": "False", }.items(), ) diff --git a/rosbot_gazebo/test/test_flake8.py b/rosbot_gazebo/test/test_flake8.py deleted file mode 100644 index 22fffcb8..00000000 --- a/rosbot_gazebo/test/test_flake8.py +++ /dev/null @@ -1,23 +0,0 @@ -# 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. - -import pytest -from ament_flake8.main import main_with_errors - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors) diff --git a/rosbot_gazebo/test/test_mecanum_simulation.py b/rosbot_gazebo/test/test_mecanum.py similarity index 90% rename from rosbot_gazebo/test/test_mecanum_simulation.py rename to rosbot_gazebo/test/test_mecanum.py index c6ab6695..e18d8796 100644 --- a/rosbot_gazebo/test/test_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_mecanum.py @@ -44,15 +44,12 @@ def generate_test_description(): ) ), launch_arguments={ - "mecanum": "True", - "headless": "True", - "world": PathJoinSubstitution( - [ - FindPackageShare("husarion_gz_worlds"), - "worlds", - "empty_with_plugins.sdf", - ] + "gz_headless_mode": "True", + "gz_world": PathJoinSubstitution( + [FindPackageShare("husarion_gz_worlds"), "worlds", "empty_with_plugins.sdf"] ), + "mecanum": "True", + "microros": "False", }.items(), ) diff --git a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py b/rosbot_gazebo/test/test_multirobot_diff_drive.py similarity index 91% rename from rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py rename to rosbot_gazebo/test/test_multirobot_diff_drive.py index 8269f543..a1b4ddbc 100644 --- a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_diff_drive.py @@ -31,19 +31,19 @@ @launch_pytest.fixture def generate_test_description(): - # IncludeLaunchDescription does not work with robots argument + gz_world_path = ( + get_package_share_directory("husarion_gz_worlds") + "/worlds/empty_with_plugins.sdf" + ) simulation_launch = ExecuteProcess( cmd=[ "ros2", "launch", "rosbot_gazebo", "simulation.launch.py", - ( - f'world:={get_package_share_directory("husarion_gz_worlds")}' - "/worlds/empty_with_plugins.sdf" - ), + "gz_headless_mode:=True", + f"gz_world:={gz_world_path}", + "microros:=False", "robots:=robot1={y: -4.0}; robot2={y: 0.0};", - "headless:=True", ], output="screen", ) diff --git a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py b/rosbot_gazebo/test/test_multirobot_mecanum.py similarity index 91% rename from rosbot_gazebo/test/test_multirobot_mecanum_simulation.py rename to rosbot_gazebo/test/test_multirobot_mecanum.py index 34c67808..16fcaf63 100644 --- a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_mecanum.py @@ -31,20 +31,20 @@ @launch_pytest.fixture def generate_test_description(): - # IncludeLaunchDescription does not work with robots argument + gz_world_path = ( + get_package_share_directory("husarion_gz_worlds") + "/worlds/empty_with_plugins.sdf" + ) simulation_launch = ExecuteProcess( cmd=[ "ros2", "launch", "rosbot_gazebo", "simulation.launch.py", - ( - f'world:={get_package_share_directory("husarion_gz_worlds")}' - "/worlds/empty_with_plugins.sdf" - ), - "robots:=robot1={y: -4.0}; robot2={y: 0.0};", + "gz_headless_mode:=True", + f"gz_world:={gz_world_path}", "mecanum:=True", - "headless:=True", + "microros:=False", + "robots:=robot1={y: -4.0}; robot2={y: 0.0};", ], output="screen", ) diff --git a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py b/rosbot_gazebo/test/test_namespaced_diff_drive.py similarity index 88% rename from rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py rename to rosbot_gazebo/test/test_namespaced_diff_drive.py index c9bef37f..52753dbb 100644 --- a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_namespaced_diff_drive.py @@ -44,15 +44,12 @@ def generate_test_description(): ) ), launch_arguments={ - "headless": "True", - "world": PathJoinSubstitution( - [ - FindPackageShare("husarion_gz_worlds"), - "worlds", - "empty_with_plugins.sdf", - ] + "gz_headless_mode": "True", + "gz_world": PathJoinSubstitution( + [FindPackageShare("husarion_gz_worlds"), "worlds", "empty_with_plugins.sdf"] ), - "namespace": "rosbot2r", + "microros": "False", + "namespace": "rosbot", }.items(), ) @@ -70,7 +67,7 @@ def generate_test_description(): def test_namespaced_diff_drive_simulation(): rclpy.init() try: - node = SimulationTestNode("test_namespaced_diff_drive_simulation", namespace="rosbot2r") + node = SimulationTestNode("test_namespaced_diff_drive_simulation", namespace="rosbot") Thread(target=lambda node: rclpy.spin(node), args=(node,)).start() diff_test(node) diff --git a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py b/rosbot_gazebo/test/test_namespaced_mecanum.py similarity index 88% rename from rosbot_gazebo/test/test_namespaced_mecanum_simulation.py rename to rosbot_gazebo/test/test_namespaced_mecanum.py index 6ba653d0..4afe29c3 100644 --- a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_namespaced_mecanum.py @@ -44,16 +44,13 @@ def generate_test_description(): ) ), launch_arguments={ - "mecanum": "True", - "world": PathJoinSubstitution( - [ - FindPackageShare("husarion_gz_worlds"), - "worlds", - "empty_with_plugins.sdf", - ] + "gz_headless_mode": "True", + "gz_world": PathJoinSubstitution( + [FindPackageShare("husarion_gz_worlds"), "worlds", "empty_with_plugins.sdf"] ), - "headless": "True", - "namespace": "rosbot2r", + "mecanum": "True", + "microros": "False", + "namespace": "rosbot", }.items(), ) @@ -71,7 +68,7 @@ def generate_test_description(): def test_namespaced_mecanum_simulation(): rclpy.init() try: - node = SimulationTestNode("test_namespaced_mecanum_simulation", namespace="rosbot2r") + node = SimulationTestNode("test_namespaced_mecanum_simulation", namespace="rosbot") Thread(target=lambda node: rclpy.spin(node), args=(node,)).start() mecanum_test(node) diff --git a/rosbot_gazebo/test/test_utils.py b/rosbot_gazebo/test/test_utils.py index 6b76dda9..1b8c1ac2 100644 --- a/rosbot_gazebo/test/test_utils.py +++ b/rosbot_gazebo/test/test_utils.py @@ -13,10 +13,11 @@ # See the License for the specific language governing permissions and # limitations under the License. +import time from threading import Event import rclpy -from geometry_msgs.msg import Twist +from geometry_msgs.msg import Twist, TwistStamped from nav_msgs.msg import Odometry from rclpy.node import Node from sensor_msgs.msg import Image, Imu, JointState, LaserScan, PointCloud2 @@ -35,7 +36,7 @@ class SimulationTestNode(Node): def __init__(self, name="test_node", namespace=None): super().__init__(name, namespace=namespace) - self.cmd_vel_pub = self.create_publisher(Twist, "cmd_vel", 10) + self.cmd_vel_pub = self.create_publisher(TwistStamped, "cmd_vel", 10) # Robot callback self.joint_sub = self.create_subscription( @@ -60,7 +61,9 @@ def __init__(self, name="test_node", namespace=None): for range_topic_name in self.RANGE_SENSORS_TOPICS: sub = self.create_subscription(LaserScan, range_topic_name, self.ranges_callback, 10) self.range_subs.append(sub) - self.scan_sub = self.create_subscription(LaserScan, "scan", self.scan_callback, 10) + self.scan_sub = self.create_subscription( + LaserScan, "scan_filtered", self.scan_callback, 10 + ) # Timer - send cmd_vel and check if the time needed for speed stabilization has elapsed self.timer = self.create_timer(0.1, self.timer_callback) @@ -128,26 +131,26 @@ def is_initialized(self): self.robot_initialized_event.set() return self.robot_initialized_event.is_set() - def controller_callback(self, data: Odometry): + def controller_callback(self, msg: Odometry): if not self.is_controller_msg: self.get_logger().info("Controller message arrived") self.is_controller_msg = True - self.controller_twist = data.twist.twist - self.is_controller_odom_correct = self.is_twist_ok(data.twist.twist) + self.controller_twist = msg.twist.twist + self.is_controller_odom_correct = self.is_twist_ok(msg.twist.twist) - def ekf_callback(self, data: Odometry): + def ekf_callback(self, msg: Odometry): if not self.is_ekf_msg: self.get_logger().info("EKF message arrived") self.is_ekf_msg = True - self.ekf_twist = data.twist.twist - self.is_ekf_odom_correct = self.is_twist_ok(data.twist.twist) + self.ekf_twist = msg.twist.twist + self.is_ekf_odom_correct = self.is_twist_ok(msg.twist.twist) - def imu_callback(self, data): + def imu_callback(self, msg: Imu): if not self.is_imu_msg: self.get_logger().info("IMU message arrived") self.is_imu_msg = True - def joint_states_callback(self, data): + def joint_states_callback(self, msg: JointState): if not self.is_joint_msg: self.get_logger().info("Joint State message arrived") self.is_joint_msg = True @@ -164,30 +167,31 @@ def timer_callback(self): ) self.vel_stabilization_time_event.set() - def scan_callback(self, data: LaserScan): - self.get_logger().debug(f"Received scan length: {len(data.ranges)}") - if data.ranges: + def scan_callback(self, msg: LaserScan): + self.get_logger().debug(f"Received scan length: {len(msg.ranges)}") + if msg.ranges: self.scan_event.set() - def ranges_callback(self, data: LaserScan): - index = self.RANGE_SENSORS_FRAMES.index(data.header.frame_id) - if len(data.ranges) == 1: + def ranges_callback(self, msg: LaserScan): + index = self.RANGE_SENSORS_FRAMES.index(msg.header.frame_id) + if len(msg.ranges) == 1: self.ranges_events[index].set() - def camera_image_callback(self, data: Image): - if data.data: + def camera_image_callback(self, msg: Image): + if msg.data: self.camera_color_event.set() - def camera_points_callback(self, data: PointCloud2): - if data.data: + def camera_points_callback(self, msg: PointCloud2): + if msg.data: self.camera_points_event.set() def publish_cmd_vel_msg(self): - twist_msg = Twist() + twist_msg = TwistStamped() - twist_msg.linear.x = self.v_x - twist_msg.linear.y = self.v_y - twist_msg.angular.z = self.v_yaw + twist_msg.header.stamp = self.get_clock().now().to_msg() + twist_msg.twist.linear.x = self.v_x + twist_msg.twist.linear.y = self.v_y + twist_msg.twist.angular.z = self.v_yaw self.cmd_vel_pub.publish(twist_msg) @@ -209,13 +213,14 @@ def x_speed_test(node: SimulationTestNode, v_x=0.0, v_y=0.0, v_yaw=0.0, robot_na f" {(node.current_time - node.goal_received_time):.1f}." ) assert node.is_controller_odom_correct, ( - f"{robot_name}: does not move properly in x direction. Check" - f" rosbot_base_controller! Twist: {node.twist}" + f"{robot_name}: does not move properly in x direction. Check rosbot_base_controller!" + f"\nTwist: {node.controller_twist}" f"\nCommand: x: {v_x}, y:{v_y}, yaw:{v_yaw}" ) assert node.is_ekf_odom_correct, ( f"{robot_name}: does not move properly in x direction. Check ekf_filter_node!" - f" Twist: {node.twist}" + f" Twist: {node.ekf_twist}" + f"\nCommand: x: {v_x}, y:{v_y}, yaw:{v_yaw}" ) @@ -228,13 +233,14 @@ def y_speed_test(node: SimulationTestNode, v_x=0.0, v_y=0.0, v_yaw=0.0, robot_na f" {(node.current_time - node.goal_received_time):.1f}." ) assert node.is_controller_odom_correct, ( - f"{robot_name} does not move properly in y direction. Check" - f" rosbot_base_controller! Twist: {node.twist}" + f"{robot_name}: does not move properly in y direction. Check rosbot_base_controller!" + f"\nTwist: {node.controller_twist}" f"\nCommand: x: {v_x}, y:{v_y}, yaw:{v_yaw}" ) assert node.is_ekf_odom_correct, ( - f"{robot_name} does not move properly in y direction. Check ekf_filter_node!" - f" Twist: {node.twist}" + f"{robot_name}: does not move properly in y direction. Check ekf_filter_node!" + f"\nTwist: {node.ekf_twist}" + f"\nCommand: x: {v_x}, y:{v_y}, yaw:{v_yaw}" ) @@ -256,21 +262,49 @@ def yaw_speed_test(node: SimulationTestNode, v_x=0.0, v_y=0.0, v_yaw=0.0, robot_ ), f"{robot_name} does not rotate properly. Check ekf_filter_node! Twist: {node.ekf_twist}" -def sensors_readings_test(node: SimulationTestNode, robot_name="ROSbot"): - flag = node.scan_event.wait(timeout=20.0) - assert flag, f"{robot_name}'s lidar does not work properly!" +def wait_for_all_events(events, timeout): + start_time = time.time() + while time.time() - start_time < timeout: + if all(event.is_set() for event in events): + return True, [] + time.sleep(0.1) - for i in range(len(node.RANGE_SENSORS_TOPICS)): - flag = node.ranges_events[i].wait(timeout=20.0) - assert ( - flag - ), f"{robot_name}'s range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!" + not_set_events = [i for i, event in enumerate(events) if not event.is_set()] + return False, not_set_events - flag = node.camera_color_event.wait(timeout=20.0) - assert flag, f"{robot_name}'s camera color image does not work properly!" - flag = node.camera_points_event.wait(timeout=20.0) - assert flag, f"{robot_name}'s camera point cloud does not work properly!" +def sensors_readings_test(node: SimulationTestNode, robot_name="ROSbot"): + events = [ + node.scan_event, + node.ranges_events[0], + node.ranges_events[1], + node.ranges_events[2], + node.ranges_events[3], + # FIXME: Camera topics are not available in the current simulation + # node.camera_color_event, + # node.camera_points_event, + ] + + event_names = [ + "Scan", + "Range FL", + "Range FR", + "Range RL", + "Range RR", + "Camera Color", + "Camera Points", + ] + + msgs_received_flag, not_set_indices = wait_for_all_events(events, timeout=20.0) + + if not msgs_received_flag: + not_set_event_names = [event_names[i] for i in not_set_indices] + missing_events = ", ".join(not_set_event_names) + raise AssertionError( + f"{robot_name}: Not all expected messages were received. Missing: {missing_events}." + ) + + print(f"{robot_name}: All messages received successfully.") def diff_test(node: SimulationTestNode, robot_name="ROSbot"): diff --git a/rosbot_utils/package.xml b/rosbot_utils/package.xml index f8d99b9f..36d19e1e 100644 --- a/rosbot_utils/package.xml +++ b/rosbot_utils/package.xml @@ -4,7 +4,9 @@ rosbot_utils 0.13.2 Utilities for ROSbot 2R and 2 PRO + Husarion + Apache License 2.0 https://husarion.com/ @@ -15,16 +17,15 @@ Jakub Delicat Rafal Gorecki + launch_ros python3-libgpiod python3-pyftdi-pip python3-requests python3-serial python3-sh usbutils - launch_ros ament_copyright - ament_flake8 ament_pep257 python3-pytest diff --git a/rosbot_utils/rosbot_utils/flash-firmware-usb.py b/rosbot_utils/rosbot_utils/flash-firmware-usb.py index 310af7a8..1c11a4cd 100755 --- a/rosbot_utils/rosbot_utils/flash-firmware-usb.py +++ b/rosbot_utils/rosbot_utils/flash-firmware-usb.py @@ -60,34 +60,41 @@ def exit_bootloader_mode(self): time.sleep(0.1) self.ftdi.close() - def try_flash_operation(self, operation_name, flash_command, flash_args): + def try_flash_operation(self, operation_name): + print(f"\n{operation_name} operation started") + self.enter_bootloader_mode() + sh.usbreset("0403:6015") for i in range(self.max_approach_no): + print(f"Attempt {i + 1}/{self.max_approach_no}") try: - self.enter_bootloader_mode() - sh.usbreset("0403:6015") - time.sleep(2.0) - flash_command(self.port, *flash_args, _out=sys.stdout) - self.exit_bootloader_mode() - time.sleep(0.2) + if operation_name == "Flashing": + flash_args = ["-v", "-w", self.binary_file, "-b", "115200"] + sh.stm32flash(self.port, *flash_args, _out=sys.stdout) + print("Success! The robot firmware has been uploaded.") + elif operation_name == "Write-UnProtection": + sh.stm32flash(self.port, "-u") + elif operation_name == "Read-UnProtection": + sh.stm32flash(self.port, "-k") + else: + raise ("Unknown operation.") break except Exception as e: - print(f"{operation_name} error! Trying again.") - print(f"Error: {e}") - print("---------------------------------------") - else: - print(f"WARNING! {operation_name} went wrong.") + stderr = e.stderr.decode("utf-8") + if stderr: + print(f"ERROR: {stderr.strip()}") + + print("Success!") + self.exit_bootloader_mode() def flash_firmware(self): # Disable the flash write-protection - self.try_flash_operation("Write-UnProtection", sh.stm32flash, ["-u"]) + self.try_flash_operation("Write-UnProtection") # Disable the flash read-protection - self.try_flash_operation("Read-UnProtection", sh.stm32flash, ["-k"]) + self.try_flash_operation("Read-UnProtection") # Flashing the firmware - # /usr/bin/stm32flash /dev/ttyUSB0 -v -w /root/firmware.bin -b 115200 - flash_args = ["-v", "-w", self.binary_file, "-b", "115200"] - self.try_flash_operation("Flashing", sh.stm32flash, flash_args) + self.try_flash_operation("Flashing") sh.usbreset("0403:6015") @@ -117,7 +124,6 @@ def main(): flasher = FirmwareFlasher(binary_file, port) flasher.flash_firmware() - print("Done.") if __name__ == "__main__": diff --git a/rosbot_utils/rosbot_utils/flash-firmware.py b/rosbot_utils/rosbot_utils/flash-firmware.py index 10f0901f..a1793a8d 100755 --- a/rosbot_utils/rosbot_utils/flash-firmware.py +++ b/rosbot_utils/rosbot_utils/flash-firmware.py @@ -25,6 +25,7 @@ import gpiod import sh + def get_raspberry_pi_model(): try: with open("/proc/cpuinfo", "r") as f: @@ -40,36 +41,37 @@ def get_raspberry_pi_model(): except FileNotFoundError: return "Not a Raspberry Pi" + class FirmwareFlasher: - def __init__(self, sys_arch, binary_file): + def __init__(self, binary_file): self.binary_file = binary_file - self.sys_arch = sys_arch + sys_arch = str(sh.uname("-m")).strip() self.max_approach_no = 3 - print(f"System architecture: {self.sys_arch}") + print(f"System architecture: {sys_arch}") - if self.sys_arch == "armv7l": + if sys_arch == "armv7l": # Setups ThinkerBoard pins print("Device: ThinkerBoard\n") - self.serial_port = "/dev/ttyS1" + self.port = "/dev/ttyS1" gpio_chip = "/dev/gpiochip0" boot0_pin_no = 164 reset_pin_no = 184 - elif self.sys_arch == "x86_64": + elif sys_arch == "x86_64": # Setups UpBoard pins print("Device: UpBoard\n") - self.serial_port = "/dev/ttyS4" + self.port = "/dev/ttyS4" gpio_chip = "/dev/gpiochip4" boot0_pin_no = 17 reset_pin_no = 18 - elif self.sys_arch == "aarch64": + elif sys_arch == "aarch64": # Setups RPi pins model = get_raspberry_pi_model() print(f"Device: {model}\n") - self.serial_port = "/dev/ttyAMA0" + self.port = "/dev/ttyAMA0" if model == "Raspberry Pi 4": gpio_chip = "/dev/gpiochip0" @@ -77,7 +79,7 @@ def __init__(self, sys_arch, binary_file): gpio_chip = "/dev/gpiochip4" else: gpio_chip = "/dev/gpiochip0" # Default or error handling - + boot0_pin_no = 17 reset_pin_no = 18 else: @@ -104,33 +106,40 @@ def exit_bootloader_mode(self): self.reset_pin.set_value(0) time.sleep(0.2) - def try_flash_operation(self, operation_name, flash_command, flash_args): + def try_flash_operation(self, operation_name): + print(f"\n{operation_name} operation started") + self.enter_bootloader_mode() for i in range(self.max_approach_no): + print(f"Attempt {i + 1}/{self.max_approach_no}") try: - flash_command(self.serial_port, *flash_args, _out=sys.stdout) - time.sleep(0.2) + if operation_name == "Flashing": + flash_args = ["-v", "-w", self.binary_file, "-b", "115200"] + sh.stm32flash(self.port, *flash_args, _out=sys.stdout) + print("Success! The robot firmware has been uploaded.") + elif operation_name == "Write-UnProtection": + sh.stm32flash(self.port, "-u") + elif operation_name == "Read-UnProtection": + sh.stm32flash(self.port, "-k") + else: + raise ("Unknown operation.") break except Exception as e: - print(f"{operation_name} error! Trying again.") - print(f"Error: {e}") - print("---------------------------------------") - else: - print(f"WARNING! {operation_name} went wrong.") + stderr = e.stderr.decode("utf-8") + if stderr: + print(f"ERROR: {stderr.strip()}") - def flash_firmware(self): - self.enter_bootloader_mode() + print("Success!") + self.exit_bootloader_mode() + def flash_firmware(self): # Disable the flash write-protection - self.try_flash_operation("Write-UnProtection", sh.stm32flash, ["-u"]) + self.try_flash_operation("Write-UnProtection") # Disable the flash read-protection - self.try_flash_operation("Read-UnProtection", sh.stm32flash, ["-k"]) + self.try_flash_operation("Read-UnProtection") # Flashing the firmware - flash_args = ["-v", "-w", self.binary_file, "-b", "115200"] - self.try_flash_operation("Flashing", sh.stm32flash, flash_args) - - self.exit_bootloader_mode() + self.try_flash_operation("Flashing") def main(): @@ -147,11 +156,9 @@ def main(): ) binary_file = parser.parse_args().file - sys_arch = sh.uname("-m").stdout.decode().strip() - flasher = FirmwareFlasher(sys_arch, binary_file) + flasher = FirmwareFlasher(binary_file) flasher.flash_firmware() - print("Done!") if __name__ == "__main__": diff --git a/rosbot_utils/test/test_flake8.py b/rosbot_utils/test/test_flake8.py deleted file mode 100644 index 22fffcb8..00000000 --- a/rosbot_utils/test/test_flake8.py +++ /dev/null @@ -1,23 +0,0 @@ -# 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. - -import pytest -from ament_flake8.main import main_with_errors - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors) diff --git a/tools/Dockerfile b/tools/Dockerfile deleted file mode 100644 index ebbecb14..00000000 --- a/tools/Dockerfile +++ /dev/null @@ -1,97 +0,0 @@ -ARG ROS_DISTRO=jazzy -ARG PREFIX= -ARG ROSBOT_FW_RELEASE=0.8.0 - -## =========================== ROS builder =============================== -FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-base AS ros_builder - -ARG ROS_DISTRO -ARG PREFIX - -SHELL ["/bin/bash", "-c"] - -WORKDIR /ros2_ws -RUN mkdir src - -COPY tools/healthcheck.cpp / - -COPY rosbot src/rosbot -COPY rosbot_bringup src/rosbot_bringup -COPY rosbot_controller src/rosbot_controller -COPY rosbot_description src/rosbot_description -COPY rosbot_utils src/rosbot_utils - -RUN apt-get update && apt-get install -y \ - python3-pip - -RUN vcs import src < src/rosbot/rosbot_hardware.repos && \ - cp -r src/ros2_controllers/diff_drive_controller src/ && \ - cp -r src/ros2_controllers/imu_sensor_broadcaster src/ && \ - rm -rf src/ros2_controllers && \ - # without this line (using vulcanexus base image) rosdep init throws error: "ERROR: default sources list file already exists:" - rm -rf /etc/ros/rosdep/sources.list.d/20-default.list && \ - rosdep init && \ - rosdep update --rosdistro $ROS_DISTRO && \ - rosdep install --from-paths src --ignore-src -y - -# Create health check package -RUN cd src/ && \ - MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ - source /opt/$MYDISTRO/$ROS_DISTRO/setup.bash && \ - ros2 pkg create healthcheck_pkg --build-type ament_cmake --dependencies rclcpp nav_msgs && \ - sed -i '/find_package(nav_msgs REQUIRED)/a \ - add_executable(healthcheck_node src/healthcheck.cpp)\n \ - ament_target_dependencies(healthcheck_node rclcpp nav_msgs)\n \ - install(TARGETS healthcheck_node DESTINATION lib/${PROJECT_NAME})' \ - /ros2_ws/src/healthcheck_pkg/CMakeLists.txt && \ - mv /healthcheck.cpp /ros2_ws/src/healthcheck_pkg/src/ - -# Build -RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ - source /opt/$MYDISTRO/$ROS_DISTRO/setup.bash && \ - colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release && \ - rm -rf build log - -## =========================== Final Stage =============================== -FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-core - -ARG ROS_DISTRO -ARG PREFIX - -SHELL ["/bin/bash", "-c"] - -WORKDIR /ros2_ws - -COPY --from=ros_builder /ros2_ws /ros2_ws - -RUN apt-get update && apt-get install -y \ - python3-rosdep \ - python3-pip \ - stm32flash \ - ros-$ROS_DISTRO-teleop-twist-keyboard && \ - rm -rf /etc/ros/rosdep/sources.list.d/20-default.list && \ - rosdep init && \ - rosdep update --rosdistro $ROS_DISTRO && \ - rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y && \ - apt-get remove -y \ - python3-rosdep \ - python3-pip && \ - apt-get clean && \ - echo $(cat /ros2_ws/src/rosbot/package.xml | grep '' | sed -r 's/.*([0-9]+.[0-9]+.[0-9]+)<\/version>/\1/g') >> /version.txt && \ - rm -rf src && \ - rm -rf /var/lib/apt/lists/* - -COPY tools/healthcheck.sh / - -# copy firmware built in previous stage and downloaded repository -# COPY --from=stm32flash_builder /firmware.bin /root/firmware.bin -# COPY --from=stm32flash_builder /firmware.hex /root/firmware.hex -# COPY --from=stm32flash_builder /stm32flash /usr/bin/stm32flash - -# copy scripts -# COPY tools/flash-firmware.py / -# COPY tools/flash-firmware.py /usr/bin/ -# COPY tools/flash-firmware-usb.py /usr/bin/ - -HEALTHCHECK --interval=7s --timeout=2s --start-period=5s --retries=5 \ - CMD ["/healthcheck.sh"] diff --git a/tools/Dockerfile.dev b/tools/Dockerfile.dev deleted file mode 100644 index b69bfd27..00000000 --- a/tools/Dockerfile.dev +++ /dev/null @@ -1,36 +0,0 @@ -ARG ROS_DISTRO=jazzy -ARG PREFIX= -ARG HUSARION_ROS_BUILD_TYPE=hardware - -## =========================== ROS builder =============================== -FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-base AS ros_builder - -ARG ROS_DISTRO -ARG PREFIX -ARG HUSARION_ROS_BUILD_TYPE - -ENV HUSARION_ROS_BUILD_TYPE=${HUSARION_ROS_BUILD_TYPE} - -WORKDIR /ros2_ws -RUN mkdir src - -COPY ./ src/ - -RUN apt-get update && apt-get install -y \ - python3-pip \ - stm32flash - -RUN vcs import src < src/rosbot/rosbot_hardware.repos && \ - if [ "$HUSARION_ROS_BUILD_TYPE" == "simulation" ]; then \ - vcs import src < src/rosbot/rosbot_simulation.repos; \ - else \ - rm -rf src/rosbot_gazebo; \ - fi && \ - rm -rf /etc/ros/rosdep/sources.list.d/20-default.list && \ - rosdep init && \ - rosdep update --rosdistro $ROS_DISTRO && \ - rosdep install --from-paths src --ignore-src -y - -RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ - source /opt/$MYDISTRO/$ROS_DISTRO/setup.bash && \ - colcon build --packages-up-to rosbot --cmake-args -DCMAKE_BUILD_TYPE=Release diff --git a/tools/compose.dev.yaml b/tools/compose.dev.yaml deleted file mode 100644 index daf1af49..00000000 --- a/tools/compose.dev.yaml +++ /dev/null @@ -1,25 +0,0 @@ -# Quick Start -# -# 1. run `docker compose up` on the robot -# 3. You can use teleop_twist_keyboard to control the robot in robot1 namespace: -# `ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r __ns:=/robot1` - -services: - - rosbot: - build: - context: ../ - dockerfile: tools/Dockerfile.dev - # privileged: true # GPIO - devices: - - ${SERIAL_PORT:?err} - - /dev/bus/usb/ #FTDI - volumes: - - ../rosbot_utils:/ros2_ws/src/rosbot_utils - command: tail -f /dev/null - # command: > - # ros2 launch rosbot_bringup combined.launch.py - # mecanum:=${MECANUM:-False} - # serial_port:=$SERIAL_PORT - # serial_baudrate:=576000 - # namespace:=robot1 diff --git a/tools/compose.ros2router.yaml b/tools/compose.ros2router.yaml deleted file mode 100644 index 5e7da281..00000000 --- a/tools/compose.ros2router.yaml +++ /dev/null @@ -1,52 +0,0 @@ -# Quick Start -# -# 1. run `docker compose up` on the robot -# 3. You can use teleop_twist_keyboard to control the robot in robot1 namespace: -# `ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r __ns:=/robot1` - -services: - - rosbot: - build: - context: ../ - dockerfile: tools/Dockerfile - network_mode: host - ipc: host - devices: - - ${SERIAL_PORT:?err} - environment: - - ROS_LOCALHOST_ONLY=0 - - ROS_DOMAIN_ID=42 - - FASTRTPS_DEFAULT_PROFILES_FILE=/shm-only.xml - command: > - ros2 launch rosbot_bringup combined.launch.py - mecanum:=${MECANUM:-False} - serial_port:=$SERIAL_PORT - serial_baudrate:=576000 - namespace:=robot1 - - ros2router: - image: husarnet/ros2router:1.4.0 - network_mode: host - ipc: host - environment: - - USE_HUSARNET=FALSE - - ROS_LOCALHOST_ONLY=1 - - ROS_DISTRO - - | - LOCAL_PARTICIPANT= - - name: LocalParticipant - kind: local - domain: 0 - transport: udp - - name: LocalDockerParticipant - kind: local - domain: 42 - transport: shm - - | - FILTER= - allowlist: - - name: "rt/robot1/cmd_vel" - type: "geometry_msgs::msg::dds_::Twist_" - blocklist: [] - builtin-topics: [] diff --git a/tools/compose.yaml b/tools/compose.yaml deleted file mode 100644 index ef4dbc8f..00000000 --- a/tools/compose.yaml +++ /dev/null @@ -1,25 +0,0 @@ -# Quick Start -# -# 1. run `docker compose up` on the robot -# 3. You can use teleop_twist_keyboard to control the robot in robot1 namespace: -# `ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r __ns:=/robot1` - -services: - - rosbot: - build: - context: ../ - dockerfile: tools/Dockerfile - devices: - - ${SERIAL_PORT:?err} - - /dev/bus/usb/ - environment: - - ROS_LOCALHOST_ONLY=0 - - ROS_DOMAIN_ID=42 - # command: tail -f /dev/null - command: > - ros2 launch rosbot_bringup combined.launch.py - mecanum:=${MECANUM:-False} - serial_port:=$SERIAL_PORT - serial_baudrate:=576000 - namespace:=robot1 diff --git a/tools/healthcheck.cpp b/tools/healthcheck.cpp deleted file mode 100644 index 34f22455..00000000 --- a/tools/healthcheck.cpp +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright 2024 Husarion sp. z o.o. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "fstream" -#include "nav_msgs/msg/odometry.hpp" -#include "rclcpp/rclcpp.hpp" - -using namespace std::chrono_literals; - -#define LOOP_PERIOD 2s -#define MSG_VALID_TIME 5s - -std::chrono::steady_clock::time_point last_msg_time; - -void write_health_status(const std::string &status) { - std::ofstream healthFile("/var/tmp/health_status.txt"); - healthFile << status; -} - -void msg_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { - last_msg_time = std::chrono::steady_clock::now(); -} - -void healthy_check() { - std::chrono::steady_clock::time_point current_time = - std::chrono::steady_clock::now(); - std::chrono::duration elapsed_time = current_time - last_msg_time; - bool is_msg_valid = elapsed_time.count() < MSG_VALID_TIME.count(); - - if (is_msg_valid) { - write_health_status("healthy"); - } else { - write_health_status("unhealthy"); - } -} - -int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("healthcheck_node"); - auto sub = node->create_subscription( - "odometry/filtered", rclcpp::SensorDataQoS(), msg_callback); - - while (rclcpp::ok()) { - rclcpp::spin_some(node); - healthy_check(); - std::this_thread::sleep_for(LOOP_PERIOD); - } - - return 0; -} diff --git a/tools/healthcheck.sh b/tools/healthcheck.sh deleted file mode 100755 index 7cae7a1c..00000000 --- a/tools/healthcheck.sh +++ /dev/null @@ -1,17 +0,0 @@ -#!/bin/bash - -HEALTHCHECK_FILE="/var/tmp/health_status.txt" - - -# Now check the health status -if [ -f "$HEALTHCHECK_FILE" ]; then - status=$(cat "$HEALTHCHECK_FILE") - if [ "$status" == "healthy" ]; then - exit 0 - else - exit 1 - fi -else - echo "Healthcheck file still not found. There may be an issue with the node." - exit 1 -fi