diff --git a/.github/workflows/tests.yaml b/.github/workflows/tests.yaml index e8803988..dcdd3e9c 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-22.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-22.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: humble industrial_ci: name: Industrial CI @@ -36,20 +22,15 @@ jobs: ROS_DISTRO: [humble] 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 - cp -r src/ros2_controllers/diff_drive_controller src/ + 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/imu_sensor_broadcaster src/ rm -rf src/ros2_controllers @@ -58,7 +39,6 @@ jobs: - name: Leave only ROSbot tests shell: bash run: | - sed '/if(BUILD_TESTING)/,/endif()/d' src/diff_drive_controller/CMakeLists.txt -i sed '/if(BUILD_TESTING)/,/endif()/d' src/imu_sensor_broadcaster/CMakeLists.txt -i sed '/if(BUILD_TESTING)/,/endif()/d' src/micro_ros_msgs/CMakeLists.txt -i diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b238ea30..389b9f0f 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/ + - 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: @@ -68,11 +85,17 @@ repos: stages: [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 88273154..00000000 --- a/.wordlist.txt +++ /dev/null @@ -1,161 +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 -spawner diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 00000000..351e9d40 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,59 @@ +# 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..4cf6656b 100644 --- a/README.md +++ b/README.md @@ -1,204 +1,136 @@ # 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/2r_colour_perspective-14e3679e451eb9fe4e79eeecf7b82e65.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 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 +vcs import src < src/rosbot_ros/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 +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 +colcon build --symlink-install --packages-up-to rosbot --cmake-args -DCMAKE_BUILD_TYPE=Release ``` -Flash firmware: - -```bash -source install/setup.bash -ros2 run rosbot_utils flash_firmware -``` +#### Run the Robot -Running: +1. Flash the firmware: -```bash -source install/setup.bash -ros2 launch rosbot_bringup combined.launch.py -``` + ```bash + sudo su + source install/setup.bash + ros2 run rosbot_utils flash_firmware + exit + ``` -### Build and run Gazebo simulation +> [!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. -Prerequisites: +2. Launch the robot: -> [!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 -> ``` + ```bash + source install/setup.bash + ros2 launch rosbot_bringup bringup.launch.py + ``` -If you have installed multiple versions of Gazebo use the global variable to select the correct one: - -```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 +vcs import src < src/rosbot_ros/rosbot/rosbot_hardware.repos +vcs import src < src/rosbot_ros/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 +# Build only imu_sensor_broadcaster from ros2_controllers +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 +### 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` | -### 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 -``` - -### 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 c92327a6..537db6a5 100644 --- a/ROS_API.md +++ b/ROS_API.md @@ -1,46 +1,120 @@ -Use `bringup.launch.py` from `rosbot_bringup` to start all base functionalities for ROSbot 2, 2 PRO, 2R. It consists of following parts: +# ROSbot - Software -- `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 +Detailed information about content of rosbot package for ROS2. - **Subscribes** - - `/rosbot_base_controller/odom` (_nav_msgs/Odometry_) - - `/imu_broadcaster/imu` (_sensor_msgs/Imu_) +## Package Description - **Publishes** - - `/tf` (_tf2_msgs/TFMessage_) - `base_link`->`odom` transform - - `/odometry/filtered` (_nav_msgs/Odometry_) +### `rosbot` -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: +Metapackage that contains dependencies to other repositories. It is also used to define whether simulation dependencies should be used. -- `joint_state_broadcaster` -- `rosbot_base_controller` -- `imu_broadcaster` +### `rosbot_bringup` - **Subscribes** - - `/cmd_vel` (_geometry_msgs/Twist_) - - `/_motors_responses` (_sensor_msgs/JointState_) - - `/_imu/data_raw` (_sensor_msgs/Imu_) +Package that contains launch, which starts all base functionalities with the microros agent. Also configs for `robot_localization` and `laser_filters` are defined there. - **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_) +**Available Launch Files:** -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.: +- `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. -```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}' -``` -If you want to use your own world add to the world's sdf file gazebo sensors plugins inside any `` tag: +### `rosbot_controller` -```xml - - -``` +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). -> **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_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 index 7725261e..05b532ca 100644 --- a/docker/.env +++ b/docker/.env @@ -1,2 +1 @@ -SERIAL_PORT=/dev/ttyUSB0 -ROS_NAMESPACE=robot1 \ No newline at end of file +ROBOT_NAMESPACE=rosbot diff --git a/docker/Dockerfile b/docker/Dockerfile deleted file mode 100644 index d89cffbe..00000000 --- a/docker/Dockerfile +++ /dev/null @@ -1,32 +0,0 @@ -ARG ROS_DISTRO=humble - -FROM husarnet/ros:${ROS_DISTRO}-ros-base - -SHELL ["/bin/bash", "-c"] - -WORKDIR /ros2_ws - -COPY . src/ - -RUN apt-get update && apt-get install -y \ - python3-pip \ - stm32flash \ - ros-${ROS_DISTRO}-teleop-twist-keyboard - -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 && \ - rm -r src/rosbot_gazebo - -# Create a script to install runtime dependencies for final image -RUN apt update && \ - rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} --reinstall --simulate -y --dependency-types exec >> /rosdep_install.sh && \ - chmod +x /rosdep_install.sh - -RUN apt update && \ - rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y && \ - source /opt/ros/$ROS_DISTRO/setup.bash && \ - colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release && \ - 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 build log src \ No newline at end of file diff --git a/docker/Dockerfile.hardware b/docker/Dockerfile.hardware new file mode 100644 index 00000000..19d85ec2 --- /dev/null +++ b/docker/Dockerfile.hardware @@ -0,0 +1,34 @@ +ARG ROS_DISTRO=humble + +FROM husarnet/ros:${ROS_DISTRO}-ros-core + +WORKDIR /ros2_ws + +ENV HUSARION_ROS_BUILD_TYPE=hardware + +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_hardware.repos && \ + 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..3dc13122 --- /dev/null +++ b/docker/Dockerfile.simulation @@ -0,0 +1,35 @@ +ARG ROS_DISTRO=humble + +FROM husarnet/ros:${ROS_DISTRO}-ros-core + +WORKDIR /ros2_ws + +ENV HUSARION_ROS_BUILD_TYPE=simulation + +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_hardware.repos && \ + vcs import src < src/rosbot_ros/rosbot/rosbot_simulation.repos && \ + 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/compose.yaml b/docker/compose.yaml deleted file mode 100644 index a4fff438..00000000 --- a/docker/compose.yaml +++ /dev/null @@ -1,19 +0,0 @@ -services: - rosbot: - build: - context: .. - dockerfile: docker/Dockerfile - network_mode: host - ipc: host - devices: - - ${SERIAL_PORT:?err} - - /dev/bus/usb/ # FTDI - # environment: - # - ROS_NAMESPACE - # command: tail -f /dev/null - command: > - ros2 launch rosbot_bringup combined.launch.py - mecanum:=${MECANUM:-False} - serial_port:=$SERIAL_PORT - serial_baudrate:=576000 - # namespace:=${ROS_NAMESPACE:-rosbot} \ No newline at end of file diff --git a/docker/justfile b/docker/justfile index ff972714..0e6f5ef4 100644 --- a/docker/justfile +++ b/docker/justfile @@ -13,7 +13,7 @@ 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:=/${ROS_NAMESPACE}" + 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 @@ -29,9 +29,9 @@ flash-firmware: _install-yq _run-as-user gpio_chip=/dev/gpiochip4 serial_port=/dev/ttyS4 else - echo "Probably user laptop" + echo "Probably user computer" gpio_chip=/dev/bus/usb - serial_port=$SERIAL_PORT + serial_port=/dev/ttyUSB0 enable_usb="--usb" fi @@ -42,13 +42,13 @@ flash-firmware: _install-yq _run-as-user docker-rosbot \ ros2 run rosbot_utils flash_firmware ${enable_usb} -run: +run_hardware: #/bin/bash - docker compose up + docker compose -f compose.hardware.yaml up -build: +build_hardware: #/bin/bash - docker compose build + docker compose -f compose.hardware.yaml build _run-as-root: #!/bin/bash @@ -86,4 +86,4 @@ _install-yq: 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 \ No newline at end of file + fi diff --git a/rosbot/package.xml b/rosbot/package.xml index 6533fef6..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,9 +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 1a054c48..fe402a5f 100644 --- a/rosbot/rosbot_hardware.repos +++ b/rosbot/rosbot_hardware.repos @@ -1,21 +1,21 @@ repositories: - rosbot_hardware_interfaces: - type: git - url: https://github.com/husarion/rosbot_hardware_interfaces.git - version: main - ros_components_description: - type: git - url: https://github.com/husarion/ros_components_description.git - version: 1cb25600afa5941d21d48c0af8e63ad2eb3afaa0 husarion_controllers: type: git url: https://github.com/husarion/husarion_controllers - version: main + version: 217b09830f5f42930098b9992eda41710702b625 micro_ros_msgs: type: git url: https://github.com/micro-ROS/micro_ros_msgs.git - version: humble + version: 10be4d005fbc7d8dd60dbb213b65f4171419bfe9 micro-ROS-Agent: type: git url: https://github.com/micro-ROS/micro-ROS-Agent.git - version: humble + version: 30377bbd86ff7ea93ca69a3b37997fd235385e1f + ros_components_description: + type: git + url: https://github.com/husarion/ros_components_description.git + version: 1cb25600afa5941d21d48c0af8e63ad2eb3afaa0 + rosbot_hardware_interfaces: + type: git + url: https://github.com/husarion/rosbot_hardware_interfaces.git + version: da1805839aaa21b8341a9c39498c96d9a1a4f87d diff --git a/rosbot/rosbot_simulation.repos b/rosbot/rosbot_simulation.repos index bded3899..9c168c64 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: 217b09830f5f42930098b9992eda41710702b625 husarion_gz_worlds: type: git url: https://github.com/husarion/husarion_gz_worlds - version: main + version: c0ff83a476f6e0bc250c763a806bf1769a00f515 + ros_components_description: + type: git + url: https://github.com/husarion/ros_components_description.git + version: 1cb25600afa5941d21d48c0af8e63ad2eb3afaa0 + ros2_controllers: # Bug: There is no nice way to change `sensor_name` imu_bradcaster param when spawning multiple robots -> ros2_control refer only to single imu entity + type: git + url: https://github.com/husarion/ros2_controllers/ + version: 9da42a07a83bbf3faf5cad11793fff22f25068af diff --git a/rosbot_bringup/config/laser_filter.yaml b/rosbot_bringup/config/laser_filter.yaml new file mode 100644 index 00000000..d00d4ea0 --- /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..9ad050c9 100644 --- a/rosbot_bringup/launch/bringup.launch.py +++ b/rosbot_bringup/launch/bringup.launch.py @@ -13,78 +13,61 @@ # 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 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( @@ -100,15 +83,35 @@ def generate_launch_description(): namespace=namespace, ) + laser_filter_config = PathJoinSubstitution([rosbot_bringup, "config", "laser_filter.yaml"]) + + laser_filter_node = Node( + package="laser_filters", + executable="scan_to_scan_filter_chain", + parameters=[laser_filter_config], + remappings=[ + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ], + 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..8cdc4c65 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,59 @@ 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, [] # All events have been set + time.sleep(0.1) # Short interval between checks + + # Identify which events were not set + 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/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index 5a218674..0863d9d7 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -14,8 +14,15 @@ # limitations under the License. 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, @@ -23,47 +30,25 @@ PathJoinSubstitution, PythonExpression, ) -from launch_ros.actions import Node, SetParameter +from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): namespace = LaunchConfiguration("namespace") + mecanum = LaunchConfiguration("mecanum") + use_sim = LaunchConfiguration("use_sim", default="False") + declare_namespace_arg = DeclareLaunchArgument( "namespace", default_value="", 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"], + description="Whether to use mecanum drive controller (otherwise diff drive controller is used)", ) controller_config_name = PythonExpression( @@ -83,7 +68,6 @@ def generate_launch_description(): default=[namespace_ext, "controller_manager"], ) - # Get URDF via xacro robot_description_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), @@ -97,14 +81,10 @@ def generate_launch_description(): ), " mecanum:=", mecanum, - " use_sim:=", - use_sim, - " use_gpu:=", - use_gpu, - " simulation_engine:=", - simulation_engine, " namespace:=", namespace, + " use_sim:=", + use_sim, ] ) robot_description = {"robot_description": robot_description_content} @@ -120,10 +100,7 @@ def generate_launch_description(): control_node = Node( package="controller_manager", executable="ros2_control_node", - parameters=[ - robot_description, - robot_controllers, - ], + parameters=[robot_description, robot_controllers], remappings=[ ("imu_sensor_node/imu", "/_imu/data_raw"), ("~/motors_cmd", "/_motors_cmd"), @@ -183,27 +160,51 @@ def generate_launch_description(): remappings=[("/tf", "tf"), ("/tf_static", "tf_static")], ) - # 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, ], ) + 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), + ) + ) + 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, + joint_state_monitor, + robot_controller_monitor, + imu_broadcaster_monitor, ] ) diff --git a/rosbot_controller/package.xml b/rosbot_controller/package.xml index 608a0348..6e162921 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,39 +17,23 @@ ament_cmake + controller_manager + diff_drive_controller + imu_sensor_broadcaster + joint_state_broadcaster launch launch_ros - - xacro - controller_manager + mecanum_drive_controller 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 100% rename from rosbot_description/env-hooks/rosbot_description.sh.in rename to rosbot_description/hooks/setup_envs.sh.in diff --git a/rosbot_description/package.xml b/rosbot_description/package.xml index 64f8f4f3..eb9c1995 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/ diff --git a/rosbot_description/urdf/body.urdf.xacro b/rosbot_description/urdf/body.urdf.xacro index 647087b0..459b4a9a 100644 --- a/rosbot_description/urdf/body.urdf.xacro +++ b/rosbot_description/urdf/body.urdf.xacro @@ -1,7 +1,7 @@ - + diff --git a/rosbot_description/urdf/rosbot.urdf.xacro b/rosbot_description/urdf/rosbot.urdf.xacro index eccac3db..99b8c88d 100644 --- a/rosbot_description/urdf/rosbot.urdf.xacro +++ b/rosbot_description/urdf/rosbot.urdf.xacro @@ -2,7 +2,6 @@ - @@ -11,7 +10,6 @@ @@ -20,7 +18,7 @@ parent_link="cover_link" xyz="0.02 0.0 0.0" rpy="0.0 0.0 0.0" - use_gpu="$(arg use_gpu)" + use_gpu="true" namespace="$(arg namespace)" simulation_engine="$(arg simulation_engine)" /> diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index 48b30f2c..1df8eaca 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 - + @@ -52,7 +41,7 @@ - + rosbot_hardware_interfaces/RosbotSystem @@ -101,7 +90,7 @@ - + @@ -140,16 +129,16 @@ - + true 25 - ${namespace_ext}imu/data_raw + ${ns}imu/data_raw false false imu_link imu_link - ${namespace_ext} + ${namespace} diff --git a/rosbot_gazebo/config/gz_remappings.yaml b/rosbot_gazebo/config/gz_remappings.yaml index 550cc139..25bb71a7 100644 --- a/rosbot_gazebo/config/gz_remappings.yaml +++ b/rosbot_gazebo/config/gz_remappings.yaml @@ -4,48 +4,58 @@ gz_type_name: ignition.msgs.Clock direction: GZ_TO_ROS + - topic_name: /cmd_vel + ros_type_name: geometry_msgs/msg/Twist + gz_type_name: ignition.msgs.Twist + direction: GZ_TO_ROS + - topic_name: /scan ros_type_name: sensor_msgs/msg/LaserScan gz_type_name: ignition.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 + 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 + 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 + 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 + 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 + direction: GZ_TO_ROS - topic_name: /range/fl ros_type_name: sensor_msgs/msg/LaserScan gz_type_name: ignition.msgs.LaserScan + direction: GZ_TO_ROS - topic_name: /range/fr ros_type_name: sensor_msgs/msg/LaserScan gz_type_name: ignition.msgs.LaserScan + direction: GZ_TO_ROS - topic_name: /range/rl ros_type_name: sensor_msgs/msg/LaserScan gz_type_name: ignition.msgs.LaserScan + direction: GZ_TO_ROS - topic_name: /range/rr ros_type_name: sensor_msgs/msg/LaserScan gz_type_name: ignition.msgs.LaserScan + direction: GZ_TO_ROS diff --git a/rosbot_gazebo/launch/simulation.launch.py b/rosbot_gazebo/launch/simulation.launch.py index ab6afe7e..d79df561 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="0.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..d0b28cee 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="0.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,28 +82,47 @@ 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", @@ -102,7 +132,6 @@ def generate_launch_description(): ("/tf", "tf"), ("/tf_static", "tf_static"), ], - output="screen", namespace=namespace, ) @@ -117,10 +146,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 +154,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 e45d4845..5fa4a0c9 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,26 @@ https://github.com/husarion/rosbot_ros/issues Jakub Delicat - Rafal Gorecki Krzysztof Wojciechowski - - rosbot_bringup - - launch - launch_ros + Rafal Gorecki husarion_gz_worlds - ros_gz_sim - - ros_gz_bridge ign_ros2_control + launch + launch_ros nav2_common + ros_gz_bridge + ros_gz_sim + rosbot_bringup - python3-pytest + geometry_msgs launch - launch_ros launch_pytest - - tf_transformations - python-transforms3d-pip + launch_ros nav_msgs - geometry_msgs + python-transforms3d-pip + 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..5e4c61a7 100644 --- a/rosbot_gazebo/test/test_utils.py +++ b/rosbot_gazebo/test/test_utils.py @@ -60,7 +60,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 +130,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,22 +166,22 @@ 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): 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..389916fc 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 dd16d2b8..ca25a630 100755 --- a/rosbot_utils/rosbot_utils/flash-firmware.py +++ b/rosbot_utils/rosbot_utils/flash-firmware.py @@ -43,35 +43,35 @@ def get_raspberry_pi_model(): 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" @@ -106,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(): @@ -149,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 b10f8516..00000000 --- a/tools/Dockerfile +++ /dev/null @@ -1,97 +0,0 @@ -ARG ROS_DISTRO=humble -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 76ff00e0..00000000 --- a/tools/Dockerfile.dev +++ /dev/null @@ -1,36 +0,0 @@ -ARG ROS_DISTRO=humble -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