- 目录 -
- 如何在 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 profile1.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 *.pymira_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.pyMira 现在应该跟随red-haro-robot，从左到右，从上到下。请注意，机器人在关节处有物理限制，无法跟随red-haro-robot到处移动。
