感知笔记：ROS 视觉- 跟随红球

- 目录 -

• 如何在 ROS 中可视化 RGB 相机。
• 如何作为机器人切换主题。
• 如何创建 blob 检测器。
• 如何获取要跟踪的颜色的颜色编码。
• 如何使用 blob 检测数据并移动 RGB 相机以跟踪 blob。

机器人技术中最常见的传感器是不起眼的 RGB 摄像头。它用于从基本颜色跟踪（blob 跟踪）到人工智能 (AI) 自动驾驶等所有领域。因此，了解这种基本的感知传感器以及如何在 ROS 中使用它至关重要。

在本单元中，您将使用 ROS 中的摄像头，并以非常粗略但有效的方式使用 OpenCV 进行 blob 跟踪。在第 2 章中，您将更深入地了解如何进行 blob 跟踪以及如何处理图像。

1.1 机器人的第一张图像

1.2 Roll Pitch Yaw

让我们开始工作吧！在上图中，您会看到 Mira 在一个房间里，房间里有一个红色板球（red-harrow-robot）。

Mira 是一个 3 自由度机器人，它的头部可以进行Roll-Pitch-Yaw运动，这对于摄像机运动来说非常容易。它是这个图像介绍的完美机器人。

对于 Mira 来说，轴略有不同，更多的是机器人技术而不是航空航天技术（在航空航天技术中，它们是倒置的）：

横滚轴运动：

俯仰轴运动：

偏航轴运动：

 您还可以使用一个脚本来自动移动红色哈罗机器人。那么，让我们移动它吧。

rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=/haro/cmd_vel

您可以使用以下基本键盘命令移动球。

现在，您将看到 Mira 所看到的内容。您将使用名为 rqt_image_view 的 ROS 图形工具，该工具允许您查看机器人中的相机正在发布的内容。

要打开该工具，请输入以下内容：

rosrun rqt_image_view rqt_image_view

屏幕上应会出现一个 rqt_image_view 应用程序窗口。

在应用程序中，选择 /mira/mira/camera1/image_raw 图像主题并等待几秒钟，直到图像源建立。您应该会看到类似于下图的内容。

1.3   颜色编码

现在，您将创建一个程序来跟踪图像中的色块。色块是图像中具有相似颜色编码的区域。第一步是获取定义要跟踪的对象的颜色编码。让我们用 red-haro-robot 来做这件事。

要获取颜色编码，我们将使用已安装的 Python 脚本，该脚本从相机接收图像并允许您移动滑块以获取所需的颜色编码值。

有两种不同的颜色编码：

RGB：它基于红-绿-蓝值的组合进行编码，范围从 0-255

HSV：它基于色相-饱和度-值进行编码，值在 0-255 之间。

我们将在这里使用 HSV，因为它往往对光照条件的变化更具鲁棒性。

在终端中启动以下命令并转到“图形界面”选项卡：

rosrun blob_tracking_v2 range_detector.py --filter HSV --preview

这将启动类似以下的 GUI：

现在你必须移动滑块，直到预览中只有red-haro-robot。请参考以下结果：

效果最佳的值应类似于以下值：

H_MIN = 0
S_MIN = 234
V_MIN = 0
H_MAX = 0
S_MAX = 255
V_MAX = 255

1.4 创建 Blob 跟踪包

现在，让我们创建一个包来启动跟踪 red-haro-robot 所需的所有软件。

• 首先，创建一个名为 my_blob_tracking_pkg 的新包，它依赖于 rospy。
• 在该包中，我们将在脚本文件夹中创建所需的脚本以使其工作。

cd ~/catkin_ws/src
catkin_create_pkg my_blob_tracking_pkg rospy cv_bridge image_transport sensor_msgs
cd ~/catkin_ws/
catkin_make
source devel/setup.bash
rospack profile

1.5 使用 OpenCV 开始 Blob 跟踪

要跟踪 blob，我们需要以下脚本：

• 访问 RGB 相机图像：mira_sensors.py
• 用于检测图像中 blob 的 blob 检测器：blob_detector.py
• 用于移动 Mira 头部的 blob 跟踪器：mira_follow_blob.py

roscd my_blob_tracking_pkg
mkdir scripts;cd scripts
# We create empty files
touch mira_sensors.py
touch blob_detector.py
touch mira_follow_blob.py
# We make all the python scripts executable
chmod +x *.py

mira_sensors.py

#!/usr/bin/env pythonimport sys
import rospy
import cv2
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Imageclass MiraSensors(object):def __init__(self, show_raw_image=False):# 初始化MiraSensors类self._show_raw_image = show_raw_image  # 是否显示原始图像self.bridge_object = CvBridge()  # 创建CvBridge对象self.camera_topic = "/mira/mira/camera1/image_raw"  # 摄像头话题self._check_cv_image_ready()  # 检查CV图像是否准备好self.image_sub = rospy.Subscriber(self.camera_topic, Image, self.camera_callback)  # 订阅摄像头图像话题def _check_cv_image_ready(self):self.cv_image = None  # 初始化图像变量while self.cv_image is None and not rospy.is_shutdown():try:# 等待获取图像消息raw_cv_image = rospy.wait_for_message("/mira/mira/camera1/image_raw", 
Image, timeout=1.0)
# 我们使用 cv_bridge，这是一个 ROS 包，可让您将 ROS 图像消息转换为 OpenCV 对象。这将打开 ROS 程序，以便使用 OpenCV 进行任何您想要的操作。然后，我们会保存最新的图像。self.cv_image = self.bridge_object.imgmsg_to_cv2(raw_cv_image, desired_encoding="bgr8")  # 转换图像格式rospy.logdebug("Current " + self.camera_topic + " READY=>")except:# 如果获取图像失败，打印错误信息rospy.logerr("Current " + self.camera_topic + " not ready yet, retrying for getting " + self.camera_topic)return self.cv_imagedef camera_callback(self, data):try:# 选择bgr8编码，因为它是OpenCV默认编码self.cv_image = self.bridge_object.imgmsg_to_cv2(data, desired_encoding="bgr8")  # 转换图像格式except CvBridgeError as e:print(e)if self._show_raw_image:# 如果需要，显示原始图像cv2.imshow("Image window", self.cv_image)cv2.waitKey(1)def get_image(self):# 这是用于访问相机上的最新图像的方法。return self.cv_image  def main():mira_sensors_object = MiraSensors()  # 创建MiraSensors对象rospy.init_node('mira_sensors_node', anonymous=True)  # 初始化ROS节点try:rospy.spin()  # 保持节点运行except KeyboardInterrupt:print("Shutting down")cv2.destroyAllWindows()  # 关闭所有OpenCV窗口if __name__ == '__main__':main()  # 运行主函数

blob_detector.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-import rospy
import cv2
import numpy as np
from mira_sensors import MiraSensors
from geometry_msgs.msg import Pointclass BlobTracker(object):def __init__(self):self.point_blob_topic = "/blob/point_blob"  # 定义发布的话题# 这个发布者使用Point消息发布# x,y: 检测到的blob中心相对于图像中心的相对位置# z: 检测到的blob的大小self.pub_blob = rospy.Publisher(self.point_blob_topic, Point, queue_size=1)def blob_detect(self,image,                  #-- 输入图像（cv标准）hsv_min,                #-- HSV滤波的最小阈值 [h_min, s_min, v_min]hsv_max,                #-- HSV滤波的最大阈值 [h_max, s_max, v_max]blur=0,                 #-- 模糊值（默认0）blob_params=None,       #-- blob参数（默认None）search_window=None,     #-- 搜索窗口 [x_min, y_min, x_max, y_max] 无量纲 (0.0到1.0)，从左上角开始imshow=False):"""blob检测函数：返回关键点和反向掩码return keypoints, reversemask"""#-- 模糊图像以去除噪声if blur > 0: image = cv2.blur(image, (blur, blur))#-- 显示结果if imshow:cv2.imshow("Blur", image)cv2.waitKey(0)#-- 搜索窗口if search_window is None: search_window = [0.0, 0.0, 1.0, 1.0]#-- 将图像从BGR转换为HSVhsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)#-- 应用HSV阈值mask = cv2.inRange(hsv, hsv_min, hsv_max)#-- 显示HSV掩码if imshow:cv2.imshow("HSV Mask", mask)#-- 膨胀使范围内区域更大mask = cv2.dilate(mask, None, iterations=2)#-- 显示膨胀后的掩码if imshow:cv2.imshow("Dilate Mask", mask)   cv2.waitKey(0)mask = cv2.erode(mask, None, iterations=2)#-- 显示腐蚀后的掩码if imshow:cv2.imshow("Erode Mask", mask)cv2.waitKey(0)#-- 使用搜索掩码裁剪图像mask = self.apply_search_window(mask, search_window)if imshow:cv2.imshow("Searching Mask", mask)cv2.waitKey(0)#-- 如果没有提供blob检测参数，则构建默认参数if blob_params is None:# 设置SimpleBlobDetector的默认参数params = cv2.SimpleBlobDetector_Params()# 修改阈值params.minThreshold = 0params.maxThreshold = 100# 按区域过滤params.filterByArea = Trueparams.minArea = 30params.maxArea = 20000# 按圆形度过滤params.filterByCircularity = Falseparams.minCircularity = 0.1# 按凸性过滤params.filterByConvexity = Falseparams.minConvexity = 0.5# 按惯性过滤params.filterByInertia = Trueparams.minInertiaRatio = 0.5else:params = blob_params     #-- 应用blob检测detector = cv2.SimpleBlobDetector_create(params)# 反转掩码：blob在白色上是黑色的reversemask = 255 - maskif imshow:cv2.imshow("Reverse Mask", reversemask)cv2.waitKey(0)keypoints = detector.detect(reversemask)return keypoints, reversemaskdef draw_keypoints(self,image,                   #-- 输入图像keypoints,               #-- CV关键点line_color=(0, 255, 0),  #-- 线的颜色 (b,g,r)imshow=False             #-- 显示结果):"""绘制检测到的blob：返回图像return(im_with_keypoints)"""#-- 将检测到的blob绘制为绿色圆圈#-- cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS 确保圆圈的大小与blob的大小相对应im_with_keypoints = cv2.drawKeypoints(image, keypoints, np.array([]), line_color, cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)if imshow:# 显示关键点cv2.imshow("Keypoints", im_with_keypoints)return im_with_keypointsdef draw_window(self,image,              #-- 输入图像window_adim,        #-- 窗口的无量纲单位color=(255, 0, 0),  #-- 线的颜色line=5,             #-- 线的厚度imshow=False        #-- 显示图像):"""绘制搜索窗口：返回图像return(image)"""rows = image.shape[0]cols = image.shape[1]x_min_px = int(cols * window_adim[0])y_min_px = int(rows * window_adim[1])x_max_px = int(cols * window_adim[2])y_max_px = int(rows * window_adim[3])  #-- 从左上角绘制一个矩形到右下角image = cv2.rectangle(image, (x_min_px, y_min_px), (x_max_px, y_max_px), color, line)if imshow:# 显示关键点cv2.imshow("Keypoints", image)return imagedef draw_frame(self,image,dimension=0.3,      #-- 相对于框架大小的维度line=2              #-- 线的厚度):"""绘制X Y坐标系return : image"""rows = image.shape[0]cols = image.shape[1]size = min([rows, cols])center_x = int(cols / 2.0)center_y = int(rows / 2.0)line_length = int(size * dimension)#-- 绘制X轴image = cv2.line(image, (center_x, center_y), (center_x + line_length, center_y), (0, 0, 255), line)#-- 绘制Y轴image = cv2.line(image, (center_x, center_y), (center_x, center_y + line_length), (0, 255, 0), line)return imagedef apply_search_window(self, image, window_adim=[0.0, 0.0, 1.0, 1.0]):"""应用搜索窗口return: image"""rows = image.shape[0]cols = image.shape[1]x_min_px = int(cols * window_adim[0])y_min_px = int(rows * window_adim[1])x_max_px = int(cols * window_adim[2])y_max_px = int(rows * window_adim[3])    #--- 初始化掩码为黑色图像mask = np.zeros(image.shape, np.uint8)#--- 复制原图像中对应于窗口的像素mask[y_min_px:y_max_px, x_min_px:x_max_px] = image[y_min_px:y_max_px, x_min_px:x_max_px]   #--- 返回掩码return maskdef blur_outside(self, image, blur=5, window_adim=[0.0, 0.0, 1.0, 1.0]):"""对搜索区域外部应用模糊"""rows = image.shape[0]cols = image.shape[1]x_min_px = int(cols * window_adim[0])y_min_px = int(rows * window_adim[1])x_max_px = int(cols * window_adim[2])y_max_px = int(rows * window_adim[3])    # 初始化掩码为黑色图像mask = cv2.blur(image, (blur, blur))# 复制原图像中对应于窗口的像素mask[y_min_px:y_max_px, x_min_px:x_max_px] = image[y_min_px:y_max_px, x_min_px:x_max_px]   return maskdef get_blob_relative_position(self, image, keyPoint):"""获取单个关键点的相机相对框架坐标return(x,y)"""rows = float(image.shape[0])cols = float(image.shape[1])center_x = 0.5 * colscenter_y = 0.5 * rowsx = (keyPoint.pt[0] - center_x) / (center_x)y = (keyPoint.pt[1] - center_y) / (center_y)return x, ydef publish_blob(self, x, y, size):blob_point = Point()blob_point.x = xblob_point.y = yblob_point.z = size self.pub_blob.publish(blob_point)if __name__ == "__main__":rospy.init_node("blob_detector_node", log_level=rospy.DEBUG)  # 初始化节点mira_sensors_obj = MiraSensors()  # 创建MiraSensors对象cv_image = mira_sensors_obj.get_image()  # 获取图像blob_detector_object = BlobTracker()  # 创建BlobTracker对象# 红色Haro的HSV限制hsv_min = (0, 234, 0)hsv_max = (0, 255, 255) # 定义检测区域 [x_min, y_min, x_max, y_max] 无量纲 (0.0到1.0)，从左上角开始window = [0.0, 0.0, 1.0, 0.9]while not rospy.is_shutdown():# 获取最新图像cv_image = mira_sensors_obj.get_image()# 检测blobkeypoints, _ = blob_detector_object.blob_detect(cv_image, hsv_min, hsv_max, blur=3, blob_params=None, search_window=window, imshow=False)# 绘制检测区域窗口cv_image = blob_detector_object.draw_window(cv_image, window)for keypoint in keypoints:x, y = blob_detector_object.get_blob_relative_position(cv_image, keypoint)blob_size = keypoint.sizeblob_detector_object.publish_blob(x, y, blob_size)# 绘制检测结果blob_detector_object.draw_keypoints(cv_image, keypoints, imshow=True)#-- 按q键退出if cv2.waitKey(1) & 0xFF == ord('q'):breakrospy.logwarn("Shutting down")    cv2.destroyAllWindows()  # 关闭所有OpenCV窗口

将斑点检测发布到主题 /blob/point_blob 中。我们不会深入讨论 OpenCV 的细节。我们只会介绍如何使用它并适应不同的颜色斑点：

mira_sensors_obj = MiraSensors()
cv_image = mira_sensors_obj.get_image()

使用之前创建的类来访问 RGB 相机记录的图像。

• 这是一个例子。值可以变化：图像的最小值为 225，但我们将其设置为 234。重要的是它适合您。

# HSV limits for RED Haro
hsv_min = (0,234,0)
hsv_max = (0, 255, 255) # We define the detection area [x_min, y_min, x_max, y_max] adimensional (0.0 to 1.0) starting from top left corner
window = [0.0, 0.0, 1.0, 0.9]

定义我们之前为跟踪红色哈罗机器人而进行的 HSV 编码的限制。如果您想跟踪其他颜色，可以在此处进行更改。

我们还为检测定义了一个窗口。这通常用于性能目的。它使用与人眼相同的技巧。眼睛的中心比眼睛的其余部分具有更多的定义，这允许更快的处理。这里也是一样。我们必须处理更少的图像，因此我们可以更快地检测。这对于机器人所需的半实时性至关重要。

# Detect blobs
keypoints, _ = blob_detector_object.blob_detect(cv_image, hsv_min, hsv_max, blur=3, blob_params=None, search_window=window, imshow=False)
# Draw window where we make detections
cv_image = blob_detector_object.draw_window(cv_image, window)for keypoint in keypoints:x , y = blob_detector_object.get_blob_relative_position(cv_image, keypoint)blob_size =  keypoint.sizeblob_detector_object.publish_blob(x,y,blob_size)# Draw Detection
blob_detector_object.draw_keypoints(cv_image, keypoints, imshow=True)

然后，我们进行斑点检测，并在原始图像、窗口和边界框上绘图，以检测斑点。

blob_detector_object.publish_blob(x,y,blob_size)

将检测结果发布到 /blob/point_blob 主题，下一个脚本将使用该主题来移动 Mira 的头部以跟随检测到的斑点。这些示例中未使用斑点的大小，但可以使用它来很好地估计与物体的距离。

mira_follow_blob.py

#!/usr/bin/env python
import time
import rospy
from math import pi, sin, cos, acos
import random
from std_msgs.msg import Float64
from sensor_msgs.msg import JointState
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Point"""
Topics To Write on:
type: std_msgs/Float64
/mira/pitch_joint_position_controller/command
/mira/roll_joint_position_controller/command
/mira/yaw_joint_position_controller/command
"""class MiraBlobFollower(object):def __init__(self, is_2D = True):rospy.loginfo("Mira Initialising Blob Follower...")self.move_rate = rospy.Rate(10)self._is_2D = is_2Dself.acceptable_error = 0.2self.current_yaw = 0.0self.twist_obj = Twist()self.pub_mira_move = rospy.Publisher('/mira/commands/velocity',  Twist, queue_size=1)self.point_blob_topic = "/blob/point_blob"self._check_cv_image_ready()rospy.Subscriber(self.point_blob_topic, Point, self.point_blob_callback)rospy.loginfo("Mira Initialising Blob Follower...")def _check_cv_image_ready(self):self.point_blob = Nonewhile self.point_blob is None and not rospy.is_shutdown():try:self.point_blob = rospy.wait_for_message(self.point_blob_topic, Point, timeout=1.0)rospy.logdebug("Current "+self.point_blob_topic+" READY=>")except:rospy.logerr("Current "+self.point_blob_topic+" not ready yet, retrying for getting "+self.point_blob_topic+"")return self.point_blobdef point_blob_callback(self, msg):if msg.x > self.acceptable_error:self.twist_obj.angular.z = -1.0elif msg.x < -1*self.acceptable_error:self.twist_obj.angular.z = 1.0else:self.twist_obj.angular.z = 0.0if msg.y > self.acceptable_error:self.twist_obj.angular.x = - 1.0elif msg.y < -1*self.acceptable_error:self.twist_obj.angular.x = 1.0else:self.twist_obj.angular.x = 0.0def loop(self):while not rospy.is_shutdown():self.pub_mira_move.publish(self.twist_obj)self.move_rate.sleep()if __name__ == "__main__":rospy.init_node('mira_follow_blob_node', anonymous=True, log_level=rospy.DEBUG)mira_jointmover_object = MiraBlobFollower()mira_jointmover_object.loop()

最后一个脚本获取 blob 检测并相应地移动 Mira 的头部。 Mira 有一个名为 /mira/commands/velocity 的主题。根据发布的 Twist 消息，机器人将移动其头部。

在这种情况下：

• 在 angular.x 中发布速度：移动滚动轴。发布正值：将头部向上移动。负值 >> 向下。
• 在 angular.z 中发布速度：移动偏航轴。发布正值：将头部向左转。负值 >> 向右转。

1.6   启动并测试 Blob 跟踪器

现在是时候看看它的实际效果了。启动 blob_tracker.py。

rosrun my_blob_tracking_pkg blob_detector.py

通过单击图形界面图标打开图形界面，这样您就可以看到斑点检测：

在第二个终端中，启动 red-haro-robot的遥控操作，并移动它，以便 Mira 可以看到它。

rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=/haro/cmd_vel

检查 blob 主题信息：

rostopic list | grep /blob/point_blob
rostopic echo /blob/point_blob

观察距离越大尺寸越小的情况。观察 x 和 y 如何根据位置变化。注意 x 和 y 位置的值是相对于图像中心的。

太棒了！现在让我们运行 mira_follow_blob.py 并看看它的表现如何：

rosrun my_blob_tracking_pkg mira_follow_blob.py

Mira 现在应该跟随red-haro-robot，从左到右，从上到下。请注意，机器人在关节处有物理限制，无法跟随red-haro-robot到处移动。