感知笔记1：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到处移动。