【ROS2】高级:模拟器-Webots-设置机器人模拟（高级）

目标：用障碍物避让节点扩展机器人模拟。

 教程级别：高级

 时间：20 分钟

 目录

•  背景

•  先决条件

•  任务

  •  1 更新 my_robot.urdf

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

  • 3 更新附加文件

  • 4 测试避障代码

•  摘要

•  下一步

 背景

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

 先决条件

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

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

 任务

 1 更新 my_robot.urdf 

如设置机器人模拟（基础）中所述， webots_ros2_driver 包含可将大多数 Webots 设备直接与 ROS 2 连接的插件。这些插件可以使用机器人的 URDF 文件中的 <device> 标签加载。 reference 属性应与 Webots 设备的 name 参数匹配。所有现有接口及相应参数的列表可以在设备参考页面 https://github.com/cyberbotics/webots_ros2/wiki/References-Devices 上找到。对于未在 URDF 文件中配置的可用设备，接口将自动创建，并且 ROS 参数将使用默认值（例如 update rate 、 topic name 和 frame name ）。

在 my_robot.urdf 中替换整个内容为：

Python:

<?xml version="1.0" ?>
<robot name="My robot">
    <!-- 定义机器人的名称为 "My robot" -->
    <webots>
        <!-- Webots 机器人仿真器相关配置 -->
        <device reference="ds0" type="DistanceSensor">
            <!-- 定义一个名为 ds0 的距离传感器设备 -->
            <ros>
                <!-- ROS 相关配置 -->
                <topicName>/left_sensor</topicName>
                <!-- 定义 ROS 话题名称为 /left_sensor -->
                <alwaysOn>true</alwaysOn>
                <!-- 设置传感器设备始终开启 -->
            </ros>
        </device>
        <device reference="ds1" type="DistanceSensor">
            <!-- 定义另一个名为 ds1 的距离传感器设备 -->
            <ros>
                <!-- ROS 相关配置 -->
                <topicName>/right_sensor</topicName>
                <!-- 定义 ROS 话题名称为 /right_sensor -->
                <alwaysOn>true</alwaysOn>
                <!-- 设置传感器设备始终开启 -->
            </ros>
        </device>
        <plugin type="my_package.my_robot_driver.MyRobotDriver" />
        <!-- 加载自定义插件 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: 定义了一个名为ObstacleAvoider的类，继承自Node类。该类初始化时创建了两个订阅者，分别订阅left_sensor和right_sensor话题，并定义了相应的回调函数。在回调函数中，根据传感器的距离值，发布控制机器人运动的Twist消息。主函数中，初始化rclpy，创建ObstacleAvoider节点对象，并进入循环等待消息。

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

import rclpy  # 导入rclpy库
from rclpy.node import Node  # 从rclpy.node模块导入Node类
from sensor_msgs.msg import Range  # 从sensor_msgs.msg模块导入Range消息类型
from geometry_msgs.msg import Twist  # 从geometry_msgs.msg模块导入Twist消息类型


MAX_RANGE = 0.15  # 定义最大范围常量为0.15米


class ObstacleAvoider(Node):  # 定义一个名为ObstacleAvoider的类，继承自Node类
    def __init__(self):  # 初始化方法
        super().__init__('obstacle_avoider')  # 调用父类的初始化方法，并命名节点为'obstacle_avoider'


        self.__publisher = self.create_publisher(Twist, 'cmd_vel', 1)  # 创建一个发布者，发布Twist消息到'cmd_vel'话题


        self.create_subscription(Range, 'left_sensor', self.__left_sensor_callback, 1)  # 创建一个订阅者，订阅'left_sensor'话题，并指定回调函数
        self.create_subscription(Range, 'right_sensor', self.__right_sensor_callback, 1)  # 创建一个订阅者，订阅'right_sensor'话题，并指定回调函数


    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()  # 创建一个Twist消息对象


        command_message.linear.x = 0.1  # 设置线速度为0.1米/秒


        if self.__left_sensor_value < 0.9 * MAX_RANGE or self.__right_sensor_value < 0.9 * MAX_RANGE:  # 如果任一传感器的距离值小于最大范围的90%
            command_message.angular.z = -2.0  # 设置角速度为-2.0弧度/秒


        self.__publisher.publish(command_message)  # 发布Twist消息


def main(args=None):  # 定义主函数
    rclpy.init(args=args)  # 初始化rclpy
    avoider = ObstacleAvoider()  # 创建ObstacleAvoider节点对象
    rclpy.spin(avoider)  # 让节点进入循环，等待消息
    avoider.destroy_node()  # 显式销毁节点（可选）
    rclpy.shutdown()  # 关闭rclpy


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)

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::SharedPtr msg);
  // 左传感器回调函数，处理传感器数据


  void rightSensorCallback(const sensor_msgs::msg::Range::SharedPtr msg);
  // 右传感器回调函数，处理传感器数据


  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
  // 发布器，用于发布Twist消息


  rclcpp::Subscription<sensor_msgs::msg::Range>::SharedPtr left_sensor_sub_;
  // 订阅器，用于订阅左传感器的Range消息


  rclcpp::Subscription<sensor_msgs::msg::Range>::SharedPtr right_sensor_sub_;
  // 订阅器，用于订阅右传感器的Range消息


  double left_sensor_value{0.0};
  // 左传感器的测量值，初始值为0.0


  double right_sensor_value{0.0};
  // 右传感器的测量值，初始值为0.0
};

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

#include "my_package/ObstacleAvoider.hpp"


#define MAX_RANGE 0.15
// 定义最大距离为0.15米


ObstacleAvoider::ObstacleAvoider() : Node("obstacle_avoider") {
  publisher_ = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 1);
  // 创建一个发布器，发布Twist消息到"/cmd_vel"话题


  left_sensor_sub_ = create_subscription<sensor_msgs::msg::Range>(
      "/left_sensor", 1,
      [this](const sensor_msgs::msg::Range::SharedPtr msg){
        return this->leftSensorCallback(msg);
      }
  );
  // 创建一个订阅器，订阅"/left_sensor"话题，并将回调函数设为leftSensorCallback


  right_sensor_sub_ = create_subscription<sensor_msgs::msg::Range>(
      "/right_sensor", 1,
      [this](const sensor_msgs::msg::Range::SharedPtr msg){
        return this->rightSensorCallback(msg);
      }
  );
  // 创建一个订阅器，订阅"/right_sensor"话题，并将回调函数设为rightSensorCallback
}


void ObstacleAvoider::leftSensorCallback(
    const sensor_msgs::msg::Range::SharedPtr msg) {
  left_sensor_value = msg->range;
  // 更新左传感器的测量值
}


void ObstacleAvoider::rightSensorCallback(
    const sensor_msgs::msg::Range::SharedPtr msg) {
  right_sensor_value = msg->range;
  // 更新右传感器的测量值


  auto command_message = std::make_unique<geometry_msgs::msg::Twist>();
  // 创建一个Twist消息对象，用于存储速度命令


  command_message->linear.x = 0.1;
  // 设置线速度为0.1米/秒


  if (left_sensor_value < 0.9 * MAX_RANGE ||
      right_sensor_value < 0.9 * MAX_RANGE) {
    command_message->angular.z = -2.0;
    // 如果任一传感器的值小于最大距离的90%，则设置角速度为-2.0弧度/秒
  }


  publisher_->publish(std::move(command_message));
  // 发布速度命令消息
}


int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  // 初始化rclcpp库


  auto avoider = std::make_shared<ObstacleAvoider>();
  // 创建ObstacleAvoider节点对象


  rclcpp::spin(avoider);
  // 让节点进入循环，处理回调函数


  rclcpp::shutdown();
  // 关闭rclcpp库


  return 0;
  // 返回0，表示程序正常结束
}

此节点将在此处为命令创建一个发布者并订阅传感器主题

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::SharedPtr msg){
        return this->leftSensorCallback(msg);
      }
  );


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

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

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

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

void ObstacleAvoider::rightSensorCallback(
    const sensor_msgs::msg::Range::SharedPtr 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 节点添加一个入口点。

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

import os  # 导入os模块，用于处理操作系统相关的功能
import launch  # 导入launch模块，用于创建Launch描述符
from launch_ros.actions import Node  # 从launch_ros.actions模块导入Node类，用于创建ROS 2节点
from launch import LaunchDescription  # 从launch模块导入LaunchDescription类，用于定义Launch描述符
from ament_index_python.packages import get_package_share_directory  # 从ament_index_python.packages模块导入get_package_share_directory函数，用于获取包共享目录
from webots_ros2_driver.webots_launcher import WebotsLauncher  # 从webots_ros2_driver.webots_launcher模块导入WebotsLauncher类，用于启动Webots仿真器
from webots_ros2_driver.webots_controller import WebotsController  # 从webots_ros2_driver.webots_controller模块导入WebotsController类，用于控制Webots中的机器人


def generate_launch_description():
    package_dir = get_package_share_directory('my_package')  # 获取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')  # 创建WebotsLauncher实例，加载指定的世界文件my_world.wbt
    )


    my_robot_driver = WebotsController(
        robot_name='my_robot',  # 指定机器人名称为my_robot
        parameters=[
            {'robot_description': robot_description_path},  # 设置机器人描述参数
        ]
    )


    obstacle_avoider = Node(
        package='my_package',  # 指定节点所属的包为my_package
        executable='obstacle_avoider',  # 指定可执行文件名称为obstacle_avoider
    )


    return LaunchDescription([
        webots,  # 启动Webots仿真器
        my_robot_driver,  # 启动Webots中的机器人控制器
        obstacle_avoider,  # 启动障碍物回避节点
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=webots,  # 目标动作是Webots仿真器
                on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],  # 当Webots仿真器进程退出时，触发关闭事件
            )
        )
    ])

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

C++:

编辑 CMakeLists.txt 并添加 obstacle_avoider 的编译和安装：

cmake_minimum_required(VERSION 3.5)  # 设置CMake的最低版本要求为3.5
project(my_package)  # 定义项目名称为my_package


if(NOT CMAKE_CXX_STANDARD)  # 如果没有设置C++标准
  set(CMAKE_CXX_STANDARD 14)  # 设置C++标准为14
endif()


# 除了包特定的依赖项，我们还需要`pluginlib`和`webots_ros2_driver`
find_package(ament_cmake REQUIRED)  # 查找ament_cmake包
find_package(rclcpp REQUIRED)  # 查找rclcpp包
find_package(std_msgs REQUIRED)  # 查找std_msgs包
find_package(geometry_msgs REQUIRED)  # 查找geometry_msgs包
find_package(pluginlib REQUIRED)  # 查找pluginlib包
find_package(webots_ros2_driver REQUIRED)  # 查找webots_ros2_driver包


# 导出插件配置文件
pluginlib_export_plugin_description_file(webots_ros2_driver my_robot_driver.xml)


# 障碍物避让器
include_directories(
  include  # 包含头文件目录
)
add_executable(obstacle_avoider
  src/ObstacleAvoider.cpp  # 添加可执行文件obstacle_avoider，源文件为src/ObstacleAvoider.cpp
)
ament_target_dependencies(obstacle_avoider
  rclcpp  # 设置obstacle_avoider的依赖项为rclcpp
  geometry_msgs  # 设置obstacle_avoider的依赖项为geometry_msgs
  sensor_msgs  # 设置obstacle_avoider的依赖项为sensor_msgs
)
install(TARGETS
  obstacle_avoider  # 安装目标obstacle_avoider
  DESTINATION lib/${PROJECT_NAME}  # 安装路径为lib/${PROJECT_NAME}
)
install(
  DIRECTORY include/  # 安装include目录
  DESTINATION include  # 安装路径为include
)


# MyRobotDriver库
add_library(# 添加共享库，源文件为src/MyRobotDriver.cpp
  ${PROJECT_NAME}  # 添加库，名称为项目名称
  SHARED  # 设置为共享库
  src/MyRobotDriver.cpp  # 源文件为src/MyRobotDriver.cpp
)
target_include_directories( # 指定包含目录
  ${PROJECT_NAME}  # 设置库的名称为项目名称
  PRIVATE  # 设置为私有
  include  # 包含头文件目录
)
ament_target_dependencies(# 指定目标依赖项
  ${PROJECT_NAME}  # 设置库的名称为项目名称
  pluginlib  # 设置库的依赖项为pluginlib
  rclcpp  # 设置库的依赖项为rclcpp
  webots_ros2_driver  # 设置库的依赖项为webots_ros2_driver
)
install(TARGETS
  ${PROJECT_NAME}  # 安装目标为项目名称
  ARCHIVE DESTINATION lib  # 安装路径为lib
  LIBRARY DESTINATION lib  # 安装路径为lib
  RUNTIME DESTINATION bin  # 安装路径为bin
)
# 安装其他目录
install(DIRECTORY
  launch  # 安装launch目录
  resource  # 安装resource目录
  worlds  # 安装worlds目录
  DESTINATION share/${PROJECT_NAME}/  # 安装路径为share/${PROJECT_NAME}/
)


ament_export_include_directories(
  include  # 导出include目录
)
ament_export_libraries(
  ${PROJECT_NAME}  # 导出库，名称为项目名称
)
ament_package()  # 定义ament包

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

import os  # 导入os模块，用于处理操作系统相关的功能
import launch  # 导入launch模块，用于创建Launch描述符
from launch_ros.actions import Node  # 从launch_ros.actions模块导入Node类，用于创建ROS 2节点
from launch import LaunchDescription  # 从launch模块导入LaunchDescription类，用于定义Launch描述符
from ament_index_python.packages import get_package_share_directory  # 从ament_index_python.packages模块导入get_package_share_directory函数，用于获取包共享目录
from webots_ros2_driver.webots_launcher import WebotsLauncher  # 从webots_ros2_driver.webots_launcher模块导入WebotsLauncher类，用于启动Webots仿真器
from webots_ros2_driver.webots_controller import WebotsController  # 从webots_ros2_driver.webots_controller模块导入WebotsController类，用于控制Webots中的机器人


def generate_launch_description():
    package_dir = get_package_share_directory('my_package')  # 获取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')  # 创建WebotsLauncher实例，加载指定的世界文件my_world.wbt
    )


    my_robot_driver = WebotsController(
        robot_name='my_robot',  # 指定机器人名称为my_robot
        parameters=[
            {'robot_description': robot_description_path},  # 设置机器人描述参数
        ]
    )


    obstacle_avoider = Node(
        package='my_package',  # 指定节点所属的包为my_package
        executable='obstacle_avoider',  # 指定可执行文件名称为obstacle_avoider
    )


    return LaunchDescription([
        webots,  # 启动Webots仿真器
        my_robot_driver,  # 启动Webots中的机器人控制器
        obstacle_avoider,  # 启动障碍物回避节点
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=webots,  # 目标动作是Webots仿真器
                on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],  # 当Webots仿真器进程退出时，触发关闭事件
            )
        )
    ])

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

4 测试避障代码

从你的 ROS 2 工作区的终端启动模拟：

Linux: 在你的 ROS 2 工作区的终端中运行：

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

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

cxy@ubuntu2404-cxy:~/ros2_ws$ colcon build --packages-select my_package
Starting >>> my_package
Finished <<< my_package [10.7s]            


Summary: 1 package finished [21.7s]
cxy@ubuntu2404-cxy:~/ros2_ws$ source install/local_setup.bash
cxy@ubuntu2404-cxy:~/ros2_ws$ ros2 launch my_package robot_launch.py
[INFO] [launch]: All log files can be found below /home/cxy/.ros/log/2024-07-18-10-06-49-551812-ubuntu2404-cxy-9720
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [webots-1]: process started with pid [9731]
[INFO] [webots_controller_my_robot-2]: process started with pid [9732]
[INFO] [obstacle_avoider-3]: process started with pid [9733]
[webots_controller_my_robot-2] The specified robot is not in the list of robots with <extern> controllers, retrying for another 50 seconds...
[webots_controller_my_robot-2] The Webots simulation world is not yet ready, pending until loading is done...
[webots_controller_my_robot-2] [INFO] [1721268424.071231410] [my_robot]: Controller successfully connected to robot in Webots simulation.

摘要

在本教程中，您扩展了基本仿真，添加了一个障碍物回避器 ROS 2 节点，该节点根据机器人距离传感器的值发布速度命令。

 下一步

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

• 设置复位处理程序。https://docs.ros.org/en/jazzy/Tutorials/Advanced/Simulators/Webots/Simulation-Reset-Handler.html