简介：在上一节的内容中，我们通过ROS2的话题发布功能将小车实时视频信息发布了出来，同时使用GUI工具进行查看，在这一节内容中，我们学习一下如何订阅话题并处理话题消息，具体实现的功能就是通过方向键控制小车行驶。
1、安装pynput
2、创建小车控制功能节点
3、修改duckiebot节点
1）配置文件
2）节点源码
4、编译并运行
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_node3、修改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启动后可以看到实时图像，通过方向键可以控制小车移动。
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
