ROS2自学笔记：话题

话题实现节点之间的通讯。话题有这样几个基本特定：
1 发布、订阅模型：发出一个话题的节点称为发布者，接受一个话题的节点称为订阅者
2 订阅者和发布者不唯一：每一个节点都可以发布或订阅话题，实现多对多通信
3 异步通信：一个话题发布后可以在之后被订阅
4 .msg文件定义通信的消息结构：话题通信有着标准的通信格式，称为消息

示例：hello world发布及订阅
发布者：

import rclpy
from rclpy.node import Node
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)

	def timer_callback(self):
		msg = String()
		msg.data = 'Hello World'
		self.pub.publish(msg)
		self.get_logger().info('Publishing: "%s"' % msg.data)

def main(args=None):
	rclpy.init(args=args)
	node = PublisherNode("topic_helloworld_pub")
	rclpy.spin(node)
	node.destroy_node()
	rclpy.shutdown()

1 from std_msgs.msg import String
导入String消息类型

2 self.pub = self.create_publisher(String, “chatter”, 10)
创建发布者对象self.pub，构造方法为create_publisher，该方法三个参数：消息类型，消息名称，缓存长度（对于无法及时发出的话题，会缓存10桢，之后删去老信息）

3 self.timer = self.create_timer(0.5, self.timer_callback)
创建计时器self.timer，参数：以秒为单位的时间，每次计时结束后执行方法

4 self.pub.publish(msg)
发布消息msg，msg为之前创建的String

5 self.get_logger().info(‘Publishing: “%s”’ % msg.data)
在终端打出信息Publishing: Hello World

订阅者：

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_subscription(String, "chatter", self.listener_callback, 10)

	def listener_callback(self, msg):
		self.get_logger().info('I heard: "%s"' % msg.data)

def main(args=None):
	rclpy.init(args=args)
	node = SubscriberNode("topic_helloworld_sub")
	rclpy.spin(node)
	node.destroy_node()
	rclpy.shutdown（）

1 self.sub=self.create_subscription(String, “chatter”, self.listener_callback, 10)
创建订阅者对象self.sub，构造方法create_subscription参数：消息类型，消息名称，收到消息后执行的函数，缓存大小

注意这里由于是异步通信，不需要考虑收到消息的时间，只要收到消息就会进行listener_callback操作（类似于中断原理）

2 self.get_logger().info(‘I heard: “%s”’ % msg.data)
在终端打出I heard: Hello World

不要忘记在setup文件声明入口

entry_points={
	'console_scripts': [
		'topic_helloworld_pub = learning_topic.topic_helloworld_pub:main',
		'topic_helloworld_sub = learning_topic.topic_helloworld_sub:main'
	]
}

话题通信流程：
发布者：
1 创建发布者对象
2 创建话题内容
3 发布话题
订阅者：
1 创建订阅者对象
2 创建回调函数
3 设置要接受的话题以创建回调函数

示例2：相机检测红色
该程序使用电脑相机检测红色，并用绿框标注
这里使用了opencv,因此先确认是否下载

apt install python3-opencv

该程序中发布者为相机程序topic_webcam_pub，订阅者为topic_webcam_sub

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2

class ImagePublisher(Node):

	def __init__(self, name):
		super().__init__(name)
		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()

	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(args=None):
	rclpy.init(args=args)
	node = ImagePublisher("topic_webcam_pub")
	rclpy.spin(node)
	node.destroy_node()
	rclpy.shutdown()

说明：
1 import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
引入必要的库，其中CvBridge是opencv和ROS2消息图片类型转换工具，Image是图片消息类型

2 self.publisher = self.create_publisher(Image, ‘image_raw’, 10)
创建发布者对象，类型为Image，名称为image_raw，缓存区为10

3 self.cap = cv2.VideoCapture(0)
获取相机图片的对象

4 self.publisher.publish(self.cv_bridge.cv2_to_imgmsg(frame, ‘bgr8’))
发布图片，这里使用cv_bridge对图片进行类型转换

5 rclpy.spin(node)
循环等待程序退出

订阅者:

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import numpy as np

lower_red = np.array([0, 90, 128])
upper_red = np.array([180, 255, 255])

class ImageSubscriber(Node):
	def __init__(self, name):
		super().__init__(name)
		self.sub = self.create_subscription(Image, 'image_raw', self.listener_callback, 10)
		self.cv_bridge = CvBridge()

	def object_detect(self, image):
		hsv_img = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
		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)
			 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)
			 cv2.waitKey(10)

	
	def listener_callback(self, data):
		self.get_logger().info('Receiving video frame')
		image = self.cv_bridge.imgmsg_to_cv2(data, 'bgr8')
		self.object_detect(image)

def main(args=None):
	rclpy.init(args=args)
	rclpy.ImageSubscriber("topic_webcam_sub")
	rclpy.spin(node)
	node.destroy_node()
	rclpy.shutdown()

1 lower_red = np.array([0, 90, 128])
upper_red = np.array([180, 255, 255])
设置颜色范围，这一范围内的颜色会被判断为红色

2 sub.sub = self.create_subscription(Image, ‘image_raw’, self.listener_callback, 10)
创建订阅者对象，接收消息类型Image，消息名称image_raw，接收消息后执行方法self.listener_callback，缓存大小10

3 hsv_img = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
将BGR图像转化为更容易处理的HSV图像

4 mask_red = cv2.inRange(hsv_img, lower_red, upper_red)
图像二值化

5 contours, hierarchy = cv2.findContours(mask_red, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE)
检测轮廓

6 if cnt.shape[0] < 150:
continue
去除过小的轮廓

7 (x, y, w, h) = cv2.boundingRect(cnt)
得到x：左上角x坐标，y:左上角y坐标，w：宽，h：高

8 cv2.drawContours(image, [cnt], -1, (0, 255, 0), 2)
画出轮廓

9 cv2.circle(image, (int(x + w / 2), int(y + h / 2), 5, (0, 255, 0), -1)
点出中心点

10 self.get_logger().info(‘Receiving video frame’)
在终端打出信息Receiving video frame

11 self.object_detect(image)
执行图像处理函数

12 rclpy.ImageSubscriber(“topic_webcam_sub”)
创建节点对象

最后别忘了声明入口

entry_points={
	'console_scripts': [
			'topic_webcam_pub	= learning_topic.topic_webcam_pub:main',
			'topic_webcam_sub	= learning_topic.topic_webcam_sub:main'
	],
}

安装USB相机驱动

apt install ros-foxy-usb-cam

启动发布者：

ros2 run usb_cam usb_cam_node_exe

启动订阅者：

ros2 run learning_topic topic_webcam_sub

执行结果如下

监测话题：
1 获取话题列表

ros2 topic list

2 获取话题信息

ros2 topic info （话题名）

3 获取话题带宽

ros2 topic bw （话题名）

4 获取话题详细信息

ros2 topic echo （话题名）

显示当前节点结构图

rqt_graph