From 65154a92a0abdeb5701912dc9c7b21b714571306 Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Mon, 15 Jan 2024 08:07:52 +0100 Subject: [PATCH] Add gazebo namespace (#93) * added namespaces to ros2_control Signed-off-by: Jakub Delicat * removed action group Signed-off-by: Jakub Delicat * Added namespace to robot description Signed-off-by: Jakub Delicat * added tests for namespaces in rosbot_controller Signed-off-by: Jakub Delicat * update worldlist Signed-off-by: Jakub Delicat * fixed ros2_control namespaces | added namespace to ekf Signed-off-by: Jakub Delicat * added mecanum Signed-off-by: Jakub Delicat * added ros2_controllers to .gitignore Signed-off-by: Jakub Delicat * ignored ros2_controllers during Industrial CI Signed-off-by: Jakub Delicat * fixed tests Signed-off-by: Jakub Delicat * added tf_prefix to imu_sensor_broadcaster Signed-off-by: Jakub Delicat * copied only diff_drive and imu_broadcaster Signed-off-by: Jakub Delicat * changed comments | changed timeout for controller test Signed-off-by: Jakub Delicat * param changed to parameter Signed-off-by: Jakub Delicat * Multi robot diff drive example Signed-off-by: Jakub Delicat * imu also works Signed-off-by: Jakub Delicat * Added use_multirobot_system | found imu error Signed-off-by: Jakub Delicat * changed all topics to ns Signed-off-by: Jakub Delicat * removed tf_prefix from xacro tests Signed-off-by: Jakub Delicat * removed comments | add use_multirobot_system arg Signed-off-by: Jakub Delicat * moved multi robot system controllers config to rosbot_controller Signed-off-by: Jakub Delicat * fixed rosbot_gazebo test Signed-off-by: Jakub Delicat * separated namespaced tests | added multirobot gazebo example with broken imu Signed-off-by: Jakub Delicat * add multicontroller test Signed-off-by: Jakub Delicat * add multibringup test Signed-off-by: Jakub Delicat * add multibringup test Signed-off-by: Jakub Delicat * Added remove rosbot_macro Signed-off-by: Jakub Delicat * fixed miss spelling Signed-off-by: Jakub Delicat * The change from robot_hardware_interface seems to be unnecessary Signed-off-by: Jakub Delicat * moved multirobot tests to arrays Signed-off-by: Jakub Delicat * move rosbot_hardware_interfaces to main Signed-off-by: Jakub Delicat * Fixed len Signed-off-by: Jakub Delicat * Added updated controllers info to readme Signed-off-by: Jakub Delicat * Gazebo test (#86) * Wait for velocity stabilization * Apply suggestions from code review Co-authored-by: Jakub Delicat <109142865+delihus@users.noreply.github.com> * Apply suggestions from code review * refactor * test * headless * fix sim time * update * Update rosbot_gazebo/test/test_mecanum_simulation.py Co-authored-by: Jakub Delicat <109142865+delihus@users.noreply.github.com> --------- Co-authored-by: Jakub Delicat <109142865+delihus@users.noreply.github.com> * Update changelog * 0.10.5 * Added gazebo remappings file Signed-off-by: Jakub Delicat * Added namespace to mappings Signed-off-by: Jakub Delicat * fixed clock | added camera Signed-off-by: Jakub Delicat * working without multisystem Signed-off-by: Jakub Delicat * parse robots arg | removed delays from rosbot_controller Signed-off-by: Jakub Delicat * TEsting Signed-off-by: Jakub Delicat * fixed imu while testing Signed-off-by: Jakub Delicat * Multirobots tests Signed-off-by: Jakub Delicat * all gazebo tests Signed-off-by: Jakub Delicat * changed to namespaced repositories Signed-off-by: Jakub Delicat * revert headless to tests Signed-off-by: Jakub Delicat * IMU name with slash Signed-off-by: Jakub Delicat * removed delay from spawning Signed-off-by: Jakub Delicat * added name for ign_ros2_controller Signed-off-by: Jakub Delicat * fixed submodule Signed-off-by: Jakub Delicat * checking mecanum controller sim time true Signed-off-by: Jakub Delicat * Update industrial_ci.yaml * removed coltroller test for testing timeout Signed-off-by: Jakub Delicat * removed bringup tests for testing timeout Signed-off-by: Jakub Delicat * used launch_ros features Signed-off-by: Jakub Delicat * Added unbuffered to skip list Signed-off-by: Jakub Delicat * added microros submodules to ignore and removed ros2 control demos from tests Signed-off-by: Jakub Delicat * Revert "removed bringup tests for testing timeout" This reverts commit f58a96780568bc4edd9cd976dc4f4d8bb29ab554. * Revert "removed coltroller test for testing timeout" This reverts commit 50c90e5028450640f9b2ce78584a76bceb3e6989. * Added nav2_common to dependencies Signed-off-by: Jakub Delicat * added readme Signed-off-by: Jakub Delicat * Get the newest version of gz_ros2_control Signed-off-by: Jakub Delicat * removed sim_time from yaml Signed-off-by: Jakub Delicat * Added range sensor Signed-off-by: Jakub Delicat * Added tests for ranges and camera Signed-off-by: Jakub Delicat * removed unnecesary if Signed-off-by: Jakub Delicat * Added me to the authors Signed-off-by: Jakub Delicat * Update rosbot_hardware.repos * Update rosbot_simulation.repos * Update .github/workflows/industrial_ci.yaml Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> * Update .github/workflows/industrial_ci.yaml Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> * default simulator is ignition-gazebo Signed-off-by: Jakub Delicat * merged multiple_simulation.launch.py with simulation.launch.py Signed-off-by: Jakub Delicat * unnecessary event Signed-off-by: Jakub Delicat * fix simulation.launch.py * test max 3 robots + env(NS) + readme * Better diagnostic in multirobot test * simplify test --------- Signed-off-by: Jakub Delicat Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Co-authored-by: action-bot Co-authored-by: rafal-gorecki --- .github/workflows/industrial_ci.yaml | 9 +- .gitignore | 2 + .wordlist.txt | 1 + README.md | 7 +- ROS_API.md | 13 +- rosbot/rosbot_hardware.repos | 4 +- rosbot/rosbot_simulation.repos | 2 +- rosbot_bringup/config/ekf.yaml | 2 +- rosbot_bringup/launch/bringup.launch.py | 15 +- rosbot_bringup/test/test_multirobot_ekf.py | 11 +- .../config/diff_drive_controller.yaml | 2 +- .../config/mecanum_drive_controller.yaml | 7 +- .../config/multi_diff_drive_controller.yaml | 180 ----------------- .../multi_mecanum_drive_controller.yaml | 191 ------------------ rosbot_controller/launch/controller.launch.py | 35 +--- .../test/test_diff_drive_controllers.py | 15 +- .../test/test_mecanum_controllers.py | 15 +- .../test/test_multirobot_controllers.py | 29 +-- .../test_namespaced_diff_drive_controllers.py | 15 +- .../test_namespaced_mecanum_controllers.py | 15 +- rosbot_controller/test/test_utils.py | 17 ++ rosbot_description/urdf/body.urdf.xacro | 10 +- .../urdf/components/vl53lox.urdf.xacro | 21 +- rosbot_description/urdf/rosbot.urdf.xacro | 37 ++-- .../urdf/rosbot_macro.urdf.xacro | 47 ++--- rosbot_description/urdf/wheel.urdf.xacro | 11 +- rosbot_gazebo/config/gz_remappings.yaml | 40 ++++ .../launch/multirobot_simulation.launch.py | 151 -------------- rosbot_gazebo/launch/simulation.launch.py | 172 ++++++++++------ rosbot_gazebo/launch/spawn.launch.py | 79 ++------ rosbot_gazebo/package.xml | 2 + rosbot_gazebo/setup.py | 1 + .../test/test_diff_drive_simulation.py | 62 +++--- rosbot_gazebo/test/test_ign_kill_utils.py | 22 +- rosbot_gazebo/test/test_mecanum_simulation.py | 75 +++---- .../test_multirobot_diff_drive_simulation.py | 96 +++++++++ .../test_multirobot_mecanum_simulation.py | 97 +++++++++ .../test_namespaced_diff_drive_simulation.py | 89 ++++++++ .../test_namespaced_mecanum_simulation.py | 90 +++++++++ rosbot_gazebo/test/test_utils.py | 164 +++++++++++++-- 40 files changed, 878 insertions(+), 975 deletions(-) delete mode 100644 rosbot_controller/config/multi_diff_drive_controller.yaml delete mode 100644 rosbot_controller/config/multi_mecanum_drive_controller.yaml create mode 100644 rosbot_gazebo/config/gz_remappings.yaml delete mode 100644 rosbot_gazebo/launch/multirobot_simulation.launch.py create mode 100644 rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py create mode 100644 rosbot_gazebo/test/test_multirobot_mecanum_simulation.py create mode 100644 rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py create mode 100644 rosbot_gazebo/test/test_namespaced_mecanum_simulation.py diff --git a/.github/workflows/industrial_ci.yaml b/.github/workflows/industrial_ci.yaml index 87c6c5f1..d1f86469 100644 --- a/.github/workflows/industrial_ci.yaml +++ b/.github/workflows/industrial_ci.yaml @@ -29,7 +29,7 @@ jobs: ros_industrial_ci: name: ROS Industrial CI runs-on: ubuntu-22.04 - timeout-minutes: 30 # Set the job timeout to 30 minutes + timeout-minutes: 30 needs: - black - spellcheck @@ -54,7 +54,8 @@ jobs: shell: bash run: | 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/rosbot_hardware.repos && + vcs import src < src/rosbot/rosbot_simulation.repos - name: Copy only diff_drive_controller and imu_sensor_broadcaster, waits for features from ros2-control shell: bash @@ -63,6 +64,10 @@ jobs: cp -r src/ros2_controllers/imu_sensor_broadcaster src/ rm -rf src/ros2_controllers + - name: Remove ign_ros2_control demo and tests + shell: bash + run: rm -rf src/gazebosim/gz_ros2_control/ign_ros2_control_demos src/gazebosim/gz_ros2_control/gz_ros2_control_tests + # Package micro_ros_msgs does not have industrial ci and tests does not pass. # For more information see https://github.com/micro-ROS/micro_ros_msgs/issues/7 - name: Remove tests from micro_ros_msgs diff --git a/.gitignore b/.gitignore index 2ef10e57..35a1da3b 100644 --- a/.gitignore +++ b/.gitignore @@ -14,6 +14,8 @@ husarion/husarion_office_gz gazebosim/gz_ros2_control diff_drive_controller imu_sensor_broadcaster +micro-ROS-Agent/ +micro_ros_msgs/ # pyspelling *.dic diff --git a/.wordlist.txt b/.wordlist.txt index cb6fa9be..62f3753d 100644 --- a/.wordlist.txt +++ b/.wordlist.txt @@ -148,3 +148,4 @@ stm ttyUSB subprocess cbus +unbuffered diff --git a/README.md b/README.md index 26aea31f..5ab328d1 100644 --- a/README.md +++ b/README.md @@ -62,7 +62,7 @@ rm -r src/rosbot_gazebo sudo rosdep init rosdep update --rosdistro $ROS_DISTRO rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y -colcon build +colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release ``` > **Prerequisites** @@ -100,10 +100,13 @@ vcs import src < src/rosbot/rosbot_simulation.repos # Build only diff_drive_controller and imu_sensor_broadcaster from ros2_controllers cp -r src/ros2_controllers/diff_drive_controller src && cp -r src/ros2_controllers/imu_sensor_broadcaster src && rm -rf src/ros2_controllers +# Remove ign_ros2_control demo and test +rm -rf src/gazebosim/gz_ros2_control/ign_ros2_control_demos src/gazebosim/gz_ros2_control/gz_ros2_control_tests + sudo rosdep init rosdep update --rosdistro $ROS_DISTRO rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y -colcon build +colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release ``` Running: diff --git a/ROS_API.md b/ROS_API.md index 63336694..3889c04d 100644 --- a/ROS_API.md +++ b/ROS_API.md @@ -11,7 +11,7 @@ Use `bringup.launch.py` from `rosbot_bringup` to start all base functionalities - `/odometry/filtered` (_nav_msgs/Odometry_) -- `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: +Use `controller.launch.py` from `rosbot_controller`, it loads robot model defined in `rosbot_description` as well as ros2 control [rosbot_hardware_interfaces](https://github.com/husarion/rosbot_hardware_interfaces). It also starts controllers: * `joint_state_broadcaster` * `rosbot_base_controller` * `imu_broadcaster` @@ -29,5 +29,16 @@ Use `bringup.launch.py` from `rosbot_bringup` to start all base functionalities - `/imu_broadcaster/imu` (_sensor_msgs/Imu_) Use `simulation.launch.py` from `rosbot_gazebo` to start all base functionalities for ROSbot 2, 2 PRO, 2R in the Gazebo simulator. +If you want to spawn multiple robots use `simulation.launch.py` with the `robots` argument e. g.: +```bash +ros2 launch rosbot_gazebo simulation.launch.py robots:='robot1={x: 0.0, y: -1.0}; robot2={x: 1.0, y: -1.0}; robot3={x: 2.0, y: -1.0}; robot4={x: 3.0, y: -1.0}' +``` + +If you want to use your own world add to the world's sdf file gazebo sensors plugins inside any `` tag: +```xml + + +``` + > **Warning** > The distance sensors' topics types from Gazebo simulation mismatch with the real ones. The range sensors are not implemented yet in the Gazebo Ignition (for more information look [here](https://github.com/gazebosim/gz-sensors/issues/19)). The real type is [sensor_msgs/msg/Range](https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/Range.msg) but simulated [sensor_msgs/msg/LaserScan](https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/LaserScan.msg). The first value of the `ranges` in [sensor_msgs/msg/LaserScan](https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/LaserScan.msg) is the `range` field of [sensor_msgs/msg/Range](https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/Range.msg). diff --git a/rosbot/rosbot_hardware.repos b/rosbot/rosbot_hardware.repos index 80f61e7d..d94215af 100644 --- a/rosbot/rosbot_hardware.repos +++ b/rosbot/rosbot_hardware.repos @@ -6,7 +6,7 @@ repositories: ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git - version: ros2 + version: ros2 rosbot_controllers: type: git url: https://github.com/husarion/rosbot_controllers @@ -17,7 +17,7 @@ repositories: ros2_controllers: type: git url: https://github.com/delihus/ros2_controllers - version: humble-backport + version: humble micro_ros_msgs: type: git url: https://github.com/micro-ROS/micro_ros_msgs.git diff --git a/rosbot/rosbot_simulation.repos b/rosbot/rosbot_simulation.repos index affc3289..40ba729f 100644 --- a/rosbot/rosbot_simulation.repos +++ b/rosbot/rosbot_simulation.repos @@ -1,7 +1,7 @@ repositories: gazebosim/gz_ros2_control: type: git - url: https://github.com/ros-controls/gz_ros2_control.git + url: https://github.com/ros-controls/gz_ros2_control version: humble husarion/husarion_office_gz: diff --git a/rosbot_bringup/config/ekf.yaml b/rosbot_bringup/config/ekf.yaml index c2d5a8f9..b8bb1672 100644 --- a/rosbot_bringup/config/ekf.yaml +++ b/rosbot_bringup/config/ekf.yaml @@ -3,7 +3,7 @@ ## ekf config file ### /**/ekf_filter_node: ros__parameters: - frequency: 25.0 + frequency: 20.0 sensor_timeout: 0.05 two_d_mode: true diff --git a/rosbot_bringup/launch/bringup.launch.py b/rosbot_bringup/launch/bringup.launch.py index 4225276b..4785a804 100644 --- a/rosbot_bringup/launch/bringup.launch.py +++ b/rosbot_bringup/launch/bringup.launch.py @@ -17,14 +17,14 @@ from ament_index_python.packages import get_package_share_directory from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution def generate_launch_description(): namespace = LaunchConfiguration("namespace") declare_namespace_arg = DeclareLaunchArgument( "namespace", - default_value="", + default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), description="Namespace for all topics and tfs", ) @@ -62,13 +62,6 @@ def generate_launch_description(): ), ) - use_multirobot_system = LaunchConfiguration("use_multirobot_system") - declare_use_multirobot_system_arg = DeclareLaunchArgument( - "use_multirobot_system", - default_value="false", - description="Enable correct Ignition Gazebo configuration in URDF", - ) - controller_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( @@ -85,7 +78,6 @@ def generate_launch_description(): "use_gpu": use_gpu, "simulation_engine": simulation_engine, "namespace": namespace, - "use_multirobot_system": use_multirobot_system, }.items(), ) @@ -94,10 +86,10 @@ def generate_launch_description(): robot_localization_node = Node( package="robot_localization", executable="ekf_node", - name="ekf_filter_node", output="screen", parameters=[ekf_config], remappings=[ + ("/diagnostics", "diagnostics"), ("/tf", "tf"), ("/tf_static", "tf_static"), ], @@ -110,7 +102,6 @@ def generate_launch_description(): declare_use_sim_arg, declare_use_gpu_arg, declare_simulation_engine_arg, - declare_use_multirobot_system_arg, SetParameter(name="use_sim_time", value=use_sim), controller_launch, robot_localization_node, diff --git a/rosbot_bringup/test/test_multirobot_ekf.py b/rosbot_bringup/test/test_multirobot_ekf.py index ca3d5685..57ae7a6c 100644 --- a/rosbot_bringup/test/test_multirobot_ekf.py +++ b/rosbot_bringup/test/test_multirobot_ekf.py @@ -20,12 +20,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, TimerAction +from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource from test_utils import BringupTestNode -robot_names = ["rosbot1", "rosbot2", "rosbot3", "rosbot4"] +robot_names = ["robot1", "robot2", "robot3"] @launch_pytest.fixture @@ -50,11 +50,8 @@ def generate_test_description(): "namespace": robot_names[i], }.items(), ) - if i > 0: - delayed_bringup_launch = TimerAction(period=i * 10.0, actions=[bringup_launch]) - actions.append(delayed_bringup_launch) - else: - actions.append(bringup_launch) + + actions.append(bringup_launch) return LaunchDescription(actions) diff --git a/rosbot_controller/config/diff_drive_controller.yaml b/rosbot_controller/config/diff_drive_controller.yaml index 1755236d..df7a0af4 100644 --- a/rosbot_controller/config/diff_drive_controller.yaml +++ b/rosbot_controller/config/diff_drive_controller.yaml @@ -1,6 +1,5 @@ /**/controller_manager: ros__parameters: - use_sim_time: False update_rate: 20 # Hz joint_state_broadcaster: @@ -13,6 +12,7 @@ /**/imu_broadcaster: ros__parameters: tf_frame_prefix_enable: false + use_namespace_as_sensor_name_prefix: true sensor_name: imu frame_id: imu_link diff --git a/rosbot_controller/config/mecanum_drive_controller.yaml b/rosbot_controller/config/mecanum_drive_controller.yaml index 9eec0f0e..4ee7d87e 100644 --- a/rosbot_controller/config/mecanum_drive_controller.yaml +++ b/rosbot_controller/config/mecanum_drive_controller.yaml @@ -1,6 +1,5 @@ /**/controller_manager: ros__parameters: - use_sim_time: false update_rate: 20 # Hz joint_state_broadcaster: @@ -13,6 +12,7 @@ /**/imu_broadcaster: ros__parameters: tf_frame_prefix_enable: false + use_namespace_as_sensor_name_prefix: true sensor_name: imu frame_id: imu_link @@ -33,9 +33,8 @@ wheel_separation_y: 0.192 wheel_radius: 0.047 - - wheel_separation_multiplier_x: 1.0 - wheel_separation_multiplier_y: 1.0 + wheel_separation_x_multiplier: 1.0 + wheel_separation_y_multiplier: 1.0 wheel_radius_multiplier: 1.0 diff --git a/rosbot_controller/config/multi_diff_drive_controller.yaml b/rosbot_controller/config/multi_diff_drive_controller.yaml deleted file mode 100644 index 9b0e3a6a..00000000 --- a/rosbot_controller/config/multi_diff_drive_controller.yaml +++ /dev/null @@ -1,180 +0,0 @@ -# This file has controllers configuration for 2 robots because the ign_ros2_control::IgnitionROS2ControlPlugin -# parameter parses only one file. -# -/rosbot1/controller_manager: - ros__parameters: - use_sim_time: True - update_rate: 20 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - rosbot_base_controller: - type: diff_drive_controller/DiffDriveController - imu_broadcaster: - type: imu_sensor_broadcaster/IMUSensorBroadcaster - -/rosbot1/imu_broadcaster: - ros__parameters: - use_sim_time: True - sensor_name: rosbot1/imu - tf_frame_prefix_enable: False - frame_id: imu_link - static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet - static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally - static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally - -/rosbot1/rosbot_base_controller: - ros__parameters: - use_sim_time: True - tf_frame_prefix_enable: False - - left_wheel_names: ["rosbot1/fl_wheel_joint", "rosbot1/rl_wheel_joint"] - right_wheel_names: ["rosbot1/fr_wheel_joint", "rosbot1/rr_wheel_joint"] - - wheel_separation: 0.186 - wheel_radius: 0.0425 - - # For skid drive kinematics it will act as ICR coefficient - # Kinematic model with ICR coefficient isn't totally accurate and this coefficient can differ - # for various ground types, but checking on our office floor 1.3 looked approximately alright - wheel_separation_multiplier: 1.45 - - left_wheel_radius_multiplier: 1.0 - right_wheel_radius_multiplier: 1.0 - - # Feedback from motors is published at around 20 Hz - publish_rate: 20.0 - odom_frame_id: odom - base_frame_id: base_link - pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values measured experimentally - - # Whether to use feedback or commands for odometry calculations - open_loop: false - - # Update odometry from velocity - #! IMPORTANT at the time of writing it is bugged on deb galactic version and require fork of diff drive controller installed from sources - # in sensor fusion only velocity is used and with this setting it is more accurate - position_feedback: false - # velocity computation filtering - velocity_rolling_window_size: 1 - - enable_odom_tf: false - - cmd_vel_timeout: 0.5 - #publish_limited_velocity: true - use_stamped_vel: false - - # Velocity and acceleration limits - # Whenever a min_* is unspecified, default to -max_* - linear: - x: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - # top speed is around 1.2m/s - max_velocity: 0.9 # m/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 1.0 # m/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # m/s^3 - - angular: - z: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - max_velocity: 3.0 # rad/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 4.0 # rad/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # rad/s^3 - -/rosbot2/controller_manager: - ros__parameters: - use_sim_time: True - update_rate: 20 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - rosbot_base_controller: - type: diff_drive_controller/DiffDriveController - imu_broadcaster: - type: imu_sensor_broadcaster/IMUSensorBroadcaster - -/rosbot2/imu_broadcaster: - ros__parameters: - use_sim_time: True - sensor_name: rosbot2/imu - tf_frame_prefix_enable: False - frame_id: imu_link - static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet - static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally - static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally - -/rosbot2/rosbot_base_controller: - ros__parameters: - use_sim_time: True - tf_frame_prefix_enable: False - - left_wheel_names: ["rosbot2/fl_wheel_joint", "rosbot2/rl_wheel_joint"] - right_wheel_names: ["rosbot2/fr_wheel_joint", "rosbot2/rr_wheel_joint"] - - wheel_separation: 0.186 - wheel_radius: 0.0425 - - # For skid drive kinematics it will act as ICR coefficient - # Kinematic model with ICR coefficient isn't totally accurate and this coefficient can differ - # for various ground types, but checking on our office floor 1.3 looked approximately alright - wheel_separation_multiplier: 1.45 - - left_wheel_radius_multiplier: 1.0 - right_wheel_radius_multiplier: 1.0 - - # Feedback from motors is published at around 20 Hz - publish_rate: 20.0 - odom_frame_id: odom - base_frame_id: base_link - pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values measured experimentally - - # Whether to use feedback or commands for odometry calculations - open_loop: false - - # Update odometry from velocity - #! IMPORTANT at the time of writing it is bugged on deb galactic version and require fork of diff drive controller installed from sources - # in sensor fusion only velocity is used and with this setting it is more accurate - position_feedback: false - # velocity computation filtering - velocity_rolling_window_size: 1 - - enable_odom_tf: false - - cmd_vel_timeout: 0.5 - #publish_limited_velocity: true - use_stamped_vel: false - - # Velocity and acceleration limits - # Whenever a min_* is unspecified, default to -max_* - linear: - x: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - # top speed is around 1.2m/s - max_velocity: 0.9 # m/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 1.0 # m/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # m/s^3 - - angular: - z: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - max_velocity: 3.0 # rad/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 4.0 # rad/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # rad/s^3 diff --git a/rosbot_controller/config/multi_mecanum_drive_controller.yaml b/rosbot_controller/config/multi_mecanum_drive_controller.yaml deleted file mode 100644 index f352830e..00000000 --- a/rosbot_controller/config/multi_mecanum_drive_controller.yaml +++ /dev/null @@ -1,191 +0,0 @@ -/rosbot1/controller_manager: - ros__parameters: - use_sim_time: false - update_rate: 20 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - rosbot_base_controller: - type: mecanum_drive_controller/MecanumDriveController - imu_broadcaster: - type: imu_sensor_broadcaster/IMUSensorBroadcaster - -/rosbot1/imu_broadcaster: - ros__parameters: - sensor_name: rosbot1/imu - tf_frame_prefix_enable: False - frame_id: imu_link - static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet - static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values based on diff_drive - static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values based on diff_drive - -/rosbot1/rosbot_base_controller: - ros__parameters: - tf_frame_prefix_enable: False - front_left_wheel_name: rosbot1/fl_wheel_joint - front_right_wheel_name: rosbot1/fr_wheel_joint - rear_left_wheel_name: rosbot1/rl_wheel_joint - rear_right_wheel_name: rosbot1/rr_wheel_joint - - wheel_separation_x: 0.106 - wheel_separation_y: 0.192 - wheel_radius: 0.047 - - - wheel_separation_multiplier_x: 1.0 - wheel_separation_multiplier_y: 1.0 - - wheel_radius_multiplier: 1.0 - - # Feedback from motors is published at around 20 Hz - publish_rate: 25.0 - odom_frame_id: odom - base_frame_id: base_link - pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values measured experimentally - - # Whether to use feedback or commands for odometry calculations - open_loop: false - - # Update odometry from velocity - # in sensor fusion only velocity is used and with this setting it is more accurate - position_feedback: false - # velocity computation filtering - velocity_rolling_window_size: 1 - - enable_odom_tf: false - - cmd_vel_timeout: 0.5 - #publish_limited_velocity: true - use_stamped_vel: false - - # Velocity and acceleration limits - # Whenever a min_* is unspecified, default to -max_* - linear: - x: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - # top speed is around 1.2m/s - max_velocity: 0.9 # m/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 1.0 # m/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # m/s^3 - y: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - # top speed is around 1.2m/s - max_velocity: 0.9 # m/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 1.0 # m/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # m/s^3 - - angular: - z: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - max_velocity: 3.0 # rad/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 4.0 # rad/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # rad/s^3 - -/rosbot2/controller_manager: - ros__parameters: - use_sim_time: false - update_rate: 20 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - rosbot_base_controller: - type: mecanum_drive_controller/MecanumDriveController - imu_broadcaster: - type: imu_sensor_broadcaster/IMUSensorBroadcaster - -/rosbot2/imu_broadcaster: - ros__parameters: - sensor_name: rosbot2/imu - tf_frame_prefix_enable: False - frame_id: imu_link - static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet - static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values based on diff_drive - static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values based on diff_drive - -/rosbot2/rosbot_base_controller: - ros__parameters: - tf_frame_prefix_enable: False - front_left_wheel_name: rosbot2/fl_wheel_joint - front_right_wheel_name: rosbot2/fr_wheel_joint - rear_left_wheel_name: rosbot2/rl_wheel_joint - rear_right_wheel_name: rosbot2/rr_wheel_joint - - wheel_separation_x: 0.106 - wheel_separation_y: 0.192 - wheel_radius: 0.047 - - - wheel_separation_multiplier_x: 1.0 - wheel_separation_multiplier_y: 1.0 - - wheel_radius_multiplier: 1.0 - - # Feedback from motors is published at around 20 Hz - publish_rate: 25.0 - odom_frame_id: odom - base_frame_id: base_link - pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values measured experimentally - - # Whether to use feedback or commands for odometry calculations - open_loop: false - - # Update odometry from velocity - # in sensor fusion only velocity is used and with this setting it is more accurate - position_feedback: false - # velocity computation filtering - velocity_rolling_window_size: 1 - - enable_odom_tf: false - - cmd_vel_timeout: 0.5 - #publish_limited_velocity: true - use_stamped_vel: false - - # Velocity and acceleration limits - # Whenever a min_* is unspecified, default to -max_* - linear: - x: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - # top speed is around 1.2m/s - max_velocity: 0.9 # m/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 1.0 # m/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # m/s^3 - y: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - # top speed is around 1.2m/s - max_velocity: 0.9 # m/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 1.0 # m/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # m/s^3 - - angular: - z: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - max_velocity: 3.0 # rad/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 4.0 # rad/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # rad/s^3 diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index 779a3b47..e94a3b9a 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -14,9 +14,8 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import RegisterEventHandler, DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument from launch.conditions import UnlessCondition -from launch.event_handlers import OnProcessExit from launch.substitutions import ( Command, PythonExpression, @@ -68,13 +67,6 @@ def generate_launch_description(): choices=["ignition-gazebo", "gazebo-classic", "webots"], ) - use_multirobot_system = LaunchConfiguration("use_multirobot_system") - declare_use_multirobot_system_arg = DeclareLaunchArgument( - "use_multirobot_system", - default_value="false", - description="Enable correct Ignition Gazebo configuration in URDF", - ) - controller_config_name = PythonExpression( [ "'mecanum_drive_controller.yaml' if ", @@ -86,6 +78,7 @@ def generate_launch_description(): namespace_ext = PythonExpression( ["''", " if '", namespace, "' == '' ", "else ", "'", namespace, "/'"] ) + controller_manager_name = LaunchConfiguration( "controller_manager_name", default=[namespace_ext, "controller_manager"], @@ -113,8 +106,6 @@ def generate_launch_description(): simulation_engine, " namespace:=", namespace, - " use_multirobot_system:=", - use_multirobot_system, ] ) robot_description = {"robot_description": robot_description_content} @@ -182,14 +173,6 @@ def generate_launch_description(): ], ) - # Delay start of robot_controller after joint_state_broadcaster - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], - ) - ) - imu_broadcaster_spawner = Node( package="controller_manager", executable="spawner", @@ -204,15 +187,6 @@ def generate_launch_description(): ], ) - # Delay start of imu_broadcaster after robot_controller - # when spawning without delay ros2_control_node sometimes crashed - delay_imu_broadcaster_spawner_after_robot_controller_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=robot_controller_spawner, - on_exit=[imu_broadcaster_spawner], - ) - ) - return LaunchDescription( [ declare_namespace_arg, @@ -220,12 +194,11 @@ def generate_launch_description(): declare_use_sim_arg, declare_use_gpu_arg, declare_simulation_engine_arg, - declare_use_multirobot_system_arg, SetParameter("use_sim_time", value=use_sim), control_node, robot_state_pub_node, joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, - delay_imu_broadcaster_spawner_after_robot_controller_spawner, + robot_controller_spawner, + imu_broadcaster_spawner, ] ) diff --git a/rosbot_controller/test/test_diff_drive_controllers.py b/rosbot_controller/test/test_diff_drive_controllers.py index 3b69459c..9a9aa6fb 100644 --- a/rosbot_controller/test/test_diff_drive_controllers.py +++ b/rosbot_controller/test/test_diff_drive_controllers.py @@ -23,7 +23,7 @@ from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import ControllersTestNode +from test_utils import ControllersTestNode, controller_readings_test @launch_pytest.fixture @@ -85,17 +85,6 @@ def test_controllers_startup_success(): node.start_publishing_fake_hardware() node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Odom message but it was not received. Check rosbot_base_controller!" - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Imu message but it was not received. Check imu_broadcaster!" + controller_readings_test(node) finally: rclpy.shutdown() diff --git a/rosbot_controller/test/test_mecanum_controllers.py b/rosbot_controller/test/test_mecanum_controllers.py index 5f2607e3..9b8aa53c 100644 --- a/rosbot_controller/test/test_mecanum_controllers.py +++ b/rosbot_controller/test/test_mecanum_controllers.py @@ -23,7 +23,7 @@ from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import ControllersTestNode +from test_utils import ControllersTestNode, controller_readings_test @launch_pytest.fixture @@ -85,17 +85,6 @@ def test_controllers_startup_success(): node.start_publishing_fake_hardware() node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Odom message but it was not received. Check rosbot_base_controller!" - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Imu message but it was not received. Check imu_broadcaster!" + 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 index e6117a2f..0b2e9084 100644 --- a/rosbot_controller/test/test_multirobot_controllers.py +++ b/rosbot_controller/test/test_multirobot_controllers.py @@ -20,12 +20,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, TimerAction +from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import ControllersTestNode +from test_utils import ControllersTestNode, controller_readings_test -robot_names = ["rosbot1", "rosbot2", "rosbot3", "rosbot4"] +robot_names = ["robot1", "robot2", "robot3"] @launch_pytest.fixture @@ -50,11 +50,8 @@ def generate_test_description(): "namespace": robot_names[i], }.items(), ) - if i > 0: - delayed_bringup_launch = TimerAction(period=i * 10.0, actions=[controller_launch]) - actions.append(delayed_bringup_launch) - else: - actions.append(controller_launch) + + actions.append(controller_launch) return LaunchDescription(actions) @@ -69,19 +66,7 @@ def test_multirobot_controllers_startup_success(): node.start_publishing_fake_hardware() node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert msgs_received_flag, ( - f"Expected JointStates message but it was not received. Check {robot_name}/" - "joint_state_broadcaster!" - ) - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert msgs_received_flag, ( - f"Expected Odom message but it was not received. Check {robot_name}/" - "rosbot_base_controller!" - ) - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), f"Expected Imu message but it was not received. Check {robot_name}/imu_broadcaster!" + + 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 index fdab2de6..7371ab7d 100644 --- a/rosbot_controller/test/test_namespaced_diff_drive_controllers.py +++ b/rosbot_controller/test/test_namespaced_diff_drive_controllers.py @@ -23,7 +23,7 @@ from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import ControllersTestNode +from test_utils import ControllersTestNode, controller_readings_test @launch_pytest.fixture @@ -86,17 +86,6 @@ def test_namespaced_controllers_startup_success(): node.start_publishing_fake_hardware() node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Odom message but it was not received. Check rosbot_base_controller!" - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Imu message but it was not received. Check imu_broadcaster!" + 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 index e2321fd0..d1ccd623 100644 --- a/rosbot_controller/test/test_namespaced_mecanum_controllers.py +++ b/rosbot_controller/test/test_namespaced_mecanum_controllers.py @@ -23,7 +23,7 @@ from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import ControllersTestNode +from test_utils import ControllersTestNode, controller_readings_test @launch_pytest.fixture @@ -86,17 +86,6 @@ def test_namespaced_controllers_startup_success(): node.start_publishing_fake_hardware() node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Odom message but it was not received. Check rosbot_base_controller!" - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Imu message but it was not received. Check imu_broadcaster!" + controller_readings_test(node) finally: rclpy.shutdown() diff --git a/rosbot_controller/test/test_utils.py b/rosbot_controller/test/test_utils.py index 831eadad..f5fdd34f 100644 --- a/rosbot_controller/test/test_utils.py +++ b/rosbot_controller/test/test_utils.py @@ -91,3 +91,20 @@ def publish_fake_hardware_messages(self): self.imu_publisher.publish(imu_msg) self.joint_states_publisher.publish(joint_state_msg) + + +def controller_readings_test(node, robot_name="ROSbot"): + msgs_received_flag = node.joint_state_msg_event.wait(timeout=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(timeout=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(timeout=10.0) + assert ( + msgs_received_flag + ), f"{robot_name}: expected Imu message but it was not received. Check imu_broadcaster!" diff --git a/rosbot_description/urdf/body.urdf.xacro b/rosbot_description/urdf/body.urdf.xacro index 16ec6563..647087b0 100644 --- a/rosbot_description/urdf/body.urdf.xacro +++ b/rosbot_description/urdf/body.urdf.xacro @@ -1,7 +1,7 @@ - + @@ -79,10 +79,10 @@ - - - - + + + + diff --git a/rosbot_description/urdf/components/vl53lox.urdf.xacro b/rosbot_description/urdf/components/vl53lox.urdf.xacro index 83d60c8b..6518a943 100644 --- a/rosbot_description/urdf/components/vl53lox.urdf.xacro +++ b/rosbot_description/urdf/components/vl53lox.urdf.xacro @@ -1,7 +1,14 @@ - + + + + + + + + @@ -15,9 +22,9 @@ https://github.com/gazebosim/gz-sensors/issues/19 --> - + - /range/${prefix} + ${namespace_ext}range/${prefix} ${prefix}_range ${prefix}_range @@ -44,12 +51,12 @@ true false + + + ${namespace_ext} + - - ogre2 - - diff --git a/rosbot_description/urdf/rosbot.urdf.xacro b/rosbot_description/urdf/rosbot.urdf.xacro index 1fdaee18..eccac3db 100644 --- a/rosbot_description/urdf/rosbot.urdf.xacro +++ b/rosbot_description/urdf/rosbot.urdf.xacro @@ -4,9 +4,8 @@ - + - + namespace="$(arg namespace)" /> - - - + + + + + - - - diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index 697b8e93..48b30f2c 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -1,13 +1,6 @@ - - - - - - + @@ -28,13 +21,13 @@ - + - - - - + + + + @@ -85,22 +78,22 @@ - + - + - + - + @@ -130,18 +123,11 @@ - - - - - - - - $(find rosbot_controller)/config/${controller_config_prefix}mecanum_drive_controller.yaml + $(find rosbot_controller)/config/mecanum_drive_controller.yaml - $(find rosbot_controller)/config/${controller_config_prefix}diff_drive_controller.yaml + $(find rosbot_controller)/config/diff_drive_controller.yaml @@ -155,7 +141,6 @@ - true 25 ${namespace_ext}imu/data_raw @@ -163,11 +148,9 @@ false imu_link imu_link - - - ${namespace} - - + + ${namespace_ext} + diff --git a/rosbot_description/urdf/wheel.urdf.xacro b/rosbot_description/urdf/wheel.urdf.xacro index 770bb2e0..e9d5057e 100644 --- a/rosbot_description/urdf/wheel.urdf.xacro +++ b/rosbot_description/urdf/wheel.urdf.xacro @@ -1,14 +1,7 @@ - - - - - - - - + @@ -65,7 +58,7 @@ - + diff --git a/rosbot_gazebo/config/gz_remappings.yaml b/rosbot_gazebo/config/gz_remappings.yaml new file mode 100644 index 00000000..6f7a6730 --- /dev/null +++ b/rosbot_gazebo/config/gz_remappings.yaml @@ -0,0 +1,40 @@ +- topic_name: "/clock" + ros_type_name: "rosgraph_msgs/msg/Clock" + gz_type_name: "ignition.msgs.Clock" + direction: GZ_TO_ROS + +- topic_name: "/scan" + ros_type_name: "sensor_msgs/msg/LaserScan" + gz_type_name: "ignition.msgs.LaserScan" + +- topic_name: "/camera/camera_info" + ros_type_name: "sensor_msgs/msg/CameraInfo" + gz_type_name: "ignition.msgs.CameraInfo" + +- topic_name: "/camera/depth_image" + ros_type_name: "sensor_msgs/msg/Image" + gz_type_name: "ignition.msgs.Image" + +- topic_name: "/camera/image" + ros_type_name: "sensor_msgs/msg/Image" + gz_type_name: "ignition.msgs.Image" + +- topic_name: "/camera/points" + ros_type_name: "sensor_msgs/msg/PointCloud2" + gz_type_name: "ignition.msgs.PointCloudPacked" + +- topic_name: "/range/fl" + ros_type_name: "sensor_msgs/msg/LaserScan" + gz_type_name: "ignition.msgs.LaserScan" + +- topic_name: "/range/fr" + ros_type_name: "sensor_msgs/msg/LaserScan" + gz_type_name: "ignition.msgs.LaserScan" + +- topic_name: "/range/rl" + ros_type_name: "sensor_msgs/msg/LaserScan" + gz_type_name: "ignition.msgs.LaserScan" + +- topic_name: "/range/rr" + ros_type_name: "sensor_msgs/msg/LaserScan" + gz_type_name: "ignition.msgs.LaserScan" diff --git a/rosbot_gazebo/launch/multirobot_simulation.launch.py b/rosbot_gazebo/launch/multirobot_simulation.launch.py deleted file mode 100644 index ec846c68..00000000 --- a/rosbot_gazebo/launch/multirobot_simulation.launch.py +++ /dev/null @@ -1,151 +0,0 @@ -# Copyright 2023 Husarion -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, TimerAction -from launch.substitutions import ( - PathJoinSubstitution, - PythonExpression, - LaunchConfiguration, -) -from launch.launch_description_sources import PythonLaunchDescriptionSource - -from launch_ros.actions import SetParameter - -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - mecanum = LaunchConfiguration("mecanum") - declare_mecanum_arg = DeclareLaunchArgument( - "mecanum", - default_value="False", - description=( - "Whether to use mecanum drive controller (otherwise diff drive controller is used)" - ), - ) - - world_package = get_package_share_directory("husarion_office_gz") - world_file = PathJoinSubstitution([world_package, "worlds", "husarion_world.sdf"]) - world_cfg = LaunchConfiguration("world") - declare_world_arg = DeclareLaunchArgument( - "world", default_value=world_file, description="SDF world file" - ) - - headless = LaunchConfiguration("headless") - declare_headless_arg = DeclareLaunchArgument( - "headless", - default_value="False", - description="Run Gazebo Ignition in the headless mode", - ) - - headless_cfg = PythonExpression( - [ - "'--headless-rendering -s -v 4 -r' if ", - headless, - " else '-r -v 4 '", - ] - ) - gz_args = [headless_cfg, " ", world_cfg] - - use_gpu = LaunchConfiguration("use_gpu") - declare_use_gpu_arg = DeclareLaunchArgument( - "use_gpu", - default_value="True", - description="Whether GPU acceleration is used", - ) - - gz_sim = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - get_package_share_directory("ros_gz_sim"), - "launch", - "gz_sim.launch.py", - ] - ) - ), - launch_arguments={ - "gz_args": gz_args, - "on_exit_shutdown": "True", - }.items(), - ) - - spawn_robot1_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - get_package_share_directory("rosbot_gazebo"), - "launch", - "spawn.launch.py", - ] - ) - ), - launch_arguments={ - "mecanum": mecanum, - "use_multirobot_system": "True", - "use_sim": "True", - "use_gpu": use_gpu, - "simulation_engine": "ignition-gazebo", - "namespace": "rosbot1", - "x": LaunchConfiguration("x1", default="0.00"), - "y": LaunchConfiguration("y1", default="2.00"), - "z": LaunchConfiguration("z1", default="0.20"), - "roll": LaunchConfiguration("roll1", default="0.00"), - "pitch": LaunchConfiguration("pitch1", default="0.00"), - "yaw": LaunchConfiguration("yaw1", default="0.00"), - }.items(), - ) - - spawn_robot2_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - get_package_share_directory("rosbot_gazebo"), - "launch", - "spawn.launch.py", - ] - ) - ), - launch_arguments={ - "mecanum": mecanum, - "use_multirobot_system": "True", - "use_sim": "True", - "use_gpu": use_gpu, - "simulation_engine": "ignition-gazebo", - "namespace": "rosbot2", - "x": LaunchConfiguration("x2", default="1.00"), - "y": LaunchConfiguration("y2", default="2.00"), - "z": LaunchConfiguration("z2", default="0.20"), - "roll": LaunchConfiguration("roll2", default="0.00"), - "pitch": LaunchConfiguration("pitch2", default="0.00"), - "yaw": LaunchConfiguration("yaw2", default="0.00"), - }.items(), - ) - - delayed_spawn_robot2 = TimerAction(period=10.0, actions=[spawn_robot2_launch]) - return LaunchDescription( - [ - declare_mecanum_arg, - declare_world_arg, - declare_headless_arg, - declare_use_gpu_arg, - # Sets use_sim_time for all nodes started below - # (doesn't work for nodes started from ignition gazebo) - SetParameter(name="use_sim_time", value=True), - gz_sim, - spawn_robot1_launch, - delayed_spawn_robot2, - ] - ) diff --git a/rosbot_gazebo/launch/simulation.launch.py b/rosbot_gazebo/launch/simulation.launch.py index be66c2d5..962d9517 100644 --- a/rosbot_gazebo/launch/simulation.launch.py +++ b/rosbot_gazebo/launch/simulation.launch.py @@ -16,28 +16,117 @@ from launch.actions import ( IncludeLaunchDescription, DeclareLaunchArgument, + LogInfo, + GroupAction, + OpaqueFunction, ) from launch.substitutions import ( + EnvironmentVariable, PathJoinSubstitution, - PythonExpression, LaunchConfiguration, + TextSubstitution, ) from launch.launch_description_sources import PythonLaunchDescriptionSource - from launch_ros.actions import SetParameter - from ament_index_python.packages import get_package_share_directory +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) + + gz_args = f"--headless-rendering -s -v 4 -r {world}" if eval(headless) else f"-r {world}" + + gz_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + get_package_share_directory("ros_gz_sim"), + "launch", + "gz_sim.launch.py", + ] + ) + ), + launch_arguments={ + "gz_args": gz_args, + "on_exit_shutdown": "True", + }.items(), + ) + + robots_list = ParseMultiRobotPose("robots").value() + if len(robots_list) == 0: + robots_list = { + namespace: { + "x": x, + "y": y, + "z": z, + "roll": roll, + "pitch": pitch, + "yaw": yaw, + } + } + + spawn_group = [] + for robot_name in robots_list: + init_pose = robots_list[robot_name] + group = GroupAction( + [ + LogInfo( + msg=[ + "Launching namespace=", + robot_name, + " init_pose=", + str(init_pose), + ] + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + get_package_share_directory("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(), + ), + ] + ) + spawn_group.append(group) + + return [gz_sim, *spawn_group] def generate_launch_description(): - namespace = LaunchConfiguration("namespace") declare_namespace_arg = DeclareLaunchArgument( "namespace", - default_value="", + default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), description="Namespace for all topics and tfs", ) - mecanum = LaunchConfiguration("mecanum") declare_mecanum_arg = DeclareLaunchArgument( "mecanum", default_value="False", @@ -46,97 +135,46 @@ def generate_launch_description(): ), ) - use_multirobot_system = LaunchConfiguration("use_multirobot_system") - declare_use_multirobot_system_arg = DeclareLaunchArgument( - "use_multirobot_system", - default_value="false", - description="Enable correct Ignition Gazebo configuration in URDF", - ) - world_package = get_package_share_directory("husarion_office_gz") world_file = PathJoinSubstitution([world_package, "worlds", "husarion_world.sdf"]) - world_cfg = LaunchConfiguration("world") declare_world_arg = DeclareLaunchArgument( "world", default_value=world_file, description="SDF world file" ) - headless = LaunchConfiguration("headless") declare_headless_arg = DeclareLaunchArgument( "headless", default_value="False", description="Run Gazebo Ignition in the headless mode", + choices=["True", "False"], ) - headless_cfg = PythonExpression( - [ - "'--headless-rendering -s -v 4 -r' if ", - headless, - " else '-r -v 4 '", - ] - ) - gz_args = [headless_cfg, " ", world_cfg] - - use_gpu = LaunchConfiguration("use_gpu") declare_use_gpu_arg = DeclareLaunchArgument( "use_gpu", default_value="True", description="Whether GPU acceleration is used", ) - gz_sim = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - get_package_share_directory("ros_gz_sim"), - "launch", - "gz_sim.launch.py", - ] - ) - ), - launch_arguments={ - "gz_args": gz_args, - "on_exit_shutdown": "True", - }.items(), - ) - - spawn_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - get_package_share_directory("rosbot_gazebo"), - "launch", - "spawn.launch.py", - ] - ) + 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}'" ), - launch_arguments={ - "mecanum": mecanum, - "use_multirobot_system": use_multirobot_system, - "use_sim": "True", - "use_gpu": use_gpu, - "simulation_engine": "ignition-gazebo", - "namespace": namespace, - "x": LaunchConfiguration("x", default="0.00"), - "y": LaunchConfiguration("y", default="2.00"), - "z": LaunchConfiguration("z", default="0.20"), - "roll": LaunchConfiguration("roll", default="0.00"), - "pitch": LaunchConfiguration("pitch", default="0.00"), - "yaw": LaunchConfiguration("yaw", default="0.00"), - }.items(), ) return LaunchDescription( [ declare_namespace_arg, - declare_use_multirobot_system_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), - gz_sim, - spawn_launch, + OpaqueFunction(function=launch_setup), ] ) diff --git a/rosbot_gazebo/launch/spawn.launch.py b/rosbot_gazebo/launch/spawn.launch.py index fc8baab9..e9c92eec 100644 --- a/rosbot_gazebo/launch/spawn.launch.py +++ b/rosbot_gazebo/launch/spawn.launch.py @@ -17,9 +17,13 @@ IncludeLaunchDescription, DeclareLaunchArgument, ) -from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, PythonExpression +from launch.substitutions import ( + PathJoinSubstitution, + LaunchConfiguration, + PythonExpression, +) from launch.launch_description_sources import PythonLaunchDescriptionSource - +from nav2_common.launch import ReplaceString from launch_ros.actions import Node, SetParameter from ament_index_python.packages import get_package_share_directory @@ -33,6 +37,10 @@ def generate_launch_description(): description="Namespace for all topics and tfs", ) + namespace_ext = PythonExpression( + ["''", " if '", namespace, "' == '' ", "else ", "'/", namespace, "'"] + ) + mecanum = LaunchConfiguration("mecanum") declare_mecanum_arg = DeclareLaunchArgument( "mecanum", @@ -42,13 +50,6 @@ def generate_launch_description(): ), ) - use_multirobot_system = LaunchConfiguration("use_multirobot_system") - declare_use_multirobot_system_arg = DeclareLaunchArgument( - "use_multirobot_system", - default_value="false", - description="Enable correct Ignition Gazebo configuration in URDF", - ) - use_gpu = LaunchConfiguration("use_gpu") declare_use_gpu_arg = DeclareLaunchArgument( "use_gpu", @@ -60,6 +61,15 @@ def generate_launch_description(): ["'rosbot'", " if '", namespace, "' == '' ", "else ", "'", namespace, "'"] ) + gz_remappings_file = PathJoinSubstitution( + [get_package_share_directory("rosbot_gazebo"), "config", "gz_remappings.yaml"] + ) + + namespaced_gz_remappings_file = ReplaceString( + source_file=gz_remappings_file, + replacements={"": (namespace_ext)}, + ) + gz_spawn_entity = Node( package="ros_gz_sim", executable="create", @@ -91,29 +101,8 @@ def generate_launch_description(): package="ros_gz_bridge", executable="parameter_bridge", name="ros_gz_bridge", - arguments=[ - "/scan@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan", - "/camera/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo", - "/camera/depth_image@sensor_msgs/msg/Image[ignition.msgs.Image", - "/camera/image@sensor_msgs/msg/Image[ignition.msgs.Image", - "/camera/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked", - "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock", - # an IR sensor is not implemented yet https://github.com/gazebosim/gz-sensors/issues/19 - "/range/fl@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan", - "/range/fr@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan", - "/range/rl@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan", - "/range/rr@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan", - ], + parameters=[{"config_file": namespaced_gz_remappings_file}], remappings=[ - ("/camera/camera_info", "camera/color/camera_info"), - ("/camera/image", "camera/color/image_raw"), - ("/camera/depth_image", "camera/depth/image_raw"), - ("/camera/points", "camera/depth/points"), - ("/scan", "scan"), - ("/range/fl", "range/fl"), - ("/range/fr", "range/fr"), - ("/range/rl", "range/rl"), - ("/range/rr", "range/rr"), ("/tf", "tf"), ("/tf_static", "tf_static"), ], @@ -137,40 +126,13 @@ def generate_launch_description(): "use_gpu": use_gpu, "simulation_engine": "ignition-gazebo", "namespace": namespace, - "use_multirobot_system": use_multirobot_system, }.items(), ) - # The frame of the pointcloud from ignition gazebo 6 isn't provided by . - # See https://github.com/gazebosim/gz-sensors/issues/239 - depth_cam_frame_fixer = Node( - package="tf2_ros", - executable="static_transform_publisher", - name="depth_to_camera", - output="log", - arguments=[ - "0.0", - "0.0", - "0.0", - "1.57", - "-1.57", - "0.0", - # Here should be namespace - "camera_depth_optical_frame", - "rosbot/base_link/camera_orbbec_astra_camera", - ], - remappings=[ - ("/tf", "tf"), - ("/tf_static", "tf_static"), - ], - namespace=namespace, - ) - return LaunchDescription( [ declare_namespace_arg, declare_mecanum_arg, - declare_use_multirobot_system_arg, declare_use_gpu_arg, # Sets use_sim_time for all nodes started below # (doesn't work for nodes started from ignition gazebo) @@ -178,6 +140,5 @@ def generate_launch_description(): ign_bridge, gz_spawn_entity, bringup_launch, - depth_cam_frame_fixer, ] ) diff --git a/rosbot_gazebo/package.xml b/rosbot_gazebo/package.xml index 0ad22c95..278efd55 100644 --- a/rosbot_gazebo/package.xml +++ b/rosbot_gazebo/package.xml @@ -7,6 +7,7 @@ Gazebo Ignition simulation for ROSbot 2, 2R, PRO Apache License 2.0 + Jakub Delicat Krzysztof Wojciechowski Husarion @@ -26,6 +27,7 @@ for details refer to the ros_gz_sim package --> ros_gz_bridge ign_ros2_control + nav2_common python3-pytest launch diff --git a/rosbot_gazebo/setup.py b/rosbot_gazebo/setup.py index 0d432bd4..26443e75 100644 --- a/rosbot_gazebo/setup.py +++ b/rosbot_gazebo/setup.py @@ -26,6 +26,7 @@ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml"]), (os.path.join("share", package_name, "launch"), glob("launch/*.launch.py")), + (os.path.join("share", package_name, "config"), glob("config/*.yaml")), ], install_requires=["setuptools"], zip_safe=True, diff --git a/rosbot_gazebo/test/test_diff_drive_simulation.py b/rosbot_gazebo/test/test_diff_drive_simulation.py index 135735fa..130b1fec 100644 --- a/rosbot_gazebo/test/test_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_diff_drive_simulation.py @@ -17,19 +17,26 @@ import launch_pytest import pytest import rclpy +import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc -from test_utils import SimulationTestNode +from test_utils import SimulationTestNode, tf_test, diff_test from test_ign_kill_utils import kill_ign_linux_processes @launch_pytest.fixture def generate_test_description(): + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env["PYTHONUNBUFFERED"] = "1" + rosbot_gazebo = get_package_share_directory("rosbot_gazebo") simulation_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -41,10 +48,26 @@ def generate_test_description(): ] ) ), - launch_arguments={"headless": "True", "world": "empty.sdf"}.items(), + launch_arguments={ + "headless": "True", + "world": PathJoinSubstitution( + [ + get_package_share_directory("husarion_office_gz"), + "worlds", + "empty_with_plugins.sdf", + ] + ), + }.items(), ) - return LaunchDescription([simulation_launch]) + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) @pytest.mark.launch(fixture=generate_test_description) @@ -55,37 +78,8 @@ def test_diff_drive_simulation(): node.create_test_subscribers_and_publishers() node.start_node_thread() - flag = node.odom_tf_event.wait(timeout=60.0) - assert ( - flag - ), "Expected odom to base_link tf but it was not received. Check robot_localization!" - - # 0.9 m/s and 3.0 rad/s are controller's limits defined in - # rosbot_controller/config/diff_drive_controller.yaml - node.set_destination_speed(0.9, 0.0, 0.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting the" - f" target speed is: {(node.current_time - node.goal_received_time):.1f}." - ) - assert ( - node.controller_odom_flag - ), "ROSbot does not move properly in x direction. Check rosbot_base_controller!" - assert ( - node.ekf_odom_flag - ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" - - node.set_destination_speed(0.0, 0.0, 3.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting the" - f" target speed is: {(node.current_time - node.goal_received_time):.1f}." - ) - assert ( - node.controller_odom_flag - ), "ROSbot does not rotate properly. Check rosbot_base_controller!" - assert node.ekf_odom_flag, "ROSbot does not rotate properly. Check ekf_filter_node!" - - flag = node.scan_event.wait(timeout=20.0) - assert flag, "ROSbot's lidar does not work properly!" + tf_test(node) + diff_test(node) finally: # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching diff --git a/rosbot_gazebo/test/test_ign_kill_utils.py b/rosbot_gazebo/test/test_ign_kill_utils.py index 7f62f936..c961d22d 100644 --- a/rosbot_gazebo/test/test_ign_kill_utils.py +++ b/rosbot_gazebo/test/test_ign_kill_utils.py @@ -1,4 +1,5 @@ # Copyright 2023 Husarion +# Copyright 2023 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. @@ -12,27 +13,18 @@ # See the License for the specific language governing permissions and # limitations under the License. -import subprocess +import psutil # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching # several tests in a row. +# https://github.com/ros-controls/gz_ros2_control/blob/master/gz_ros2_control_tests/tests/position_test.py def kill_ign_linux_processes(): - try: - result = subprocess.run( - ["pgrep", "-f", "ign gazebo"], - capture_output=True, - text=True, - check=True, - ) - - pids = result.stdout.strip().split("\n") - for pid in pids: - subprocess.run(["kill", pid], check=True) - print("Killed all Ignition Gazebo processes") - except subprocess.CalledProcessError as e: - print(e) + for proc in psutil.process_iter(): + # check whether the process name matches + if proc.name() == "ruby": + proc.kill() kill_ign_linux_processes() diff --git a/rosbot_gazebo/test/test_mecanum_simulation.py b/rosbot_gazebo/test/test_mecanum_simulation.py index ed6a2627..eed30286 100644 --- a/rosbot_gazebo/test/test_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_mecanum_simulation.py @@ -17,19 +17,26 @@ import launch_pytest import pytest import rclpy +import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc -from test_utils import SimulationTestNode +from test_utils import SimulationTestNode, tf_test, mecanum_test from test_ign_kill_utils import kill_ign_linux_processes @launch_pytest.fixture def generate_test_description(): + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env["PYTHONUNBUFFERED"] = "1" + rosbot_gazebo = get_package_share_directory("rosbot_gazebo") simulation_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -41,10 +48,27 @@ def generate_test_description(): ] ) ), - launch_arguments={"mecanum": "True", "headless": "True", "world": "empty.sdf"}.items(), + launch_arguments={ + "mecanum": "True", + "headless": "True", + "world": PathJoinSubstitution( + [ + get_package_share_directory("husarion_office_gz"), + "worlds", + "empty_with_plugins.sdf", + ] + ), + }.items(), ) - return LaunchDescription([simulation_launch]) + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) @pytest.mark.launch(fixture=generate_test_description) @@ -55,49 +79,8 @@ def test_mecanum_simulation(): node.create_test_subscribers_and_publishers() node.start_node_thread() - flag = node.odom_tf_event.wait(timeout=60.0) - assert ( - flag - ), "Expected odom to base_link tf but it was not received. Check robot_localization!" - - # 0.9 m/s and 3.0 rad/s are controller's limits defined in - # rosbot_controller/config/mecanum_drive_controller.yaml - node.set_destination_speed(0.9, 0.0, 0.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting the" - f" target speed is: {(node.current_time - node.goal_received_time):.1f}." - ) - assert ( - node.controller_odom_flag - ), "ROSbot does not move properly in x direction. Check rosbot_base_controller!" - assert ( - node.ekf_odom_flag - ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" - - node.set_destination_speed(0.0, 0.9, 0.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting the" - f" target speed is: {(node.current_time - node.goal_received_time):.1f}." - ) - assert ( - node.controller_odom_flag - ), "ROSbot does not move properly in y direction. Check rosbot_base_controller!" - assert ( - node.ekf_odom_flag - ), "ROSbot does not move properly in y direction. Check ekf_filter_node!" - - node.set_destination_speed(0.0, 0.0, 3.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting the" - f" target speed is: {(node.current_time - node.goal_received_time):.1f}." - ) - assert ( - node.controller_odom_flag - ), "ROSbot does not rotate properly. Check rosbot_base_controller!" - assert node.ekf_odom_flag, "ROSbot does not rotate properly. Check ekf_filter_node!" - - flag = node.scan_event.wait(timeout=20.0) - assert flag, "ROSbot's lidar does not work properly!" + tf_test(node) + mecanum_test(node) finally: # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching diff --git a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py new file mode 100644 index 00000000..6894e8ab --- /dev/null +++ b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py @@ -0,0 +1,96 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch_pytest +import pytest +import rclpy +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc +from rclpy.executors import MultiThreadedExecutor +from threading import Thread + +from test_utils import SimulationTestNode, diff_test + +from test_ign_kill_utils import kill_ign_linux_processes + + +@launch_pytest.fixture +def generate_test_description(): + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env["PYTHONUNBUFFERED"] = "1" + + # IncludeLaunchDescription does not work with robots argument + simulation_launch = ExecuteProcess( + cmd=[ + "ros2", + "launch", + "rosbot_gazebo", + "simulation.launch.py", + ( + f'world:={get_package_share_directory("husarion_office_gz")}' + "/worlds/empty_with_plugins.sdf" + ), + "robots:=robot1={y: -4.0}; robot2={y: 0.0}; robot3={y: 4.0};", + "headless:=True", + ], + output="screen", + ) + + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_multirobot_simulation(): + robot_names = ["robot1", "robot2", "robot3"] + rclpy.init() + try: + nodes = {} + executor = MultiThreadedExecutor(num_threads=len(robot_names)) + + for robot_name in robot_names: + node = SimulationTestNode("test_simulation", namespace=robot_name) + node.create_test_subscribers_and_publishers() + nodes[robot_name] = node + executor.add_node(node) + + ros_spin_thread = Thread(target=lambda executor: executor.spin(), args=(executor,)) + ros_spin_thread.start() + + for robot_name in robot_names: + node = nodes[robot_name] + diff_test(node, robot_name) + + node.destroy_node() + + finally: + rclpy.shutdown() + + # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching + # several tests in a row. + kill_ign_linux_processes() diff --git a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py new file mode 100644 index 00000000..65d76431 --- /dev/null +++ b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py @@ -0,0 +1,97 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch_pytest +import pytest +import rclpy +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc +from rclpy.executors import MultiThreadedExecutor +from threading import Thread + +from test_utils import SimulationTestNode, mecanum_test +from test_ign_kill_utils import kill_ign_linux_processes + + +@launch_pytest.fixture +def generate_test_description(): + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env["PYTHONUNBUFFERED"] = "1" + + # IncludeLaunchDescription does not work with robots argument + simulation_launch = ExecuteProcess( + cmd=[ + "ros2", + "launch", + "rosbot_gazebo", + "simulation.launch.py", + ( + f'world:={get_package_share_directory("husarion_office_gz")}' + "/worlds/empty_with_plugins.sdf" + ), + "robots:=robot1={y: -4.0}; robot2={y: 0.0}; robot3={y: 4.0};", + "headless:=True", + "mecanum:=True", + ], + output="screen", + ) + + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_multirobot_mecanum_simulation(): + robot_names = ["robot1", "robot2", "robot3"] + rclpy.init() + try: + nodes = {} + executor = MultiThreadedExecutor(num_threads=len(robot_names)) + + for robot_name in robot_names: + node = SimulationTestNode("test_simulation", namespace=robot_name) + node.create_test_subscribers_and_publishers() + nodes[robot_name] = node + executor.add_node(node) + + ros_spin_thread = Thread(target=lambda executor: executor.spin(), args=(executor,)) + ros_spin_thread.start() + + # Speed test + for robot_name in robot_names: + node = nodes[robot_name] + mecanum_test(node, robot_name) + + node.destroy_node() + + finally: + rclpy.shutdown() + + # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching + # several tests in a row. + kill_ign_linux_processes() diff --git a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py new file mode 100644 index 00000000..e4a15ff0 --- /dev/null +++ b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py @@ -0,0 +1,89 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch_pytest +import pytest +import rclpy +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc + +from test_utils import SimulationTestNode, tf_test, diff_test +from test_ign_kill_utils import kill_ign_linux_processes + + +@launch_pytest.fixture +def generate_test_description(): + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env["PYTHONUNBUFFERED"] = "1" + + rosbot_gazebo = get_package_share_directory("rosbot_gazebo") + simulation_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_gazebo, + "launch", + "simulation.launch.py", + ] + ) + ), + launch_arguments={ + "headless": "True", + "world": PathJoinSubstitution( + [ + get_package_share_directory("husarion_office_gz"), + "worlds", + "empty_with_plugins.sdf", + ] + ), + "namespace": "rosbot2r", + }.items(), + ) + + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_diff_drive_simulation(): + rclpy.init() + try: + node = SimulationTestNode("test_simulation", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + node.start_node_thread() + + tf_test(node) + diff_test(node) + + finally: + # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching + # several tests in a row. + kill_ign_linux_processes() + rclpy.shutdown() diff --git a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py new file mode 100644 index 00000000..138fb47d --- /dev/null +++ b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py @@ -0,0 +1,90 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch_pytest +import pytest +import rclpy +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc + +from test_utils import SimulationTestNode, tf_test, mecanum_test +from test_ign_kill_utils import kill_ign_linux_processes + + +@launch_pytest.fixture +def generate_test_description(): + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env["PYTHONUNBUFFERED"] = "1" + + rosbot_gazebo = get_package_share_directory("rosbot_gazebo") + simulation_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_gazebo, + "launch", + "simulation.launch.py", + ] + ) + ), + launch_arguments={ + "mecanum": "True", + "world": PathJoinSubstitution( + [ + get_package_share_directory("husarion_office_gz"), + "worlds", + "empty_with_plugins.sdf", + ] + ), + "headless": "True", + "namespace": "rosbot2r", + }.items(), + ) + + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_mecanum_simulation(): + rclpy.init() + try: + node = SimulationTestNode("test_simulation", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + node.start_node_thread() + + tf_test(node) + mecanum_test(node) + + finally: + # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching + # several tests in a row. + kill_ign_linux_processes() + rclpy.shutdown() diff --git a/rosbot_gazebo/test/test_utils.py b/rosbot_gazebo/test/test_utils.py index ed111f40..8457dfc5 100644 --- a/rosbot_gazebo/test/test_utils.py +++ b/rosbot_gazebo/test/test_utils.py @@ -22,11 +22,7 @@ from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry -from sensor_msgs.msg import LaserScan - -from tf2_ros import TransformException -from tf2_ros.buffer import Buffer -from tf2_ros.transform_listener import TransformListener +from sensor_msgs.msg import LaserScan, Image, PointCloud2 class SimulationTestNode(Node): @@ -35,25 +31,33 @@ class SimulationTestNode(Node): # cause the rosbot_base_controller to determine inaccurate odometry. ACCURACY = 0.10 # 10% accuracy - def __init__(self, name="test_node"): - super().__init__(name) + RANGE_SENSORS_TOPICS = ["range/fl", "range/fr", "range/rl", "range/rr"] + RANGE_SENSORS_FRAMES = ["fl_range", "fr_range", "rl_range", "rr_range"] + + def __init__(self, name="test_node", namespace=None): + super().__init__(name, namespace=namespace) # Use simulation time to correct run on slow machine use_sim_time = rclpy.parameter.Parameter("use_sim_time", rclpy.Parameter.Type.BOOL, True) self.set_parameters([use_sim_time]) self.VELOCITY_STABILIZATION_DELAY = 3 + self.current_time = 1e-9 * self.get_clock().now().nanoseconds self.goal_received_time = 1e-9 * self.get_clock().now().nanoseconds self.vel_stabilization_time_event = Event() self.v_x = 0.0 self.v_y = 0.0 self.v_yaw = 0.0 + self.twist = None # Debug info self.controller_odom_flag = False self.ekf_odom_flag = False self.odom_tf_event = Event() self.scan_event = Event() + self.ranges_events = [Event() for _ in range(len(self.RANGE_SENSORS_TOPICS))] + self.camera_color_event = Event() + self.camera_points_event = Event() def clear_odom_flag(self): self.controller_odom_flag = False @@ -71,23 +75,33 @@ def create_test_subscribers_and_publishers(self): self.cmd_vel_publisher = self.create_publisher(Twist, "cmd_vel", 10) self.controller_odom_sub = self.create_subscription( - Odometry, "/rosbot_base_controller/odom", self.controller_callback, 10 + Odometry, "rosbot_base_controller/odom", self.controller_callback, 10 ) self.ekf_odom_sub = self.create_subscription( - Odometry, "/odometry/filtered", self.ekf_callback, 10 + Odometry, "odometry/filtered", self.ekf_callback, 10 ) - self.scan_sub = self.create_subscription(LaserScan, "/scan", self.scan_callback, 10) - self.tf_buffer = Buffer() - self.tf_listener = TransformListener(self.tf_buffer, self) + self.scan_sub = self.create_subscription(LaserScan, "scan", self.scan_callback, 10) - self.timer = None + self.range_subs = [] + 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.camera_color_sub = self.create_subscription( + Image, "camera/image", self.camera_image_callback, 10 + ) + + self.camera_points_sub = self.create_subscription( + PointCloud2, "camera/points", self.camera_points_callback, 10 + ) + + self.timer = self.create_timer(1.0 / 10.0, self.timer_callback) def start_node_thread(self): self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,)) self.ros_spin_thread.start() - self.timer = self.create_timer(1.0 / 10.0, self.timer_callback) def is_twist_ok(self, twist: Twist): def are_close_to_each_other(current_value, dest_value, tolerance=self.ACCURACY, eps=0.01): @@ -101,30 +115,41 @@ def are_close_to_each_other(current_value, dest_value, tolerance=self.ACCURACY, return x_ok and y_ok and yaw_ok def controller_callback(self, data: Odometry): + self.get_logger().debug(f"Received twist from controller: {data.twist.twist}") self.controller_odom_flag = self.is_twist_ok(data.twist.twist) + self.twist = data.twist.twist def ekf_callback(self, data: Odometry): - self.ekf_odom_flag = self.is_twist_ok(data.twist.twist) + self.get_logger().debug(f"Received twist filtered: {data.twist.twist}") - def lookup_transform_odom(self): - try: - self.tf_buffer.lookup_transform("odom", "base_link", rclpy.time.Time()) - self.odom_tf_event.set() - except TransformException as ex: - self.get_logger().error(f"Could not transform odom to base_link: {ex}") + self.odom_tf_event.set() + self.ekf_odom_flag = self.is_twist_ok(data.twist.twist) def timer_callback(self): self.publish_cmd_vel_messages() - self.lookup_transform_odom() self.current_time = 1e-9 * self.get_clock().now().nanoseconds if self.current_time > self.goal_received_time + self.VELOCITY_STABILIZATION_DELAY: 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: 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: + self.ranges_events[index].set() + + def camera_image_callback(self, data: Image): + if data.data: + self.camera_color_event.set() + + def camera_points_callback(self, data: PointCloud2): + if data.data: + self.camera_points_event.set() + def publish_cmd_vel_messages(self): twist_msg = Twist() @@ -132,4 +157,99 @@ def publish_cmd_vel_messages(self): twist_msg.linear.y = self.v_y twist_msg.angular.z = self.v_yaw + self.get_logger().debug(f"Publishing twist: {twist_msg}") self.cmd_vel_publisher.publish(twist_msg) + + +def x_speed_test(node, v_x=0.0, v_y=0.0, v_yaw=0.0, robot_name="ROSbot"): + node.set_destination_speed(v_x, v_y, v_yaw) + + assert node.vel_stabilization_time_event.wait(timeout=120.0), ( + f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" + " since setting the target speed is:" + f" {(node.current_time - node.goal_received_time):.1f}." + ) + assert node.controller_odom_flag, ( + f"{robot_name}: does not move properly in x direction. Check" + f" rosbot_base_controller! Twist: {node.twist}" + ) + assert node.ekf_odom_flag, ( + f"{robot_name}: does not move properly in x direction. Check ekf_filter_node!" + f" Twist: {node.twist}" + ) + + +def y_speed_test(node, v_x=0.0, v_y=0.0, v_yaw=0.0, robot_name="ROSbot"): + node.set_destination_speed(v_x, v_y, v_yaw) + + assert node.vel_stabilization_time_event.wait(timeout=120.0), ( + f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" + " since setting the target speed is:" + f" {(node.current_time - node.goal_received_time):.1f}." + ) + assert node.controller_odom_flag, ( + f"{robot_name} does not move properly in y direction. Check" + f" rosbot_base_controller! Twist: {node.twist}" + ) + assert node.ekf_odom_flag, ( + f"{robot_name} does not move properly in y direction. Check ekf_filter_node!" + f" Twist: {node.twist}" + ) + + +def yaw_speed_test(node, v_x=0.0, v_y=0.0, v_yaw=0.0, robot_name="ROSbot"): + node.set_destination_speed(v_x, v_y, v_yaw) + + assert node.vel_stabilization_time_event.wait(timeout=120.0), ( + f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" + " since setting the target speed is:" + f" {(node.current_time - node.goal_received_time):.1f}." + ) + assert ( + node.controller_odom_flag + ), f"{robot_name} does not rotate properly. Check rosbot_base_controller! Twist: {node.twist}" + assert ( + node.ekf_odom_flag + ), f"{robot_name} does not rotate properly. Check ekf_filter_node! Twist: {node.twist}" + + +def sensors_readings_test(node, robot_name="ROSbot"): + flag = node.scan_event.wait(timeout=20.0) + assert flag, f"{robot_name}'s lidar does not work properly!" + + for i in range(len(node.RANGE_SENSORS_TOPICS)): + flag = node.ranges_events[i].wait(timeout=20.0) + assert ( + flag + ), f"{robot_name}'s range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!" + + flag = node.camera_color_event.wait(timeout=20.0) + assert flag, f"{robot_name}'s camera color image does not work properly!" + + flag = node.camera_points_event.wait(timeout=20.0) + assert flag, f"{robot_name}'s camera point cloud does not work properly!" + + +def tf_test(node, robot_name="ROSbot"): + flag = node.odom_tf_event.wait(timeout=20.0) + assert flag, ( + f"{robot_name}: expected odom to base_link tf but it was not received. Check" + " robot_localization!" + ) + + +def diff_test(node, robot_name="ROSbot"): + sensors_readings_test(node, robot_name) + # 0.9 m/s and 3.0 rad/s are controller's limits defined in + # rosbot_controller/config/mecanum_drive_controller.yaml + x_speed_test(node, v_x=0.9, robot_name=robot_name) + yaw_speed_test(node, v_yaw=3.0, robot_name=robot_name) + + +def mecanum_test(node, robot_name="ROSbot"): + sensors_readings_test(node, robot_name) + # 0.9 m/s and 3.0 rad/s are controller's limits defined in + # rosbot_controller/config/mecanum_drive_controller.yaml + x_speed_test(node, v_x=0.9, robot_name=robot_name) + y_speed_test(node, v_y=0.9, robot_name=robot_name) + yaw_speed_test(node, v_yaw=3.0, robot_name=robot_name)