设置机器人模拟（高级）

目标： 通过添加避障节点扩展机器人模拟。

教程级别： 高级

**时间：**20分钟

背景

在本教程中，您将扩展第一部分教程中创建的软件包：设置机器人仿真（基础）。目标是使用机器人的距离传感器实现一个ROS 2节点，用于避开障碍物。本教程重点介绍使用带有``webots_ros2_driver``接口的机器人设备。

先决条件

这是教程的第一部分的延续：设置机器人仿真（基础）。必须从第一部分开始设置自定义软件包和必要文件。

本教程与 webots_ros2 的版本 2023.1.0 和 Webots R2023b 以及即将发布的版本兼容。

任务

1 更新 my_robot.urdf

如在 设置机器人仿真（基础） 中所述，webots_ros2_driver 包含了直接与 ROS 2 接口的大多数 Webots 设备的插件。可以使用 URDF 文件中的 <device> 标签加载这些插件。reference 属性应与 Webots 设备的 name 参数匹配。所有现有接口及其对应的参数列表可以在 设备参考页面 找到。对于在 URDF 文件中未配置的可用设备，将自动创建接口，并使用 ROS 参数的默认值（例如 update rate、topic name 和 frame name）。

在 my_robot.urdf 中，用以下内容替换整个内容：

<?xml version="1.0" ?>
<robot name="My robot">
    <webots>
        <device reference="ds0" type="DistanceSensor">
            <ros>
                <topicName>/left_sensor</topicName>
                <alwaysOn>true</alwaysOn>
            </ros>
        </device>
        <device reference="ds1" type="DistanceSensor">
            <ros>
                <topicName>/right_sensor</topicName>
                <alwaysOn>true</alwaysOn>
            </ros>
        </device>
        <plugin type="my_package.my_robot_driver.MyRobotDriver" />
    </webots>
</robot>

除了您自定义的插件外，``webots_ros2_driver``将解析引用**DistanceSensor**节点的``<device>``标签，并使用``<ros>``标签中的标准参数来启用传感器并命名其主题。

2 创建一个ROS节点以避免障碍物

机器人将使用一个标准的ROS节点来检测墙壁并发送电机指令以避免碰撞。在``my_package/my_package/``文件夹中创建一个名为``obstacle_avoider.py``的文件，并添加以下代码：

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Range
from geometry_msgs.msg import Twist


MAX_RANGE = 0.15


class ObstacleAvoider(Node):
    def __init__(self):
        super().__init__('obstacle_avoider')

        self.__publisher = self.create_publisher(Twist, 'cmd_vel', 1)

        self.create_subscription(Range, 'left_sensor', self.__left_sensor_callback, 1)
        self.create_subscription(Range, 'right_sensor', self.__right_sensor_callback, 1)

    def __left_sensor_callback(self, message):
        self.__left_sensor_value = message.range

    def __right_sensor_callback(self, message):
        self.__right_sensor_value = message.range

        command_message = Twist()

        command_message.linear.x = 0.1

        if self.__left_sensor_value < 0.9 * MAX_RANGE or self.__right_sensor_value < 0.9 * MAX_RANGE:
            command_message.angular.z = -2.0

        self.__publisher.publish(command_message)


def main(args=None):
    rclpy.init(args=args)
    avoider = ObstacleAvoider()
    rclpy.spin(avoider)
    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    avoider.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

此节点将在此处创建一个用于指令发布的发布者，并订阅传感器主题：

self.__publisher = self.create_publisher(Twist, 'cmd_vel', 1)

self.create_subscription(Range, 'left_sensor', self.__left_sensor_callback, 1)
self.create_subscription(Range, 'right_sensor', self.__right_sensor_callback, 1)

当左侧传感器接收到测量值时，它将被复制到一个成员字段中：

def __left_sensor_callback(self, message):
    self.__left_sensor_value = message.range

最后，当右侧传感器接收到测量值时，将向``/cmd_vel``主题发送一条消息。``command_message``将至少在``linear.x``中注册前进速度，以便在没有检测到障碍物时使机器人移动。如果任何两个传感器都检测到障碍物，``command_message``还将在``angular.z``中注册旋转速度，以使机器人向右转动。

def __right_sensor_callback(self, message):
    self.__right_sensor_value = message.range

    command_message = Twist()

    command_message.linear.x = 0.1

    if self.__left_sensor_value < 0.9 * MAX_RANGE or self.__right_sensor_value < 0.9 * MAX_RANGE:
        command_message.angular.z = -2.0

    self.__publisher.publish(command_message)

3 更新附加文件

您需要修改这另外两个文件以启动您的新节点。

编辑 setup.py，将 'console_scripts' 替换为：

'console_scripts': [
    'my_robot_driver = my_package.my_robot_driver:main',
    'obstacle_avoider = my_package.obstacle_avoider:main'
],

这将为 obstacle_avoider 节点添加一个入口点。

打开 robot_launch.py 文件，并将 def generate_launch_description(): 替换为：

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},
        ]
    )

    obstacle_avoider = Node(
        package='my_package',
        executable='obstacle_avoider',
    )

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

这将创建一个包含在 LaunchDescription 中的 obstacle_avoider 节点。

4. 测试避障代码。

在ROS 2工作空间的终端中启动仿真：

在ROS 2工作空间的终端中运行：

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

您的机器人应该向前移动，在撞到墙壁之前应该顺时针转向。您可以在Webots中按下``Ctrl+F10``，或转到“View”菜单、“Optional Rendering”和“Show DistanceSensor Rays”以显示机器人的距离传感器范围。

../../../../_images/Robot_turning_clockwise.png

总结

在本教程中，您通过基于机器人的距离传感器值发布速度命令的障碍物避让ROS 2节点扩展了基本仿真。

下一步

您可能希望改进插件或创建新节点以更改机器人的行为。您还可以实现重置处理程序，以在从 Webots 界面重置仿真时自动重新启动 ROS 节点：