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

目标：设置机器人仿真并通过 ROS 2 控制它。

 教程级别：高级

 时间：30 分钟

 目录

•  背景

•  先决条件

•  任务

  • 1 创建包结构

  • 2 设置模拟世界

  • 3 编辑 my_robot_driver 插件

  • 4 创建 my_robot.urdf 文件

  • 5 创建启动文件

  • 6 编辑其他文件

  • 7 测试代码

•  摘要

•  下一步

 背景

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

webots_ros2 包提供了 ROS 2 和 Webots 之间的接口。它包括几个子包，但在本教程中，您将只使用 webots_ros2_driver 子包来实现控制模拟机器人的 Python 或 C++ 插件。其他一些子包包含不同机器人（如 TurtleBot3）的演示。它们在 Webots ROS 2 示例页面中有记录。

先决条件

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

Linux:

本教程的 Linux 和 ROS 命令可以在标准的 Linux 终端中运行。以下页面安装 (Ubuntu) 解释了如何在 Linux 上安装 webots_ros2 包。

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

任务

1 创建包结构

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

Python:

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

--node-name my_robot_driver 选项将在 my_package 子文件夹中创建一个 my_robot_driver.py 模板 Python 插件，您稍后将进行修改。 --dependencies rclpy geometry_msgs webots_ros2_driver 选项指定 package.xml 文件中 my_robot_driver.py 插件所需的包。

让我们在 my_package 文件夹中添加一个 launch 文件夹和一个 worlds 文件夹。

cxy@ubuntu2404-cxy:~/ros2_ws/src/my_package$ mkdir launch
cxy@ubuntu2404-cxy:~/ros2_ws/src/my_package$ mkdir worlds
cxy@ubuntu2404-cxy:~/ros2_ws/src/my_package$ tree
.
├── launch
├── LICENSE
├── my_package
│   ├── __init__.py
│   └── my_robot_driver.py
├── package.xml
├── resource
│   └── my_package
├── setup.cfg
├── setup.py
├── test
│   ├── test_copyright.py
│   ├── test_flake8.py
│   └── test_pep257.py
└── worlds


6 directories, 10 files

C++ :

ros2 pkg create --build-type ament_cmake --license Apache-2.0 --node-name MyRobotDriver my_package --dependencies rclcpp geometry_msgs webots_ros2_driver pluginlib

--node-name MyRobotDriver 选项将在 my_package/src 子文件夹中创建一个 MyRobotDriver.cpp 模板 C++ 插件，您稍后将进行修改。 --dependencies rclcpp geometry_msgs webots_ros2_driver pluginlib 选项指定 package.xml 文件中 MyRobotDriver 插件所需的包。

让我们在 my_package 文件夹中添加一个 launch 、一个 worlds 和一个 resource 文件夹。

cd my_package
mkdir launch
mkdir worlds
mkdir resource

必须创建两个附加文件： MyRobotDriver 的头文件和 my_robot_driver.xml 的 pluginlib 描述文件。

touch my_robot_driver.xml
touch include/my_package/MyRobotDriver.hpp

你应该最终得到以下文件夹结构：

src/
└── my_package/
    ├── include/
    │   └── my_package/
    │       └── MyRobotDriver.hpp
    ├── launch/
    ├── resource/
    ├── src/
    │   └── MyRobotDriver.cpp
    ├── worlds/
    ├── CMakeList.txt
    ├── my_robot_driver.xml
    └── package.xml

2 设置模拟世界

您将需要一个包含机器人的世界文件来启动您的模拟。 Download this world file 并将其移动到 my_package/worlds/ 内。https://docs.ros.org/en/jazzy/_downloads/5ad123fc6a8f1ea79553d5039728084a/my_world.wbt

#VRML_SIM R2022b utf8  // VRML_SIM 文件格式，版本 R2022b，使用 UTF-8 编码


EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackground.proto"  // 引用外部原型文件 TexturedBackground.proto
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto"  // 引用外部原型文件 TexturedBackgroundLight.proto
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/floors/protos/CircleArena.proto"  // 引用外部原型文件 CircleArena.proto


WorldInfo {  // 世界信息节点
}
Viewpoint {  // 视点节点
  orientation -0.33185733874619844 -0.09874274160469809 0.9381474178937331 3.686018050088086  // 视点的方向
  position 1.700313773507203 1.0549607538959629 1.4846240848267684  // 视点的位置
  follow "my_robot"  // 视点跟随的对象
}
TexturedBackground {  // 纹理背景节点
}
TexturedBackgroundLight {  // 纹理背景光节点
}
CircleArena {  // 圆形竞技场节点
}
Robot {  // 机器人节点
  children [  // 子节点列表
    HingeJoint {  // 铰链关节节点
      jointParameters HingeJointParameters {  // 铰链关节参数
        axis 0 1 0  // 关节轴
        anchor 0 0 0.025  // 关节锚点
      }
      device [  // 设备列表
        RotationalMotor {  // 旋转电机节点
          name "left wheel motor"  // 电机名称
        }
      ]
      endPoint Solid {  // 终点实体节点
        translation 0 0.045 0.025  // 终点位置
        children [  // 子节点列表
          DEF WHEEL Transform {  // 定义轮子变换节点
            rotation 1 0 0 1.5707996938995747  // 轮子旋转
            children [  // 子节点列表
              Shape {  // 形状节点
                appearance PBRAppearance {  // PBR 外观节点
                  baseColor 1 0 0  // 基础颜色
                  roughness 1  // 粗糙度
                  metalness 0  // 金属度
                }
                geometry Cylinder {  // 几何体为圆柱体
                  height 0.01  // 圆柱体高度
                  radius 0.025  // 圆柱体半径
                }
              }
            ]
          }
        ]
        name "left wheel"  // 轮子名称
        boundingObject USE WHEEL  // 使用定义的轮子作为边界对象
        physics Physics {  // 物理属性
        }
      }
    }
    HingeJoint {  // 铰链关节节点
      jointParameters HingeJointParameters {  // 铰链关节参数
        axis 0 1 0  // 关节轴
        anchor 0 0 0.025  // 关节锚点
      }
      device [  // 设备列表
        RotationalMotor {  // 旋转电机节点
          name "right wheel motor"  // 电机名称
        }
      ]
      endPoint Solid {  // 终点实体节点
        translation 0 -0.045 0.025  // 终点位置
        children [  // 子节点列表
          USE WHEEL  // 使用定义的轮子
        ]
        name "right wheel"  // 轮子名称
        boundingObject USE WHEEL  // 使用定义的轮子作为边界对象
        physics Physics {  // 物理属性
        }
      }
    }
    Transform {  // 变换节点
      translation 0 0 0.0415  // 变换位置
      children [  // 子节点列表
        Shape {  // 形状节点
          appearance PBRAppearance {  // PBR 外观节点
            baseColor 0 0 1  // 基础颜色
            roughness 1  // 粗糙度
            metalness 0  // 金属度
          }
          geometry DEF BODY Cylinder {  // 定义几何体为圆柱体
            height 0.08  // 圆柱体高度
            radius 0.045  // 圆柱体半径
          }
        }
      ]
    }
    DistanceSensor {  // 距离传感器节点
      translation 0.042 0.02 0.063  // 传感器位置
      rotation 0 0 1 0.5236003061004253  // 传感器旋转
      children [  // 子节点列表
        DEF SENSOR Transform {  // 定义传感器变换节点
          rotation 0 1 0 1.5708  // 传感器旋转
          children [  // 子节点列表
            Shape {  // 形状节点
              appearance PBRAppearance {  // PBR 外观节点
                baseColor 1 1 0  // 基础颜色
                roughness 1  // 粗糙度
                metalness 0  // 金属度
              }
              geometry Cylinder {  // 几何体为圆柱体
                height 0.004  // 圆柱体高度
                radius 0.008  // 圆柱体半径
              }
            }
          ]
        }
      ]
      name "ds0"  // 传感器名称
      lookupTable [  // 查找表
        0 1020 0
        0.05 1020 0
        0.15 0 0
      ]
      numberOfRays 2  // 射线数量
      aperture 1  // 光圈
    }
    DistanceSensor {  // 距离传感器节点
      translation 0.042 -0.02 0.063  // 传感器位置
      rotation 0 0 1 -0.5235996938995747  // 传感器旋转
      children [  // 子节点列表
        USE SENSOR  // 使用定义的传感器
      ]
      name "ds1"  // 传感器名称
      lookupTable [  // 查找表
        0 1020 0
        0.05 1020 0
        0.15 0 0
      ]
      numberOfRays 2  // 射线数量
      aperture 1  // 光圈
    }
  ]
  boundingObject Transform {  // 边界对象变换节点
    translation 0 0 0.0415  // 变换位置
    children [  // 子节点列表
      USE BODY  // 使用定义的主体
    ]
  }
  physics Physics {  // 物理属性
  }
  controller "<extern>"  // 控制器
  name "my_robot"  // 机器人名称
}

这实际上是一个相当简单的文本文件，您可以在文本编辑器中可视化。一个简单的机器人已经包含在这个 my_world.wbt 世界文件中。

 便条

如果你想学习如何在 Webots 中创建自己的机器人模型，可以查看本教程。https://cyberbotics.com/doc/guide/tutorial-6-4-wheels-robot

3 编辑 my_robot_driver 插件

webots_ros2_driver 子包会自动为大多数传感器创建一个 ROS 2 接口。有关现有设备接口的更多详细信息以及如何配置它们，请参见教程的第二部分：设置机器人仿真（高级）。在此任务中，您将通过创建自己的自定义插件来扩展此接口。此自定义插件是一个相当于机器人控制器的 ROS 节点。您可以使用它访问 Webots 机器人 API，并创建自己的主题和服务来控制您的机器人。

 便条

本教程的目的是展示一个具有最少依赖项的基本示例。然而，您可以通过使用另一个名为 webots_ros2_control 的 webots_ros2 子包来避免使用此插件，从而引入一个新的依赖项。这个其他子包创建了一个与 ros2_control 包的接口，便于控制差动轮式机器人。

Python:

在您喜欢的编辑器中打开 my_package/my_package/my_robot_driver.py 并将其内容替换为以下内容：

import rclpy  # 导入 ROS2 客户端库
from geometry_msgs.msg import Twist  # 从 geometry_msgs 模块导入 Twist 消息类型


HALF_DISTANCE_BETWEEN_WHEELS = 0.045  # 定义车轮间距的一半
WHEEL_RADIUS = 0.025  # 定义车轮半径


class MyRobotDriver:  # 定义 MyRobotDriver 类
    def init(self, webots_node, properties):  # 初始化方法
        self.__robot = webots_node.robot  # 获取 Webots 机器人节点


        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)  # 设置左轮电机速度为 0


        self.__right_motor.setPosition(float('inf'))  # 设置右轮电机位置为无限
        self.__right_motor.setVelocity(0)  # 设置右轮电机速度为 0


        self.__target_twist = Twist()  # 初始化目标 Twist 消息


        rclpy.init(args=None)  # 初始化 ROS2 客户端库
        self.__node = rclpy.create_node('my_robot_driver')  # 创建 ROS2 节点
        self.__node.create_subscription(Twist, 'cmd_vel', self.__cmd_vel_callback, 1)  # 创建订阅者，订阅 'cmd_vel' 话题


    def __cmd_vel_callback(self, twist):  # 回调函数，处理接收到的 Twist 消息
        self.__target_twist = twist  # 更新目标 Twist 消息


    def step(self):  # 步进方法
        rclpy.spin_once(self.__node, timeout_sec=0)  # 处理一次 ROS2 消息


        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)  # 设置右轮电机速度

如您所见， MyRobotDriver 类实现了三种方法。

第一种方法，名为 init(self, ...) ，实际上是 Python __init__(self, ...) 构造函数的 ROS 节点对应物。 init 方法总是需要两个参数：

• webots_node 参数包含对 Webots 实例的引用。

• properties 参数是从 URDF 文件中给出的 XML 标签创建的字典（4 创建 my_robot.urdf 文件），并允许您将参数传递给控制器。

仿真中的机器人实例 self.__robot 可用于访问 Webots 机器人 API  https://cyberbotics.com/doc/reference/robot?tab-language=python。然后，它获取两个电机实例并用目标位置和目标速度初始化它们。最后，创建一个 ROS 节点，并为名为 /cmd_vel 的 ROS 主题注册一个回调方法，该方法将处理 Twist 消息。

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)

然后是 __cmd_vel_callback(self, twist) 回调私有方法的实现，该方法将在 /cmd_vel 主题上收到的每个 Twist 消息时调用，并将其保存在 self.__target_twist 成员变量中。

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

最后，在每个仿真时间步调用 step(self) 方法。调用 rclpy.spin_once() 是为了保持 ROS 节点平稳运行。在每个时间步，方法将从 self.__target_twist 中检索所需的 forward_speed 和 angular_speed 。由于电机是通过角速度控制的，该方法将把 forward_speed 和 angular_speed 转换为每个车轮的单独命令。此转换取决于机器人的结构，更具体地说，取决于车轮的半径和它们之间的距离。

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)

C++:

在您喜欢的编辑器中打开 my_package/include/my_package/MyRobotDriver.hpp 并将其内容替换为以下内容：

#ifndef WEBOTS_ROS2_PLUGIN_EXAMPLE_HPP
#define WEBOTS_ROS2_PLUGIN_EXAMPLE_HPP
// 防止头文件重复包含的宏定义


#include "rclcpp/macros.hpp"
// 包含rclcpp的宏定义


#include "webots_ros2_driver/PluginInterface.hpp"
// 包含Webots ROS2驱动的插件接口


#include "webots_ros2_driver/WebotsNode.hpp"
// 包含Webots ROS2驱动的节点


#include "geometry_msgs/msg/twist.hpp"
// 包含几何消息中的Twist消息类型


#include "rclcpp/rclcpp.hpp"
// 包含rclcpp库


namespace my_robot_driver {
// 定义命名空间my_robot_driver


class MyRobotDriver : public webots_ros2_driver::PluginInterface {
// 定义类MyRobotDriver，继承自webots_ros2_driver::PluginInterface
public:
  void step() override;
  // 重写step函数


  void init(webots_ros2_driver::WebotsNode *node,
            std::unordered_map<std::string, std::string> &parameters) override;
  // 重写init函数，初始化节点和参数


private:


  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr
      cmd_vel_subscription_;
  // 定义一个Twist消息类型的订阅者


  geometry_msgs::msg::Twist cmd_vel_msg;
  // 定义一个Twist消息类型的变量


  WbDeviceTag right_motor;
  // 定义右轮电机标签


  WbDeviceTag left_motor;
  // 定义左轮电机标签
};
} // namespace my_robot_driver
// 命名空间my_robot_driver结束


#endif
// 头文件结束的宏定义

定义了类 MyRobotDriver ，它继承自 webots_ros2_driver::PluginInterface 类。插件必须重写 step(...) 和 init(...) 函数。更多细节在 MyRobotDriver.cpp 文件中给出。插件内部使用的几个辅助方法、回调和成员变量被私有声明。

然后，在您喜欢的编辑器中打开 my_package/src/MyRobotDriver.cpp ，并将其内容替换为以下内容：

#include "my_package/MyRobotDriver.hpp"
// 包含自定义的MyRobotDriver头文件


#include "rclcpp/rclcpp.hpp"
// 包含rclcpp库


#include <cstdio>
// 包含标准输入输出库


#include <functional>
// 包含函数库


#include <webots/motor.h>
// 包含Webots电机库


#include <webots/robot.h>
// 包含Webots机器人库


#define HALF_DISTANCE_BETWEEN_WHEELS 0.045
// 定义轮子之间的一半距离


#define WHEEL_RADIUS 0.025
// 定义轮子的半径


namespace my_robot_driver {
// 定义命名空间my_robot_driver


void MyRobotDriver::init(
    webots_ros2_driver::WebotsNode *node,
    std::unordered_map<std::string, std::string> &parameters) {


  right_motor = wb_robot_get_device("right wheel motor");
  // 获取右轮电机设备


  left_motor = wb_robot_get_device("left wheel motor");
  // 获取左轮电机设备


  wb_motor_set_position(left_motor, INFINITY);
  // 设置左轮电机的位置为无限大


  wb_motor_set_velocity(left_motor, 0.0);
  // 设置左轮电机的速度为0


  wb_motor_set_position(right_motor, INFINITY);
  // 设置右轮电机的位置为无限大


  wb_motor_set_velocity(right_motor, 0.0);
  // 设置右轮电机的速度为0


  cmd_vel_subscription_ = node->create_subscription<geometry_msgs::msg::Twist>(
      "/cmd_vel", rclcpp::SensorDataQoS().reliable(),
      [this](const geometry_msgs::msg::Twist::SharedPtr msg){
        this->cmd_vel_msg.linear = msg->linear;
        this->cmd_vel_msg.angular = msg->angular;
      }
  );
  // 创建订阅者，订阅/cmd_vel话题，并在接收到消息时更新cmd_vel_msg
}


void MyRobotDriver::step() {
  auto forward_speed = cmd_vel_msg.linear.x;
  // 获取线速度


  auto angular_speed = cmd_vel_msg.angular.z;
  // 获取角速度


  auto command_motor_left =
      (forward_speed - angular_speed * HALF_DISTANCE_BETWEEN_WHEELS) /
      WHEEL_RADIUS;
  // 计算左轮电机的速度命令


  auto command_motor_right =
      (forward_speed + angular_speed * HALF_DISTANCE_BETWEEN_WHEELS) /
      WHEEL_RADIUS;
  // 计算右轮电机的速度命令


  wb_motor_set_velocity(left_motor, command_motor_left);
  // 设置左轮电机的速度


  wb_motor_set_velocity(right_motor, command_motor_right);
  // 设置右轮电机的速度
}
} // namespace my_robot_driver
// 命名空间my_robot_driver结束


#include "pluginlib/class_list_macros.hpp"
// 包含插件库的宏定义


PLUGINLIB_EXPORT_CLASS(my_robot_driver::MyRobotDriver,
                       webots_ros2_driver::PluginInterface)
// 导出MyRobotDriver类作为插件

MyRobotDriver::init 方法在插件被 webots_ros2_driver 包加载后执行。它接受两个参数：

• 指向由 webots_ros2_driver 定义的 WebotsNode 的指针，它允许访问 ROS 2 节点功能。

• parameters 参数是一个字符串的无序映射，由 URDF 文件中给定的 XML 标签创建（4 创建 my_robot.urdf 文件），并允许将参数传递给控制器。在此示例中未使用。

它通过设置机器人电机、设置它们的位置和速度以及订阅 /cmd_vel 主题来初始化插件。

void MyRobotDriver::init(
    webots_ros2_driver::WebotsNode *node,
    std::unordered_map<std::string, std::string> &parameters) {


  right_motor = wb_robot_get_device("right wheel motor");
  left_motor = wb_robot_get_device("left wheel motor");


  wb_motor_set_position(left_motor, INFINITY);
  wb_motor_set_velocity(left_motor, 0.0);


  wb_motor_set_position(right_motor, INFINITY);
  wb_motor_set_velocity(right_motor, 0.0);


  cmd_vel_subscription_ = node->create_subscription<geometry_msgs::msg::Twist>(
      "/cmd_vel", rclcpp::SensorDataQoS().reliable(),
      [this](const geometry_msgs::msg::Twist::SharedPtr msg){
        this->cmd_vel_msg.linear = msg->linear;
        this->cmd_vel_msg.angular = msg->angular;
      }
  );
}

订阅回调被声明为一个 lambda 函数，它将在接收到 /cmd_vel 主题的每个 Twist 消息时被调用，并将其保存在 cmd_vel_msg 成员变量中。

[this](const geometry_msgs::msg::Twist::SharedPtr msg){
        this->cmd_vel_msg.linear = msg->linear;
        this->cmd_vel_msg.angular = msg->angular;
      }

step() 方法在模拟的每个时间步调用。在每个时间步，方法将从 cmd_vel_msg 中检索所需的 forward_speed 和 angular_speed 。由于电机是通过角速度控制的，因此该方法会将 forward_speed 和 angular_speed 转换为每个车轮的单独命令。此转换取决于机器人的结构，更具体地说，取决于车轮的半径和它们之间的距离。

void MyRobotDriver::step() {
  auto forward_speed = cmd_vel_msg.linear.x;
  auto angular_speed = cmd_vel_msg.angular.z;


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


  wb_motor_set_velocity(left_motor, command_motor_left);
  wb_motor_set_velocity(right_motor, command_motor_right);
}

文件的最后几行定义了 my_robot_driver 命名空间的结束，并包含一个宏，用于使用 PLUGINLIB_EXPORT_CLASS 宏将 MyRobotDriver 类导出为插件。这允许插件在运行时由 Webots ROS2 驱动程序加载。

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(my_robot_driver::MyRobotDriver,
                       webots_ros2_driver::PluginInterface)

 便条

虽然插件是用 C++实现的，但必须使用 C API 与 Webots 控制器库进行交互。

4 创建 my_robot.urdf 文件

现在你需要创建一个 URDF 文件来声明 MyRobotDriver 插件。这将允许 webots_ros2_driver ROS 节点启动插件并将其连接到目标机器人。

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

Python:

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

type 属性指定由文件的层次结构给出的类的路径。 webots_ros2_driver 负责根据指定的包和模块加载类。

C++:

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

type 属性指定要加载的命名空间和类名。 pluginlib 负责根据指定的信息加载类。

 便条

这个简单的 URDF 文件不包含任何关于机器人的连杆或关节信息，因为在本教程中不需要这些信息。然而，URDF 文件通常包含更多的信息，如 URDF 教程中所解释的那样。https://docs.ros.org/en/jazzy/Tutorials/Intermediate/URDF/URDF-Main.html

 便条

此插件不接受任何输入参数，但可以通过包含参数名称的标签来实现。

Python:

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

C++:

<plugin type="my_robot_driver::MyRobotDriver">
    <parameterName>someValue</parameterName>
</plugin>

这主要用于将参数传递给现有的 Webots 设备插件（参见设置机器人仿真（高级））。https://docs.ros.org/en/jazzy/Tutorials/Advanced/Simulators/Webots/Setting-Up-Simulation-Webots-Advanced.html

5 创建启动文件

让我们创建启动文件，以便通过一个命令轻松启动仿真和 ROS 控制器。在 my_package/launch 文件夹中创建一个名为 robot_launch.py 的新文本文件，并使用以下代码：

import os
# 导入操作系统模块


import launch
# 导入launch模块


from launch import LaunchDescription
# 从launch模块导入LaunchDescription类


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类


from webots_ros2_driver.webots_controller import WebotsController
# 从webots_ros2_driver.webots_controller模块导入WebotsController类




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_robot_driver = WebotsController(
        robot_name='my_robot',
        parameters=[
            {'robot_description': robot_description_path},
        ]
    )
    # 创建WebotsController对象，指定机器人名称和参数


    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())],
            )
        )
    ])
    # 返回LaunchDescription对象，包含WebotsLauncher和WebotsController对象，并注册事件处理程序

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

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

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

Linux: 

该节点将能够通过使用基于 IPC 和共享内存的自定义协议与模拟机器人进行通信。

在你的情况下，你需要运行该节点的单个实例，因为你在模拟中只有一个机器人。但如果你在模拟中有更多的机器人，你将不得不为每个机器人运行该节点的一个实例。 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())],
    )
)

 便条

有关 WebotsController 和 WebotsLauncher 参数的更多详细信息，请参阅节点参考页面。https://github.com/cyberbotics/webots_ros2/wiki/References-Nodes

6 编辑其他文件 

Python:

在启动文件之前，您必须修改 setup.py 文件以包含您添加的额外文件。打开 my_package/setup.py 并将其内容替换为：

from setuptools import find_packages, setup


# 定义包的名称
package_name = 'my_package'


# 初始化data_files列表，用于存储数据文件的信息
data_files = []


# 添加ament_index的资源索引文件
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']))


# 添加URDF机器人模型文件
data_files.append(('share/' + package_name + '/resource', ['resource/my_robot.urdf']))


# 添加包的XML描述文件
data_files.append(('share/' + package_name, ['package.xml']))


# 调用setup函数进行包的配置
setup(
    name=package_name,  # 包的名称
    version='0.0.0',  # 包的版本
    packages=find_packages(exclude=['test']),  # 自动发现包，排除test目录
    data_files=data_files,  # 包含的数据文件
    install_requires=['setuptools'],  # 安装时的依赖
    zip_safe=True,  # 表示包可以安全地作为zip文件分发
    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',  # 定义控制台脚本入口点
        ],
    },
)

这将设置包并在 data_files 变量中添加新添加的文件： my_world.wbt ， my_robot.urdf 和 robot_launch.py 。

C++:

在启动文件之前，您必须修改 CMakeLists.txt 和 my_robot_driver.xml 文件：

• CMakeLists.txt 定义了插件的编译规则。

• my_robot_driver.xml 是 pluginlib 查找您的 Webots ROS 2 插件所必需的。

打开 my_package/my_robot_driver.xml 并替换其内容：

<library path="my_package">
  <!-- The `type` attribute is a reference to the plugin class. -->
  <!-- The `base_class_type` attribute is always `webots_ros2_driver::PluginInterface`. -->
  <class type="my_robot_driver::MyRobotDriver" base_class_type="webots_ros2_driver::PluginInterface">
    <description>
      This is a Webots ROS 2 plugin example
    </description>
  </class>
</library>

打开 my_package/CMakeLists.txt 并替换其内容：

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


# Besides the package specific dependencies we also need the `pluginlib` and `webots_ros2_driver`
# 除了包特定的依赖项，我们还需要`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包


# Export the plugin configuration file
# 导出插件配置文件
pluginlib_export_plugin_description_file(webots_ros2_driver my_robot_driver.xml)  # 导出插件描述文件


# MyRobotDriver library
# MyRobotDriver库
add_library(
  ${PROJECT_NAME}  # 添加库，库名为项目名称
  SHARED  # 共享库
  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 additional directories.
# 安装附加目录
install(DIRECTORY
  launch  # 安装launch目录
  resource  # 安装resource目录
  worlds  # 安装worlds目录
  DESTINATION share/${PROJECT_NAME}/  # 安装到share/项目名称/目录
)
ament_export_include_directories(
  include  # 导出包含目录
)
ament_export_libraries(
  ${PROJECT_NAME}  # 导出库
)
ament_package()  # 声明 ament 包

CMakeLists.txt 使用 pluginlib_export_plugin_description_file() 导出插件配置文件，定义 C++ 插件的共享库 src/MyRobotDriver.cpp ，并使用 ament_target_dependencies() 设置包含和库依赖项。

然后，该文件将库、目录 launch 、 resource 和 worlds 安装到 share/my_package 目录。最后，它分别使用 ament_export_include_directories() 和 ament_export_libraries() 导出包含目录和库，并使用 ament_package() 声明包。

7 测试代码

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

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

这将启动模拟。如果尚未安装，Webots 将在首次运行时自动安装。

 便条

如果您想手动安装 Webots，可以在此下载。https://github.com/cyberbotics/webots/releases/latest

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

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

机器人现在正在前进。

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

cxy@ubuntu2404-cxy:~$ ros2 topic pub /cmd_vel geometry_msgs/Twist  "linear: { x: 0.1 }"
publisher: beginning loop
publishing #1: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #2: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #3: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #4: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #5: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #6: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #7: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #8: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #9: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #10: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #11: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #12: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #13: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #14: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #15: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #16: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #17: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #18: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #19: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #20: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #21: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #22: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #23: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #24: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #25: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #26: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #27: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #28: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #29: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #30: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #31: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #32: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #33: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #34: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #35: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #36: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #37: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #38: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #39: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #40: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #41: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #42: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #43: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #44: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #45: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #46: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #47: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #48: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #49: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #50: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #51: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #52: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))


publishing #53: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.1, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0))

关闭 Webots 窗口，这也应该关闭从启动器启动的 ROS 节点。还要关闭第二个终端中的 Ctrl+C 主题命令。

 摘要

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

 下一步

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

• 设置机器人模拟（高级）。https://docs.ros.org/en/jazzy/Tutorials/Advanced/Simulators/Webots/Setting-Up-Simulation-Webots-Advanced.html