100字范文 > ROS2学习笔记（四）-- 用方向键控制小车行走

ROS2学习笔记（四）-- 用方向键控制小车行走

时间：2023-07-01 01:35:14

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

1、安装pynput

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

3、修改duckiebot节点

1）配置文件

2）节点源码

4、编译并运行

1、安装pynput

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

$ pip3 install pynput

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

from pynput import keyboarddef on_press(key):print('{0} pressed'.format(key))def on_release(key):print('{0} released'.format(key))if key == keyboard.Key.esc:return Falsewith 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 python3import rclpyfrom rclpy.node import Nodeimport cv2import numpy as npfrom pynput import keyboard #引入键盘监听功能库from sensor_msgs.msg import Image from geometry_msgs.msg import TwistStampedfrom cv_bridge import CvBridgeclass ControlNode(Node):def __init__(self,name):super().__init__(name)#初始化控制消息，设置header.frame_idself.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.0self.action.twist.angular.x = 0.0self.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 python3import rclpyfrom rclpy.node import Nodeimport gymfrom pyglet.window import keyfrom gym_duckietown.envs import DuckietownEnvimport cv2import numpy as npfrom 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] = vself.action[1] = omegadef main(args=None):rclpy.init(args=args)node = DuckiebotNode(name="duckiebot_node")rclpy.spin(node=node)rclpy.shutdown()