Improve TIAGo for better compatibility with real robot

webots_ros2/CHANGELOG.rst

@@ -4,8 +4,9 @@ Changelog for package webots_ros2
 
 2023.0.4 (2023-XX-XX)
 ------------------
-* Added support for painted point clouds
+* Added support for painted point clouds.
 * Fixed ability to launch RViz without other tools in e-puck example.
+* Added new TIAGo project to webots_ros2_tiago to run real robot configuration.
 
 2023.0.3 (2023-04-12)
 ------------------

webots_ros2_driver/CHANGELOG.rst

@@ -5,6 +5,7 @@ Changelog for package webots_ros2_driver
 2023.0.4 (2023-XX-XX)
 ------------------
 * Added support for painted point clouds
+* Added parameters to rename Camera and RangeFinder topics.
 
 2023.0.3 (2023-04-12)
 ------------------

webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp

@@ -55,6 +55,9 @@ namespace webots_ros2_driver {
 
     WbDeviceTag mCamera;
 
+    std::string mCameraInfoSuffix;
+    std::string mImageSuffix;
+
     rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr mImagePublisher;
     sensor_msgs::msg::Image mImageMessage;
 

webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2RangeFinder.hpp

@@ -36,6 +36,10 @@ namespace webots_ros2_driver {
 
     WbDeviceTag mRangeFinder;
 
+    std::string mCameraInfoSuffix;
+    std::string mImageSuffix;
+    std::string mPointCloudSuffix;
+
     rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr mImagePublisher;
     sensor_msgs::msg::Image mImageMessage;
     rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr mCameraInfoPublisher;

webots_ros2_driver/src/plugins/static/Ros2Camera.cpp

@@ -23,10 +23,14 @@ namespace webots_ros2_driver {
     mRecognitionIsEnabled = false;
     mCamera = wb_robot_get_device(parameters["name"].c_str());
 
+    mCameraInfoSuffix = parameters.count("cameraInfoSuffix") ? parameters["cameraInfoSuffix"] : "/camera_info";
+    mImageSuffix = parameters.count("imageSuffix") ? parameters["imageSuffix"] : "";
+
     assert(mCamera != 0);
 
     // Image publisher
-    mImagePublisher = mNode->create_publisher<sensor_msgs::msg::Image>(mTopicName, rclcpp::SensorDataQoS().reliable());
+    mImagePublisher =
+      mNode->create_publisher<sensor_msgs::msg::Image>(mTopicName + mImageSuffix, rclcpp::SensorDataQoS().reliable());
     mImageMessage.header.frame_id = mFrameName;
     mImageMessage.height = wb_camera_get_height(mCamera);
     mImageMessage.width = wb_camera_get_width(mCamera);
@@ -37,7 +41,7 @@ namespace webots_ros2_driver {
 
     // CameraInfo publisher
     mCameraInfoPublisher =
-      mNode->create_publisher<sensor_msgs::msg::CameraInfo>(mTopicName + "/camera_info", rclcpp::SensorDataQoS().reliable());
+      mNode->create_publisher<sensor_msgs::msg::CameraInfo>(mTopicName + mCameraInfoSuffix, rclcpp::SensorDataQoS().reliable());
     mCameraInfoMessage.header.stamp = mNode->get_clock()->now();
     mCameraInfoMessage.header.frame_id = mFrameName;
     mCameraInfoMessage.height = wb_camera_get_height(mCamera);

webots_ros2_driver/src/plugins/static/Ros2RangeFinder.cpp

@@ -25,13 +25,18 @@ namespace webots_ros2_driver {
     mIsEnabled = false;
     mRangeFinder = wb_robot_get_device(parameters["name"].c_str());
 
+    mCameraInfoSuffix = parameters.count("cameraInfoSuffix") ? parameters["cameraInfoSuffix"] : "/camera_info";
+    mImageSuffix = parameters.count("imageSuffix") ? parameters["imageSuffix"] : "";
+    mPointCloudSuffix = parameters.count("pointCloudSuffix") ? parameters["pointCloudSuffix"] : "/point_cloud";
+
     assert(mRangeFinder != 0);
 
     const int width = wb_range_finder_get_width(mRangeFinder);
     const int height = wb_range_finder_get_height(mRangeFinder);
 
     // Image publisher
-    mImagePublisher = mNode->create_publisher<sensor_msgs::msg::Image>(mTopicName, rclcpp::SensorDataQoS().reliable());
+    mImagePublisher =
+      mNode->create_publisher<sensor_msgs::msg::Image>(mTopicName + mImageSuffix, rclcpp::SensorDataQoS().reliable());
     mImageMessage.header.frame_id = mFrameName;
     mImageMessage.height = height;
     mImageMessage.width = width;
@@ -42,7 +47,7 @@ namespace webots_ros2_driver {
 
     // CameraInfo publisher
     mCameraInfoPublisher =
-      mNode->create_publisher<sensor_msgs::msg::CameraInfo>(mTopicName + "/camera_info", rclcpp::SensorDataQoS().reliable());
+      mNode->create_publisher<sensor_msgs::msg::CameraInfo>(mTopicName + mCameraInfoSuffix, rclcpp::SensorDataQoS().reliable());
     mCameraInfoMessage.header.stamp = mNode->get_clock()->now();
     mCameraInfoMessage.header.frame_id = mFrameName;
     mCameraInfoMessage.height = height;
@@ -57,8 +62,8 @@ namespace webots_ros2_driver {
                             1.0,          0.0};
 
     // Point cloud publisher
-    mPointCloudPublisher =
-      mNode->create_publisher<sensor_msgs::msg::PointCloud2>(mTopicName + "/point_cloud", rclcpp::SensorDataQoS().reliable());
+    mPointCloudPublisher = mNode->create_publisher<sensor_msgs::msg::PointCloud2>(mTopicName + mPointCloudSuffix,
+                                                                                  rclcpp::SensorDataQoS().reliable());
     mPointCloudMessage.header.frame_id = mFrameName;
     mPointCloudMessage.fields.resize(3);
     mPointCloudMessage.fields[0].name = "x";

webots_ros2_tiago/CHANGELOG.rst

@@ -2,6 +2,10 @@
 Changelog for package webots_ros2_tiago
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2023.0.4 (2023-XX-XX)
+------------------
+* Added new world, resources and launch file to start the TIAGo with real robot configuration.
+
 2023.0.3 (2023-04-12)
 ------------------
 * Fixed the calibration of the TIAGo.

webots_ros2_tiago/launch/robot_bringup_launch.py

@@ -0,0 +1,141 @@
+#!/usr/bin/env python
+
+# Copyright 1996-2023 Cyberbotics Ltd.
+#
+# 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 and the controller."""
+
+import os
+import pathlib
+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 launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.actions import IncludeLaunchDescription
+from webots_ros2_driver.webots_launcher import WebotsLauncher
+from webots_ros2_driver.utils import controller_url_prefix
+
+
+def launch_spawners(event, nodes):
+    # Start ros2_control spawners once the controller_manager is ready
+    if 'Successful \'activate\' of hardware' in event.text.decode().strip():
+        return nodes
+    return
+
+
+def get_ros2_nodes(*args):
+    package_dir = get_package_share_directory('webots_ros2_tiago')
+    use_rviz = LaunchConfiguration('rviz', default=False)
+    robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'tiago_bringup_webots.urdf')).read_text()
+    ros2_control_params = os.path.join(package_dir, 'resource', 'ros2_control_bringup.yml')
+    use_sim_time = LaunchConfiguration('use_sim_time', default=True)
+
+    tiago_driver = Node(
+        package='webots_ros2_driver',
+        executable='driver',
+        output='screen',
+        additional_env={'WEBOTS_CONTROLLER_URL': controller_url_prefix() + 'Tiago'},
+        parameters=[
+            {'robot_description': robot_description,
+             'use_sim_time': use_sim_time},
+            ros2_control_params
+        ]
+    )
+
+    tiago_bringup_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(os.path.join(
+            get_package_share_directory('tiago_bringup'), 'launch', 'tiago_bringup.launch.py')),
+    )
+
+    spawners = []
+    spawners.append(tiago_bringup_launch)
+    spawners_handler = launch.actions.RegisterEventHandler(
+        event_handler=launch.event_handlers.OnProcessIO(
+            target_action=tiago_driver,
+            on_stderr=lambda event: launch_spawners(event, spawners)
+        )
+    )
+
+    # RViz
+    rviz_config = os.path.join(get_package_share_directory('webots_ros2_tiago'), 'resource', 'default_bringup.rviz')
+    rviz = Node(
+        package='rviz2',
+        executable='rviz2',
+        output='screen',
+        arguments=['--display-config=' + rviz_config],
+        parameters=[{'use_sim_time': use_sim_time}],
+        condition=launch.conditions.IfCondition(use_rviz)
+    )
+
+    return [
+        tiago_driver,
+        spawners_handler,
+        rviz,
+    ]
+
+
+def generate_launch_description():
+    package_dir = get_package_share_directory('webots_ros2_tiago')
+    world = LaunchConfiguration('world')
+    mode = LaunchConfiguration('mode')
+
+    webots = WebotsLauncher(
+        world=PathJoinSubstitution([package_dir, 'worlds', world]),
+        mode=mode,
+        ros2_supervisor=True
+    )
+
+    # The following lines are 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='default_bringup.wbt',
+            description='Choose one of the world files from `/webots_ros2_tiago/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_tiago/launch/robot_launch.py

@@ -31,8 +31,14 @@
 from webots_ros2_driver.utils import controller_url_prefix
 
 
+def launch_spawners(event, nodes):
+    # Start ros2_control spawners once the controller_manager is ready
+    if 'Successful \'activate\' of hardware' in event.text.decode().strip():
+        return nodes
+    return
+
+
 def get_ros2_nodes(*args):
-    optional_nodes = []
     package_dir = get_package_share_directory('webots_ros2_tiago')
     use_rviz = LaunchConfiguration('rviz', default=False)
     use_nav = LaunchConfiguration('nav', default=False)
@@ -47,6 +53,25 @@ def get_ros2_nodes(*args):
     cartographer_config_basename = 'cartographer.lua'
     use_sim_time = LaunchConfiguration('use_sim_time', default=True)
 
+    mappings = [('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel')]
+    if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] in ['humble', 'rolling']:
+        mappings.append(('/diffdrive_controller/odom', '/odom'))
+
+    tiago_driver = Node(
+        package='webots_ros2_driver',
+        executable='driver',
+        output='screen',
+        additional_env={'WEBOTS_CONTROLLER_URL': controller_url_prefix() + 'Tiago_Lite'},
+        parameters=[
+            {'robot_description': robot_description,
+             'use_sim_time': use_sim_time,
+             'set_robot_state_publisher': True},
+            ros2_control_params
+        ],
+        remappings=mappings
+    )
+
+    # ROS2_control
     controller_manager_timeout = ['--controller-manager-timeout', '500']
     controller_manager_prefix = 'python.exe' if os.name == 'nt' else ''
 
@@ -68,24 +93,17 @@ def get_ros2_nodes(*args):
         arguments=['joint_state_broadcaster'] + controller_manager_timeout,
     )
 
-    mappings = [('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel')]
-    if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] in ['humble', 'rolling']:
-        mappings.append(('/diffdrive_controller/odom', '/odom'))
-
-    tiago_driver = Node(
-        package='webots_ros2_driver',
-        executable='driver',
-        output='screen',
-        additional_env={'WEBOTS_CONTROLLER_URL': controller_url_prefix() + 'Tiago_Lite'},
-        parameters=[
-            {'robot_description': robot_description,
-             'use_sim_time': use_sim_time,
-             'set_robot_state_publisher': True},
-            ros2_control_params
-        ],
-        remappings=mappings
+    spawners = []
+    spawners.append(diffdrive_controller_spawner)
+    spawners.append(joint_state_broadcaster_spawner)
+    spawners_handler = launch.actions.RegisterEventHandler(
+        event_handler=launch.event_handlers.OnProcessIO(
+            target_action=tiago_driver,
+            on_stderr=lambda event: launch_spawners(event, spawners)
+        )
     )
 
+    # State publishers
     robot_state_publisher = Node(
         package='robot_state_publisher',
         executable='robot_state_publisher',
@@ -102,6 +120,7 @@ def get_ros2_nodes(*args):
         arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_footprint'],
     )
 
+    # RViz
     rviz_config = os.path.join(get_package_share_directory('webots_ros2_tiago'), 'resource', 'default.rviz')
     rviz = Node(
         package='rviz2',
@@ -113,6 +132,7 @@ def get_ros2_nodes(*args):
     )
 
     # Navigation
+    optional_nodes = []
     if 'nav2_bringup' in get_packages_with_prefixes():
         optional_nodes.append(IncludeLaunchDescription(
             PythonLaunchDescriptionSource(os.path.join(
@@ -170,8 +190,7 @@ def get_ros2_nodes(*args):
     )
 
     return [
-        joint_state_broadcaster_spawner,
-        diffdrive_controller_spawner,
+        spawners_handler,
         nav_handler,
         robot_state_publisher,
         tiago_driver,