感知笔记：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 python

import 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 Image


class 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_image

    def 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 Point


class 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转换为HSV
        hsv = 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 = 0
            params.maxThreshold = 100
            
            # 按区域过滤
            params.filterByArea = True
            params.minArea = 30
            params.maxArea = 20000
            
            # 按圆形度过滤
            params.filterByCircularity = False
            params.minCircularity = 0.1
            
            # 按凸性过滤
            params.filterByConvexity = False
            params.minConvexity = 0.5
            
            # 按惯性过滤
            params.filterByInertia = True
            params.minInertiaRatio = 0.5
            
        else:
            params = blob_params     

        #-- 应用blob检测
        detector = cv2.SimpleBlobDetector_create(params)

        # 反转掩码：blob在白色上是黑色的
        reversemask = 255 - mask
        
        if imshow:
            cv2.imshow("Reverse Mask", reversemask)
            cv2.waitKey(0)
            
        keypoints = detector.detect(reversemask)

        return keypoints, reversemask

    def 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_keypoints

    def 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 image

    def 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 image

    def 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 mask
        
    def 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 mask
        

    def get_blob_relative_position(self, image, keyPoint):
        """
        获取单个关键点的相机相对框架坐标
        return(x,y)
        """
        rows = float(image.shape[0])
        cols = float(image.shape[1])
        center_x = 0.5 * cols
        center_y = 0.5 * rows
        x = (keyPoint.pt[0] - center_x) / (center_x)
        y = (keyPoint.pt[1] - center_y) / (center_y)
        return x, y
    
    def publish_blob(self, x, y, size):
        blob_point = Point()
        blob_point.x = x
        blob_point.y = y
        blob_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()
        
        # 检测blob
        keypoints, _ = 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.size
            blob_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'):
            break
    

    rospy.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.size
    blob_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_2D
        self.acceptable_error = 0.2

        self.current_yaw = 0.0
        self.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 = None
        while 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_blob
    
        
    def point_blob_callback(self, msg):
                
        if msg.x > self.acceptable_error:
            self.twist_obj.angular.z = -1.0
        elif msg.x < -1*self.acceptable_error:
            self.twist_obj.angular.z = 1.0
        else:
            self.twist_obj.angular.z = 0.0

        if msg.y > self.acceptable_error:
            self.twist_obj.angular.x = - 1.0
        elif msg.y < -1*self.acceptable_error:
            self.twist_obj.angular.x = 1.0
        else:
            self.twist_obj.angular.x = 0.0
        
        


    def 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到处移动。