diff --git a/.github/workflows/industrial_ci.yaml b/.github/workflows/industrial_ci.yaml index ad5f6cf1..4b4b1b79 100644 --- a/.github/workflows/industrial_ci.yaml +++ b/.github/workflows/industrial_ci.yaml @@ -39,6 +39,9 @@ jobs: - name: Clone installation requirements shell: bash run: python3 -m pip install -U vcstool && vcs import . < ./rosbot/rosbot_hardware.repos && vcs import . < ./rosbot/rosbot_simulation.repos + - name: Copy only diff_drive_controller and imu_sensor_broadcaster, waits for features from ros2-control + shell: bash + run: cp -r ros2_controllers/diff_drive_controller . && cp -r ros2_controllers/imu_sensor_broadcaster . && rm -rf ros2_controllers - uses: ros-industrial/industrial_ci@master env: ROS_DISTRO: ${{matrix.ROS_DISTRO}} diff --git a/.gitignore b/.gitignore index f044df5f..2ef10e57 100644 --- a/.gitignore +++ b/.gitignore @@ -12,6 +12,8 @@ ros_components_description/ rosbot_controllers/ husarion/husarion_office_gz gazebosim/gz_ros2_control +diff_drive_controller +imu_sensor_broadcaster # pyspelling *.dic diff --git a/.wordlist.txt b/.wordlist.txt index 5a21da62..5600e10d 100644 --- a/.wordlist.txt +++ b/.wordlist.txt @@ -120,3 +120,8 @@ Palacios env pids pgrep +namespace +TODO +delipl +microros +namespaces diff --git a/README.md b/README.md index 79fbf259..807439a4 100644 --- a/README.md +++ b/README.md @@ -54,6 +54,9 @@ source /opt/ros/$ROS_DISTRO/setup.bash vcs import src < src/rosbot/rosbot_hardware.repos +# Build only diff_drive_controller and imu_sensor_broadcaster from ros2_controllers +cp -r src/ros2_controllers/diff_drive_controller src && cp -r src/ros2_controllers/imu_sensor_broadcaster src && rm -rf src/ros2_controllers + rm -r src/rosbot_gazebo rosdep init @@ -94,6 +97,9 @@ source /opt/ros/$ROS_DISTRO/setup.bash vcs import src < src/rosbot/rosbot_hardware.repos vcs import src < src/rosbot/rosbot_simulation.repos +# Build only diff_drive_controller and imu_sensor_broadcaster from ros2_controllers +cp -r src/ros2_controllers/diff_drive_controller src && cp -r src/ros2_controllers/imu_sensor_broadcaster src && rm -rf src/ros2_controllers + rosdep init rosdep update --rosdistro $ROS_DISTRO rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y diff --git a/rosbot/rosbot_hardware.repos b/rosbot/rosbot_hardware.repos index 27f4a344..0ffde78d 100644 --- a/rosbot/rosbot_hardware.repos +++ b/rosbot/rosbot_hardware.repos @@ -11,3 +11,10 @@ repositories: type: git url: https://github.com/husarion/rosbot_controllers version: main + # Waiting for backporting to the humble version + # https://github.com/ros-controls/ros2_controllers/pull/726 and + # https://github.com/ros-controls/ros2_controllers/pull/831 + ros2_controllers: + type: git + url: https://github.com/delihus/ros2_controllers + version: humble-backport diff --git a/rosbot/rosbot_simulation.repos b/rosbot/rosbot_simulation.repos index a4014f93..affc3289 100644 --- a/rosbot/rosbot_simulation.repos +++ b/rosbot/rosbot_simulation.repos @@ -2,9 +2,7 @@ repositories: gazebosim/gz_ros2_control: type: git url: https://github.com/ros-controls/gz_ros2_control.git - # on branch humble hardware isn't activated - # recently on master API breaking change was introduced, it is necessary to use commit before this change - version: b296ff2f5c3758b637a70bd496fe6ed875ab03ce + version: humble husarion/husarion_office_gz: type: git diff --git a/rosbot_bringup/config/ekf.yaml b/rosbot_bringup/config/ekf.yaml index 7ca6901d..c2d5a8f9 100644 --- a/rosbot_bringup/config/ekf.yaml +++ b/rosbot_bringup/config/ekf.yaml @@ -1,7 +1,7 @@ # Ref: http://docs.ros.org/en/noetic/api/robot_localization/html/state_estimation_nodes.html ## ekf config file ### -ekf_filter_node: +/**/ekf_filter_node: ros__parameters: frequency: 25.0 sensor_timeout: 0.05 @@ -17,7 +17,7 @@ ekf_filter_node: publish_tf: true publish_acceleration: false - odom0: /rosbot_base_controller/odom + odom0: rosbot_base_controller/odom odom0_config: [false, false, false, # X , Y , Z false, false, false, # roll , pitch ,yaw true, true, false, # dX , dY , dZ @@ -29,7 +29,7 @@ ekf_filter_node: odom0_differential: false odom0_relative: true - imu0: /imu_broadcaster/imu + imu0: imu_broadcaster/imu imu0_config: [false, false, false, # X , Y , Z false, false, true, # roll , pitch ,yaw false, false, false, # dX , dY , dZ diff --git a/rosbot_bringup/launch/bringup.launch.py b/rosbot_bringup/launch/bringup.launch.py index 30b85f65..4225276b 100644 --- a/rosbot_bringup/launch/bringup.launch.py +++ b/rosbot_bringup/launch/bringup.launch.py @@ -21,6 +21,13 @@ def generate_launch_description(): + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value="", + description="Namespace for all topics and tfs", + ) + use_sim = LaunchConfiguration("use_sim") declare_use_sim_arg = DeclareLaunchArgument( "use_sim", @@ -55,6 +62,13 @@ 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( @@ -70,6 +84,8 @@ def generate_launch_description(): "mecanum": mecanum, "use_gpu": use_gpu, "simulation_engine": simulation_engine, + "namespace": namespace, + "use_multirobot_system": use_multirobot_system, }.items(), ) @@ -81,13 +97,20 @@ def generate_launch_description(): name="ekf_filter_node", output="screen", parameters=[ekf_config], + remappings=[ + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ], + namespace=namespace, ) actions = [ + declare_namespace_arg, declare_mecanum_arg, declare_use_sim_arg, declare_use_gpu_arg, declare_simulation_engine_arg, + declare_use_multirobot_system_arg, SetParameter(name="use_sim_time", value=use_sim), controller_launch, robot_localization_node, diff --git a/rosbot_bringup/package.xml b/rosbot_bringup/package.xml index b3f84d63..b33998ac 100644 --- a/rosbot_bringup/package.xml +++ b/rosbot_bringup/package.xml @@ -25,6 +25,7 @@ robot_localization rosbot_controller + nav_msgs ament_python diff --git a/rosbot_bringup/test/test_diff_drive_ekf.py b/rosbot_bringup/test/test_diff_drive_ekf.py index c9b1bea8..5da7a3b1 100644 --- a/rosbot_bringup/test/test_diff_drive_ekf.py +++ b/rosbot_bringup/test/test_diff_drive_ekf.py @@ -58,10 +58,10 @@ def test_bringup_startup_success(): node.start_publishing_fake_hardware() node.start_node_thread() - msgs_received_flag = node.odom_tf_event.wait(timeout=10.0) + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) assert ( msgs_received_flag - ), "Expected odom to base_link tf but it was not received. Check robot_localization!" + ), "Expected odometry/filtered message but it was not received. Check robot_localization!" finally: rclpy.shutdown() diff --git a/rosbot_bringup/test/test_mecanum_ekf.py b/rosbot_bringup/test/test_mecanum_ekf.py index d46b29cc..fcdfc633 100644 --- a/rosbot_bringup/test/test_mecanum_ekf.py +++ b/rosbot_bringup/test/test_mecanum_ekf.py @@ -58,10 +58,10 @@ def test_bringup_startup_success(): node.start_publishing_fake_hardware() node.start_node_thread() - msgs_received_flag = node.odom_tf_event.wait(timeout=10.0) + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) assert ( msgs_received_flag - ), "Expected odom to base_link tf but it was not received. Check robot_localization!" + ), "Expected odometry/filtered message but it was not received. Check robot_localization!" finally: rclpy.shutdown() diff --git a/rosbot_bringup/test/test_multirobot_ekf.py b/rosbot_bringup/test/test_multirobot_ekf.py new file mode 100644 index 00000000..ca3d5685 --- /dev/null +++ b/rosbot_bringup/test/test_multirobot_ekf.py @@ -0,0 +1,79 @@ +# 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 + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, TimerAction +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import BringupTestNode + +robot_names = ["rosbot1", "rosbot2", "rosbot3", "rosbot4"] + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_bringup = get_package_share_directory("rosbot_bringup") + actions = [] + for i in range(len(robot_names)): + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_bringup, + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "False", + "use_gpu": "False", + "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) + + return LaunchDescription(actions) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_multirobot_bringup_startup_success(): + for robot_name in robot_names: + rclpy.init() + try: + node = BringupTestNode("test_bringup", namespace=robot_name) + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.odom_msg_event.wait(timeout=20.0) + assert msgs_received_flag, ( + f"Expected {robot_name}/odometry/filtered message but it was not received. " + "Check robot_localization!" + ) + + finally: + rclpy.shutdown() diff --git a/rosbot_bringup/test/test_namespaced_diff_drive_ekf.py b/rosbot_bringup/test/test_namespaced_diff_drive_ekf.py new file mode 100644 index 00000000..f78629a7 --- /dev/null +++ b/rosbot_bringup/test/test_namespaced_diff_drive_ekf.py @@ -0,0 +1,68 @@ +# 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 + +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 test_utils import BringupTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_bringup = get_package_share_directory("rosbot_bringup") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_bringup, + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "False", + "use_gpu": "False", + "namespace": "rosbot2r", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_bringup_startup_success(): + rclpy.init() + try: + node = BringupTestNode("test_bringup", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected odometry/filtered message but it was not received. Check robot_localization!" + + finally: + rclpy.shutdown() diff --git a/rosbot_bringup/test/test_namespaced_mecanum_ekf.py b/rosbot_bringup/test/test_namespaced_mecanum_ekf.py new file mode 100644 index 00000000..a49b8faf --- /dev/null +++ b/rosbot_bringup/test/test_namespaced_mecanum_ekf.py @@ -0,0 +1,68 @@ +# 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 + +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 test_utils import BringupTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_bringup = get_package_share_directory("rosbot_bringup") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_bringup, + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "True", + "use_gpu": "False", + "namespace": "rosbot2r", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_bringup_startup_success(): + rclpy.init() + try: + node = BringupTestNode("test_bringup", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected odometry/filtered message but it was not received. Check robot_localization!" + + finally: + rclpy.shutdown() diff --git a/rosbot_bringup/test/test_utils.py b/rosbot_bringup/test/test_utils.py index 36ff36f4..cc483be1 100644 --- a/rosbot_bringup/test/test_utils.py +++ b/rosbot_bringup/test/test_utils.py @@ -21,9 +21,7 @@ from rclpy.node import Node from sensor_msgs.msg import JointState, Imu -from tf2_ros import TransformException -from tf2_ros.buffer import Buffer -from tf2_ros.transform_listener import TransformListener +from nav_msgs.msg import Odometry class BringupTestNode(Node): @@ -31,27 +29,20 @@ class BringupTestNode(Node): __test__ = False - def __init__(self, name="test_node"): - super().__init__(name) - self.odom_tf_event = Event() + def __init__(self, name="test_node", namespace=None): + super().__init__(name, namespace=namespace) + self.odom_msg_event = Event() def create_test_subscribers_and_publishers(self): - self.imu_publisher = self.create_publisher(Imu, "_imu/data_raw", 10) + self.imu_publisher = self.create_publisher(Imu, "/_imu/data_raw", 10) - self.joint_states_publisher = self.create_publisher(JointState, "_motors_response", 10) - - self.tf_buffer = Buffer() - self.tf_listener = TransformListener(self.tf_buffer, self) + self.joint_states_publisher = self.create_publisher(JointState, "/_motors_response", 10) + self.odom_sub = self.create_subscription( + Odometry, "odometry/filtered", self.odometry_callback, 10 + ) self.timer = None - 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}") - def start_node_thread(self): self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,)) self.ros_spin_thread.start() @@ -64,7 +55,9 @@ def start_publishing_fake_hardware(self): def timer_callback(self): self.publish_fake_hardware_messages() - self.lookup_transform_odom() + + def odometry_callback(self, data): + self.odom_msg_event.set() def publish_fake_hardware_messages(self): imu_msg = Imu() diff --git a/rosbot_controller/config/diff_drive_controller.yaml b/rosbot_controller/config/diff_drive_controller.yaml index f69481cd..1755236d 100644 --- a/rosbot_controller/config/diff_drive_controller.yaml +++ b/rosbot_controller/config/diff_drive_controller.yaml @@ -1,12 +1,6 @@ -simulation_ignition_ros_control: +/**/controller_manager: ros__parameters: - use_sim_time: true - -# Separate controller manager used for simulation - only difference is -# the use_sim_time parameter (it is the easiest way to do it with ign ros2 control) -simulation_controller_manager: - ros__parameters: - use_sim_time: true + use_sim_time: False update_rate: 20 # Hz joint_state_broadcaster: @@ -16,29 +10,20 @@ simulation_controller_manager: imu_broadcaster: type: imu_sensor_broadcaster/IMUSensorBroadcaster - -controller_manager: +/**/imu_broadcaster: ros__parameters: - use_sim_time: false - update_rate: 20 # Hz + tf_frame_prefix_enable: false - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - rosbot_base_controller: - type: diff_drive_controller/DiffDriveController - imu_broadcaster: - type: imu_sensor_broadcaster/IMUSensorBroadcaster - -imu_broadcaster: - ros__parameters: sensor_name: imu frame_id: imu_link static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet 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 -rosbot_base_controller: +/**/rosbot_base_controller: ros__parameters: + tf_frame_prefix_enable: false + left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"] right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"] diff --git a/rosbot_controller/config/mecanum_drive_controller.yaml b/rosbot_controller/config/mecanum_drive_controller.yaml index 56c56c94..9eec0f0e 100644 --- a/rosbot_controller/config/mecanum_drive_controller.yaml +++ b/rosbot_controller/config/mecanum_drive_controller.yaml @@ -1,4 +1,4 @@ -controller_manager: +/**/controller_manager: ros__parameters: use_sim_time: false update_rate: 20 # Hz @@ -10,30 +10,20 @@ controller_manager: imu_broadcaster: type: imu_sensor_broadcaster/IMUSensorBroadcaster -# Separate controller manager used for simulation - only difference is -# the use_sim_time parameter (it is the easiest way to do it with ign ros2 control) -simulation_controller_manager: +/**/imu_broadcaster: ros__parameters: - use_sim_time: true - 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 + tf_frame_prefix_enable: false -imu_broadcaster: - ros__parameters: - sensor_name: "imu" - frame_id: "imu_link" + sensor_name: imu + frame_id: imu_link static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet 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 -rosbot_base_controller: +/**/rosbot_base_controller: ros__parameters: + tf_frame_prefix_enable: false + front_left_wheel_name: fl_wheel_joint front_right_wheel_name: fr_wheel_joint rear_left_wheel_name: rl_wheel_joint diff --git a/rosbot_controller/config/multi_diff_drive_controller.yaml b/rosbot_controller/config/multi_diff_drive_controller.yaml new file mode 100644 index 00000000..9b0e3a6a --- /dev/null +++ b/rosbot_controller/config/multi_diff_drive_controller.yaml @@ -0,0 +1,180 @@ +# 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 new file mode 100644 index 00000000..f352830e --- /dev/null +++ b/rosbot_controller/config/multi_mecanum_drive_controller.yaml @@ -0,0 +1,191 @@ +/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 f6382f6b..779a3b47 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -30,6 +30,13 @@ def generate_launch_description(): + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value="", + description="Namespace for all topics and tfs", + ) + mecanum = LaunchConfiguration("mecanum") declare_mecanum_arg = DeclareLaunchArgument( "mecanum", @@ -61,6 +68,13 @@ 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 ", @@ -69,12 +83,12 @@ def generate_launch_description(): ] ) - controller_manager_name = PythonExpression( - [ - "'/simulation_controller_manager' if ", - use_sim, - " else '/controller_manager'", - ] + namespace_ext = PythonExpression( + ["''", " if '", namespace, "' == '' ", "else ", "'", namespace, "/'"] + ) + controller_manager_name = LaunchConfiguration( + "controller_manager_name", + default=[namespace_ext, "controller_manager"], ) # Get URDF via xacro @@ -97,6 +111,10 @@ def generate_launch_description(): use_gpu, " simulation_engine:=", simulation_engine, + " namespace:=", + namespace, + " use_multirobot_system:=", + use_multirobot_system, ] ) robot_description = {"robot_description": robot_description_content} @@ -112,20 +130,28 @@ def generate_launch_description(): control_node = Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_description, robot_controllers], + parameters=[ + robot_description, + robot_controllers, + ], remappings=[ - ("/imu_sensor_node/imu", "/_imu/data_raw"), + ("imu_sensor_node/imu", "/_imu/data_raw"), ("~/motors_cmd", "/_motors_cmd"), ("~/motors_response", "/_motors_response"), - ("/rosbot_base_controller/cmd_vel_unstamped", "/cmd_vel"), + ("rosbot_base_controller/cmd_vel_unstamped", "cmd_vel"), + ("/tf", "tf"), + ("/tf_static", "tf_static"), ], condition=UnlessCondition(use_sim), + namespace=namespace, ) robot_state_pub_node = Node( package="robot_state_publisher", executable="robot_state_publisher", parameters=[robot_description], + remappings=[("/tf", "tf"), ("/tf_static", "tf_static")], + namespace=namespace, ) joint_state_broadcaster_spawner = Node( @@ -137,6 +163,8 @@ def generate_launch_description(): controller_manager_name, "--controller-manager-timeout", "120", + "--namespace", + namespace, ], ) @@ -149,6 +177,8 @@ def generate_launch_description(): controller_manager_name, "--controller-manager-timeout", "120", + "--namespace", + namespace, ], ) @@ -169,6 +199,8 @@ def generate_launch_description(): controller_manager_name, "--controller-manager-timeout", "120", + "--namespace", + namespace, ], ) @@ -181,17 +213,19 @@ def generate_launch_description(): ) ) - actions = [ - declare_mecanum_arg, - declare_use_sim_arg, - declare_use_gpu_arg, - declare_simulation_engine_arg, - SetParameter(name="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, - ] - - return LaunchDescription(actions) + return LaunchDescription( + [ + declare_namespace_arg, + declare_mecanum_arg, + 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, + ] + ) diff --git a/rosbot_controller/test/test_multirobot_controllers.py b/rosbot_controller/test/test_multirobot_controllers.py new file mode 100644 index 00000000..e6117a2f --- /dev/null +++ b/rosbot_controller/test/test_multirobot_controllers.py @@ -0,0 +1,87 @@ +# 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 + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, TimerAction +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import ControllersTestNode + +robot_names = ["rosbot1", "rosbot2", "rosbot3", "rosbot4"] + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_controller = get_package_share_directory("rosbot_controller") + actions = [] + for i in range(len(robot_names)): + controller_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_controller, + "launch", + "controller.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "True", + "use_gpu": "False", + "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) + + return LaunchDescription(actions) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_multirobot_controllers_startup_success(): + for robot_name in robot_names: + rclpy.init() + try: + node = ControllersTestNode(f"test_{robot_name}_controllers", namespace=robot_name) + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + 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!" + 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 new file mode 100644 index 00000000..fdab2de6 --- /dev/null +++ b/rosbot_controller/test/test_namespaced_diff_drive_controllers.py @@ -0,0 +1,102 @@ +# 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 + +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 test_utils import ControllersTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_controller = get_package_share_directory("rosbot_controller") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_controller, + "launch", + "controller.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "False", + "use_gpu": "False", + "namespace": "rosbot2r", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_controllers_startup_fail(): + rclpy.init() + try: + node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + + node.start_node_thread() + msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received JointStates message that should not have appeared. Check whether other" + " robots are connected to your network.!" + ) + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received Odom message that should not have appeared. Check whether other robots are" + " connected to your network.!" + ) + msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received Imu message that should not have appeared. Check whether other robots are" + " connected to your network.!" + ) + finally: + rclpy.shutdown() + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_controllers_startup_success(): + rclpy.init() + try: + node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + 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!" + finally: + rclpy.shutdown() diff --git a/rosbot_controller/test/test_namespaced_mecanum_controllers.py b/rosbot_controller/test/test_namespaced_mecanum_controllers.py new file mode 100644 index 00000000..e2321fd0 --- /dev/null +++ b/rosbot_controller/test/test_namespaced_mecanum_controllers.py @@ -0,0 +1,102 @@ +# 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 + +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 test_utils import ControllersTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_controller = get_package_share_directory("rosbot_controller") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_controller, + "launch", + "controller.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "True", + "use_gpu": "False", + "namespace": "rosbot2r", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_controllers_startup_fail(): + rclpy.init() + try: + node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + + node.start_node_thread() + msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received JointStates message that should not have appeared. Check whether other" + " robots are connected to your network.!" + ) + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received Odom message that should not have appeared. Check whether other robots are" + " connected to your network.!" + ) + msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received Imu message that should not have appeared. Check whether other robots are" + " connected to your network.!" + ) + finally: + rclpy.shutdown() + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_controllers_startup_success(): + rclpy.init() + try: + node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + 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!" + finally: + rclpy.shutdown() diff --git a/rosbot_controller/test/test_utils.py b/rosbot_controller/test/test_utils.py index db48c06a..831eadad 100644 --- a/rosbot_controller/test/test_utils.py +++ b/rosbot_controller/test/test_utils.py @@ -29,26 +29,27 @@ class ControllersTestNode(Node): __test__ = False - def __init__(self, name="test_node"): - super().__init__(name) + def __init__(self, name="test_node", namespace=None): + super().__init__(name, namespace=namespace) self.joint_state_msg_event = Event() self.odom_msg_event = Event() self.imu_msg_event = Event() def create_test_subscribers_and_publishers(self): self.joint_state_sub = self.create_subscription( - JointState, "/joint_states", self.joint_states_callback, 10 + JointState, "joint_states", self.joint_states_callback, 10 ) self.odom_sub = self.create_subscription( - Odometry, "/rosbot_base_controller/odom", self.odometry_callback, 10 + Odometry, "rosbot_base_controller/odom", self.odometry_callback, 10 ) - self.imu_sub = self.create_subscription(Imu, "/imu_broadcaster/imu", self.imu_callback, 10) + self.imu_sub = self.create_subscription(Imu, "imu_broadcaster/imu", self.imu_callback, 10) - self.imu_publisher = self.create_publisher(Imu, "_imu/data_raw", 10) + # TODO: @delipl namespaces have not been implemented in microros yet + self.imu_publisher = self.create_publisher(Imu, "/_imu/data_raw", 10) - self.joint_states_publisher = self.create_publisher(JointState, "_motors_response", 10) + self.joint_states_publisher = self.create_publisher(JointState, "/_motors_response", 10) self.timer = None @@ -71,6 +72,7 @@ def start_publishing_fake_hardware(self): self.publish_fake_hardware_messages, ) + # TODO: @delipl namespaces have not been implemented in microros yet def publish_fake_hardware_messages(self): imu_msg = Imu() imu_msg.header.stamp = self.get_clock().now().to_msg() diff --git a/rosbot_controller/test/test_xacro.py b/rosbot_controller/test/test_xacro.py index 17216c75..0e52cfe6 100644 --- a/rosbot_controller/test/test_xacro.py +++ b/rosbot_controller/test/test_xacro.py @@ -23,18 +23,26 @@ def test_rosbot_description_parsing(): use_sim_values = ["true", "false"] use_gpu_values = ["true", "false"] simulation_engine_values = ["ignition-gazebo", "webots"] # 'gazebo-classic' + use_multirobot_system_values = ["true", "false"] all_combinations = list( - itertools.product(mecanum_values, use_sim_values, use_gpu_values, simulation_engine_values) + itertools.product( + mecanum_values, + use_sim_values, + use_gpu_values, + simulation_engine_values, + use_multirobot_system_values, + ) ) for combination in all_combinations: - mecanum, use_sim, use_gpu, simulation_engine = combination + mecanum, use_sim, use_gpu, simulation_engine, use_multirobot_system = combination mappings = { "mecanum": mecanum, "use_sim": use_sim, "use_gpu": use_gpu, "simulation_engine": simulation_engine, + "use_multirobot_system": use_multirobot_system, } rosbot_description = get_package_share_directory("rosbot_description") xacro_path = os.path.join(rosbot_description, "urdf/rosbot.urdf.xacro") diff --git a/rosbot_description/urdf/body.urdf.xacro b/rosbot_description/urdf/body.urdf.xacro index c178f1f8..16ec6563 100644 --- a/rosbot_description/urdf/body.urdf.xacro +++ b/rosbot_description/urdf/body.urdf.xacro @@ -1,7 +1,7 @@ - + diff --git a/rosbot_description/urdf/components/vl53lox.urdf.xacro b/rosbot_description/urdf/components/vl53lox.urdf.xacro index bcbe6ec6..83d60c8b 100644 --- a/rosbot_description/urdf/components/vl53lox.urdf.xacro +++ b/rosbot_description/urdf/components/vl53lox.urdf.xacro @@ -45,6 +45,9 @@ true false + + ogre2 + diff --git a/rosbot_description/urdf/rosbot.urdf.xacro b/rosbot_description/urdf/rosbot.urdf.xacro index d3b27224..1fdaee18 100644 --- a/rosbot_description/urdf/rosbot.urdf.xacro +++ b/rosbot_description/urdf/rosbot.urdf.xacro @@ -5,23 +5,32 @@ + + - - - - + simulation_engine="$(arg simulation_engine)" + namespace="$(arg namespace)" + use_multirobot_system="$(arg use_multirobot_system)" /> - - + + + + + + diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index 4f3d8613..697b8e93 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -1,6 +1,20 @@ - + + + + + + + + + + + + + @@ -14,23 +28,23 @@ - + - - - - + + + + - + rosbot_hardware_interfaces/RosbotImuSensor 120000 500 - + @@ -45,14 +59,14 @@ - + rosbot_hardware_interfaces/RosbotSystem 120000 500 - + rr_wheel_joint, rl_wheel_joint, @@ -71,22 +85,22 @@ - + - + - + - + @@ -94,7 +108,7 @@ - + @@ -115,28 +129,45 @@ + + + + + + + + - $(find rosbot_controller)/config/mecanum_drive_controller.yaml + $(find rosbot_controller)/config/${controller_config_prefix}mecanum_drive_controller.yaml - $(find rosbot_controller)/config/diff_drive_controller.yaml + $(find rosbot_controller)/config/${controller_config_prefix}diff_drive_controller.yaml - simulation + - /rosbot_base_controller/cmd_vel_unstamped:=/cmd_vel + + ${namespace} + + rosbot_base_controller/cmd_vel_unstamped:=cmd_vel + /tf:=tf - + true 25 - imu/data_raw + ${namespace_ext}imu/data_raw false false imu_link imu_link + + + ${namespace} + + diff --git a/rosbot_description/urdf/wheel.urdf.xacro b/rosbot_description/urdf/wheel.urdf.xacro index 4624f78f..770bb2e0 100644 --- a/rosbot_description/urdf/wheel.urdf.xacro +++ b/rosbot_description/urdf/wheel.urdf.xacro @@ -1,7 +1,14 @@ - + + + + + + + + @@ -29,28 +36,28 @@ - + - + - + - + @@ -58,16 +65,16 @@ - + - + - + @@ -90,7 +97,7 @@ - + diff --git a/rosbot_gazebo/launch/multirobot_simulation.launch.py b/rosbot_gazebo/launch/multirobot_simulation.launch.py new file mode 100644 index 00000000..ec846c68 --- /dev/null +++ b/rosbot_gazebo/launch/multirobot_simulation.launch.py @@ -0,0 +1,151 @@ +# 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 786c21a0..be66c2d5 100644 --- a/rosbot_gazebo/launch/simulation.launch.py +++ b/rosbot_gazebo/launch/simulation.launch.py @@ -24,12 +24,19 @@ ) from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import Node, SetParameter +from launch_ros.actions import SetParameter from ament_index_python.packages import get_package_share_directory def generate_launch_description(): + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value="", + description="Namespace for all topics and tfs", + ) + mecanum = LaunchConfiguration("mecanum") declare_mecanum_arg = DeclareLaunchArgument( "mecanum", @@ -39,6 +46,13 @@ 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") @@ -55,9 +69,9 @@ def generate_launch_description(): headless_cfg = PythonExpression( [ - "'--headless-rendering -s -r' if ", + "'--headless-rendering -s -v 4 -r' if ", headless, - " else '-r'", + " else '-r -v 4 '", ] ) gz_args = [headless_cfg, " ", world_cfg] @@ -85,91 +99,36 @@ def generate_launch_description(): }.items(), ) - gz_spawn_entity = Node( - package="ros_gz_sim", - executable="create", - arguments=[ - "-name", - "rosbot", - "-allow_renaming", - "true", - "-topic", - "robot_description", - "-x", - "0", - "-y", - "2.0", - "-z", - "0.2", - ], - output="screen", - ) - - ign_bridge = Node( - 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", - ], - 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"), - ], - output="screen", - ) - - bringup_launch = IncludeLaunchDescription( + spawn_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( [ - get_package_share_directory("rosbot_bringup"), + get_package_share_directory("rosbot_gazebo"), "launch", - "bringup.launch.py", + "spawn.launch.py", ] ) ), 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(), ) - # 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", - "camera_depth_optical_frame", - "rosbot/base_link/camera_orbbec_astra_camera", - ], - ) - return LaunchDescription( [ + declare_namespace_arg, + declare_use_multirobot_system_arg, declare_mecanum_arg, declare_world_arg, declare_headless_arg, @@ -178,9 +137,6 @@ def generate_launch_description(): # (doesn't work for nodes started from ignition gazebo) SetParameter(name="use_sim_time", value=True), gz_sim, - ign_bridge, - gz_spawn_entity, - bringup_launch, - depth_cam_frame_fixer, + spawn_launch, ] ) diff --git a/rosbot_gazebo/launch/spawn.launch.py b/rosbot_gazebo/launch/spawn.launch.py new file mode 100644 index 00000000..fc8baab9 --- /dev/null +++ b/rosbot_gazebo/launch/spawn.launch.py @@ -0,0 +1,183 @@ +# 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, +) +from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, PythonExpression +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from launch_ros.actions import Node, SetParameter + +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value="", + description="Namespace for all topics and tfs", + ) + + mecanum = LaunchConfiguration("mecanum") + declare_mecanum_arg = DeclareLaunchArgument( + "mecanum", + default_value="False", + description=( + "Whether to use mecanum drive controller (otherwise diff drive controller is used)" + ), + ) + + use_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", + default_value="True", + description="Whether GPU acceleration is used", + ) + + robot_name = PythonExpression( + ["'rosbot'", " if '", namespace, "' == '' ", "else ", "'", namespace, "'"] + ) + + gz_spawn_entity = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-name", + robot_name, + "-allow_renaming", + "true", + "-topic", + "robot_description", + "-x", + LaunchConfiguration("x", default="0.00"), + "-y", + LaunchConfiguration("y", default="0.00"), + "-z", + LaunchConfiguration("z", default="0.00"), + "-R", + LaunchConfiguration("roll", default="0.00"), + "-P", + LaunchConfiguration("pitch", default="0.00"), + "-Y", + LaunchConfiguration("yaw", default="0.00"), + ], + output="screen", + namespace=namespace, + ) + + ign_bridge = Node( + 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", + ], + 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"), + ], + output="screen", + namespace=namespace, + ) + + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + get_package_share_directory("rosbot_bringup"), + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "mecanum": mecanum, + "use_sim": "True", + "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) + SetParameter(name="use_sim_time", value=True), + ign_bridge, + gz_spawn_entity, + bringup_launch, + depth_cam_frame_fixer, + ] + )