Webots 机器人仿真设置（进阶）

目标： 使用障碍物避障节点扩展机器人仿真。

教程级别： 进阶

预计时间： 20 分钟

背景

在本教程中，你将扩展教程第一部分中创建的包：Webots 机器人仿真设置（基础）。

目标是实现一个使用机器人距离传感器来避开障碍物的 ROS 2 节点。

本教程侧重于使用 webots_ros2_driver 接口的机器人设备。

前提条件

这是教程第一部分的延续：Webots 机器人仿真设置（基础）。

必须先完成第一部分，设置好自定义包和所需文件。

本教程兼容 webots_ros2 版本 2023.1.0 和 Webots R2023b 及后续版本。

任务

1 更新 `my_robot.urdf`

如 Webots 机器人仿真设置（基础）中所述，webots_ros2_driver 包含插件，可将大多数 Webots 设备直接与 ROS 2 连接。

这些插件可以通过机器人 URDF 文件中的 <device> 标签来加载。

reference 属性需要与 Webots 设备的 name 参数匹配。

所有现有接口及其对应参数的列表可以在设备参考页面中找到。

对于 URDF 文件中未配置的可用设备，接口会自动创建，并使用 ROS 参数的默认值（例如 update rate、topic name 和 frame name）。

将 my_robot.urdf 的全部内容替换为：

Python

<?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>

C++

<?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_robot_driver::MyRobotDriver" />
    </webots>
</robot>

除了你的自定义插件外，webots_ros2_driver 还会解析引用 DistanceSensor 节点的 <device> 标签，并根据 <ros> 标签中的标准参数来启用传感器并为其话题命名。

2 创建避障 ROS 节点

Python

机器人将使用一个标准的 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()

此节点会为速度命令创建一个发布者，并订阅以下传感器话题：

    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)

C++

机器人将使用一个标准的 ROS 节点来检测墙壁并发送电机命令以避开障碍。

在 my_package/include/my_package 文件夹中，创建一个名为 ObstacleAvoider.hpp 的头文件，代码如下：

#include <memory>

#include "geometry_msgs/msg/twist.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/range.hpp"

class ObstacleAvoider : public rclcpp::Node {
public:
  explicit ObstacleAvoider();

private:
  void leftSensorCallback(const sensor_msgs::msg::Range::ConstSharedPtr msg);
  void rightSensorCallback(const sensor_msgs::msg::Range::ConstSharedPtr msg);

  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
  rclcpp::Subscription<sensor_msgs::msg::Range>::SharedPtr left_sensor_sub_;
  rclcpp::Subscription<sensor_msgs::msg::Range>::SharedPtr right_sensor_sub_;

  double left_sensor_value{0.0};
  double right_sensor_value{0.0};
};

在 my_package/src 文件夹中，创建一个名为 ObstacleAvoider.cpp 的源文件，代码如下：

#include "my_package/ObstacleAvoider.hpp"

#define MAX_RANGE 0.15

ObstacleAvoider::ObstacleAvoider() : Node("obstacle_avoider") {
  publisher_ = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 1);

  left_sensor_sub_ = create_subscription<sensor_msgs::msg::Range>(
      "/left_sensor", 1,
      [this](const sensor_msgs::msg::Range::ConstSharedPtr msg){
        return this->leftSensorCallback(msg);
      }
  );

  right_sensor_sub_ = create_subscription<sensor_msgs::msg::Range>(
      "/right_sensor", 1,
      [this](const sensor_msgs::msg::Range::ConstSharedPtr msg){
        return this->rightSensorCallback(msg);
      }
  );
}

void ObstacleAvoider::leftSensorCallback(
    const sensor_msgs::msg::Range::ConstSharedPtr msg) {
  left_sensor_value = msg->range;
}

void ObstacleAvoider::rightSensorCallback(
    const sensor_msgs::msg::Range::ConstSharedPtr msg) {
  right_sensor_value = msg->range;

  auto command_message = std::make_unique<geometry_msgs::msg::Twist>();

  command_message->linear.x = 0.1;

  if (left_sensor_value < 0.9 * MAX_RANGE ||
      right_sensor_value < 0.9 * MAX_RANGE) {
    command_message->angular.z = -2.0;
  }

  publisher_->publish(std::move(command_message));
}

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  auto avoider = std::make_shared<ObstacleAvoider>();
  rclcpp::spin(avoider);
  rclcpp::shutdown();
  return 0;
}

此节点会为速度命令创建一个发布者，并订阅以下传感器话题：

ObstacleAvoider::ObstacleAvoider() : Node("obstacle_avoider") {
  publisher_ = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 1);

  left_sensor_sub_ = create_subscription<sensor_msgs::msg::Range>(
      "/left_sensor", 1,
      [this](const sensor_msgs::msg::Range::ConstSharedPtr msg){
        return this->leftSensorCallback(msg);
      }
  );

  right_sensor_sub_ = create_subscription<sensor_msgs::msg::Range>(
      "/right_sensor", 1,
      [this](const sensor_msgs::msg::Range::ConstSharedPtr msg){
        return this->rightSensorCallback(msg);
      }
  );
}

当从左侧传感器接收到测量值时，会将其存入成员变量：

void ObstacleAvoider::leftSensorCallback(
    const sensor_msgs::msg::Range::ConstSharedPtr msg) {
  left_sensor_value = msg->range;
}

最后，当从右侧传感器接收到测量值时，会向 /cmd_vel 话题发送一条消息。

command_message 至少会设置 linear.x 方向的前进速度，使机器人在没有检测到障碍物时保持移动。

如果两个传感器中任意一个检测到障碍物，command_message 还会设置 angular.z 方向的旋转速度，使机器人向右转。

void ObstacleAvoider::rightSensorCallback(
    const sensor_msgs::msg::Range::ConstSharedPtr msg) {
  right_sensor_value = msg->range;

  auto command_message = std::make_unique<geometry_msgs::msg::Twist>();

  command_message->linear.x = 0.1;

  if (left_sensor_value < 0.9 * MAX_RANGE ||
      right_sensor_value < 0.9 * MAX_RANGE) {
    command_message->angular.z = -2.0;
  }

  publisher_->publish(std::move(command_message));
}

3 更新附加文件

接下来需要修改另外两个文件，以便启动新节点。

Python

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

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

这样就会为 obstacle_avoider 节点添加一个入口点。

C++

编辑 CMakeLists.txt，添加 obstacle_avoider 的编译和安装规则：

cmake_minimum_required(VERSION 3.20)
project(my_package)

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

# Besides the package specific dependencies we also need the `pluginlib` and `webots_ros2_driver`
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(pluginlib REQUIRED)
find_package(webots_ros2_driver REQUIRED)

# Export the plugin configuration file
pluginlib_export_plugin_description_file(webots_ros2_driver my_robot_driver.xml)

# Obstacle avoider
include_directories(
  include
)
add_executable(obstacle_avoider
  src/ObstacleAvoider.cpp
)
ament_target_dependencies(obstacle_avoider
  rclcpp
  geometry_msgs
  sensor_msgs
)
install(TARGETS
  obstacle_avoider
  DESTINATION lib/${PROJECT_NAME}
)
install(
  DIRECTORY include/
  DESTINATION include
)

# MyRobotDriver library
add_library(
  ${PROJECT_NAME}
  SHARED
  src/MyRobotDriver.cpp
)
target_include_directories(
  ${PROJECT_NAME}
  PRIVATE
  include
)
ament_target_dependencies(
  ${PROJECT_NAME}
  pluginlib
  rclcpp
  webots_ros2_driver
)
install(TARGETS
  ${PROJECT_NAME}
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
)
# Install additional directories.
install(DIRECTORY
  launch
  resource
  worlds
  DESTINATION share/${PROJECT_NAME}/
)

ament_export_include_directories(
  include
)
ament_export_libraries(
  ${PROJECT_NAME}
)
ament_package()

转到 robot_launch.py 文件并将其替换为：

import os
import launch
from launch_ros.actions import Node
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},
        ]
    )

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

这样就创建了一个 obstacle_avoider 节点，并将其加入了 LaunchDescription。

4 测试避障代码

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

Linux

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

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

Windows

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

$ colcon build
$ export WEBOTS_HOME=/mnt/c/Program\ Files/Webots
$ source install/local_setup.bash
$ ros2 launch my_package robot_launch.py

确保在 Webots 安装文件夹路径前使用 /mnt 前缀，以便从 WSL 访问 Windows 文件系统。

macOS

在主机（不是虚拟机）的终端中，如果尚未设置，请指定 Webots 安装文件夹（例如 /Applications/Webots.app）并使用以下命令启动服务器：

$ export WEBOTS_HOME=/Applications/Webots.app
$ python3 local_simulation_server.py

注意，ROS 2 节点结束后服务器会继续运行，无需每次启动新仿真时都重启它。

在 Linux 虚拟机的 ROS 2 工作空间的终端中，构建并启动你的自定义包：

$ cd ~/ros2_ws
$ colcon build
$ source install/local_setup.bash
$ ros2 launch my_package robot_launch.py

机器人应该会向前运动，在撞到墙壁之前顺时针转弯。

可以在 Webots 中按 Ctrl+F10，或依次进入 View 菜单 → Optional Rendering → Show DistanceSensor Rays 来显示机器人距离传感器的射线。

{/* 截图：机器人顺时针转弯 */}

总结

在本教程中，你用避障 ROS 2 节点扩展了基础仿真，该节点根据机器人距离传感器的值来发布速度命令。

下一步

你可以进一步改进插件或创建新节点来改变机器人的行为。

还可以实现一个重置处理器，以便从 Webots 界面重置仿真时自动重启 ROS 节点：

Webots 仿真重置处理器。

Webots 机器人仿真设置（进阶）

背景

前提条件

任务

1 更新 my_robot.urdf

Python

C++

2 创建避障 ROS 节点

Python

C++

3 更新附加文件

Python

C++

4 测试避障代码

Linux

Windows

macOS

总结

下一步

1 更新 `my_robot.urdf`