设置机器人模拟（基础）

目标： 设置机器人模拟并从 ROS 2 控制它。

教程级别： 高级

时间： 30 分钟

背景

在本教程中，您将使用 Webots 机器人模拟器来设置和运行一个非常简单的 ROS 2 模拟场景。

webots_ros2 包提供了 ROS 2 和 Webots 之间的接口。

它包含几个子包，但在本教程中，您将仅使用``webots_ros2_driver`` 子包来实现控制模拟机器人的 Python 或 C++ 插件。

其他一些子包包含不同机器人的演示，例如 TurtleBot3。

它们记录在`Webots ROS 2 示例 <https://github.com/cyberbotics/webots_ros2/wiki/Examples>`_ 页面中。

先决条件

建议了解初学者 教程 中涵盖的基本 ROS 原理。 特别是，使用 turtlesim, ros2, 和 rqt、理解主题、创建工作区、创建包 和 创建启动文件 都是有用的先决条件。

LinuxWindowsmacOS

The Linux and ROS commands of this tutorial can be run in a standard Linux terminal. The following page 安装 (Ubuntu) explains how to install the webots_ros2 package on Linux.

本教程与“webots_ros2”和 Webots R2023b 的 2023.1.0 版本以及即将推出的版本兼容。

任务

1 创建包结构

让我们在自定义 ROS 2 包中组织代码。 从 ROS 2 工作区的“src”文件夹中创建一个名为“my_package”的新包。 将终端的当前目录更改为“ros2_ws/src”并运行：

PythonC++

ros2 pkg create --build-type ament_python --license Apache-2.0 --node-name my_robot_driver my_package --dependencies rclpy geometry_msgs webots_ros2_driver

The --node-name my_robot_driver option will create a my_robot_driver.py template Python plugin in the my_package subfolder that you will modify later. The --dependencies rclpy geometry_msgs webots_ros2_driver option specifies the packages needed by the my_robot_driver.py plugin in the package.xml file.

Let’s add a launch and a worlds folder inside the my_package folder.

cd my_package
mkdir launch
mkdir worlds

You should end up with the following folder structure:

src/
└── my_package/
    ├── launch/
    ├── my_package/
    │   ├── __init__.py
    │   └── my_robot_driver.py
    ├── resource/
    │   └── my_package
    ├── test/
    │   ├── test_copyright.py
    │   ├── test_flake8.py
    │   └── test_pep257.py
    ├── worlds/
    ├── package.xml
    ├── setup.cfg
    └── setup.py

2 设置模拟世界

您需要一个包含机器人的世界文件来启动模拟。 下载此世界文件 并将其移动到 my_package/worlds/ 内。

这实际上是一个相当简单的文本文件，您可以在文本编辑器中查看。

此 my_world.wbt 世界文件中已经包含一个简单的机器人。

如果您想了解如何在 Webots 中创建自己的机器人模型，您可以查看此 教程。

3 编辑 my_robot_driver 插件

webots_ros2_driver 子包会自动为大多数传感器创建 ROS 2 接口。 有关现有设备接口及其配置方法的更多详细信息，请参阅本教程的第二部分：Setting up a robot simulation (Advanced)。 在此任务中，您将通过创建自己的自定义插件来扩展此接口。 此自定义插件是相当于机器人控制器的 ROS 节点。 您可以使用它来访问 Webots 机器人 API 并创建自己的主题和服务来控制您的机器人。

Note

The purpose of this tutorial is to show a basic example with a minimum number of dependencies. However, you could avoid the use of this plugin by using another webots_ros2 sub-package named webots_ros2_control, introducing a new dependency. This other sub-package creates an interface with the ros2_control package that facilitates the control of a differential wheeled robot.

PythonC++

Open my_package/my_package/my_robot_driver.py in your favorite editor and replace its contents with the following:

import rclpy
from geometry_msgs.msg import Twist

HALF_DISTANCE_BETWEEN_WHEELS = 0.045
WHEEL_RADIUS = 0.025

class MyRobotDriver:
    def init(self, webots_node, properties):
        self.__robot = webots_node.robot

        self.__left_motor = self.__robot.getDevice('left wheel motor')
        self.__right_motor = self.__robot.getDevice('right wheel motor')

        self.__left_motor.setPosition(float('inf'))
        self.__left_motor.setVelocity(0)

        self.__right_motor.setPosition(float('inf'))
        self.__right_motor.setVelocity(0)

        self.__target_twist = Twist()

        rclpy.init(args=None)
        self.__node = rclpy.create_node('my_robot_driver')
        self.__node.create_subscription(Twist, 'cmd_vel', self.__cmd_vel_callback, 1)

    def __cmd_vel_callback(self, twist):
        self.__target_twist = twist

    def step(self):
        rclpy.spin_once(self.__node, timeout_sec=0)

        forward_speed = self.__target_twist.linear.x
        angular_speed = self.__target_twist.angular.z

        command_motor_left = (forward_speed - angular_speed * HALF_DISTANCE_BETWEEN_WHEELS) / WHEEL_RADIUS
        command_motor_right = (forward_speed + angular_speed * HALF_DISTANCE_BETWEEN_WHEELS) / WHEEL_RADIUS

        self.__left_motor.setVelocity(command_motor_left)
        self.__right_motor.setVelocity(command_motor_right)

As you can see, the MyRobotDriver class implements three methods.

The first method, named init(self, ...), is actually the ROS node counterpart of the Python __init__(self, ...) constructor. The init method always takes two arguments:

• The webots_node argument contains a reference on the Webots instance.

• The properties argument is a dictionary created from the XML tags given in the URDF files (4 创建 my_robot.urdf 文件) and allows you to pass parameters to the controller.

The robot instance from the simulation self.__robot can be used to access the Webots robot API. Then, it gets the two motor instances and initializes them with a target position and a target velocity. Finally a ROS node is created and a callback method is registered for a ROS topic named /cmd_vel that will handle Twist messages.

def init(self, webots_node, properties):
    self.__robot = webots_node.robot

    self.__left_motor = self.__robot.getDevice('left wheel motor')
    self.__right_motor = self.__robot.getDevice('right wheel motor')

    self.__left_motor.setPosition(float('inf'))
    self.__left_motor.setVelocity(0)

    self.__right_motor.setPosition(float('inf'))
    self.__right_motor.setVelocity(0)

    self.__target_twist = Twist()

    rclpy.init(args=None)
    self.__node = rclpy.create_node('my_robot_driver')
    self.__node.create_subscription(Twist, 'cmd_vel', self.__cmd_vel_callback, 1)

Then comes the implementation of the __cmd_vel_callback(self, twist) callback private method that will be called for each Twist message received on the /cmd_vel topic and will save it in the self.__target_twist member variable.

def __cmd_vel_callback(self, twist):
    self.__target_twist = twist

Finally, the step(self) method is called at every time step of the simulation. The call to rclpy.spin_once() is needed to keep the ROS node running smoothly. At each time step, the method will retrieve the desired forward_speed and angular_speed from self.__target_twist. As the motors are controlled with angular velocities, the method will then convert the forward_speed and angular_speed into individual commands for each wheel. This conversion depends on the structure of the robot, more specifically on the radius of the wheel and the distance between them.

def step(self):
    rclpy.spin_once(self.__node, timeout_sec=0)

    forward_speed = self.__target_twist.linear.x
    angular_speed = self.__target_twist.angular.z

    command_motor_left = (forward_speed - angular_speed * HALF_DISTANCE_BETWEEN_WHEELS) / WHEEL_RADIUS
    command_motor_right = (forward_speed + angular_speed * HALF_DISTANCE_BETWEEN_WHEELS) / WHEEL_RADIUS

    self.__left_motor.setVelocity(command_motor_left)
    self.__right_motor.setVelocity(command_motor_right)

4 创建 my_robot.urdf 文件

现在您必须创建一个 URDF 文件来声明 MyRobotDriver 插件。

这将允许 webots_ros2_driver ROS 节点启动插件并将其连接到目标机器人。

在 my_package/resource 文件夹中创建一个名为 my_robot.urdf 的文本文件，内容如下：

PythonC++

<?xml version="1.0" ?>
<robot name="My robot">
    <webots>
        <plugin type="my_package.my_robot_driver.MyRobotDriver" />
    </webots>
</robot>

The type attribute specifies the path to the class given by the hierarchical structure of files. webots_ros2_driver is responsible for loading the class based on the specified package and modules.

Note

This simple URDF file doesn’t contain any link or joint information about the robot as it is not needed in this tutorial. However, URDF files usually contain much more information as explained in the URDF tutorial.

Note

Here the plugin does not take any input parameter, but this can be achieved with a tag containing the parameter name.

PythonC++

<plugin type="my_package.my_robot_driver.MyRobotDriver">
    <parameterName>someValue</parameterName>
</plugin>

This is namely used to pass parameters to existing Webots device plugins (see Setting up a robot simulation (Advanced)).

5 创建启动文件

让我们创建启动文件，以便使用单个命令轻松启动模拟和 ROS 控制器。 在 my_package/launch 文件夹中创建一个名为 robot_launch.py​​ 的新文本文件，其中包含以下代码：

import os
import launch
from launch import LaunchDescription
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


def generate_launch_description():
    package_dir = get_package_share_directory('my_package')
    robot_description_path = os.path.join(package_dir, 'resource', 'my_robot.urdf')

    webots = WebotsLauncher(
        world=os.path.join(package_dir, 'worlds', 'my_world.wbt')
    )

    my_robot_driver = WebotsController(
        robot_name='my_robot',
        parameters=[
            {'robot_description': robot_description_path},
        ]
    )

    return LaunchDescription([
        webots,
        my_robot_driver,
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=webots,
                on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
            )
        )
    ])

“WebotsLauncher”对象是一个自定义操作，允许您启动Webots模拟实例。 您必须在构造函数中指定模拟器将打开哪个世界文件。

webots = WebotsLauncher(
    world=os.path.join(package_dir, 'worlds', 'my_world.wbt')
)

然后，创建与模拟机器人交互的 ROS 节点。 这个名为“WebotsController”的节点位于“webots_ros2_driver”包中。

LinuxWindowsmacOS

The node will be able to communicate with the simulated robot by using a custom protocol based on IPC and shared memory.

在您的案例中，您需要运行此节点的单个实例，因为模拟中只有一个机器人。 但是，如果您在模拟中有更多机器人，则必须为每个机器人运行此节点的一个实例。 robot_name 参数用于定义驱动程序应连接到的机器人的名称。 robot_description 参数保存指向 MyRobotDriver 插件的 URDF 文件的路径。 您可以将 WebotsController 节点视为将控制器插件连接到目标机器人的接口。

my_robot_driver = WebotsController(
    robot_name='my_robot',
    parameters=[
        {'robot_description': robot_description_path},
    ]
)

之后在``LaunchDescription``构造函数中设置两个节点进行启动：

return LaunchDescription([
    webots,
    my_robot_driver,

最后，添加一个可选部分，以便在 Webots 终止时（例如，从图形用户界面关闭时）关闭所有节点。

launch.actions.RegisterEventHandler(
    event_handler=launch.event_handlers.OnProcessExit(
        target_action=webots,
        on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
    )
)

Note

有关“WebotsController”和“WebotsLauncher”参数的更多详细信息，请参见 on the nodes reference page.

6 Edit additional files

PythonC++

Before you can start the launch file, you have to modify the setup.py file to include the extra files you added. Open my_package/setup.py and replace its contents with:

from setuptools import setup

package_name = 'my_package'
data_files = []
data_files.append(('share/ament_index/resource_index/packages', ['resource/' + package_name]))
data_files.append(('share/' + package_name + '/launch', ['launch/robot_launch.py']))
data_files.append(('share/' + package_name + '/worlds', ['worlds/my_world.wbt']))
data_files.append(('share/' + package_name + '/resource', ['resource/my_robot.urdf']))
data_files.append(('share/' + package_name, ['package.xml']))

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=data_files,
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='user',
    maintainer_email='user.name@mail.com',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'my_robot_driver = my_package.my_robot_driver:main',
        ],
    },
)

This sets-up the package and adds in the data_files variable the newly added files: my_world.wbt, my_robot.urdf and robot_launch.py.

7 测试代码

LinuxWindowsmacOS

From a terminal in your ROS 2 workspace run:

colcon build
source install/local_setup.bash
ros2 launch my_package robot_launch.py

This will launch the simulation. Webots will be automatically installed on the first run in case it was not already installed.

Note

如果你想手动安装Webots，你可以下载 here.

然后，打开第二个终端并发送命令：

ros2 topic pub /cmd_vel geometry_msgs/Twist  "linear: { x: 0.1 }"

机器人正在向前移动。

此时，机器人能够盲目地遵循你的电机命令。 但当你命令它向前移动时，它最终会撞到墙上。

关闭 Webots 窗口，这也会关闭从启动器启动的 ROS 节点。 在第二个终端中，使用“Ctrl+C”关闭主题命令。

摘要

在本教程中，您使用 Webots 设置了一个真实的机器人模拟，并实现了一个自定义插件来控制机器人的电机。

后续步骤

为了改进模拟，可以使用机器人的传感器来检测障碍物并避开它们。 本教程的第二部分展示了如何实现这种行为：

• Setting up a robot simulation (Advanced).