简介：在上一节的内容中，我们通过ROS2的话题发布功能将小车实时视频信息发布了出来，同时使用GUI工具进行查看，在这一节内容中，我们学习一下如何订阅话题并处理话题消息，具体实现的功能就是通过方向键控制小车行驶。

1、安装pynput

我们要通过键盘方向键控制小车行走，需要用python编码实现键盘事件监听，我这里使用了pynput库，默认系统时没有安装的，需要手动安装一下：

$ pip3 install pynput

安装后我们测试一下，源码如下：

from pynput import keyboard

def on_press(key):
    print('{0} pressed'.format(key))

def on_release(key):
    print('{0} released'.format(key))
    if key == keyboard.Key.esc:
        return False

with keyboard.Listener(on_press=on_press,on_release=on_release) as listener:
    listener.join()

以上代码创建了一个阻塞式（with…as….）的键盘事件监听功能，监听了键盘所有按键的按下和释放事件，在程序中应用时，不能使用阻塞式编程，会影响程序其他线程的执行，可以换一个方式:

listener = keyboard.Listener(on_press=on_press,on_release=on_release)
listener.start()

2、创建小车控制功能节点

进入ROS2工作空间源码目录

$ cd ~/ros2_ws/src

创建小车控制功能包，包名control，编译类型选python，默认创建control_node节点，依赖rclpy、sensor_msgs、geometry_msgs

$ ros2 pkg create control --build-type ament_python --node-name control_node --dependencies rclpy sensor_msgs geometry_msgs

功能包目录结构如下图：

编辑源码文件control_node.py

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node

import cv2
import numpy as np
from pynput import keyboard #引入键盘监听功能库

from sensor_msgs.msg import Image 
from geometry_msgs.msg import TwistStamped
from cv_bridge import CvBridge

class ControlNode(Node):

    def __init__(self,name):
        super().__init__(name)
        #初始化控制消息，设置header.frame_id
        self.action = TwistStamped()
        self.action.header.frame_id = name
        #创建控制消息发布接口
        self.pub_action = self.create_publisher(TwistStamped, "control_node/action", 10)
        #创建图像消息接收接口（消息类型，话题名称，回调函数，消息队列长度）
        self.sub_img = self.create_subscription(Image, "duckiebot_node/image", self.cb_image, 10)
        #创建图像转换工具
        self.bridge = CvBridge()
        #创建键盘事件监听器，并启动
        self.listener = keyboard.Listener(on_press=self.on_press,on_release=self.on_release)
        self.listener.start()

    #图像处理回调函数
    def cb_image(self,imgmsg):
        #ROS图像消息转化为opencv格式，第二个参数指定图像颜色编码格式
        image = self.bridge.imgmsg_to_cv2(imgmsg, 'bgr8')
        #显示图像
        cv2.imshow("image", image)
        cv2.waitKey(1)       
    #键盘按键按下事件处理，按下方向键时设定线速度和角速度数据并发布
    def on_press(self, key):
        #判断是否是方向键，只处理方向键事件
        if key == keyboard.Key.up or key == keyboard.Key.down or key == keyboard.Key.left or key == keyboard.Key.right:
            if key == keyboard.Key.up:      #上：向前
                self.action.twist.linear.x = 0.44  #设置线速度
                self.action.twist.angular.x = 0.0  #设置角速度
            elif key == keyboard.Key.down:  #下：向后
                self.action.twist.linear.x = -0.44 #设置线速度
                self.action.twist.angular.x = 0.0  #设置角速度
            elif key == keyboard.Key.left:  #左：左转
                self.action.twist.linear.x = 0.2   #设置线速度
                self.action.twist.angular.x = 1.0  #设置角速度
            elif key == keyboard.Key.right: #右：右转
                self.action.twist.linear.x = 0.2   #设置线速度
                self.action.twist.angular.x = -1.0 #设置角速度
            #设置消息时间数据
            self.action.header.stamp = self.get_clock().now().to_msg()
            #发布消息
            self.pub_action.publish(self.action)
     #键盘按键松开事件处理，松开方向键时设定线速度和角速度为0并发布
    def on_release(self, key):
         #判断是否是方向键，只处理方向键事件
        if key == keyboard.Key.up or key == keyboard.Key.down or key == keyboard.Key.left or key == keyboard.Key.right:
            self.action.twist.linear.x = 0.0
            self.action.twist.angular.x = 0.0
            self.action.header.stamp = self.get_clock().now().to_msg()
            self.pub_action.publish(self.action)

def main(args=None):
    rclpy.init(args=args)
    node = ControlNode(name="control_node")
    rclpy.spin(node=node)
    rclpy.shutdown()

实现代码完成后，可以先编译测试代码有没有错误，也可以等duckiebot节点修改完再编译测试，提前编译测试可以按以下步骤，运行时因为没有图像发布出来，不会显示图像，如需显示图像，新开一个终端，启动上一节的duckiebot节点。

返回工作空间

$ cd ~/ros2_ws
$ colcon build

或者单独编译

$ colcon build --packages-select control

设置环境变量

$ source install/setup.bash

启动节点

$ ros2 run control control_node

3、修改duckiebot节点

上一节的duckiebot节点仅仅是发布了图像话题，小车动作是随机生成的，我们现在有了动作指令话题，就需要修改duckiebot节点，实现动作指令话题数据的接收和处理，需要修改内容有以下几点：

1）配置文件

动作指令话题control_node/action发布的是geometry_msgs.msg的TwistStamped类型，duckiebot节点创建时没有设置依赖这种消息类型，需要手动添加：

2）节点源码

节点源码duckiebot_node.py中需要添加几何数据消息类型的引入，control_node/action话题的订阅以及消息处理，另外为了方便消息传递，需要定义一个action数据，详见下文源码注释：

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node

import gym
from pyglet.window import key
from gym_duckietown.envs import DuckietownEnv

import cv2
import numpy as np

from sensor_msgs.msg import Image  #发布图像使用Image消息类型
from geometry_msgs.msg import TwistStamped #控制消息类型
from cv_bridge import CvBridge #opencv和ros图像数据转换工具

class DuckiebotNode(Node):

    def __init__(self, name):
        super().__init__(name)
        self.env = DuckietownEnv(seed=1,map_name="udem1",draw_curve=False,draw_bbox=False,domain_rand=False,frame_skip=1,distortion=False,camera_rand=False,dynamics_rand=False)
        self.env.reset()
        #定义图像发布接口
        self.pub_img = self.create_publisher(Image,"duckiebot_node/image",10)
        self.timer = self.create_timer(0.05, self.timer_callback)
        #创建图像转换工具
        self.bridge = CvBridge()
        #定义全局动作变量，默认线速度和角速度都是0，车辆停止
        self.action = np.array([0.0,0.0])
        #订阅控制指令话题
        self.sub_action = self.create_subscription(TwistStamped, "control_node/action", self.cb_action, 10)

    def timer_callback(self):
        #这里不再生成随机动作指令，直接使用全局动作变量
        obs, reward, done, info = self.env.step(self.action)
        #发布图像数据，obs是rgb编码，转化时指定编码，解码时就有据可查
        self.pub_img.publish(self.bridge.cv2_to_imgmsg(obs, 'rgb8'))
        if done:
            self.env.reset()
    #控制指令处理函数
    def cb_action(self, msg):
        v = msg.twist.linear.x       #线速度
        omega = msg.twist.angular.x  #角速度
        self.action[0] = v
        self.action[1] = omega

def main(args=None):
    rclpy.init(args=args)
    node = DuckiebotNode(name="duckiebot_node")
    rclpy.spin(node=node)
    rclpy.shutdown()

4、编译并运行

返回工作空间

$ cd ~/ros2_ws

编译所有节点

$ colcon build

设置环境变量

$ source install/setup.bash

启动duckiebot节点

$ ros2 run duckiebot duckiebot_node

新建终端启动control节点

$ cd ~/ros2_ws
$ source install/setup.bash
$ ros2 run control control_node

启动后可以看到实时图像，通过方向键可以控制小车移动。