ROS2学习笔记之编写Python发布订阅节点篇

学习目标：通过Python实现编写并运行发布订阅节点

背景

在本篇教程当中，我们编写两个节点通过话题的方式进行字符串消息的传递。在这个例子中我们使用“talker” 和 “listener” 的模式，一个发布数据一个接收数据，让他们实现数据的交流。

前期准备

知道如何创建工作空间和节点，了解Python基本语法。

学习内容

1. 创建Python功能包

新打开一个终端，进入dev_ws/src，准备创建功能包

cd ~/dev_ws/src

创建一个py_pubsub的功能包

ros2 pkg create --build-type ament_python py_pubsub

终端会显示创建了一些文件和目录表示创建成功。

2. 编写发布节点

之前的教程我们讲过Python脚本一般放在和功能包目录下面和功能同名的一个文件夹下，首先进入这个文件夹

cd py_pubsub/py_pubsub

通过下面的命令下载实例代码

wget https://raw.githubusercontent.com/ros2/examples/master/rclpy/topics/minimal_publisher/examples_rclpy_minimal_publisher/publisher_member_function.py

选择一个编辑器打开publisher_member_function.py文件，代码如下

import rclpy
from rclpy.node import Node

from std_msgs.msg import String


class MinimalPublisher(Node):

    def __init__(self):
        super().__init__('minimal_publisher')
        self.publisher_ = self.create_publisher(String, 'topic', 10)
        timer_period = 0.5  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0

    def timer_callback(self):
        msg = String()
        msg.data = 'Hello World: %d' % self.i
        self.publisher_.publish(msg)
        self.get_logger().info('Publishing: "%s"' % msg.data)
        self.i += 1


def main(args=None):
    rclpy.init(args=args)

    minimal_publisher = MinimalPublisher()

    rclpy.spin(minimal_publisher)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

2.1 代码解释

引入ros2的相关功能到Python

import rclpy
from rclpy.node import Node

引入消息类型

from std_msgs.msg import String

上面这些代表了依赖，需要在package.xml中添加相关信息，之后会讲。
接下来是定义了一个MinimalPublisher类，继承至Node类

class MinimalPublisher(Node):

接下来是这个类的构造函数，通过super().__init__的方式调用了父类的构造函数，将节点名称命名为minimal_publisher

create_publisher声明了话题名称为topic，消息类型为String，消息队列长度为10。
定时器timer创建了一个0.5s的回调
self.i作为一个记录消息发送数量的计数器

def __init__(self):
    super().__init__('minimal_publisher')
    self.publisher_ = self.create_publisher(String, 'topic', 10)
    timer_period = 0.5  # seconds
    self.timer = self.create_timer(timer_period, self.timer_callback)
    self.i = 0

timer_callback是定时回调函数，发送消息同时，将消息打印到终端通过get_logger().info的方式

def timer_callback(self):
    msg = String()
    msg.data = 'Hello World: %d' % self.i
    self.publisher_.publish(msg)
    self.get_logger().info('Publishing: "%s"' % msg.data)
    self.i += 1

最后一部分是main函数的定义

def main(args=None):
    rclpy.init(args=args)

    minimal_publisher = MinimalPublisher()

    rclpy.spin(minimal_publisher)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_publisher.destroy_node()
    rclpy.shutdown()

首先rclpy进行了初始化，然后创建节点，之后进入回调。

2.2 添加依赖

进入dev_ws/src/py_pubsub目录，我们可以看到setup.py、setup.cfg和 package.xml 文件都给我们自动创建好了。

打开package.xml，首先对description、 maintainer and license进行填写。

<description>Examples of minimal publisher/subscriber using rclpy</description>
<maintainer email="you@email.com">Your Name</maintainer>
<license>Apache License 2.0</license>

在ament_python构建依赖后面加上执行时需要的依赖。

<exec_depend>rclpy</exec_depend>
<exec_depend>std_msgs</exec_depend>

保存文件

2.3 添加发布者程序入口

打开setup.py，首先填写maintainer、maintainer_email、 description 、 license 和package.xml保持一致

maintainer='YourName',
maintainer_email='you@email.com',
description='Examples of minimal publisher/subscriber using rclpy',
license='Apache License 2.0',

然后在console_scripts 下一行括号之内添加程序入口:

entry_points={
        'console_scripts': [
                'talker = py_pubsub.publisher_member_function:main',
        ],
},

保存文件

2.4 检查setup.cfg

文件内容应该是创建包的时候自动填充的

[develop]
script-dir=$base/lib/py_pubsub
[install]
install-scripts=$base/lib/py_pubsub

这些就是告诉setuptools把可执行文件放到lib中，让ros2 run可以找到它。

到此为止发布者就完全编写完成了，我们可以编译过后source一下就可以运行了，但是我们还是等订阅者也写好过后在运行，方便看到整体的效果。

3. 编译订阅者

我们首先进入文件夹然后下载示例代码

cd ~/dev_ws/src/py_pubsub/py_pubsub
wget https://raw.githubusercontent.com/ros2/examples/master/rclpy/topics/minimal_subscriber/examples_rclpy_minimal_subscriber/subscriber_member_function.py

3.1 代码解释

打开subscriber_member_function.py内容如下：

import rclpy
from rclpy.node import Node

from std_msgs.msg import String


class MinimalSubscriber(Node):

    def __init__(self):
        super().__init__('minimal_subscriber')
        self.subscription = self.create_subscription(
            String,
            'topic',
            self.listener_callback,
            10)
        self.subscription  # prevent unused variable warning

    def listener_callback(self, msg):
        self.get_logger().info('I heard: "%s"' % msg.data)


def main(args=None):
    rclpy.init(args=args)

    minimal_subscriber = MinimalSubscriber()

    rclpy.spin(minimal_subscriber)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_subscriber.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

前面的大部分代码都和发布者相似。在构造函数里面，用和发布者相同的消息类型、话题、消息队列长度定义了一个订阅者

self.subscription = self.create_subscription(
    String,
    'topic',
    self.listener_callback,
    10)

由于我们只需要处理接收到的消息就没有了定时器，话题的回调函数里面将接收到的消息打印到控制台。

def listener_callback(self, msg):
    self.get_logger().info('I heard: "%s"' % msg.data)

main函数部分和发布者部分的代码几乎一样，唯一不同的地方就是把相应的地方换为了订阅者的内容。

minimal_subscriber = MinimalSubscriber()

rclpy.spin(minimal_subscriber)

订阅和发布者节点的依赖相同，所以我们并不需要修改package.xml。setup.cfg也保持默认。

3.2 添加订阅者程序入口

打开setup.py文件，在刚刚添加发布者的下面添加订阅者的程序入口。

entry_points={
        'console_scripts': [
                'talker = py_pubsub.publisher_member_function:main',
                'listener = py_pubsub.subscriber_member_function:main',
        ],
},

保存文件，到此我们发布和订阅的节点都准备完成了。

4. 编译运行节点

我们进入工作空间根目录开始编译节点

cd ~/dev_ws
colcon build --packages-select py_pubsub

新打开一个终端运行talker节点

source dev_ws/install/setup.bash
ros2 run py_pubsub talker

节点开始运行在终端不断打印出发布的消息内容

[INFO] [minimal_publisher]: Publishing: "Hello World: 0"
[INFO] [minimal_publisher]: Publishing: "Hello World: 1"
[INFO] [minimal_publisher]: Publishing: "Hello World: 2"
[INFO] [minimal_publisher]: Publishing: "Hello World: 3"
[INFO] [minimal_publisher]: Publishing: "Hello World: 4"
...

再新打开一个终端运行listener节点

source dev_ws/install/setup.bash
ros2 run py_pubsub listener

节点开始运行，并且将接收到的消息打印显示到终端。

[INFO] [minimal_subscriber]: I heard: "Hello World: 10"
[INFO] [minimal_subscriber]: I heard: "Hello World: 11"
[INFO] [minimal_subscriber]: I heard: "Hello World: 12"
[INFO] [minimal_subscriber]: I heard: "Hello World: 13"
[INFO] [minimal_subscriber]: I heard: "Hello World: 14"

总结

这个例程讲述了ros2中Python编写话题订阅发布，基本操作就需要添加依赖和添加程序入口。