ROS2中的发布/订阅模型（古月居ros2学习笔记）

关于话题通讯

节点实现了机械人各种各样的功能，但这些功能并不是独立的，他们之间会有千丝万缕的联系，其中一种方法就是基于DDS的发布订阅模型：话题（message）

话题数据传输的特性是从一个节点到另外一个节点，发送数据的对象称之为发布者，接收数据的对象称之为订阅者，每一个话题都需要有一个名字，传输的数据也需要有固定的数据类型。

特点：
1.多对多

2.异步通信

适合对象：周期发布的数据：传感器数据、运动控制指令不适合逻辑性较强的指令。(发布者和订阅者代码中都用到了回调函数)

接口方式：.msg
与编程语言无关，我们也可以通过.msg后缀的文件自行定义，有了这样的接口，各种节点就像积木块一样，通过各种各样的接口进行拼接，组成复杂的机器人系统。

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)
		self.pub=self.create_publisher(String,"chatter",10)  #创建一个发布者对象
		self.timer=self.create_timer(0.5,self.timer_callback)#创建一个定时器（周期s，回调函数）
	def timer_callback(self):
		msg=String()
		mag.data = "Hello World"
		self.pub.publish(msg)
		self.get_logger().info("Publishing:'%s'" % msg.data)
def main(args=None):               # 节点主程序main函数入口
	rclpy.init(args=args)			#初始化python接口
	node=PublisherNode("learning_topic_publisher")	#创建节点
	rclpy.spin(node)			#循环等待程序退出
	node.destroy_node()			#销毁节点
	rclpy.shutdown()			#关闭接口
	  

订阅者代码如下

import rclpy
from rclpy.node import Node 
from std_msgs.msg import String  #字符串消息类型

#创建订阅者模型
class SubscriberNode(Node):
	def __init__(self,name):
		super().__init__(name)
		self.sub=self.create_subcription(String,"chatter",self.listener_calllback,10)#创建订阅者（类型，话题名，回调函数，队列长度）
	def listener_callback(self,msg):
		self.get_logger().info("I heard: %s" % msg.data)

def main(arge=None):
	rclpy.init(args=args)
	node=SubsriberNode("learning_topic_subscriber")
	rclpy.spin(node)
	node.destroy_node()
	rclpy.shutdown()

机器视觉识别

#编写一个发布图像的发布者
import rclpy
import cv2
from cv2.bridge import CvBridge #ROS与OpenCV图像转换类
from sensor_imgs.msg import Image 
from rclpy.node import Node

class ImagePublisher(Node):
	def __init__(self,name):
		super().__init__(name)  #初始化父类节点
		self.pub=self.create_publisher(Image,"img",10)#创建一个图像发布者(数据类型，话题名称，队列长度)
		self.cam=cv2.VedioCapture(0)#创建一个相机
		self.bridge=Bridge()       #创建一个转换器
		self.timer_callback=self.create_timer(0.1,timer_callback)#创建一个回调函数
	def timer_callback(self):
		ret,frame=self.cam.read()
		if ret == Ture:
			self.pub.publish(bridge.cv2_to_imgmsg(frame,"bgr8"))     #发布图像
			self.get_logger().info("Publishing vedio frame")
def main(args=None):
	rclpy.init(args=args)#Python接口初始化
	node=ImagePublisher("topic_camera_publisher") #创建节点
	rclpy.spin(node)  #等待退出
	node.destroy_node()  #销毁节点
	rclpy.shutdown()  #关闭接口

下面是图像识别的代码

import rclpy
import cv2
from cv2.bridge import CvBridge #ROS与OpenCV图像转换类
from sensor_imgs.msg import Image 
from rclpy.node import Node
import numpy as np
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, 'img', self.listener_callback, 10)   # 创建订阅者对象（消息类型、话题名、订阅者回调函数、队列长度）
        self.cv_bridge = CvBridge()  # 创建一个图像转换对象，用于OpenCV图像与ROS的图像消息的互相转换
    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 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(x+w/2), int(y+h/2)), 5,
                       (0, 255, 0), -1)                       # 将苹果的图像中心点画出来

        cv2.imshow("object", image)                           # 使用OpenCV显示处理后的图像效果
        cv2.waitKey(10)

def main(args=None):                            # ROS2节点主入口main函数
    rclpy.init(args=args)                       # ROS2 Python接口初始化
    node = ImageSubscriber("topic_webcam_sub")  # 创建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',
         'topic_helloworld_sub  = learning_topic.topic_helloworld_sub:main',
         'topic_webcam_pub      = learning_topic.topic_webcam_pub:main',
         'topic_webcam_sub      = learning_topic.topic_webcam_sub:main',
        ],
    }

用usb相机标准驱动发布图像话题

常用的usb相机驱动一般都是通用的，ROS中也集成了usb相机的标准驱动，我们只需要通过这样一行指令，就可以安装好，无论你用什么样的相机，只要符合usb接口协议，就可以直接使用ROS中的相机驱动节点，发布标准的图像话题了。

$ sudo apt install ros-humble-usb-cam

这样，我们的代码又得到了进一步精简，刚才自己写的图像发布节点换成了这样一句指令，视觉识别节点不需要做任何变化。

$ ros2 run usb_cam usb_cam_node_exe