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 4ec883ba..bfa2e01d 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: @@ -91,25 +88,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 @@ -117,16 +95,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: @@ -173,7 +151,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