鱼香ros学习第三章话题

本节内容来自古月居ros2 21讲 个人学习笔记节点实现了机器人各种各样的功能但这些功能并不是独立的之间会有千丝万缕的联系其中最重要的一种联系方式就是话题它是节点间传递数据的桥梁。通信模型以两个机器人节点为例。A节点的功能是驱动相机这个硬件设备获取得到相机拍摄的图像信息B节点的功能是视频监控将相机拍摄到的图像实时显示给用户查看。此时从节点A到节点B传递图像数据的方式在ROS中我们就称之为话题它作为一个桥梁实现了节点之间某一个方向上的数据传输。发布/订阅模型从话题本身的实现角度来看使用了基于DDS的发布/订阅模型什么叫发布和订阅呢话题数据传输的特性是从一个节点到另外一个节点发送数据的对象称之为发布者接收数据的对象称之为订阅者每一个话题都需要有一个名字传输的数据也需要有固定的数据类型。打一个比方大家平时应该也会看微信公众号比如有一个公众号它的名字叫做“古月居”这个古月居就是话题名称公众号的发布者是古月居的小编他会把组织好的机器人知识排版成要求格式的公众号文章发布出去这个文章格式就是话题的数据类型。如果大家对这个话题感兴趣就可以订阅“古月居”成为订阅者之后自然就可以收到古月居的公众号文章没有订阅的话也就无法收到。类似这样的发布/订阅模型在生活中随处可见比如订阅报纸、订阅杂志等等。多对多通信大家再仔细想下这些可以订阅的东西是不是并不是唯一的我们每个人可以订阅很多公众号、报纸、杂志这些公众号、报纸、杂志也可以被很多人订阅没错ROS里的话题也是一样发布者和订阅者的数量并不是唯一的可以称之为是多对多的通信模型。因为话题是多对多的模型发布控制指令的摇杆可以有一个也可以有2个、3个订阅控制指令的机器人可以有1个也可以有2个、3个大家可以想象一下这个画面似乎还是挺魔性的如果存在多个发送指令的节点建议大家要注意区分优先级不然机器人可能不知道该听谁的了。异步通信话题通信还有一个特性那就是异步这个词可能有同学是第一次听说所谓异步只要是指发布者发出数据后并不知道订阅者什么时候可以收到类似古月居公众号发布一篇文章你什么时候阅读的古月居根本不知道报社发出一份报纸你什么时候收到报社也是不知道的。这就叫做异步。异步的特性也让话题更适合用于一些周期发布的数据比如传感器的数据运动控制的指令等等如果某些逻辑性较强的指令比如修改某一个参数用话题传输就不太合适了。消息接口最后既然是数据传输发布者和订阅者就得统一数据的描述格式不能一个说英文一个理解成了中文。在ROS中话题通信数据的描述格式称之为消息对应编程语言中数据结构的概念。比如这里的一个图像数据就会包含图像的长宽像素值、每个像素的RGB等等在ROS中都有标准定义。消息是ROS中的一种接口定义方式与编程语言无关我们也可以通过.msg后缀的文件自行定义有了这样的接口各种节点就像积木块一样通过各种各样的接口进行拼接组成复杂的机器人系统。在src目录下创建功能包ros2 pkg create demo_cpp_topic --build-type ament_cmake --dependencies rclcpp geometry_msgs turtlesim --license Apache-2.0模板import rclpy from rclpy.node import Node class NovelPubNode(Node): def __init__(self, node_name): super().__init__(node_name) self.get_logger().info(f{node_name},启动) def main(): rclpy.init() node NovelPubNode(novel_pub) rclpy.spin(node) rclpy.shutdown()发布者代码解析我们来看下发布者的实现方法。程序实现learning_topic/topic_helloworld_pub.py 作者: 古月居(www.guyuehome.com) 说明: ROS2话题示例-发布“Hello World”话题 import rclpy # ROS2 Python接口库 from rclpy.node import Node # ROS2 节点类 from std_msgs.msg import String # 字符串消息类型 创建一个发布者节点 class PublisherNode(Node): def __init__(self, name): super().__init__(name) # ROS2节点父类初始化 self.pub self.create_publisher(String, chatter, 10) # 创建发布者对象消息类型、话题名、队列长度 self.timer self.create_timer(0.5, self.timer_callback) # 创建一个定时器单位为秒的周期定时执行的回调函数 def timer_callback(self): # 创建定时器周期执行的回调函数 msg String() # 创建一个String类型的消息对象 msg.data Hello World # 填充消息对象中的消息数据 self.pub.publish(msg) # 发布话题消息 self.get_logger().info(Publishing: %s % msg.data) # 输出日志信息提示已经完成话题发布 def main(argsNone): # ROS2节点主入口main函数 rclpy.init(argsargs) # ROS2 Python接口初始化 node PublisherNode(topic_helloworld_pub) # 创建ROS2节点对象并进行初始化 rclpy.spin(node) # 循环等待ROS2退出 node.destroy_node() # 销毁节点对象 rclpy.shutdown() # 关闭ROS2 Python接口完成代码的编写后需要设置功能包的编译选项让系统知道Python程序的入口打开功能包的setup.py文件加入如下入口点的配置entry_points{ console_scripts: [ topic_helloworld_pub learning_topic.topic_helloworld_pub:main, ], },对以上程序进行分析如果我们想要实现一个发布者流程如下编程接口初始化创建节点并初始化创建发布者对象创建并填充话题消息发布话题消息销毁节点并关闭接口订阅者代码解析我们再来看下订阅者的实现方法。程序实现learning_topic/topic_helloworld_sub.py 作者: 古月居(www.guyuehome.com) 说明: ROS2话题示例-订阅“Hello World”话题消息 import rclpy # ROS2 Python接口库 from rclpy.node import Node # ROS2 节点类 from std_msgs.msg import String # ROS2标准定义的String消息 创建一个订阅者节点 class SubscriberNode(Node): def __init__(self, name): super().__init__(name) # ROS2节点父类初始化 self.sub self.create_subscription(String, chatter, self.listener_callback, 10) # 创建订阅者对象消息类型、话题名、订阅者回调函数、队列长度 def listener_callback(self, msg): # 创建回调函数执行收到话题消息后对数据的处理 self.get_logger().info(I heard: %s % msg.data) # 输出日志信息提示订阅收到的话题消息 def main(argsNone): # ROS2节点主入口main函数 rclpy.init(argsargs) # ROS2 Python接口初始化 node SubscriberNode(topic_helloworld_sub) # 创建ROS2节点对象并进行初始化 rclpy.spin(node) # 循环等待ROS2退出 node.destroy_node() # 销毁节点对象 rclpy.shutdown() # 关闭ROS2 Python接口对以上程序进行分析如果我们想要实现一个订阅者流程如下编程接口初始化创建节点并初始化创建订阅者对象回调函数处理话题数据销毁节点并关闭接口发布小说import rclpy from rclpy.node import Node import requests#请求库 from example_interfaces.msg import String#消息接口 from queue import Queue class NovelPubNode(Node): def __init__(self, node_name): super().__init__(node_name) self.novels_queue_ Queue() # 创建队列存放小说 # 创建话题发布者发布小说 self.novel_publisher_ self.create_publisher(String, novel, 10)# 创建发布者对象消息类型、话题名、队列长度 self.timer_ self.create_timer(5, self.timer_callback) # 创建定时器 def download_novel(self, url): response requests.get(url) response.encoding utf-8 self.get_logger().info(f下载完成{url}) for line in response.text.splitlines(): # 按行分割放入队列 self.novels_queue_.put(line) def timer_callback(self): if self.novels_queue_.qsize() 0: # 队列中有数据取出发布一行 msg String() # 实例化一个消息 msg.data self.novels_queue_.get() # 对消息结构体进行赋值 self.novel_publisher_.publish(msg) # 发布消息 self.get_logger().info(f发布了一行小说{msg.data}) def main(): rclpy.init() node NovelPubNode(novel_pub) node.download_novel(http://localhost:8000/novel1.txt) rclpy.spin(node) rclpy.shutdown()订阅接受并朗读小说import rclpy from rclpy.node import Node from example_interfaces.msg import String import threading from queue import Queue import time import espeakng class NovelSubNode(Node): def __init__(self, node_name): super().__init__(node_name) self.novels_queue_ Queue()#队列 self.novel_subscriber_ self.create_subscription(String, novel, self.novel_callback, 10)# 创建订阅者对象消息类型、话题名、订阅者回调函数、队列长度 self.speech_thread_ threading.Thread(targetself.speak_thread) self.speech_thread_.start()#启动线程 def novel_callback(self, msg):#回调函数 self.novels_queue_.put(msg.data)#小说放入队列 def speak_thread(self): speaker espeakng.Speaker() speaker.voice zh#中文 while rclpy.ok():#检测当前ros上下文是否正常 if self.novels_queue_.qsize() 0: text self.novels_queue_.get() self.get_logger().info(f正在朗读 {text}) speaker.say(text)#说 speaker.wait()#说完 else: time.sleep(1)#让当前线程休眠 def main(argsNone): rclpy.init(argsargs) node NovelSubNode(novel_read) rclpy.spin(node) rclpy.shutdown()案例二机器视觉识别在节点概念的讲解过程中我们通过一个节点驱动了相机并且实现了对红色物体的识别。功能虽然没问题但是对于机器人开发来讲并没有做到程序的模块化更好的方式是将相机驱动和视觉识别做成两个节点节点间的联系就是这个图像数据通过话题周期传输即可。一个发布图像一个订阅图像。发布者代码解析learning_topic/topic_webcam_pub.py#!/usr/bin/env python3 # -*- coding: utf-8 -*- 作者: 古月居(www.guyuehome.com) 说明: ROS2话题示例-发布图像话题 import rclpy # ROS2 Python接口库 from rclpy.node import Node # ROS2 节点类 from sensor_msgs.msg import Image # 图像消息类型 from cv_bridge import CvBridge # ROS与OpenCV图像转换类 import cv2 # Opencv图像处理库 创建一个发布者节点 class ImagePublisher(Node): def __init__(self, name): super().__init__(name) # ROS2节点父类初始化 self.publisher_ self.create_publisher(Image, image_raw, 10) # 创建发布者对象消息类型、话题名、队列长度 self.timer self.create_timer(0.1, self.timer_callback) # 创建一个定时器单位为秒的周期定时执行的回调函数 self.cap cv2.VideoCapture(0) # 创建一个视频采集对象驱动相机采集图像相机设备号 self.cv_bridge CvBridge() # 创建一个图像转换对象用于稍后将OpenCV的图像转换成ROS的图像消息 def timer_callback(self): ret, frame self.cap.read() # 一帧一帧读取图像 if ret True: # 如果图像读取成功 self.publisher_.publish( self.cv_bridge.cv2_to_imgmsg(frame, bgr8)) # 发布图像消息 self.get_logger().info(Publishing video frame) # 输出日志信息提示已经完成图像话题发布 def main(argsNone): # ROS2节点主入口main函数 rclpy.init(argsargs) # ROS2 Python接口初始化 node ImagePublisher(topic_webcam_pub) # 创建ROS2节点对象并进行初始化 rclpy.spin(node) # 循环等待ROS2退出 node.destroy_node() # 销毁节点对象 rclpy.shutdown() # 关闭ROS2 Python接口订阅者代码解析learning_topic/topic_webcam_sub.py#!/usr/bin/env python3 # -*- coding: utf-8 -*- 作者: 古月居(www.guyuehome.com) 说明: ROS2话题示例-订阅图像话题 import rclpy # ROS2 Python接口库 from rclpy.node import Node # ROS2 节点类 from sensor_msgs.msg import Image # 图像消息类型 from cv_bridge import CvBridge # ROS与OpenCV图像转换类 import cv2 # Opencv图像处理库 import numpy as np # Python数值计算库 lower_red np.array([0, 90, 128]) # 红色的HSV阈值下限 upper_red np.array([180, 255, 255]) # 红色的HSV阈值上限 创建一个订阅者节点 class ImageSubscriber(Node): def __init__(self, name): super().__init__(name) # ROS2节点父类初始化 self.sub self.create_subscription( Image, image_raw, self.listener_callback, 10) # 创建订阅者对象消息类型、话题名、订阅者回调函数、队列长度 self.cv_bridge CvBridge() # 创建一个图像转换对象用于OpenCV图像与ROS的图像消息的互相转换 def object_detect(self, image): hsv_img cv2.cvtColor(image, cv2.COLOR_BGR2HSV) # 图像从BGR颜色模型转换为HSV模型 mask_red cv2.inRange(hsv_img, lower_red, upper_red) # 图像二值化 contours, hierarchy cv2.findContours( mask_red, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE) # 图像中轮廓检测 for cnt in contours: # 去除一些轮廓面积太小的噪声 if cnt.shape[0] 150: continue (x, y, w, h) cv2.boundingRect(cnt) # 得到苹果所在轮廓的左上角xy像素坐标及轮廓范围的宽和高 cv2.drawContours(image, [cnt], -1, (0, 255, 0), 2)# 将苹果的轮廓勾勒出来 cv2.circle(image, (int(xw/2), int(yh/2)), 5, (0, 255, 0), -1) # 将苹果的图像中心点画出来 cv2.imshow(object, image) # 使用OpenCV显示处理后的图像效果 cv2.waitKey(10) def listener_callback(self, data): self.get_logger().info(Receiving video frame) # 输出日志信息提示已进入回调函数 image self.cv_bridge.imgmsg_to_cv2(data, bgr8) # 将ROS的图像消息转化成OpenCV图像 self.object_detect(image) # 苹果检测 def main(argsNone): # ROS2节点主入口main函数 rclpy.init(argsargs) # ROS2 Python接口初始化 node ImageSubscriber(topic_webcam_sub) # 创建ROS2节点对象并进行初始化 rclpy.spin(node) # 循环等待ROS2退出 node.destroy_node() # 销毁节点对象 rclpy.shutdown() # 关闭ROS2 Python接口发布速度控制海龟转圈圈import rclpy from rclpy.node import Node import requests#请求库 from example_interfaces.msg import String#消息接口 from queue import Queue from geometry_msgs.msg import Twist class NovelPubNode(Node): def __init__(self, node_name): super().__init__(node_name) # 创建话题发布者发布海龟命令 self.turtle_circle_publisher_ self.create_publisher(Twist, /turtle1/cmd_vel, 10)# 创建发布者对象消息类型、话题名、队列长度 self.timer_ self.create_timer(1, self.timer_callback) # 创建定时器 def timer_callback(self): twistTwist()#组装数据 twist.linear.x1.0 twist.angular.z0.5 self.turtle_circle_publisher_.publish(twist) # 发布消息 def main(): rclpy.init() node NovelPubNode(turtle_circle) rclpy.spin(node) rclpy.shutdown()海龟控制使一直海龟到达另一只海龟位置import rclpy from rclpy.node import Node import requests#请求库 from turtlesim.msg import Pose#消息接口 from queue import Queue from geometry_msgs.msg import Twist import numpy as np import math class TurtlePubNode(Node): def __init__(self, node_name): super().__init__(node_name) # 创建话题发布者发布海龟命令 self.turtle_control_publisher_ self.create_publisher(Twist, /turtle2/cmd_vel, 10)# 创建发布者对象消息类型、话题名、队列长度 self.turtle_control_subscriber_ self.create_subscription(Pose, /turtle2/pose, self.pose_callback, 10)# 创建订阅者对象消息类型、话题名、订阅者回调函数、队列长度 self.turtle_control2_subscriber_ self.create_subscription(Pose, /turtle1/pose, self.pose_callback2, 10)# 创建订阅者对象消息类型、话题名、订阅者回调函数、队列长度 #目标位置 self.target_x_ 5.0 #// 目标位置X,设置默认值1.0 self.target_y_ 5.0 #// 目标位置Y,设置默认值1.0 self.k_ 1.0 #// 比例系数控制输出误差*比例系数 self.max_speed_ 3.0 #// 最大线速度设置默认值3.0 def pose_callback2(self,turtle_pose2): self.target_x_ turtle_pose2.x #// 目标位置X,设置默认值1.0 self.target_y_ turtle_pose2.y #// 目标位置Y,设置默认值1.0 def pose_callback(self,turtle_pose): # 1. 记录当前位置 current_x turtle_pose.x current_y turtle_pose.y #self.get_logger().info(f当前位置:(x{current_x},y{current_y})) # 2. 计算距离目标的距离与当前海龟朝向的角度差 distance math.sqrt((self.target_x_ - current_x) ** 2 (self.target_y_ - current_y) ** 2) angle math.atan2(self.target_y_ - current_y, self.target_x_ - current_x) - turtle_pose.theta # 3. 控制策略距离大于0.1继续运动角度差大于0.2则原地旋转否则直行 twist_msg Twist() if distance 0.1: if abs(angle) 0.2: twist_msg.angular.z abs(angle) else: # 通过比例控制器计算输出速度 twist_msg.linear.x self.k_ * distance # 4. 限制最大值 if twist_msg.linear.x self.max_speed_: twist_msg.linear.x self.max_speed_ # 发布消息 self.turtle_control_publisher_.publish(twist_msg) # 发布消息 def main(): rclpy.init() node TurtlePubNode(turtle_control) rclpy.spin(node) rclpy.shutdown()自定义通信接口ros2 pkg create status_interfaces --dependencies builtin_interfaces rosidl_default_genterators --license Apache-2.0builtin_interfaces/Time stamp # 记录时间戳 string host_name # 系统名称 float32 cpu_percent # CPU使用率 float32 memory_percent # 内存使用率 float32 memory_total # 内存总量 float32 memory_available # 剩余有效内存 float64 net_sent # 网络发送数据总量 float64 net_recv # 网络接收数据总量只能用C语言去建立通信接口 见视频《ROS 2机器人开发从入门到实践》3.4.2自定义通信接口_哔哩哔哩_bilibilipython发布该自定义话题import rclpy from rclpy.node import Node from status_interfaces.msg import SystemStatus # 导入消息接口 import psutil import platform class SysStatusPub(Node): def __init__(self, node_name): super().__init__(node_name) self.status_publisher_ self.create_publisher(SystemStatus, sys_status, 10)#创建发布者 self.timer self.create_timer(1, self.timer_callback)#定时器 def timer_callback(self): cpu_percent psutil.cpu_percent()#CPU使用率 memory_info psutil.virtual_memory()#内存信息 net_io_counters psutil.net_io_counters()#网络信息 msg SystemStatus() msg.stamp self.get_clock().now().to_msg()#当前时间 msg.host_name platform.node() msg.cpu_percent cpu_percent msg.memory_percent memory_info.percent msg.memory_total memory_info.total / 1024 / 1024 msg.memory_available memory_info.available / 1024 / 1024 msg.net_sent net_io_counters.bytes_sent / 1024 / 1024 msg.net_recv net_io_counters.bytes_recv / 1024 / 1024 self.get_logger().info(f发布:{str(msg)}) self.status_publisher_.publish(msg) def main(): rclpy.init() node SysStatusPub(sys_status_pub) rclpy.spin(node) rclpy.shutdown()用python接受该话题并进行可视化显示import sys import threading import rclpy from rclpy.node import Node from PyQt5.QtWidgets import QApplication, QLabel, QVBoxLayout, QWidget from PyQt5.QtCore import Qt from status_interfaces.msg import SystemStatus class SysStatusDisplay(Node): def __init__(self): super().__init__(sys_status_display) self.subscription self.create_subscription(SystemStatus,sys_status,self.listener_callback,10)#订阅者 self.label_text QLabel(等待系统状态消息...) def listener_callback(self, msg): show_str ( f系统状态可视化显示工具\n f数 据 时 间:\t{msg.stamp.sec} s\n f用 户 名:\t{msg.host_name}\t\n fCPU使用率:\t{msg.cpu_percent} %\n f内存使用率:\t{msg.memory_percent} %\n f内存总大小:\t{msg.memory_total} MB\n f剩余有效内存:\t{msg.memory_available} MB\n f网络发送量:\t{msg.net_sent} MB\n f网络接收量:\t{msg.net_recv} MB\n f ) self.label_text.setText(show_str) self.label_text.show() def main(): app QApplication(sys.argv) rclpy.init() node SysStatusDisplay(display) def spin(): rclpy.spin(node) spin_thread threading.Thread(targetspin) spin_thread.daemon True # 确保主程序退出时线程也会退出 spin_thread.start() app.exec_()