From 39504b89ef7d7ecd7c40d92b8837bb2f670a6000 Mon Sep 17 00:00:00 2001 From: Rafal Gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Wed, 27 Nov 2024 13:24:42 +0100 Subject: [PATCH 01/17] Cleaner logs from flash script (#108) * Clean up output of flash script * Pre commit * Cleaner * Specify concrete commands * Simplify * pre-commit fix * Clean up usb firmware --- README.md | 2 + .../rosbot_utils/flash-firmware-usb.py | 41 +++++++------ rosbot_utils/rosbot_utils/flash-firmware.py | 61 ++++++++++--------- 3 files changed, 59 insertions(+), 45 deletions(-) diff --git a/README.md b/README.md index 4ec883ba..aefe590a 100644 --- a/README.md +++ b/README.md @@ -78,8 +78,10 @@ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release Flash firmware: ```bash +sudo su source install/setup.bash ros2 run rosbot_utils flash_firmware +exit ``` Running: diff --git a/rosbot_utils/rosbot_utils/flash-firmware-usb.py b/rosbot_utils/rosbot_utils/flash-firmware-usb.py index 310af7a8..3b2fe9d4 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") 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__": From 1f2a8ae368c816dcb7d65966b41221644646656b Mon Sep 17 00:00:00 2001 From: Rafal Gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Thu, 28 Nov 2024 09:39:00 +0100 Subject: [PATCH 02/17] Clean up unused arguments (#118) * Add scan filter * Clean up * undo * added docker setup for local build * broken ros2 controller - previous worked fine * Clean up unused arguments * Fix workflow: Remove spellcheck (redundant), fix integration check * Fix multiple robot spawn * fixes * Delete tools folder * Pre-commit update and remove redundant flake8 * Fix some tests and set hash in vcs * Clean up * Move controller test to bringup and reduce test time * Fix lidar in simulation * Rename test files * Fix tests * Add scan_filtered test and improve workpace tree --------- Co-authored-by: Dominik Nowak --- .github/workflows/tests.yaml | 15 -- .pre-commit-config.yaml | 10 +- .pyspelling.yaml | 54 ------ .wordlist.txt | 161 ---------------- README.md | 44 ++--- docker/.env | 2 +- docker/Dockerfile | 5 +- docker/compose.yaml | 2 +- docker/justfile | 4 +- rosbot/rosbot_hardware.repos | 8 +- rosbot/rosbot_simulation.repos | 6 +- rosbot_bringup/config/laser_filter.yaml | 19 ++ rosbot_bringup/launch/bringup.launch.py | 33 ++-- rosbot_bringup/package.xml | 1 + ...t_diff_drive_ekf.py => test_diff_drive.py} | 8 +- rosbot_bringup/test/test_flake8.py | 23 --- .../{test_mecanum_ekf.py => test_mecanum.py} | 8 +- ...t_multirobot_ekf.py => test_multirobot.py} | 17 +- ...e_ekf.py => test_namespaced_diff_drive.py} | 8 +- ...anum_ekf.py => test_namespaced_mecanum.py} | 8 +- rosbot_bringup/test/test_utils.py | 125 ++++++++++-- rosbot_controller/launch/controller.launch.py | 51 ++--- .../test/test_diff_drive_controllers.py | 89 --------- rosbot_controller/test/test_flake8.py | 23 --- .../test/test_mecanum_controllers.py | 89 --------- .../test/test_multirobot_controllers.py | 71 ------- .../test_namespaced_diff_drive_controllers.py | 90 --------- .../test_namespaced_mecanum_controllers.py | 90 --------- rosbot_controller/test/test_xacro.py | 11 +- rosbot_description/urdf/body.urdf.xacro | 2 +- rosbot_description/urdf/rosbot.urdf.xacro | 4 +- .../urdf/rosbot_macro.urdf.xacro | 33 ++-- rosbot_gazebo/config/gz_remappings.yaml | 20 +- rosbot_gazebo/launch/simulation.launch.py | 182 ++++++------------ rosbot_gazebo/launch/spawn.launch.py | 104 +++++++--- ...drive_simulation.py => test_diff_drive.py} | 10 +- rosbot_gazebo/test/test_flake8.py | 23 --- ..._mecanum_simulation.py => test_mecanum.py} | 10 +- ...ation.py => test_multirobot_diff_drive.py} | 11 +- ...mulation.py => test_multirobot_mecanum.py} | 13 +- ...ation.py => test_namespaced_diff_drive.py} | 10 +- ...mulation.py => test_namespaced_mecanum.py} | 12 +- rosbot_gazebo/test/test_utils.py | 38 ++-- rosbot_utils/package.xml | 1 - rosbot_utils/test/test_flake8.py | 23 --- tools/Dockerfile | 97 ---------- tools/Dockerfile.dev | 36 ---- tools/compose.dev.yaml | 25 --- tools/compose.ros2router.yaml | 52 ----- tools/compose.yaml | 25 --- tools/healthcheck.cpp | 61 ------ tools/healthcheck.sh | 17 -- 52 files changed, 412 insertions(+), 1472 deletions(-) delete mode 100644 .pyspelling.yaml delete mode 100644 .wordlist.txt create mode 100644 rosbot_bringup/config/laser_filter.yaml rename rosbot_bringup/test/{test_diff_drive_ekf.py => test_diff_drive.py} (87%) delete mode 100644 rosbot_bringup/test/test_flake8.py rename rosbot_bringup/test/{test_mecanum_ekf.py => test_mecanum.py} (87%) rename rosbot_bringup/test/{test_multirobot_ekf.py => test_multirobot.py} (81%) rename rosbot_bringup/test/{test_namespaced_diff_drive_ekf.py => test_namespaced_diff_drive.py} (87%) rename rosbot_bringup/test/{test_namespaced_mecanum_ekf.py => test_namespaced_mecanum.py} (87%) delete mode 100644 rosbot_controller/test/test_diff_drive_controllers.py delete mode 100644 rosbot_controller/test/test_flake8.py delete mode 100644 rosbot_controller/test/test_mecanum_controllers.py delete mode 100644 rosbot_controller/test/test_multirobot_controllers.py delete mode 100644 rosbot_controller/test/test_namespaced_diff_drive_controllers.py delete mode 100644 rosbot_controller/test/test_namespaced_mecanum_controllers.py rename rosbot_gazebo/test/{test_diff_drive_simulation.py => test_diff_drive.py} (90%) delete mode 100644 rosbot_gazebo/test/test_flake8.py rename rosbot_gazebo/test/{test_mecanum_simulation.py => test_mecanum.py} (90%) rename rosbot_gazebo/test/{test_multirobot_diff_drive_simulation.py => test_multirobot_diff_drive.py} (91%) rename rosbot_gazebo/test/{test_multirobot_mecanum_simulation.py => test_multirobot_mecanum.py} (91%) rename rosbot_gazebo/test/{test_namespaced_diff_drive_simulation.py => test_namespaced_diff_drive.py} (90%) rename rosbot_gazebo/test/{test_namespaced_mecanum_simulation.py => test_namespaced_mecanum.py} (91%) delete mode 100644 rosbot_utils/test/test_flake8.py delete mode 100644 tools/Dockerfile delete mode 100644 tools/Dockerfile.dev delete mode 100644 tools/compose.dev.yaml delete mode 100644 tools/compose.ros2router.yaml delete mode 100644 tools/compose.yaml delete mode 100644 tools/healthcheck.cpp delete mode 100755 tools/healthcheck.sh diff --git a/.github/workflows/tests.yaml b/.github/workflows/tests.yaml index e8803988..3833fde5 100644 --- a/.github/workflows/tests.yaml +++ b/.github/workflows/tests.yaml @@ -17,15 +17,6 @@ jobs: 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 - industrial_ci: name: Industrial CI runs-on: ubuntu-22.04 @@ -38,10 +29,6 @@ jobs: - name: Checkout uses: actions/checkout@v3 - - name: Act + Docker fix - run: | - sudo chown runner:docker /var/run/docker.sock - - name: Setup ROS2 Workspace and Clone Repositories run: | mkdir -p src @@ -49,7 +36,6 @@ jobs: 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/ cp -r src/ros2_controllers/imu_sensor_broadcaster src/ rm -rf src/ros2_controllers @@ -58,7 +44,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..8aca7908 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,7 +1,7 @@ --- 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 @@ -16,7 +16,7 @@ repos: args: [--pytest-test-first] - repo: https://github.com/codespell-project/codespell - rev: v2.2.6 + rev: v2.3.0 hooks: - id: codespell name: codespell @@ -33,13 +33,13 @@ 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, @@ -71,7 +71,7 @@ repos: # 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] 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/README.md b/README.md index aefe590a..1114fce3 100644 --- a/README.md +++ b/README.md @@ -48,9 +48,9 @@ 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 +mkdir -p ros2_ws cd ros2_ws -git clone https://github.com/husarion/rosbot_ros src/ +git clone https://github.com/husarion/rosbot_ros src/rosbot_ros ``` ### Build and run on hardware @@ -62,17 +62,14 @@ export HUSARION_ROS_BUILD=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 --cmake-args -DCMAKE_BUILD_TYPE=Release ``` Flash firmware: @@ -93,25 +90,6 @@ ros2 launch rosbot_bringup combined.launch.py ### Build and run Gazebo simulation -Prerequisites: - -> [!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 -> ``` - -If you have installed multiple versions of Gazebo use the global variable to select the correct one: - -```bash -export GZ_VERSION=fortress -``` - Building: ```bash @@ -119,16 +97,16 @@ export HUSARION_ROS_BUILD=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 --cmake-args -DCMAKE_BUILD_TYPE=Release ``` Running: @@ -175,7 +153,7 @@ colcon test-result --verbose ### Format python code with [Black](https://github.com/psf/black) ```bash -cd src/ +cd src/rosbot_ros black rosbot* ``` diff --git a/docker/.env b/docker/.env index 7725261e..9a9566ef 100644 --- a/docker/.env +++ b/docker/.env @@ -1,2 +1,2 @@ SERIAL_PORT=/dev/ttyUSB0 -ROS_NAMESPACE=robot1 \ No newline at end of file +ROS_NAMESPACE=robot1 diff --git a/docker/Dockerfile b/docker/Dockerfile index d89cffbe..3dfb0f67 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -14,9 +14,6 @@ RUN apt-get update && apt-get install -y \ 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 @@ -29,4 +26,4 @@ RUN apt update && \ 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 + rm -rf build log src diff --git a/docker/compose.yaml b/docker/compose.yaml index a4fff438..fb9aaf96 100644 --- a/docker/compose.yaml +++ b/docker/compose.yaml @@ -16,4 +16,4 @@ services: mecanum:=${MECANUM:-False} serial_port:=$SERIAL_PORT serial_baudrate:=576000 - # namespace:=${ROS_NAMESPACE:-rosbot} \ No newline at end of file + # namespace:=${ROS_NAMESPACE:-rosbot} diff --git a/docker/justfile b/docker/justfile index ff972714..50fbe34d 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:=/${ROS_NAMESPACE}" flash-firmware: _install-yq _run-as-user #!/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/rosbot_hardware.repos b/rosbot/rosbot_hardware.repos index 1a054c48..55bca8c2 100644 --- a/rosbot/rosbot_hardware.repos +++ b/rosbot/rosbot_hardware.repos @@ -2,7 +2,7 @@ repositories: rosbot_hardware_interfaces: type: git url: https://github.com/husarion/rosbot_hardware_interfaces.git - version: main + version: da1805839aaa21b8341a9c39498c96d9a1a4f87d ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git @@ -10,12 +10,12 @@ repositories: 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 diff --git a/rosbot/rosbot_simulation.repos b/rosbot/rosbot_simulation.repos index bded3899..ee822f11 100644 --- a/rosbot/rosbot_simulation.repos +++ b/rosbot/rosbot_simulation.repos @@ -2,4 +2,8 @@ repositories: husarion_gz_worlds: type: git url: https://github.com/husarion/husarion_gz_worlds - version: main + version: c0ff83a476f6e0bc250c763a806bf1769a00f515 + 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/launch/bringup.launch.py b/rosbot_bringup/launch/bringup.launch.py index 06ab2112..e3e1588f 100644 --- a/rosbot_bringup/launch/bringup.launch.py +++ b/rosbot_bringup/launch/bringup.launch.py @@ -39,21 +39,6 @@ def generate_launch_description(): 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") @@ -79,8 +64,6 @@ def generate_launch_description(): launch_arguments={ "use_sim": use_sim, "mecanum": mecanum, - "use_gpu": use_gpu, - "simulation_engine": simulation_engine, "namespace": namespace, }.items(), ) @@ -100,15 +83,27 @@ 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, + ) + actions = [ 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, robot_localization_node, + laser_filter_node, ] return LaunchDescription(actions) diff --git a/rosbot_bringup/package.xml b/rosbot_bringup/package.xml index d88edc1b..21c94e76 100644 --- a/rosbot_bringup/package.xml +++ b/rosbot_bringup/package.xml @@ -16,6 +16,7 @@ launch launch_ros + laser_filters rosbot_controller robot_localization micro_ros_agent diff --git a/rosbot_bringup/test/test_diff_drive_ekf.py b/rosbot_bringup/test/test_diff_drive.py similarity index 87% rename from rosbot_bringup/test/test_diff_drive_ekf.py rename to rosbot_bringup/test/test_diff_drive.py index 5764616a..f81f98c3 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 @@ -41,7 +41,6 @@ def generate_test_description(): launch_arguments={ "use_sim": "False", "mecanum": "False", - "use_gpu": "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_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 87% rename from rosbot_bringup/test/test_mecanum_ekf.py rename to rosbot_bringup/test/test_mecanum.py index 066641da..2b652094 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 @@ -41,7 +41,6 @@ def generate_test_description(): launch_arguments={ "use_sim": "False", "mecanum": "True", - "use_gpu": "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 81% rename from rosbot_bringup/test/test_multirobot_ekf.py rename to rosbot_bringup/test/test_multirobot.py index 8086fb3a..2abcf4f7 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 @@ -45,18 +45,19 @@ def generate_test_description(): launch_arguments={ "use_sim": "False", "mecanum": "False", - "use_gpu": "False", "namespace": robot_names[i], }.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 +66,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 87% rename from rosbot_bringup/test/test_namespaced_diff_drive_ekf.py rename to rosbot_bringup/test/test_namespaced_diff_drive.py index 3bb6ff72..40839ad7 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 @@ -41,7 +41,6 @@ def generate_test_description(): launch_arguments={ "use_sim": "False", "mecanum": "False", - "use_gpu": "False", "namespace": "rosbot2r", }.items(), ) @@ -58,10 +57,7 @@ def test_namespaced_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_namespaced_mecanum_ekf.py b/rosbot_bringup/test/test_namespaced_mecanum.py similarity index 87% rename from rosbot_bringup/test/test_namespaced_mecanum_ekf.py rename to rosbot_bringup/test/test_namespaced_mecanum.py index 0beb2aa1..595b50d6 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 @@ -41,7 +41,6 @@ def generate_test_description(): launch_arguments={ "use_sim": "False", "mecanum": "True", - "use_gpu": "False", "namespace": "rosbot2r", }.items(), ) @@ -58,10 +57,7 @@ def test_namespaced_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_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..d169d7a8 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -39,9 +39,15 @@ def generate_launch_description(): declare_mecanum_arg = DeclareLaunchArgument( "mecanum", default_value="False", - description=( - "Whether to use mecanum drive controller (otherwise diff drive controller is used)" - ), + description="Whether to use mecanum drive controller (otherwise diff drive controller is used)", + ) + + simulation_engine = LaunchConfiguration("simulation_engine") + declare_simulation_engine_arg = DeclareLaunchArgument( + "simulation_engine", + default_value="ignition-gazebo", + description="Which simulation engine to be used", + choices=["ignition-gazebo", "webots"], ) use_sim = LaunchConfiguration("use_sim") @@ -51,21 +57,6 @@ def generate_launch_description(): description="Whether simulation is used", ) - use_gpu = LaunchConfiguration("use_gpu") - declare_use_gpu_arg = DeclareLaunchArgument( - "use_gpu", - default_value="False", - description="Whether GPU acceleration is used", - ) - - simulation_engine = LaunchConfiguration("simulation_engine") - declare_simulation_engine_arg = DeclareLaunchArgument( - "simulation_engine", - default_value="webots", - description="Which simulation engine to be used", - choices=["ignition-gazebo", "gazebo-classic", "webots"], - ) - controller_config_name = PythonExpression( [ "'mecanum_drive_controller.yaml' if ", @@ -83,7 +74,6 @@ def generate_launch_description(): default=[namespace_ext, "controller_manager"], ) - # Get URDF via xacro robot_description_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), @@ -97,14 +87,12 @@ def generate_launch_description(): ), " mecanum:=", mecanum, - " use_sim:=", - use_sim, - " use_gpu:=", - use_gpu, - " simulation_engine:=", - simulation_engine, " namespace:=", namespace, + " simulation_engine:=", + simulation_engine, + " use_sim:=", + use_sim, ] ) robot_description = {"robot_description": robot_description_content} @@ -120,10 +108,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,11 +168,10 @@ 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, @@ -198,9 +182,8 @@ def generate_launch_description(): [ declare_namespace_arg, declare_mecanum_arg, - declare_use_sim_arg, - declare_use_gpu_arg, declare_simulation_engine_arg, + declare_use_sim_arg, SetParameter("use_sim_time", value=use_sim), control_node, robot_state_pub_node, 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_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/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..44c92a71 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,46 @@ 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") + mecanum = LaunchConfiguration("mecanum") + 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") - gz_args = f"--headless-rendering -s -v 4 -r {world}" if eval(headless) else f"-r {world}" + 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)" + ), + ) + + 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 +80,47 @@ 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={ + "mecanum": mecanum, + "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..64134d39 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, @@ -26,35 +26,55 @@ def generate_launch_description(): + mecanum = LaunchConfiguration("mecanum") namespace = LaunchConfiguration("namespace") + x = LaunchConfiguration("x") + y = LaunchConfiguration("y") + z = LaunchConfiguration("z") + roll = LaunchConfiguration("roll") + pitch = LaunchConfiguration("pitch") + yaw = LaunchConfiguration("yaw") + + declare_mecanum_arg = DeclareLaunchArgument( + "mecanum", + default_value="False", + description=( + "Whether to use mecanum drive controller (otherwise diff drive controller is used)" + ), + ) + 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." + ) + + declare_pitch_arg = DeclareLaunchArgument( + "pitch", default_value="0.0", description="Initial robot 'pitch' orientation." ) - use_gpu = LaunchConfiguration("use_gpu") - declare_use_gpu_arg = DeclareLaunchArgument( - "use_gpu", - default_value="True", - description="Whether GPU acceleration is used", + declare_yaw_arg = DeclareLaunchArgument( + "yaw", default_value="0.0", description="Initial robot 'yaw' orientation." ) - robot_name = PythonExpression( - ["'rosbot'", " if '", namespace, "' == '' ", "else ", "'", namespace, "'"] + namespace_ext = PythonExpression( + ["''", " if '", namespace, "' == '' ", "else ", "'/", namespace, "'"] ) gz_remappings_file = PathJoinSubstitution( @@ -71,28 +91,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 +141,6 @@ def generate_launch_description(): ("/tf", "tf"), ("/tf_static", "tf_static"), ], - output="screen", namespace=namespace, ) @@ -119,20 +157,22 @@ def generate_launch_description(): launch_arguments={ "mecanum": mecanum, "use_sim": "True", - "use_gpu": use_gpu, - "simulation_engine": "ignition-gazebo", "namespace": namespace, }.items(), ) 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_namespace_arg, + 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/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..a16b1f8a 100644 --- a/rosbot_gazebo/test/test_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_diff_drive.py @@ -44,13 +44,9 @@ 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"] ), }.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..1ed8a83d 100644 --- a/rosbot_gazebo/test/test_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_mecanum.py @@ -45,13 +45,9 @@ 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"] ), }.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..ec054f9d 100644 --- a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_diff_drive.py @@ -31,19 +31,18 @@ @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}", "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..17162797 100644 --- a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_mecanum.py @@ -31,20 +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" - ), - "robots:=robot1={y: -4.0}; robot2={y: 0.0};", + "gz_headless_mode:=True", + f"gz_world:={gz_world_path}", "mecanum:=True", - "headless:=True", + "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 90% rename from rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py rename to rosbot_gazebo/test/test_namespaced_diff_drive.py index c9bef37f..44312c53 100644 --- a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_namespaced_diff_drive.py @@ -44,13 +44,9 @@ 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", }.items(), diff --git a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py b/rosbot_gazebo/test/test_namespaced_mecanum.py similarity index 91% rename from rosbot_gazebo/test/test_namespaced_mecanum_simulation.py rename to rosbot_gazebo/test/test_namespaced_mecanum.py index 6ba653d0..e93c29d9 100644 --- a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_namespaced_mecanum.py @@ -44,15 +44,11 @@ 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", + "mecanum": "True", "namespace": "rosbot2r", }.items(), ) diff --git a/rosbot_gazebo/test/test_utils.py b/rosbot_gazebo/test/test_utils.py index 6b76dda9..efbb8022 100644 --- a/rosbot_gazebo/test/test_utils.py +++ b/rosbot_gazebo/test/test_utils.py @@ -60,7 +60,7 @@ 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 +128,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 +164,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..8f995689 100644 --- a/rosbot_utils/package.xml +++ b/rosbot_utils/package.xml @@ -24,7 +24,6 @@ launch_ros ament_copyright - ament_flake8 ament_pep257 python3-pytest 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 From f09c16ae4353b2b44c0186259e35585ec42d5e40 Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Thu, 28 Nov 2024 11:46:36 +0100 Subject: [PATCH 03/17] Applied xml schemas for xmllint (#101) * applied package.xml template Signed-off-by: Jakub Delicat * chronology Signed-off-by: Jakub Delicat * Support Signed-off-by: Jakub Delicat * Removed unnecessary dep Signed-off-by: Jakub Delicat * DDS xml lint --------- Signed-off-by: Jakub Delicat Co-authored-by: rafal-gorecki --- .pre-commit-config.yaml | 5 +++++ rosbot/package.xml | 2 ++ rosbot_bringup/config/microros_localhost_only.xml | 8 ++++---- rosbot_bringup/package.xml | 2 ++ rosbot_controller/package.xml | 2 ++ rosbot_description/package.xml | 4 +++- rosbot_gazebo/package.xml | 4 +++- rosbot_utils/package.xml | 2 ++ 8 files changed, 23 insertions(+), 6 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 8aca7908..00bec250 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -76,3 +76,8 @@ repos: - id: doc8 args: [--max-line-length=100, --ignore=D001] exclude: ^.*\/CHANGELOG\.rst/.*$ + + - repo: https://github.com/comkieffer/pre-commit-xmllint.git + rev: 1.0.0 + hooks: + - id: xmllint diff --git a/rosbot/package.xml b/rosbot/package.xml index 6533fef6..bd038d29 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/ 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/package.xml b/rosbot_bringup/package.xml index 21c94e76..f44fddd8 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/ diff --git a/rosbot_controller/package.xml b/rosbot_controller/package.xml index 608a0348..5066af4c 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/ diff --git a/rosbot_description/package.xml b/rosbot_description/package.xml index 64f8f4f3..9c9d5ad6 100644 --- a/rosbot_description/package.xml +++ b/rosbot_description/package.xml @@ -4,15 +4,17 @@ rosbot_description 0.13.2 ROSbot 2, 2R, PRO description package + Husarion + Apache License 2.0 https://husarion.com/ https://github.com/husarion/rosbot_ros https://github.com/husarion/rosbot_ros/issues - Jakub Delicat Maciej Stepien + Jakub Delicat ament_cmake diff --git a/rosbot_gazebo/package.xml b/rosbot_gazebo/package.xml index e45d4845..5c7a01a9 100644 --- a/rosbot_gazebo/package.xml +++ b/rosbot_gazebo/package.xml @@ -4,16 +4,18 @@ rosbot_gazebo 0.13.2 Gazebo Ignition simulation for ROSbot 2, 2R, PRO + Husarion + Apache License 2.0 https://husarion.com/ https://github.com/husarion/rosbot_ros https://github.com/husarion/rosbot_ros/issues + Krzysztof Wojciechowski Jakub Delicat Rafal Gorecki - Krzysztof Wojciechowski rosbot_bringup diff --git a/rosbot_utils/package.xml b/rosbot_utils/package.xml index 8f995689..96ba3853 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/ From 678324db0513da9953a8eb613337f543ae4d4922 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 29 Nov 2024 09:59:23 +0100 Subject: [PATCH 04/17] Pre-commit --- rosbot_bringup/test/test_multirobot.py | 2 +- rosbot_gazebo/test/test_utils.py | 4 +++- rosbot_utils/rosbot_utils/flash-firmware-usb.py | 1 - 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/rosbot_bringup/test/test_multirobot.py b/rosbot_bringup/test/test_multirobot.py index 2abcf4f7..621316f4 100644 --- a/rosbot_bringup/test/test_multirobot.py +++ b/rosbot_bringup/test/test_multirobot.py @@ -49,7 +49,7 @@ def generate_test_description(): }.items(), ) - delayed_bringup = TimerAction(period=5.0*i, actions=[bringup_launch]) + delayed_bringup = TimerAction(period=5.0 * i, actions=[bringup_launch]) actions.append(delayed_bringup) return LaunchDescription(actions) diff --git a/rosbot_gazebo/test/test_utils.py b/rosbot_gazebo/test/test_utils.py index efbb8022..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_filtered", 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) diff --git a/rosbot_utils/rosbot_utils/flash-firmware-usb.py b/rosbot_utils/rosbot_utils/flash-firmware-usb.py index 3b2fe9d4..389916fc 100755 --- a/rosbot_utils/rosbot_utils/flash-firmware-usb.py +++ b/rosbot_utils/rosbot_utils/flash-firmware-usb.py @@ -124,7 +124,6 @@ def main(): flasher = FirmwareFlasher(binary_file, port) flasher.flash_firmware() - print("Done.") if __name__ == "__main__": From 9c3307cd68fc369b75d1462e916b265a20d1b3ac Mon Sep 17 00:00:00 2001 From: Rafal Gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Mon, 2 Dec 2024 10:24:50 +0100 Subject: [PATCH 05/17] Humble docker (#124) * Add hardware and sim Docker * Fix hardware docker * Fixes * Use packages-up-to * Add description to compose --- README.md | 8 ++++---- docker/.env | 3 +-- docker/Dockerfile | 29 ---------------------------- docker/Dockerfile.hardware | 34 +++++++++++++++++++++++++++++++++ docker/Dockerfile.simulation | 35 ++++++++++++++++++++++++++++++++++ docker/compose.hardware.yaml | 22 +++++++++++++++++++++ docker/compose.simulation.yaml | 32 +++++++++++++++++++++++++++++++ docker/compose.yaml | 19 ------------------ docker/justfile | 14 +++++++------- rosbot/package.xml | 5 ++--- 10 files changed, 137 insertions(+), 64 deletions(-) delete mode 100644 docker/Dockerfile create mode 100644 docker/Dockerfile.hardware create mode 100644 docker/Dockerfile.simulation create mode 100644 docker/compose.hardware.yaml create mode 100644 docker/compose.simulation.yaml delete mode 100644 docker/compose.yaml diff --git a/README.md b/README.md index 1114fce3..b6a697b9 100644 --- a/README.md +++ b/README.md @@ -58,7 +58,7 @@ git clone https://github.com/husarion/rosbot_ros src/rosbot_ros Building: ```bash -export HUSARION_ROS_BUILD=hardware +export HUSARION_ROS_BUILD_TYPE=hardware source /opt/ros/$ROS_DISTRO/setup.bash @@ -69,7 +69,7 @@ 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 --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release +colcon build --symlink-install --packages-up-to rosbot --cmake-args -DCMAKE_BUILD_TYPE=Release ``` Flash firmware: @@ -93,7 +93,7 @@ ros2 launch rosbot_bringup combined.launch.py Building: ```bash -export HUSARION_ROS_BUILD=simulation +export HUSARION_ROS_BUILD_TYPE=simulation source /opt/ros/$ROS_DISTRO/setup.bash @@ -106,7 +106,7 @@ cp -r src/ros2_controllers/imu_sensor_broadcaster src && rm -rf src/ros2_control sudo rosdep init rosdep update --rosdistro $ROS_DISTRO rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y -colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release +colcon build --symlink-install --packages-up-to rosbot --cmake-args -DCMAKE_BUILD_TYPE=Release ``` Running: diff --git a/docker/.env b/docker/.env index 9a9566ef..05b532ca 100644 --- a/docker/.env +++ b/docker/.env @@ -1,2 +1 @@ -SERIAL_PORT=/dev/ttyUSB0 -ROS_NAMESPACE=robot1 +ROBOT_NAMESPACE=rosbot diff --git a/docker/Dockerfile b/docker/Dockerfile deleted file mode 100644 index 3dfb0f67..00000000 --- a/docker/Dockerfile +++ /dev/null @@ -1,29 +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 && \ - 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 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..dc90ab5b --- /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/* \ No newline at end of file diff --git a/docker/compose.hardware.yaml b/docker/compose.hardware.yaml new file mode 100644 index 00000000..3ebcf58e --- /dev/null +++ b/docker/compose.hardware.yaml @@ -0,0 +1,22 @@ +# 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 + command: > + ros2 launch rosbot_bringup combined.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..9194cda1 --- /dev/null +++ b/docker/compose.simulation.yaml @@ -0,0 +1,32 @@ +# 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: + - DISPLAY=${DISPLAY:?err} + - NVIDIA_VISIBLE_DEVICES=all + - NVIDIA_DRIVER_CAPABILITIES=all + +x-cpu-config: + &cpu-config + environment: + - 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 fb9aaf96..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} diff --git a/docker/justfile b/docker/justfile index 50fbe34d..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 diff --git a/rosbot/package.xml b/rosbot/package.xml index bd038d29..b39ffd53 100644 --- a/rosbot/package.xml +++ b/rosbot/package.xml @@ -20,9 +20,8 @@ rosbot_bringup rosbot_controller rosbot_description - rosbot_utils - - rosbot_gazebo + rosbot_gazebo + rosbot_utils ament_cmake From a75745c9f10090dccd7cada23b922d8736675639 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Tue, 3 Dec 2024 13:15:15 +0100 Subject: [PATCH 06/17] Add healthcheck + clean up --- .github/workflows/tests.yaml | 15 +-- .pre-commit-config.yaml | 57 +++++---- docker/Dockerfile.simulation | 2 +- rosbot_bringup/launch/bringup.launch.py | 61 +++++----- rosbot_bringup/launch/healthcheck.launch.py | 98 ++++++++++++++++ rosbot_bringup/launch/microros.launch.py | 111 ++++++++++++++++++ rosbot_bringup/package.xml | 13 +- rosbot_bringup/test/test_diff_drive.py | 2 +- rosbot_bringup/test/test_mecanum.py | 3 +- rosbot_bringup/test/test_multirobot.py | 4 +- .../test/test_namespaced_diff_drive.py | 6 +- .../test/test_namespaced_mecanum.py | 7 +- rosbot_controller/launch/controller.launch.py | 17 +-- rosbot_controller/package.xml | 40 +++---- rosbot_description/CMakeLists.txt | 8 +- .../setup_envs.sh.in} | 0 rosbot_gazebo/launch/simulation.launch.py | 11 -- rosbot_gazebo/launch/spawn.launch.py | 11 -- rosbot_gazebo/package.xml | 26 ++-- rosbot_gazebo/test/test_diff_drive.py | 1 + rosbot_gazebo/test/test_mecanum.py | 3 +- .../test/test_multirobot_diff_drive.py | 1 + rosbot_gazebo/test/test_multirobot_mecanum.py | 1 + .../test/test_namespaced_diff_drive.py | 5 +- rosbot_gazebo/test/test_namespaced_mecanum.py | 5 +- rosbot_utils/package.xml | 2 +- 26 files changed, 346 insertions(+), 164 deletions(-) create mode 100644 rosbot_bringup/launch/healthcheck.launch.py create mode 100644 rosbot_bringup/launch/microros.launch.py rename rosbot_description/{env-hooks/rosbot_description.sh.in => hooks/setup_envs.sh.in} (100%) diff --git a/.github/workflows/tests.yaml b/.github/workflows/tests.yaml index 3833fde5..db48c831 100644 --- a/.github/workflows/tests.yaml +++ b/.github/workflows/tests.yaml @@ -6,16 +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 + 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 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 00bec250..d472c928 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -3,17 +3,40 @@ repos: - repo: https://github.com/pre-commit/pre-commit-hooks 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.3.0 @@ -21,7 +44,8 @@ repos: - id: codespell name: codespell description: Checks for common misspellings in text files. - entry: codespell * + entry: codespell + exclude_types: [rst, svg] language: python types: [text] @@ -42,14 +66,7 @@ repos: 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: @@ -69,7 +86,6 @@ repos: entry: ament_copyright language: system - # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 rev: v1.1.2 hooks: @@ -77,7 +93,8 @@ repos: args: [--max-line-length=100, --ignore=D001] exclude: ^.*\/CHANGELOG\.rst/.*$ - - repo: https://github.com/comkieffer/pre-commit-xmllint.git - rev: 1.0.0 + - repo: https://github.com/tier4/pre-commit-hooks-ros + rev: v0.10.0 hooks: - - id: xmllint + - id: prettier-package-xml + - id: sort-package-xml diff --git a/docker/Dockerfile.simulation b/docker/Dockerfile.simulation index dc90ab5b..3dc13122 100644 --- a/docker/Dockerfile.simulation +++ b/docker/Dockerfile.simulation @@ -32,4 +32,4 @@ RUN apt-get update && apt-get install -y \ ros-dev-tools && \ apt-get autoremove -y && \ apt-get clean && \ - rm -rf /var/lib/apt/lists/* \ No newline at end of file + rm -rf /var/lib/apt/lists/* diff --git a/rosbot_bringup/launch/bringup.launch.py b/rosbot_bringup/launch/bringup.launch.py index e3e1588f..b24285c7 100644 --- a/rosbot_bringup/launch/bringup.launch.py +++ b/rosbot_bringup/launch/bringup.launch.py @@ -20,54 +20,53 @@ LaunchConfiguration, PathJoinSubstitution, ) -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") + use_sim = LaunchConfiguration("use_sim", default="False") + + declare_healthcheck_arg = DeclareLaunchArgument( + "healthcheck", + default_value="True", + description="Check if all node are up and ready, if not emit shutdown signal.", + 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", - ) - - 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, "namespace": namespace, + "use_sim": use_sim, }.items(), ) + healthcheck_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([rosbot_bringup, "launch", "healthcheck.launch.py"]) + ) + ) + + # microros_launch = IncludeLaunchDescription( + # PythonLaunchDescriptionSource( + # PathJoinSubstitution([rosbot_bringup, "launch", "microros.launch.py"]) + # ), + # condition=UnlessCondition([use_sim]), + # ) + ekf_config = PathJoinSubstitution([rosbot_bringup, "config", "ekf.yaml"]) robot_localization_node = Node( @@ -97,13 +96,13 @@ def generate_launch_description(): ) actions = [ + declare_healthcheck_arg, declare_namespace_arg, - declare_mecanum_arg, - declare_use_sim_arg, - SetParameter(name="use_sim_time", value=use_sim), controller_launch, - robot_localization_node, + healthcheck_launch, + # microros_launch, laser_filter_node, + robot_localization_node, ] return LaunchDescription(actions) diff --git a/rosbot_bringup/launch/healthcheck.launch.py b/rosbot_bringup/launch/healthcheck.launch.py new file mode 100644 index 00000000..4bcfc1dd --- /dev/null +++ b/rosbot_bringup/launch/healthcheck.launch.py @@ -0,0 +1,98 @@ +# Copyright 2020 ros2_control Development Team +# 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 os +import subprocess + +from launch import LaunchDescription +from launch.actions import EmitEvent, OpaqueFunction, TimerAction +from launch.events import Shutdown +from launch.substitutions import LaunchConfiguration + + +def check_controller_status(context): + healthcheck = LaunchConfiguration("healthcheck").perform(context) + if healthcheck.lower() == "false": + with open("/var/tmp/rosbot_status.txt", "w") as status_file: + status_file.write("healthy") + return + else: + with open("/var/tmp/rosbot_status.txt", "w") as status_file: + status_file.write("unhealthy") + + use_sim = LaunchConfiguration("use_sim", default="False").perform(context) + nodes_to_check = [ + "/controller_manager", + "/ekf_filter_node", + "/imu_broadcaster", + "/joint_state_broadcaster", + "/laser_scan_box_filter", + "/robot_state_publisher", + "/rosbot_base_controller", + "/scan_to_scan_filter_chain", + ] + if use_sim.lower() == "true": + additional_nodes = [ + "/gz_ros2_control", + "/ros_gz_bridge", + ] + else: + additional_nodes = [ + "/imu_sensor_node", + # "/rosbot_ros2_firmware", not visible via USB port + "/rosbot_system_node", + ] + nodes_to_check.extend(additional_nodes) + + namespace = LaunchConfiguration("namespace", default="").perform(context) + ns = "/" + namespace if namespace else "" + nodes_to_check = [ns + node for node in nodes_to_check] + + def get_missing_nodes(): + env = os.environ.copy() + env["ROS_LOCALHOST_ONLY"] = "1" + + result = subprocess.run( + ["ros2", "node", "list"], + stdout=subprocess.PIPE, + text=True, + env=env, + ) + active_nodes = result.stdout.splitlines() + return [node for node in nodes_to_check if node not in active_nodes] + + missing_nodes = get_missing_nodes() + green_color = "\033[92m" + red_color = "\033[91m" + reset_color = "\033[0m" + + if missing_nodes: + print( + f"{red_color}Error: some nodes are missing: {missing_nodes}. Emitting shutdown...{reset_color}" + ) + return [EmitEvent(event=Shutdown())] + else: + print(f"{green_color}All systems are up and running!{reset_color}") + with open("/var/tmp/rosbot_status.txt", "w") as status_file: + status_file.write("healthy") + + +def generate_launch_description(): + check_controller = TimerAction( + period=15.0, + actions=[OpaqueFunction(function=check_controller_status)], + ) + + return LaunchDescription([check_controller]) diff --git a/rosbot_bringup/launch/microros.launch.py b/rosbot_bringup/launch/microros.launch.py new file mode 100644 index 00000000..512419ba --- /dev/null +++ b/rosbot_bringup/launch/microros.launch.py @@ -0,0 +1,111 @@ +# 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 os + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + LogInfo, + OpaqueFunction, + SetEnvironmentVariable, +) +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): + env_setup_actions = [] + + ros_domain_id = os.environ.get("ROS_DOMAIN_ID") + if ros_domain_id: + env_setup_actions.append( + SetEnvironmentVariable(name="XRCE_DOMAIN_ID_OVERRIDE", value=ros_domain_id) + ) + + 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) + + if os.environ.get("ROS_LOCALHOST_ONLY") == "1": + env_setup_actions.extend( + [ + LogInfo( + msg=[ + "ROS_LOCALHOST_ONLY set to 1. Using FASTRTPS_DEFAULT_PROFILES_FILE=", + localhost_only_fastrtps_profiles_file, + ".", + ] + ), + SetEnvironmentVariable(name="RMW_IMPLEMENTATION", value="rmw_fastrtps_cpp"), + SetEnvironmentVariable( + name="FASTRTPS_DEFAULT_PROFILES_FILE", + value=localhost_only_fastrtps_profiles_file, + ), + ] + ) + + microros_agent_node = Node( + package="micro_ros_agent", + executable="micro_ros_agent", + arguments=["serial", "-D", serial_port, "-b", serial_baudrate], + output="screen", + ) + + return env_setup_actions + [microros_agent_node] + else: + microros_agent_node = Node( + package="micro_ros_agent", + executable="micro_ros_agent", + arguments=["serial", "-D", serial_port, "-b", serial_baudrate], + output="screen", + ) + + return env_setup_actions + [microros_agent_node] + + +def generate_launch_description(): + declare_serial_port_arg = DeclareLaunchArgument( + "serial_port", + default_value="/dev/ttySERIAL", + description="Serial port for micro-ROS agent", + ) + + declare_serial_baudrate_arg = DeclareLaunchArgument( + "serial_baudrate", default_value="576000", description="Baud rate for serial communication" + ) + + fastrtps_profiles_file = PathJoinSubstitution( + [FindPackageShare("rosbot_bringup"), "config", "microros_localhost_only.xml"] + ) + declare_localhost_only_fastrtps_profiles_file_arg = DeclareLaunchArgument( + "localhost_only_fastrtps_profiles_file", + default_value=fastrtps_profiles_file, + description=( + "Path to the Fast RTPS default profiles file for Micro-ROS agent for localhost only" + " setup" + ), + ) + + return LaunchDescription( + [ + declare_serial_port_arg, + declare_serial_baudrate_arg, + declare_localhost_only_fastrtps_profiles_file_arg, + OpaqueFunction(function=generate_microros_agent_node), + ] + ) diff --git a/rosbot_bringup/package.xml b/rosbot_bringup/package.xml index f44fddd8..2be81752 100644 --- a/rosbot_bringup/package.xml +++ b/rosbot_bringup/package.xml @@ -16,22 +16,21 @@ Jakub Delicat Rafal Gorecki + laser_filters launch launch_ros - laser_filters - 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.py b/rosbot_bringup/test/test_diff_drive.py index f81f98c3..fa1174d6 100644 --- a/rosbot_bringup/test/test_diff_drive.py +++ b/rosbot_bringup/test/test_diff_drive.py @@ -39,8 +39,8 @@ def generate_test_description(): ) ), launch_arguments={ + "healthcheck": "False", "use_sim": "False", - "mecanum": "False", }.items(), ) diff --git a/rosbot_bringup/test/test_mecanum.py b/rosbot_bringup/test/test_mecanum.py index 2b652094..f31ab78f 100644 --- a/rosbot_bringup/test/test_mecanum.py +++ b/rosbot_bringup/test/test_mecanum.py @@ -39,8 +39,9 @@ def generate_test_description(): ) ), launch_arguments={ - "use_sim": "False", + "healthcheck": "False", "mecanum": "True", + "use_sim": "False", }.items(), ) diff --git a/rosbot_bringup/test/test_multirobot.py b/rosbot_bringup/test/test_multirobot.py index 621316f4..b234c002 100644 --- a/rosbot_bringup/test/test_multirobot.py +++ b/rosbot_bringup/test/test_multirobot.py @@ -43,9 +43,9 @@ def generate_test_description(): ) ), launch_arguments={ - "use_sim": "False", - "mecanum": "False", + "healthcheck": "False", "namespace": robot_names[i], + "use_sim": "False", }.items(), ) diff --git a/rosbot_bringup/test/test_namespaced_diff_drive.py b/rosbot_bringup/test/test_namespaced_diff_drive.py index 40839ad7..4f608bd7 100644 --- a/rosbot_bringup/test/test_namespaced_diff_drive.py +++ b/rosbot_bringup/test/test_namespaced_diff_drive.py @@ -39,9 +39,9 @@ def generate_test_description(): ) ), launch_arguments={ + "healthcheck": "False", + "namespace": "rosbot", "use_sim": "False", - "mecanum": "False", - "namespace": "rosbot2r", }.items(), ) @@ -52,7 +52,7 @@ 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() diff --git a/rosbot_bringup/test/test_namespaced_mecanum.py b/rosbot_bringup/test/test_namespaced_mecanum.py index 595b50d6..adea806e 100644 --- a/rosbot_bringup/test/test_namespaced_mecanum.py +++ b/rosbot_bringup/test/test_namespaced_mecanum.py @@ -39,9 +39,10 @@ def generate_test_description(): ) ), launch_arguments={ - "use_sim": "False", + "healthcheck": "False", "mecanum": "True", - "namespace": "rosbot2r", + "namespace": "rosbot", + "use_sim": "False", }.items(), ) @@ -52,7 +53,7 @@ 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() diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index d169d7a8..1505b89a 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -23,26 +23,28 @@ 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") + simulation_engine = LaunchConfiguration("simulation_engine") + 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)", ) - simulation_engine = LaunchConfiguration("simulation_engine") declare_simulation_engine_arg = DeclareLaunchArgument( "simulation_engine", default_value="ignition-gazebo", @@ -50,13 +52,6 @@ def generate_launch_description(): choices=["ignition-gazebo", "webots"], ) - use_sim = LaunchConfiguration("use_sim") - declare_use_sim_arg = DeclareLaunchArgument( - "use_sim", - default_value="False", - description="Whether simulation is used", - ) - controller_config_name = PythonExpression( [ "'mecanum_drive_controller.yaml' if ", @@ -183,8 +178,6 @@ def generate_launch_description(): declare_namespace_arg, declare_mecanum_arg, declare_simulation_engine_arg, - declare_use_sim_arg, - SetParameter("use_sim_time", value=use_sim), control_node, robot_state_pub_node, delayed_spawner_nodes, diff --git a/rosbot_controller/package.xml b/rosbot_controller/package.xml index 5066af4c..71e70bfd 100644 --- a/rosbot_controller/package.xml +++ b/rosbot_controller/package.xml @@ -17,39 +17,33 @@ 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 + xacro - joint_state_broadcaster - imu_sensor_broadcaster - diff_drive_controller - mecanum_drive_controller - - python3-pytest + controller_manager + diff_drive_controller + imu_sensor_broadcaster + joint_state_broadcaster launch - launch_ros launch_pytest - - xacro - sensor_msgs + launch_ros + mecanum_drive_controller nav_msgs - - rosbot_description + python3-pytest ros_components_description - - controller_manager - joint_state_broadcaster - imu_sensor_broadcaster - diff_drive_controller - mecanum_drive_controller + rosbot_description + sensor_msgs + xacro ament_python 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_gazebo/launch/simulation.launch.py b/rosbot_gazebo/launch/simulation.launch.py index 44c92a71..d79df561 100644 --- a/rosbot_gazebo/launch/simulation.launch.py +++ b/rosbot_gazebo/launch/simulation.launch.py @@ -28,7 +28,6 @@ def generate_launch_description(): namespace = LaunchConfiguration("namespace") - mecanum = LaunchConfiguration("mecanum") x = LaunchConfiguration("x", default="0.0") y = LaunchConfiguration("y", default="2.0") z = LaunchConfiguration("z", default="0.0") @@ -42,14 +41,6 @@ def generate_launch_description(): 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)" - ), - ) - declare_robots_arg = DeclareLaunchArgument( "robots", default_value="", @@ -100,7 +91,6 @@ def generate_launch_description(): ) ), launch_arguments={ - "mecanum": mecanum, "use_sim": "True", "namespace": robot_name, "x": init_pose["x"], @@ -117,7 +107,6 @@ def generate_launch_description(): return LaunchDescription( [ declare_namespace_arg, - declare_mecanum_arg, declare_robots_arg, SetParameter(name="use_sim_time", value=True), gz_sim, diff --git a/rosbot_gazebo/launch/spawn.launch.py b/rosbot_gazebo/launch/spawn.launch.py index 64134d39..d0b28cee 100644 --- a/rosbot_gazebo/launch/spawn.launch.py +++ b/rosbot_gazebo/launch/spawn.launch.py @@ -26,7 +26,6 @@ def generate_launch_description(): - mecanum = LaunchConfiguration("mecanum") namespace = LaunchConfiguration("namespace") x = LaunchConfiguration("x") y = LaunchConfiguration("y") @@ -35,14 +34,6 @@ def generate_launch_description(): pitch = LaunchConfiguration("pitch") yaw = LaunchConfiguration("yaw") - declare_mecanum_arg = DeclareLaunchArgument( - "mecanum", - default_value="False", - description=( - "Whether to use mecanum drive controller (otherwise diff drive controller is used)" - ), - ) - declare_namespace_arg = DeclareLaunchArgument( "namespace", default_value="", @@ -155,7 +146,6 @@ def generate_launch_description(): ) ), launch_arguments={ - "mecanum": mecanum, "use_sim": "True", "namespace": namespace, }.items(), @@ -163,7 +153,6 @@ def generate_launch_description(): return LaunchDescription( [ - declare_mecanum_arg, declare_namespace_arg, declare_x_arg, declare_y_arg, diff --git a/rosbot_gazebo/package.xml b/rosbot_gazebo/package.xml index 5c7a01a9..04c4e8b2 100644 --- a/rosbot_gazebo/package.xml +++ b/rosbot_gazebo/package.xml @@ -17,29 +17,23 @@ Jakub Delicat Rafal Gorecki - rosbot_bringup - - launch - launch_ros - 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.py b/rosbot_gazebo/test/test_diff_drive.py index a16b1f8a..08801364 100644 --- a/rosbot_gazebo/test/test_diff_drive.py +++ b/rosbot_gazebo/test/test_diff_drive.py @@ -48,6 +48,7 @@ def generate_test_description(): "gz_world": PathJoinSubstitution( [FindPackageShare("husarion_gz_worlds"), "worlds", "empty_with_plugins.sdf"] ), + "healthcheck": "False", }.items(), ) diff --git a/rosbot_gazebo/test/test_mecanum.py b/rosbot_gazebo/test/test_mecanum.py index 1ed8a83d..02da649e 100644 --- a/rosbot_gazebo/test/test_mecanum.py +++ b/rosbot_gazebo/test/test_mecanum.py @@ -44,11 +44,12 @@ def generate_test_description(): ) ), launch_arguments={ - "mecanum": "True", "gz_headless_mode": "True", "gz_world": PathJoinSubstitution( [FindPackageShare("husarion_gz_worlds"), "worlds", "empty_with_plugins.sdf"] ), + "healthcheck": "False", + "mecanum": "True", }.items(), ) diff --git a/rosbot_gazebo/test/test_multirobot_diff_drive.py b/rosbot_gazebo/test/test_multirobot_diff_drive.py index ec054f9d..96d75f7c 100644 --- a/rosbot_gazebo/test/test_multirobot_diff_drive.py +++ b/rosbot_gazebo/test/test_multirobot_diff_drive.py @@ -43,6 +43,7 @@ def generate_test_description(): "gz_headless_mode:=True", f"gz_world:={gz_world_path}", "robots:=robot1={y: -4.0}; robot2={y: 0.0};", + "healthcheck:=False", ], output="screen", ) diff --git a/rosbot_gazebo/test/test_multirobot_mecanum.py b/rosbot_gazebo/test/test_multirobot_mecanum.py index 17162797..87ed7d7f 100644 --- a/rosbot_gazebo/test/test_multirobot_mecanum.py +++ b/rosbot_gazebo/test/test_multirobot_mecanum.py @@ -42,6 +42,7 @@ def generate_test_description(): "simulation.launch.py", "gz_headless_mode:=True", f"gz_world:={gz_world_path}", + "healthcheck:=False", "mecanum:=True", "robots:=robot1={y: -4.0}; robot2={y: 0.0};", ], diff --git a/rosbot_gazebo/test/test_namespaced_diff_drive.py b/rosbot_gazebo/test/test_namespaced_diff_drive.py index 44312c53..60291448 100644 --- a/rosbot_gazebo/test/test_namespaced_diff_drive.py +++ b/rosbot_gazebo/test/test_namespaced_diff_drive.py @@ -48,7 +48,8 @@ def generate_test_description(): "gz_world": PathJoinSubstitution( [FindPackageShare("husarion_gz_worlds"), "worlds", "empty_with_plugins.sdf"] ), - "namespace": "rosbot2r", + "healthcheck": "False", + "namespace": "rosbot", }.items(), ) @@ -66,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.py b/rosbot_gazebo/test/test_namespaced_mecanum.py index e93c29d9..5420a9f2 100644 --- a/rosbot_gazebo/test/test_namespaced_mecanum.py +++ b/rosbot_gazebo/test/test_namespaced_mecanum.py @@ -48,8 +48,9 @@ def generate_test_description(): "gz_world": PathJoinSubstitution( [FindPackageShare("husarion_gz_worlds"), "worlds", "empty_with_plugins.sdf"] ), + "healthcheck": "False", "mecanum": "True", - "namespace": "rosbot2r", + "namespace": "rosbot", }.items(), ) @@ -67,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_utils/package.xml b/rosbot_utils/package.xml index 96ba3853..36d19e1e 100644 --- a/rosbot_utils/package.xml +++ b/rosbot_utils/package.xml @@ -17,13 +17,13 @@ Jakub Delicat Rafal Gorecki + launch_ros python3-libgpiod python3-pyftdi-pip python3-requests python3-serial python3-sh usbutils - launch_ros ament_copyright ament_pep257 From 63eb8d6096b96b23335501f77a756bffb86267dc Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Tue, 3 Dec 2024 14:16:10 +0100 Subject: [PATCH 07/17] Add healthcheck + clean up --- .github/workflows/tests.yaml | 2 +- rosbot/rosbot_hardware.repos | 16 ++++++++-------- rosbot/rosbot_simulation.repos | 8 ++++++++ 3 files changed, 17 insertions(+), 9 deletions(-) diff --git a/.github/workflows/tests.yaml b/.github/workflows/tests.yaml index db48c831..6403ffa2 100644 --- a/.github/workflows/tests.yaml +++ b/.github/workflows/tests.yaml @@ -22,7 +22,7 @@ jobs: ROS_DISTRO: [humble] steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Setup ROS2 Workspace and Clone Repositories run: | diff --git a/rosbot/rosbot_hardware.repos b/rosbot/rosbot_hardware.repos index 55bca8c2..fe402a5f 100644 --- a/rosbot/rosbot_hardware.repos +++ b/rosbot/rosbot_hardware.repos @@ -1,12 +1,4 @@ repositories: - rosbot_hardware_interfaces: - type: git - url: https://github.com/husarion/rosbot_hardware_interfaces.git - version: da1805839aaa21b8341a9c39498c96d9a1a4f87d - 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 @@ -19,3 +11,11 @@ repositories: type: git url: https://github.com/micro-ROS/micro-ROS-Agent.git 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 ee822f11..9c168c64 100644 --- a/rosbot/rosbot_simulation.repos +++ b/rosbot/rosbot_simulation.repos @@ -1,8 +1,16 @@ 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: 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/ From 48dfc63a1dc6179523db3cee38e2feb9101c1f39 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Wed, 4 Dec 2024 09:13:18 +0100 Subject: [PATCH 08/17] Remove combined.launch.py --- docker/compose.hardware.yaml | 2 +- rosbot_bringup/launch/bringup.launch.py | 25 ++-- rosbot_bringup/launch/combined.launch.py | 118 ------------------ rosbot_bringup/launch/healthcheck.launch.py | 2 +- rosbot_bringup/test/test_diff_drive.py | 2 +- rosbot_bringup/test/test_mecanum.py | 2 +- rosbot_bringup/test/test_multirobot.py | 2 +- .../test/test_namespaced_diff_drive.py | 2 +- .../test/test_namespaced_mecanum.py | 2 +- rosbot_gazebo/test/test_diff_drive.py | 1 + rosbot_gazebo/test/test_mecanum.py | 1 + .../test/test_multirobot_diff_drive.py | 3 +- rosbot_gazebo/test/test_multirobot_mecanum.py | 1 + .../test/test_namespaced_diff_drive.py | 1 + rosbot_gazebo/test/test_namespaced_mecanum.py | 1 + 15 files changed, 32 insertions(+), 133 deletions(-) delete mode 100644 rosbot_bringup/launch/combined.launch.py diff --git a/docker/compose.hardware.yaml b/docker/compose.hardware.yaml index 3ebcf58e..05b067a2 100644 --- a/docker/compose.hardware.yaml +++ b/docker/compose.hardware.yaml @@ -16,7 +16,7 @@ services: - ${SERIAL_PORT:-/dev/ttyUSB0} - /dev/bus/usb/ # FTDI command: > - ros2 launch rosbot_bringup combined.launch.py + ros2 launch rosbot_bringup bringup.launch.py mecanum:=${MECANUM:-False} namespace:=${ROBOT_NAMESPACE:-rosbot} serial_port:=${SERIAL_PORT:-/dev/ttyUSB0} diff --git a/rosbot_bringup/launch/bringup.launch.py b/rosbot_bringup/launch/bringup.launch.py index b24285c7..507bd4d9 100644 --- a/rosbot_bringup/launch/bringup.launch.py +++ b/rosbot_bringup/launch/bringup.launch.py @@ -14,17 +14,20 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +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 from launch_ros.substitutions import FindPackageShare def generate_launch_description(): + microros = LaunchConfiguration("microros") namespace = LaunchConfiguration("namespace") use_sim = LaunchConfiguration("use_sim", default="False") @@ -35,6 +38,13 @@ def generate_launch_description(): choices=["True", "true", "False", "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=""), @@ -60,12 +70,12 @@ def generate_launch_description(): ) ) - # microros_launch = IncludeLaunchDescription( - # PythonLaunchDescriptionSource( - # PathJoinSubstitution([rosbot_bringup, "launch", "microros.launch.py"]) - # ), - # condition=UnlessCondition([use_sim]), - # ) + 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"]) @@ -97,10 +107,11 @@ def generate_launch_description(): actions = [ declare_healthcheck_arg, + declare_microros_arg, declare_namespace_arg, controller_launch, healthcheck_launch, - # microros_launch, + microros_launch, laser_filter_node, robot_localization_node, ] diff --git a/rosbot_bringup/launch/combined.launch.py b/rosbot_bringup/launch/combined.launch.py deleted file mode 100644 index a3eeae66..00000000 --- a/rosbot_bringup/launch/combined.launch.py +++ /dev/null @@ -1,118 +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. - -import os - -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - IncludeLaunchDescription, - OpaqueFunction, - SetEnvironmentVariable, -) -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir -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( - SetEnvironmentVariable(name="XRCE_DOMAIN_ID_OVERRIDE", value=ros_domain_id) - ) - - 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) - - 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), - SetEnvironmentVariable( - name="FASTRTPS_DEFAULT_PROFILES_FILE", - value=localhost_only_fastrtps_profiles_file, - ), - ] - ) - - microros_agent_node = Node( - package="micro_ros_agent", - executable="micro_ros_agent", - arguments=["serial", "-D", serial_port, "-b", serial_baudrate], - output="screen", - ) - - return env_setup_actions + [microros_agent_node] - else: - microros_agent_node = Node( - package="micro_ros_agent", - executable="micro_ros_agent", - arguments=["serial", "-D", serial_port, "-b", serial_baudrate], - output="screen", - ) - - return env_setup_actions + [microros_agent_node] - - -def generate_launch_description(): - declare_serial_port_arg = DeclareLaunchArgument( - "serial_port", - default_value="/dev/ttySERIAL", - description="Serial port for micro-ROS agent", - ) - - declare_serial_baudrate_arg = DeclareLaunchArgument( - "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", - default_value=fastrtps_profiles_file, - description=( - "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, - OpaqueFunction(function=generate_microros_agent_node), - bringup_launch, - ] - ) diff --git a/rosbot_bringup/launch/healthcheck.launch.py b/rosbot_bringup/launch/healthcheck.launch.py index 4bcfc1dd..cbe12d41 100644 --- a/rosbot_bringup/launch/healthcheck.launch.py +++ b/rosbot_bringup/launch/healthcheck.launch.py @@ -80,7 +80,7 @@ def get_missing_nodes(): if missing_nodes: print( - f"{red_color}Error: some nodes are missing: {missing_nodes}. Emitting shutdown...{reset_color}" + f"{red_color}Error: Some nodes are not initialized: {missing_nodes}. Emitting shutdown...{reset_color}" ) return [EmitEvent(event=Shutdown())] else: diff --git a/rosbot_bringup/test/test_diff_drive.py b/rosbot_bringup/test/test_diff_drive.py index fa1174d6..40582433 100644 --- a/rosbot_bringup/test/test_diff_drive.py +++ b/rosbot_bringup/test/test_diff_drive.py @@ -40,7 +40,7 @@ def generate_test_description(): ), launch_arguments={ "healthcheck": "False", - "use_sim": "False", + "microros": "False", }.items(), ) diff --git a/rosbot_bringup/test/test_mecanum.py b/rosbot_bringup/test/test_mecanum.py index f31ab78f..81955878 100644 --- a/rosbot_bringup/test/test_mecanum.py +++ b/rosbot_bringup/test/test_mecanum.py @@ -41,7 +41,7 @@ def generate_test_description(): launch_arguments={ "healthcheck": "False", "mecanum": "True", - "use_sim": "False", + "microros": "False", }.items(), ) diff --git a/rosbot_bringup/test/test_multirobot.py b/rosbot_bringup/test/test_multirobot.py index b234c002..f6670bda 100644 --- a/rosbot_bringup/test/test_multirobot.py +++ b/rosbot_bringup/test/test_multirobot.py @@ -45,7 +45,7 @@ def generate_test_description(): launch_arguments={ "healthcheck": "False", "namespace": robot_names[i], - "use_sim": "False", + "microros": "False", }.items(), ) diff --git a/rosbot_bringup/test/test_namespaced_diff_drive.py b/rosbot_bringup/test/test_namespaced_diff_drive.py index 4f608bd7..837d858c 100644 --- a/rosbot_bringup/test/test_namespaced_diff_drive.py +++ b/rosbot_bringup/test/test_namespaced_diff_drive.py @@ -41,7 +41,7 @@ def generate_test_description(): launch_arguments={ "healthcheck": "False", "namespace": "rosbot", - "use_sim": "False", + "microros": "False", }.items(), ) diff --git a/rosbot_bringup/test/test_namespaced_mecanum.py b/rosbot_bringup/test/test_namespaced_mecanum.py index adea806e..7050f792 100644 --- a/rosbot_bringup/test/test_namespaced_mecanum.py +++ b/rosbot_bringup/test/test_namespaced_mecanum.py @@ -42,7 +42,7 @@ def generate_test_description(): "healthcheck": "False", "mecanum": "True", "namespace": "rosbot", - "use_sim": "False", + "microros": "False", }.items(), ) diff --git a/rosbot_gazebo/test/test_diff_drive.py b/rosbot_gazebo/test/test_diff_drive.py index 08801364..5e0c4020 100644 --- a/rosbot_gazebo/test/test_diff_drive.py +++ b/rosbot_gazebo/test/test_diff_drive.py @@ -49,6 +49,7 @@ def generate_test_description(): [FindPackageShare("husarion_gz_worlds"), "worlds", "empty_with_plugins.sdf"] ), "healthcheck": "False", + "microros": "False", }.items(), ) diff --git a/rosbot_gazebo/test/test_mecanum.py b/rosbot_gazebo/test/test_mecanum.py index 02da649e..4bf0668b 100644 --- a/rosbot_gazebo/test/test_mecanum.py +++ b/rosbot_gazebo/test/test_mecanum.py @@ -50,6 +50,7 @@ def generate_test_description(): ), "healthcheck": "False", "mecanum": "True", + "microros": "False", }.items(), ) diff --git a/rosbot_gazebo/test/test_multirobot_diff_drive.py b/rosbot_gazebo/test/test_multirobot_diff_drive.py index 96d75f7c..6698d0a8 100644 --- a/rosbot_gazebo/test/test_multirobot_diff_drive.py +++ b/rosbot_gazebo/test/test_multirobot_diff_drive.py @@ -42,8 +42,9 @@ def generate_test_description(): "simulation.launch.py", "gz_headless_mode:=True", f"gz_world:={gz_world_path}", - "robots:=robot1={y: -4.0}; robot2={y: 0.0};", "healthcheck:=False", + "microros:=False", + "robots:=robot1={y: -4.0}; robot2={y: 0.0};", ], output="screen", ) diff --git a/rosbot_gazebo/test/test_multirobot_mecanum.py b/rosbot_gazebo/test/test_multirobot_mecanum.py index 87ed7d7f..a928764b 100644 --- a/rosbot_gazebo/test/test_multirobot_mecanum.py +++ b/rosbot_gazebo/test/test_multirobot_mecanum.py @@ -44,6 +44,7 @@ def generate_test_description(): f"gz_world:={gz_world_path}", "healthcheck:=False", "mecanum:=True", + "microros:=False", "robots:=robot1={y: -4.0}; robot2={y: 0.0};", ], output="screen", diff --git a/rosbot_gazebo/test/test_namespaced_diff_drive.py b/rosbot_gazebo/test/test_namespaced_diff_drive.py index 60291448..0c3e3f03 100644 --- a/rosbot_gazebo/test/test_namespaced_diff_drive.py +++ b/rosbot_gazebo/test/test_namespaced_diff_drive.py @@ -49,6 +49,7 @@ def generate_test_description(): [FindPackageShare("husarion_gz_worlds"), "worlds", "empty_with_plugins.sdf"] ), "healthcheck": "False", + "microros": "False", "namespace": "rosbot", }.items(), ) diff --git a/rosbot_gazebo/test/test_namespaced_mecanum.py b/rosbot_gazebo/test/test_namespaced_mecanum.py index 5420a9f2..bf66fcf8 100644 --- a/rosbot_gazebo/test/test_namespaced_mecanum.py +++ b/rosbot_gazebo/test/test_namespaced_mecanum.py @@ -50,6 +50,7 @@ def generate_test_description(): ), "healthcheck": "False", "mecanum": "True", + "microros": "False", "namespace": "rosbot", }.items(), ) From a529cbdd8a3c6aa71a8b5301cc77244cb59ee40c Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Wed, 4 Dec 2024 09:15:31 +0100 Subject: [PATCH 09/17] Add User env to docker --- docker/compose.hardware.yaml | 2 ++ docker/compose.simulation.yaml | 2 ++ 2 files changed, 4 insertions(+) diff --git a/docker/compose.hardware.yaml b/docker/compose.hardware.yaml index 05b067a2..ff4a6cdc 100644 --- a/docker/compose.hardware.yaml +++ b/docker/compose.hardware.yaml @@ -15,6 +15,8 @@ services: devices: - ${SERIAL_PORT:-/dev/ttyUSB0} - /dev/bus/usb/ # FTDI + environment: + - USER command: > ros2 launch rosbot_bringup bringup.launch.py mecanum:=${MECANUM:-False} diff --git a/docker/compose.simulation.yaml b/docker/compose.simulation.yaml index 9194cda1..535181b5 100644 --- a/docker/compose.simulation.yaml +++ b/docker/compose.simulation.yaml @@ -9,6 +9,7 @@ x-gpu-config: &gpu-config runtime: nvidia environment: + - USER - DISPLAY=${DISPLAY:?err} - NVIDIA_VISIBLE_DEVICES=all - NVIDIA_DRIVER_CAPABILITIES=all @@ -16,6 +17,7 @@ x-gpu-config: x-cpu-config: &cpu-config environment: + - USER - DISPLAY=${DISPLAY:?err} services: From c74f9225133057b65f61a3d3f859eede30338fe7 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Wed, 4 Dec 2024 17:20:44 +0100 Subject: [PATCH 10/17] Change healthcheck --- rosbot_bringup/launch/bringup.launch.py | 8 -- rosbot_bringup/launch/healthcheck.launch.py | 74 ++----------------- rosbot_bringup/test/test_diff_drive.py | 1 - rosbot_bringup/test/test_mecanum.py | 1 - rosbot_bringup/test/test_multirobot.py | 1 - .../test/test_namespaced_diff_drive.py | 1 - .../test/test_namespaced_mecanum.py | 1 - rosbot_controller/launch/controller.launch.py | 51 +++++++++---- rosbot_gazebo/test/test_diff_drive.py | 1 - rosbot_gazebo/test/test_mecanum.py | 1 - .../test/test_multirobot_diff_drive.py | 1 - rosbot_gazebo/test/test_multirobot_mecanum.py | 1 - .../test/test_namespaced_diff_drive.py | 1 - rosbot_gazebo/test/test_namespaced_mecanum.py | 1 - 14 files changed, 45 insertions(+), 99 deletions(-) diff --git a/rosbot_bringup/launch/bringup.launch.py b/rosbot_bringup/launch/bringup.launch.py index 507bd4d9..2e0ecb0d 100644 --- a/rosbot_bringup/launch/bringup.launch.py +++ b/rosbot_bringup/launch/bringup.launch.py @@ -31,13 +31,6 @@ def generate_launch_description(): namespace = LaunchConfiguration("namespace") use_sim = LaunchConfiguration("use_sim", default="False") - declare_healthcheck_arg = DeclareLaunchArgument( - "healthcheck", - default_value="True", - description="Check if all node are up and ready, if not emit shutdown signal.", - choices=["True", "true", "False", "false"], - ) - declare_microros_arg = DeclareLaunchArgument( "microros", default_value="True", @@ -106,7 +99,6 @@ def generate_launch_description(): ) actions = [ - declare_healthcheck_arg, declare_microros_arg, declare_namespace_arg, controller_launch, diff --git a/rosbot_bringup/launch/healthcheck.launch.py b/rosbot_bringup/launch/healthcheck.launch.py index cbe12d41..3c171889 100644 --- a/rosbot_bringup/launch/healthcheck.launch.py +++ b/rosbot_bringup/launch/healthcheck.launch.py @@ -13,83 +13,23 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os -import subprocess - from launch import LaunchDescription -from launch.actions import EmitEvent, OpaqueFunction, TimerAction -from launch.events import Shutdown -from launch.substitutions import LaunchConfiguration +from launch.actions import OpaqueFunction, TimerAction def check_controller_status(context): - healthcheck = LaunchConfiguration("healthcheck").perform(context) - if healthcheck.lower() == "false": - with open("/var/tmp/rosbot_status.txt", "w") as status_file: - status_file.write("healthy") - return - else: - with open("/var/tmp/rosbot_status.txt", "w") as status_file: - status_file.write("unhealthy") - - use_sim = LaunchConfiguration("use_sim", default="False").perform(context) - nodes_to_check = [ - "/controller_manager", - "/ekf_filter_node", - "/imu_broadcaster", - "/joint_state_broadcaster", - "/laser_scan_box_filter", - "/robot_state_publisher", - "/rosbot_base_controller", - "/scan_to_scan_filter_chain", - ] - if use_sim.lower() == "true": - additional_nodes = [ - "/gz_ros2_control", - "/ros_gz_bridge", - ] - else: - additional_nodes = [ - "/imu_sensor_node", - # "/rosbot_ros2_firmware", not visible via USB port - "/rosbot_system_node", - ] - nodes_to_check.extend(additional_nodes) - - namespace = LaunchConfiguration("namespace", default="").perform(context) - ns = "/" + namespace if namespace else "" - nodes_to_check = [ns + node for node in nodes_to_check] - - def get_missing_nodes(): - env = os.environ.copy() - env["ROS_LOCALHOST_ONLY"] = "1" - - result = subprocess.run( - ["ros2", "node", "list"], - stdout=subprocess.PIPE, - text=True, - env=env, - ) - active_nodes = result.stdout.splitlines() - return [node for node in nodes_to_check if node not in active_nodes] - - missing_nodes = get_missing_nodes() green_color = "\033[92m" - red_color = "\033[91m" reset_color = "\033[0m" - if missing_nodes: - print( - f"{red_color}Error: Some nodes are not initialized: {missing_nodes}. Emitting shutdown...{reset_color}" - ) - return [EmitEvent(event=Shutdown())] - else: - print(f"{green_color}All systems are up and running!{reset_color}") - with open("/var/tmp/rosbot_status.txt", "w") as status_file: - status_file.write("healthy") + print(f"{green_color}All systems are up and running!{reset_color}") + with open("/var/tmp/rosbot_status.txt", "w") as status_file: + status_file.write("healthy") def generate_launch_description(): + with open("/var/tmp/rosbot_status.txt", "w") as status_file: + status_file.write("unhealthy") + check_controller = TimerAction( period=15.0, actions=[OpaqueFunction(function=check_controller_status)], diff --git a/rosbot_bringup/test/test_diff_drive.py b/rosbot_bringup/test/test_diff_drive.py index 40582433..8c25a50c 100644 --- a/rosbot_bringup/test/test_diff_drive.py +++ b/rosbot_bringup/test/test_diff_drive.py @@ -39,7 +39,6 @@ def generate_test_description(): ) ), launch_arguments={ - "healthcheck": "False", "microros": "False", }.items(), ) diff --git a/rosbot_bringup/test/test_mecanum.py b/rosbot_bringup/test/test_mecanum.py index 81955878..3871dd6f 100644 --- a/rosbot_bringup/test/test_mecanum.py +++ b/rosbot_bringup/test/test_mecanum.py @@ -39,7 +39,6 @@ def generate_test_description(): ) ), launch_arguments={ - "healthcheck": "False", "mecanum": "True", "microros": "False", }.items(), diff --git a/rosbot_bringup/test/test_multirobot.py b/rosbot_bringup/test/test_multirobot.py index f6670bda..e400c1b5 100644 --- a/rosbot_bringup/test/test_multirobot.py +++ b/rosbot_bringup/test/test_multirobot.py @@ -43,7 +43,6 @@ def generate_test_description(): ) ), launch_arguments={ - "healthcheck": "False", "namespace": robot_names[i], "microros": "False", }.items(), diff --git a/rosbot_bringup/test/test_namespaced_diff_drive.py b/rosbot_bringup/test/test_namespaced_diff_drive.py index 837d858c..4c5f9786 100644 --- a/rosbot_bringup/test/test_namespaced_diff_drive.py +++ b/rosbot_bringup/test/test_namespaced_diff_drive.py @@ -39,7 +39,6 @@ def generate_test_description(): ) ), launch_arguments={ - "healthcheck": "False", "namespace": "rosbot", "microros": "False", }.items(), diff --git a/rosbot_bringup/test/test_namespaced_mecanum.py b/rosbot_bringup/test/test_namespaced_mecanum.py index 7050f792..32d6cd00 100644 --- a/rosbot_bringup/test/test_namespaced_mecanum.py +++ b/rosbot_bringup/test/test_namespaced_mecanum.py @@ -39,7 +39,6 @@ def generate_test_description(): ) ), launch_arguments={ - "healthcheck": "False", "mecanum": "True", "namespace": "rosbot", "microros": "False", diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index 1505b89a..e8b3f661 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, @@ -30,7 +37,6 @@ def generate_launch_description(): namespace = LaunchConfiguration("namespace") mecanum = LaunchConfiguration("mecanum") - simulation_engine = LaunchConfiguration("simulation_engine") use_sim = LaunchConfiguration("use_sim", default="False") declare_namespace_arg = DeclareLaunchArgument( @@ -45,13 +51,6 @@ def generate_launch_description(): description="Whether to use mecanum drive controller (otherwise diff drive controller is used)", ) - declare_simulation_engine_arg = DeclareLaunchArgument( - "simulation_engine", - default_value="ignition-gazebo", - description="Which simulation engine to be used", - choices=["ignition-gazebo", "webots"], - ) - controller_config_name = PythonExpression( [ "'mecanum_drive_controller.yaml' if ", @@ -84,8 +83,6 @@ def generate_launch_description(): mecanum, " namespace:=", namespace, - " simulation_engine:=", - simulation_engine, " use_sim:=", use_sim, ] @@ -108,7 +105,7 @@ def generate_launch_description(): ("imu_sensor_node/imu", "/_imu/data_raw"), ("~/motors_cmd", "/_motors_cmd"), ("~/motors_response", "/_motors_response"), - ("rosbot_base_controller/cmd_vel_unstamped", "cmd_vel"), + ("rosbot_base_controller/cmd_vel", "cmd_vel"), ("/tf", "tf"), ("/tf_static", "tf_static"), ], @@ -173,13 +170,41 @@ def generate_launch_description(): ], ) + def check_if_log_is_fatal(event): + red_color = "\033[91m" + reset_color = "\033[0m" + if "fatal" 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_simulation_engine_arg, control_node, robot_state_pub_node, delayed_spawner_nodes, + joint_state_monitor, + robot_controller_monitor, + imu_broadcaster_monitor, ] ) diff --git a/rosbot_gazebo/test/test_diff_drive.py b/rosbot_gazebo/test/test_diff_drive.py index 5e0c4020..ccdeec23 100644 --- a/rosbot_gazebo/test/test_diff_drive.py +++ b/rosbot_gazebo/test/test_diff_drive.py @@ -48,7 +48,6 @@ def generate_test_description(): "gz_world": PathJoinSubstitution( [FindPackageShare("husarion_gz_worlds"), "worlds", "empty_with_plugins.sdf"] ), - "healthcheck": "False", "microros": "False", }.items(), ) diff --git a/rosbot_gazebo/test/test_mecanum.py b/rosbot_gazebo/test/test_mecanum.py index 4bf0668b..e18d8796 100644 --- a/rosbot_gazebo/test/test_mecanum.py +++ b/rosbot_gazebo/test/test_mecanum.py @@ -48,7 +48,6 @@ def generate_test_description(): "gz_world": PathJoinSubstitution( [FindPackageShare("husarion_gz_worlds"), "worlds", "empty_with_plugins.sdf"] ), - "healthcheck": "False", "mecanum": "True", "microros": "False", }.items(), diff --git a/rosbot_gazebo/test/test_multirobot_diff_drive.py b/rosbot_gazebo/test/test_multirobot_diff_drive.py index 6698d0a8..a1b4ddbc 100644 --- a/rosbot_gazebo/test/test_multirobot_diff_drive.py +++ b/rosbot_gazebo/test/test_multirobot_diff_drive.py @@ -42,7 +42,6 @@ def generate_test_description(): "simulation.launch.py", "gz_headless_mode:=True", f"gz_world:={gz_world_path}", - "healthcheck:=False", "microros:=False", "robots:=robot1={y: -4.0}; robot2={y: 0.0};", ], diff --git a/rosbot_gazebo/test/test_multirobot_mecanum.py b/rosbot_gazebo/test/test_multirobot_mecanum.py index a928764b..16fcaf63 100644 --- a/rosbot_gazebo/test/test_multirobot_mecanum.py +++ b/rosbot_gazebo/test/test_multirobot_mecanum.py @@ -42,7 +42,6 @@ def generate_test_description(): "simulation.launch.py", "gz_headless_mode:=True", f"gz_world:={gz_world_path}", - "healthcheck:=False", "mecanum:=True", "microros:=False", "robots:=robot1={y: -4.0}; robot2={y: 0.0};", diff --git a/rosbot_gazebo/test/test_namespaced_diff_drive.py b/rosbot_gazebo/test/test_namespaced_diff_drive.py index 0c3e3f03..52753dbb 100644 --- a/rosbot_gazebo/test/test_namespaced_diff_drive.py +++ b/rosbot_gazebo/test/test_namespaced_diff_drive.py @@ -48,7 +48,6 @@ def generate_test_description(): "gz_world": PathJoinSubstitution( [FindPackageShare("husarion_gz_worlds"), "worlds", "empty_with_plugins.sdf"] ), - "healthcheck": "False", "microros": "False", "namespace": "rosbot", }.items(), diff --git a/rosbot_gazebo/test/test_namespaced_mecanum.py b/rosbot_gazebo/test/test_namespaced_mecanum.py index bf66fcf8..4afe29c3 100644 --- a/rosbot_gazebo/test/test_namespaced_mecanum.py +++ b/rosbot_gazebo/test/test_namespaced_mecanum.py @@ -48,7 +48,6 @@ def generate_test_description(): "gz_world": PathJoinSubstitution( [FindPackageShare("husarion_gz_worlds"), "worlds", "empty_with_plugins.sdf"] ), - "healthcheck": "False", "mecanum": "True", "microros": "False", "namespace": "rosbot", From db0ad25b5befe2133c61708ce2f4f6c30dee4ae5 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Thu, 5 Dec 2024 09:11:09 +0100 Subject: [PATCH 11/17] Clean up --- .github/workflows/tests.yaml | 8 +- rosbot_controller/launch/controller.launch.py | 4 +- rosbot_controller/package.xml | 10 -- rosbot_controller/test/test_utils.py | 107 ------------------ 4 files changed, 6 insertions(+), 123 deletions(-) delete mode 100644 rosbot_controller/test/test_utils.py diff --git a/.github/workflows/tests.yaml b/.github/workflows/tests.yaml index 6403ffa2..dcdd3e9c 100644 --- a/.github/workflows/tests.yaml +++ b/.github/workflows/tests.yaml @@ -26,11 +26,11 @@ jobs: - name: Setup ROS2 Workspace and Clone Repositories run: | - mkdir -p src - find . -maxdepth 1 -not -name src -not -name . -exec mv {} src/ \; + mkdir -p src/rosbot_ros + find . -maxdepth 1 -not -name src -not -name . -exec mv {} src/rosbot_ros \; python3 -m pip install -U vcstool - vcs import src < src/rosbot/rosbot_hardware.repos - vcs import src < src/rosbot/rosbot_simulation.repos + vcs import src < src/rosbot_ros/rosbot/rosbot_hardware.repos + vcs import src < src/rosbot_ros/rosbot/rosbot_simulation.repos cp -r src/ros2_controllers/imu_sensor_broadcaster src/ rm -rf src/ros2_controllers diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index e8b3f661..0863d9d7 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -105,7 +105,7 @@ def generate_launch_description(): ("imu_sensor_node/imu", "/_imu/data_raw"), ("~/motors_cmd", "/_motors_cmd"), ("~/motors_response", "/_motors_response"), - ("rosbot_base_controller/cmd_vel", "cmd_vel"), + ("rosbot_base_controller/cmd_vel_unstamped", "cmd_vel"), ("/tf", "tf"), ("/tf_static", "tf_static"), ], @@ -173,7 +173,7 @@ def generate_launch_description(): def check_if_log_is_fatal(event): red_color = "\033[91m" reset_color = "\033[0m" - if "fatal" in event.text.decode().lower(): + 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")) diff --git a/rosbot_controller/package.xml b/rosbot_controller/package.xml index 71e70bfd..6e162921 100644 --- a/rosbot_controller/package.xml +++ b/rosbot_controller/package.xml @@ -30,19 +30,9 @@ rosbot_hardware_interfaces xacro - controller_manager - diff_drive_controller - imu_sensor_broadcaster - joint_state_broadcaster - launch - launch_pytest - launch_ros - mecanum_drive_controller - nav_msgs python3-pytest ros_components_description rosbot_description - sensor_msgs xacro 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!" From 482a944873f4afbfdf556f2e60e0aed98b0d75bc Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Thu, 5 Dec 2024 10:08:55 +0100 Subject: [PATCH 12/17] Update docs --- .pre-commit-config.yaml | 1 + CONTRIBUTING.md | 59 +++++++++++ README.md | 169 ++++++++--------------------- ROS_API.md | 188 +++++++++++++++++++++++++++------ rosbot_description/package.xml | 2 +- rosbot_gazebo/package.xml | 2 +- 6 files changed, 261 insertions(+), 160 deletions(-) create mode 100644 CONTRIBUTING.md diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index d472c928..389b9f0f 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -85,6 +85,7 @@ repos: stages: [commit] entry: ament_copyright language: system + exclude: ^.*\.md$ - repo: https://github.com/PyCQA/doc8 rev: v1.1.2 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 b6a697b9..5706c732 100644 --- a/README.md +++ b/README.md @@ -1,61 +1,35 @@ # 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 -cd ros2_ws -git clone https://github.com/husarion/rosbot_ros src/rosbot_ros -``` - -### Build and run on hardware - -Building: +#### Building ```bash export HUSARION_ROS_BUILD_TYPE=hardware @@ -72,25 +46,30 @@ rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y colcon build --symlink-install --packages-up-to rosbot --cmake-args -DCMAKE_BUILD_TYPE=Release ``` -Flash firmware: +#### Run the Robot -```bash -sudo su -source install/setup.bash -ros2 run rosbot_utils flash_firmware -exit -``` +1. Flash the firmware: -Running: + ```bash + sudo su + source install/setup.bash + ros2 run rosbot_utils flash_firmware + exit + ``` -```bash -source install/setup.bash -ros2 launch rosbot_bringup combined.launch.py -``` +> [!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. + +2. Launch the robot: + + ```bash + source install/setup.bash + ros2 launch rosbot_bringup bringup.launch.py + ``` -### Build and run Gazebo simulation +### πŸ–₯️ Simulation -Building: +#### Building ```bash export HUSARION_ROS_BUILD_TYPE=simulation @@ -109,76 +88,18 @@ rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y colcon build --symlink-install --packages-up-to rosbot --cmake-args -DCMAKE_BUILD_TYPE=Release ``` -Running: +#### Run the Simulation ```bash source install/setup.bash ros2 launch rosbot_gazebo simulation.launch.py ``` -## Developer info - -### pre-commit - -[pre-commit configuration](.pre-commit-config.yaml) prepares plenty of tests helping for developing and contributing. Usage: - -```bash -# install pre-commit -pip install pre-commit - -# initialize pre-commit workspace -pre-commit install - -# manually run tests -pre-commit run -a -``` - -After initialization [pre-commit configuration](.pre-commit-config.yaml) will applied on every commit. - -### Industrial CI - -```bash -colcon test -``` - -> [!NOTE] -> Command `colcon test` does not build the code. Remember to build your code after changes. - -If tests finish with errors print logs: - -``` bash -colcon test-result --verbose -``` - -### Format python code with [Black](https://github.com/psf/black) - -```bash -cd src/rosbot_ros -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: - -```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..ae4480a0 100644 --- a/ROS_API.md +++ b/ROS_API.md @@ -1,46 +1,166 @@ -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}' -``` +**Launch Params:** -If you want to use your own world add to the world's sdf file gazebo sensors plugins inside any `` tag: +| PARAMETER | DESCRIPTION | VALUE | +| ------------------------ | ----------------------------------------------------------------- | ---------- | +| **camera_model** | Add camera model to the robot URDF | **None**\* | +| **lidar_model** | Add LiDAR model to the robot URDF | **None**\* | +| **include_camera_mount** | Whether to include camera mount to the robot URDF | **False** | +| **mecanum** | Whether to use mecanum drive controller, otherwise use diff drive | **False** | +| **namespace** | Namespace for all topics and tfs | **""** | -```xml - - -``` +> \*You can check all available options using `-s`/`--show-args` flag. (e.g. `ros2 launch rosbot_bringup bringup.launch.py -s`). -> **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_controller` + +ROS2 hardware controller for ROSbot. It manages inputs and outputs data from ROS2 control, forwarding it via ROS topics to be read by microROS. The controller.launch.py file loads the robot model defined in rosbot_description along with ROS2 control dependencies from [rosbot_hardware_interfaces](https://github.com/husarion/rosbot_hardware_interfaces). + +### `rosbot_description` + +URDF model used for both simulation and as a source of transforms on physical robot. It was written to be compatible with ROS Industrial and preconfigured for ROS2 control. + +Available models: + +| MODEL | DESCRIPTION | +| ------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `rosbot` | Final configuration of rosbot with ability to attach external hardware. | +| `rosbot_base` | Base of rosbot prepared to be included into preexisting configuration. Meant to be compatible with concept of ROS Industrial ability for manipulators to have interchangeable end effectors. | + +### `rosbot_gazebo` + +Launch files for Ignition Gazebo working with ROS2 control. + +**Available Launch Files:** + +- `simulations.launch.py` - running a rosbot in Gazebo simulator and simulate all specified sensors. + +**Launch Params:** + +| PARAMETER | DESCRIPTION | VALUE | +| ------------------------ | ----------------------------------------------------------------- | ----------------------------------------------------------- | +| **camera_model** | Add camera model to the robot URDF | **None**\* | +| **lidar_model** | Add LiDAR model to the robot URDF | **None**\* | +| **include_camera_mount** | Whether to include camera mount to the robot URDF | **False** | +| **mecanum** | Whether to use mecanum drive controller, otherwise use diff drive | **False** | +| **namespace** | Namespace for all topics and tfs | **""** | +| **world** | Path to SDF world file | **`husarion_gz_worlds/`
`worlds/husarion_world.sdf`** | +| **headless** | Run Gazebo Ignition in the headless mode | **False** | +| **robots** | List of robots that will be spawn in the simulation | **[]**\*\* | + +> \*You can check all available options using `-s`/`--show-args` flag. (e.g. `ros2 launch rosbot_bringup bringup.launch.py -s`). +> +> \*\*Example of use: `robots:='robot1={x: 0.0, y: -1.0}; robot2={x: 1.0, y: -1.0};'` + +### `rosbot_utils` + +This package contains the stable firmware version with the flash script. + +### `rosbot_navigation` + +Package that contains navigation configurations and launch files for ROSbot. It integrates with `nav2` for autonomous navigation. + +**Available Launch Files:** + +- `navigation.launch.py` - launches the navigation stack with predefined parameters. + +**Launch Params:** + +| PARAMETER | DESCRIPTION | VALUE | +| ------------------------ | ----------------------------------------------------------------- | ---------- | +| **map** | Path to the map file used for navigation | **""** | +| **use_sim_time** | Whether to use simulation time | **False** | +| **params_file** | Path to the parameters file for navigation stack | **""** | + +> You can check all available options using `-s`/`--show-args` flag. (e.g. `ros2 launch rosbot_navigation navigation.launch.py -s`). + +## 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/rosbot_description/package.xml b/rosbot_description/package.xml index 9c9d5ad6..eb9c1995 100644 --- a/rosbot_description/package.xml +++ b/rosbot_description/package.xml @@ -13,8 +13,8 @@ https://github.com/husarion/rosbot_ros https://github.com/husarion/rosbot_ros/issues - Maciej Stepien Jakub Delicat + Maciej Stepien ament_cmake diff --git a/rosbot_gazebo/package.xml b/rosbot_gazebo/package.xml index 04c4e8b2..5fa4a0c9 100644 --- a/rosbot_gazebo/package.xml +++ b/rosbot_gazebo/package.xml @@ -13,8 +13,8 @@ https://github.com/husarion/rosbot_ros https://github.com/husarion/rosbot_ros/issues - Krzysztof Wojciechowski Jakub Delicat + Krzysztof Wojciechowski Rafal Gorecki husarion_gz_worlds From c15d21bfcf32c98077211be4d13680acf3d02167 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Thu, 5 Dec 2024 10:34:53 +0100 Subject: [PATCH 13/17] Add launch arguments info --- README.md | 31 ++++++++++++++++ ROS_API.md | 46 ------------------------ rosbot_bringup/launch/microros.launch.py | 17 ++++----- 3 files changed, 38 insertions(+), 56 deletions(-) diff --git a/README.md b/README.md index 5706c732..4cf6656b 100644 --- a/README.md +++ b/README.md @@ -95,6 +95,37 @@ source install/setup.bash ros2 launch rosbot_gazebo simulation.launch.py ``` +### 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` | + +> [!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`) + ## πŸ•ΉοΈ Demo Explore demos showcasing the capabilities of ROSbots: diff --git a/ROS_API.md b/ROS_API.md index ae4480a0..537db6a5 100644 --- a/ROS_API.md +++ b/ROS_API.md @@ -17,17 +17,6 @@ Package that contains launch, which starts all base functionalities with the mic - `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. -**Launch Params:** - -| PARAMETER | DESCRIPTION | VALUE | -| ------------------------ | ----------------------------------------------------------------- | ---------- | -| **camera_model** | Add camera model to the robot URDF | **None**\* | -| **lidar_model** | Add LiDAR model to the robot URDF | **None**\* | -| **include_camera_mount** | Whether to include camera mount to the robot URDF | **False** | -| **mecanum** | Whether to use mecanum drive controller, otherwise use diff drive | **False** | -| **namespace** | Namespace for all topics and tfs | **""** | - -> \*You can check all available options using `-s`/`--show-args` flag. (e.g. `ros2 launch rosbot_bringup bringup.launch.py -s`). ### `rosbot_controller` @@ -52,45 +41,10 @@ Launch files for Ignition Gazebo working with ROS2 control. - `simulations.launch.py` - running a rosbot in Gazebo simulator and simulate all specified sensors. -**Launch Params:** - -| PARAMETER | DESCRIPTION | VALUE | -| ------------------------ | ----------------------------------------------------------------- | ----------------------------------------------------------- | -| **camera_model** | Add camera model to the robot URDF | **None**\* | -| **lidar_model** | Add LiDAR model to the robot URDF | **None**\* | -| **include_camera_mount** | Whether to include camera mount to the robot URDF | **False** | -| **mecanum** | Whether to use mecanum drive controller, otherwise use diff drive | **False** | -| **namespace** | Namespace for all topics and tfs | **""** | -| **world** | Path to SDF world file | **`husarion_gz_worlds/`
`worlds/husarion_world.sdf`** | -| **headless** | Run Gazebo Ignition in the headless mode | **False** | -| **robots** | List of robots that will be spawn in the simulation | **[]**\*\* | - -> \*You can check all available options using `-s`/`--show-args` flag. (e.g. `ros2 launch rosbot_bringup bringup.launch.py -s`). -> -> \*\*Example of use: `robots:='robot1={x: 0.0, y: -1.0}; robot2={x: 1.0, y: -1.0};'` - ### `rosbot_utils` This package contains the stable firmware version with the flash script. -### `rosbot_navigation` - -Package that contains navigation configurations and launch files for ROSbot. It integrates with `nav2` for autonomous navigation. - -**Available Launch Files:** - -- `navigation.launch.py` - launches the navigation stack with predefined parameters. - -**Launch Params:** - -| PARAMETER | DESCRIPTION | VALUE | -| ------------------------ | ----------------------------------------------------------------- | ---------- | -| **map** | Path to the map file used for navigation | **""** | -| **use_sim_time** | Whether to use simulation time | **False** | -| **params_file** | Path to the parameters file for navigation stack | **""** | - -> You can check all available options using `-s`/`--show-args` flag. (e.g. `ros2 launch rosbot_navigation navigation.launch.py -s`). - ## ROS API ### Available Nodes diff --git a/rosbot_bringup/launch/microros.launch.py b/rosbot_bringup/launch/microros.launch.py index 512419ba..2510a88e 100644 --- a/rosbot_bringup/launch/microros.launch.py +++ b/rosbot_bringup/launch/microros.launch.py @@ -37,9 +37,7 @@ 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": env_setup_actions.extend( @@ -47,14 +45,14 @@ def generate_microros_agent_node(context, *args, **kwargs): LogInfo( msg=[ "ROS_LOCALHOST_ONLY set to 1. Using FASTRTPS_DEFAULT_PROFILES_FILE=", - localhost_only_fastrtps_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, ), ] ) @@ -92,12 +90,11 @@ def generate_launch_description(): fastrtps_profiles_file = PathJoinSubstitution( [FindPackageShare("rosbot_bringup"), "config", "microros_localhost_only.xml"] ) - declare_localhost_only_fastrtps_profiles_file_arg = DeclareLaunchArgument( - "localhost_only_fastrtps_profiles_file", + 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" ), ) @@ -105,7 +102,7 @@ def generate_launch_description(): [ 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), ] ) From 24f19f0598222d97217a2cdb1aa97f0b477cb35c Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Thu, 5 Dec 2024 13:49:55 +0100 Subject: [PATCH 14/17] Add status info --- rosbot_bringup/launch/bringup.launch.py | 23 ++++++++----- rosbot_bringup/launch/healthcheck.launch.py | 38 --------------------- 2 files changed, 15 insertions(+), 46 deletions(-) delete mode 100644 rosbot_bringup/launch/healthcheck.launch.py diff --git a/rosbot_bringup/launch/bringup.launch.py b/rosbot_bringup/launch/bringup.launch.py index 2e0ecb0d..c7f65934 100644 --- a/rosbot_bringup/launch/bringup.launch.py +++ b/rosbot_bringup/launch/bringup.launch.py @@ -13,7 +13,12 @@ # 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 ( @@ -57,12 +62,6 @@ def generate_launch_description(): }.items(), ) - healthcheck_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([rosbot_bringup, "launch", "healthcheck.launch.py"]) - ) - ) - microros_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution([rosbot_bringup, "launch", "microros.launch.py"]) @@ -98,14 +97,22 @@ def generate_launch_description(): 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, controller_launch, - healthcheck_launch, microros_launch, laser_filter_node, robot_localization_node, + status_info, ] return LaunchDescription(actions) diff --git a/rosbot_bringup/launch/healthcheck.launch.py b/rosbot_bringup/launch/healthcheck.launch.py deleted file mode 100644 index 3c171889..00000000 --- a/rosbot_bringup/launch/healthcheck.launch.py +++ /dev/null @@ -1,38 +0,0 @@ -# Copyright 2020 ros2_control Development Team -# Copyright 2024 Husarion sp. z o.o. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from launch import LaunchDescription -from launch.actions import OpaqueFunction, TimerAction - - -def check_controller_status(context): - green_color = "\033[92m" - reset_color = "\033[0m" - - print(f"{green_color}All systems are up and running!{reset_color}") - with open("/var/tmp/rosbot_status.txt", "w") as status_file: - status_file.write("healthy") - - -def generate_launch_description(): - with open("/var/tmp/rosbot_status.txt", "w") as status_file: - status_file.write("unhealthy") - - check_controller = TimerAction( - period=15.0, - actions=[OpaqueFunction(function=check_controller_status)], - ) - - return LaunchDescription([check_controller]) From 5037817758e5c0a28aaefaa38f747b2c085e6461 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Thu, 5 Dec 2024 13:50:14 +0100 Subject: [PATCH 15/17] add unsaved --- rosbot_bringup/launch/bringup.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbot_bringup/launch/bringup.launch.py b/rosbot_bringup/launch/bringup.launch.py index c7f65934..25f9de60 100644 --- a/rosbot_bringup/launch/bringup.launch.py +++ b/rosbot_bringup/launch/bringup.launch.py @@ -101,7 +101,7 @@ def generate_launch_description(): reset_color = "\033[0m" status_info = TimerAction( - period=15.0, + period=LaunchConfiguration("status_timeout", default_value="15.0"), actions=[LogInfo(msg=f"{green_color}All systems are up and running!{reset_color}")], ) From de63beac134f56cb13e6f6c634e0a067c628f0b4 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Thu, 5 Dec 2024 14:56:08 +0100 Subject: [PATCH 16/17] fix --- rosbot_bringup/launch/bringup.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbot_bringup/launch/bringup.launch.py b/rosbot_bringup/launch/bringup.launch.py index 25f9de60..8c57f326 100644 --- a/rosbot_bringup/launch/bringup.launch.py +++ b/rosbot_bringup/launch/bringup.launch.py @@ -101,7 +101,7 @@ def generate_launch_description(): reset_color = "\033[0m" status_info = TimerAction( - period=LaunchConfiguration("status_timeout", default_value="15.0"), + period=LaunchConfiguration("status_timeout", default="15.0"), actions=[LogInfo(msg=f"{green_color}All systems are up and running!{reset_color}")], ) From e884d5b5d4ab9b206076e329fcd50a7ae6f75b79 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Thu, 5 Dec 2024 15:03:18 +0100 Subject: [PATCH 17/17] Simplify --- rosbot_bringup/launch/bringup.launch.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rosbot_bringup/launch/bringup.launch.py b/rosbot_bringup/launch/bringup.launch.py index 8c57f326..9ad050c9 100644 --- a/rosbot_bringup/launch/bringup.launch.py +++ b/rosbot_bringup/launch/bringup.launch.py @@ -58,7 +58,6 @@ def generate_launch_description(): ), launch_arguments={ "namespace": namespace, - "use_sim": use_sim, }.items(), ) @@ -101,7 +100,7 @@ def generate_launch_description(): reset_color = "\033[0m" status_info = TimerAction( - period=LaunchConfiguration("status_timeout", default="15.0"), + period=15.0, actions=[LogInfo(msg=f"{green_color}All systems are up and running!{reset_color}")], )