ROSbot 2R, XL webots_ros2 support

.gitmodules

@@ -1,3 +1,3 @@
 [submodule "webots_ros2_importer/webots_ros2_importer/urdf2webots"]
 	path = webots_ros2_importer/webots_ros2_importer/urdf2webots
-	url = https://github.com/cyberbotics/urdf2webots.git
+	url = https://github.com/cyberbotics/urdf2webots.git

tests/sources/test_clang_format.py

@@ -56,6 +56,7 @@ def test_sources_are_clang_format_compliant(self):
             'webots_ros2_control',
             'webots_ros2_driver',
             'webots_ros2_epuck',
+            'webots_ros2_husarion',
             'webots_ros2_importer',
             'webots_ros2_mavic',
             'webots_ros2_msgs',

tests/sources/test_pep8.py

@@ -31,7 +31,10 @@
 skippedDirectories = [
     '.git',
     'webots_ros2_importer/webots_ros2_importer/urdf2webots/',
-    'webots_ros2_driver/webots/lib'
+    'webots_ros2_driver/webots/lib',
+    'webots_ros2_husarion/rosbot_ros',
+    'webots_ros2_husarion/rosbot_xl_ros',
+    'webots_ros2_husarion/ros_components_description'
 ]
 skippedDirectoriesFull = [os.path.join(ROOT_FOLDER, os.path.normpath(path)) for path in skippedDirectories]
 

webots_ros2_husarion/launch/rosbot_launch.py

@@ -0,0 +1,163 @@
+#!/usr/bin/env python
+
+# Copyright 1996-2023 Cyberbotics Ltd.
+# 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.
+
+"""Launch Webots ROSbot 2R driver."""
+
+import os
+import launch
+from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions.path_join_substitution import PathJoinSubstitution
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+from webots_ros2_driver.webots_launcher import WebotsLauncher
+from webots_ros2_driver.webots_controller import WebotsController
+from webots_ros2_driver.wait_for_controller_connection import WaitForControllerConnection
+
+
+def get_ros2_nodes(*args):
+    package_dir = get_package_share_directory('webots_ros2_husarion')
+    rosbot_webots_xacro_path = os.path.join(package_dir, 'resource', 'rosbot_webots.urdf')
+    ekf_config = os.path.join(package_dir, 'resource', 'ekf.yaml')
+
+    ros2_control_params = os.path.join(package_dir, 'resource', 'rosbot_controllers.yaml')
+    use_sim_time = LaunchConfiguration('use_sim_time', default=True)
+
+    # ROS control spawners
+    controller_manager_timeout = ['--controller-manager-timeout', '50']
+    controller_manager_prefix = 'python.exe' if os.name == 'nt' else ''
+    diff_drive_controller_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        output='screen',
+        prefix=controller_manager_prefix,
+        arguments=['rosbot_base_controller'] + controller_manager_timeout,
+    )
+    joint_state_broadcaster_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        output='screen',
+        prefix=controller_manager_prefix,
+        arguments=['joint_state_broadcaster'] + controller_manager_timeout,
+    )
+    ros_control_spawners = [diff_drive_controller_spawner, joint_state_broadcaster_spawner]
+
+    rosbot_driver = WebotsController(
+        robot_name='rosbot',
+        parameters=[
+            {'robot_description': rosbot_webots_xacro_path,
+             'use_sim_time': use_sim_time,
+             'set_robot_state_publisher': True},
+            ros2_control_params
+        ],
+        remappings=[
+            ('rosbot_base_controller/cmd_vel_unstamped', 'cmd_vel'),
+            ('odom', 'rosbot_base_controller/odom'),
+            ('rosbot/laser', '/scan'),
+            ('rosbot/rl_range', '/range/rl'),
+            ('rosbot/rr_range', '/range/rr'),
+            ('rosbot/fl_range', '/range/fl'),
+            ('rosbot/fr_range', '/range/fr')
+        ]
+    )
+
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[{
+            'robot_description': '<robot name=""><link name=""/></robot>'
+        }],
+    )
+
+    robot_localization_node = Node(
+        package='robot_localization',
+        executable='ekf_node',
+        name='ekf_filter_node',
+        output='screen',
+        parameters=[
+            ekf_config,
+            {'use_sim_time': True},
+            {'odom0': '/' + 'rosbot_base_controller/odom'}
+        ]
+    )
+
+    # Wait for the simulation to be ready to start navigation nodes
+    waiting_nodes = WaitForControllerConnection(
+        target_driver=rosbot_driver,
+        nodes_to_start=ros_control_spawners
+    )
+
+    return [
+        robot_state_publisher,
+        rosbot_driver,
+        waiting_nodes,
+        robot_localization_node
+    ]
+
+
+def generate_launch_description():
+    package_dir = get_package_share_directory('webots_ros2_husarion')
+    world = LaunchConfiguration('world')
+    mode = LaunchConfiguration('mode')
+
+    webots = WebotsLauncher(
+        world=PathJoinSubstitution([package_dir, 'worlds', world]),
+        mode=mode,
+        ros2_supervisor=True
+    )
+
+    # The following line is important!
+    # This event handler respawns the ROS 2 nodes on simulation reset (supervisor process ends).
+    reset_handler = launch.actions.RegisterEventHandler(
+        event_handler=launch.event_handlers.OnProcessExit(
+            target_action=webots._supervisor,
+            on_exit=get_ros2_nodes,
+        )
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'world',
+            default_value='rosbot.wbt',
+            description='Choose one of the world files from `/webots_ros2_turtlebot/world` directory'
+        ),
+        DeclareLaunchArgument(
+            'mode',
+            default_value='realtime',
+            description='Webots startup mode'
+        ),
+        webots,
+        webots._supervisor,
+
+        # This action will kill all nodes once the Webots simulation has exited
+        launch.actions.RegisterEventHandler(
+            event_handler=launch.event_handlers.OnProcessExit(
+                target_action=webots,
+                on_exit=[
+                    launch.actions.UnregisterEventHandler(
+                        event_handler=reset_handler.event_handler
+                    ),
+                    launch.actions.EmitEvent(event=launch.events.Shutdown())
+                ],
+            )
+        ),
+
+        # Add the reset event handler
+        reset_handler
+    ] + get_ros2_nodes())

webots_ros2_husarion/launch/rosbot_xl_launch.py

@@ -0,0 +1,167 @@
+#!/usr/bin/env python
+
+# Copyright 1996-2023 Cyberbotics Ltd.
+# 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.
+
+"""Launch Webots ROSbot XL driver."""
+
+import os
+import launch
+from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions.path_join_substitution import PathJoinSubstitution
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+from webots_ros2_driver.webots_launcher import WebotsLauncher
+from webots_ros2_driver.webots_controller import WebotsController
+from webots_ros2_driver.wait_for_controller_connection import WaitForControllerConnection
+
+
+def get_ros2_nodes(*args):
+    package_dir = get_package_share_directory('webots_ros2_husarion')
+    rosbot_xl_webots_xacro_path = os.path.join(package_dir, 'resource', 'rosbot_webots.urdf')
+    laser_filter_config = os.path.join(package_dir, 'resource', 'laser_filter.yaml')
+    ekf_config = os.path.join(package_dir, 'resource', 'ekf.yaml')
+
+    ros2_control_params = os.path.join(package_dir, 'resource', 'rosbot_xl_controllers.yaml')
+    use_sim_time = LaunchConfiguration('use_sim_time', default=True)
+
+    # ROS control spawners
+    controller_manager_timeout = ['--controller-manager-timeout', '50']
+    controller_manager_prefix = 'python.exe' if os.name == 'nt' else ''
+    diff_drive_controller_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        output='screen',
+        prefix=controller_manager_prefix,
+        arguments=['rosbot_xl_base_controller'] + controller_manager_timeout,
+    )
+    joint_state_broadcaster_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        output='screen',
+        prefix=controller_manager_prefix,
+        arguments=['joint_state_broadcaster'] + controller_manager_timeout,
+    )
+    ros_control_spawners = [diff_drive_controller_spawner, joint_state_broadcaster_spawner]
+
+    rosbot_driver = WebotsController(
+        robot_name='rosbot_xl',
+        parameters=[
+            {'robot_description': rosbot_xl_webots_xacro_path,
+             'use_sim_time': use_sim_time,
+             'set_robot_state_publisher': True},
+            ros2_control_params
+        ],
+        remappings=[
+            ('rosbot_xl_base_controller/cmd_vel_unstamped', 'cmd_vel'),
+            ('odom', 'rosbot_base_xl_controller/odom'),
+            ('rosbot_xl/laser', '/scan')
+        ]
+    )
+
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[{
+            'robot_description': '<robot name=""><link name=""/></robot>'
+        }],
+    )
+
+    robot_localization_node = Node(
+        package='robot_localization',
+        executable='ekf_node',
+        name='ekf_filter_node',
+        output='screen',
+        parameters=[
+            ekf_config,
+            {'use_sim_time': True},
+            {'odom0': '/' + 'rosbot_xl_base_controller/odom'}
+        ]
+    )
+
+    laser_filter_node = Node(
+        package='laser_filters',
+        executable='scan_to_scan_filter_chain',
+        parameters=[laser_filter_config],
+    )
+
+    # Wait for the simulation to be ready to start navigation nodes
+    waiting_nodes = WaitForControllerConnection(
+        target_driver=rosbot_driver,
+        nodes_to_start=ros_control_spawners
+    )
+
+    return [
+        robot_state_publisher,
+        rosbot_driver,
+        waiting_nodes,
+        robot_localization_node,
+        laser_filter_node
+    ]
+
+
+def generate_launch_description():
+    package_dir = get_package_share_directory('webots_ros2_husarion')
+    world = LaunchConfiguration('world')
+    mode = LaunchConfiguration('mode')
+
+    webots = WebotsLauncher(
+        world=PathJoinSubstitution([package_dir, 'worlds', world]),
+        mode=mode,
+        ros2_supervisor=True
+    )
+
+    # The following line is important!
+    # This event handler respawns the ROS 2 nodes on simulation reset (supervisor process ends).
+    reset_handler = launch.actions.RegisterEventHandler(
+        event_handler=launch.event_handlers.OnProcessExit(
+            target_action=webots._supervisor,
+            on_exit=get_ros2_nodes,
+        )
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'world',
+            default_value='rosbot_xl.wbt',
+            description='Choose one of the world files from `/webots_ros2_turtlebot/world` directory'
+        ),
+        DeclareLaunchArgument(
+            'mode',
+            default_value='realtime',
+            description='Webots startup mode'
+        ),
+        webots,
+        webots._supervisor,
+
+        # This action will kill all nodes once the Webots simulation has exited
+        launch.actions.RegisterEventHandler(
+            event_handler=launch.event_handlers.OnProcessExit(
+                target_action=webots,
+                on_exit=[
+                    launch.actions.UnregisterEventHandler(
+                        event_handler=reset_handler.event_handler
+                    ),
+                    launch.actions.EmitEvent(event=launch.events.Shutdown())
+                ],
+            )
+        ),
+
+        # Add the reset event handler
+        reset_handler
+    ] + get_ros2_nodes())

webots_ros2_husarion/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>webots_ros2_husarion</name>
+  <version>2023.0.4</version>
+  <description>Husarion ROSbot 2R and XL robots ROS2 interface for Webots.</description>
+
+  <author email="[email protected]">Jakub Delicat</author>
+  <maintainer email="[email protected]">Husarion</maintainer>
+  <license>Apache License 2.0</license>
+  <url type="website">http://wiki.ros.org/webots_ros2</url>
+  <url type="repository">https://github.com/cyberbotics/webots_ros2</url>
+  <url type="bugtracker">https://github.com/cyberbotics/webots_ros2/issues</url>
+
+  <exec_depend>webots_ros2</exec_depend>
+  <exec_depend>webots_ros2_driver</exec_depend>
+  <exec_depend>laser_filters</exec_depend>
+  <exec_depend>robot_localization</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>