识别图像物块
本案例使用 eye-to-hand 的模式，使用摄像头，通过 OpenCV 加载 Tensorflow 训练出来的模型数据，识别图像物块并定位图像物块在视频中的位置。通过相关点位，计算出物块相对于机械臂的空间坐标位置。为机械臂设置一套相关动作，将识别的物块放入桶中。在接下来的章节中将详细介绍整个案例的代码实现过程。
1 更新 mycobot_ai 包
为确保用户能够及时使用官方提供的最新程序包 （新用户无需更新），可通过文件管理器去到 /home/ubuntu/catkin_ws/src文件夹中，打开控制台终端(快捷键Ctrl+Alt+T) ，输入下面命令进行更新：
# 克隆github上的代码
mkdir -p ~/catkin_ws/src # 创建文件夹(若文件夹已存在则无需再创建)
cd ~/catkin_ws/src
git clone https://github.com/elephantrobotics/mycobot_ros.git # 请先查看下面的注意部分再判断是否需要执行该命令
cd ~/catkin_ws # 返回工作区
catkin_make # 构建工作区中的代码
source devel/setup.bash # 添加环境变量
注意： 若/home/ubuntu/catkin_ws/src (等效于 ~/catkin_ws/src)目录中已经存在mycobot_ros文件夹，则需要先删除原有的 mycobot_ros，再执行以上命令。其中，目录路径中的ubuntu为虚拟机的用户名，若不一致请修改。
2 摄像头调整
首先，需要使用python运行mycobot_ai包下的openVideo.py。若开启的摄像头为电脑摄像头则需要修改cap_num，具体可参考：注意事项。确保摄像头完全包含整个识别区域，且识别区域在视频中是正正方方的，如下图所示。若识别区域在视频中不符合要求，则需要调整摄像头的位置。
- 进入到目标文件夹
cd ~/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/
输入
python scripts/openVideo.py打开摄像头进行调整。
3 添加新图像
- 进入到目标功能包目录下:
cd ~/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/ - 输入
python scripts/add_img.py打开增加图像的程序。 - 根据终端输入的提示进行操作，在弹出的第二个图像框中进行图像的截取。
- 图像截取区域完成后，按下回车键，根据终端提示，输入数字(1~4)保存到相对应图像的文件夹，按下回车键即可保存至对应文件夹。
4 案例重现
M5版本：
- 使用 Ctrl+Alt+T 组合快捷键开启一个终端窗口，输入以下命令启动master节点：
roscore
- 在命令终端中键入 Ctrl+Shift+T 开启同目录下的另一个终端窗口，查看设备名称：
# 查看机械臂设备名称
ls /dev/ttyUSB* # 旧版 myCobot280 M5
# 若终端没有显示/dev/ttyUSB相关名称，则需要使用以下命令
ls /dev/ttyACM* # 新版 myCobot280 M5
- 赋予操作机械臂权限：
# 默认设备名称为/dev/ttyUSB0，若设备名称不是默认值则需修改。
sudo chmod 777 /dev/ttyUSB0 # 旧版 myCobot280 M5
sudo chmod 777 /dev/ttyACM0 # 新版 myCobot280 M5
- 输入以下命令启动launch文件。
# 设备名称不是默认值时需修改port值
roslaunch ai_mycobot_280 vision_m5.launch port:=/dev/ttyUSB0 baud:=115200 # 旧版 myCobot280 M5
roslaunch ai_mycobot_280 vision_wio.launch port:=/dev/ttyACM0 baud:=115200 # 新版 myCobot280 M5
- 使用 Ctrl+Alt+T 快捷键开启另一个终端窗口，进入机械臂相对应的目录：
cd ~/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/
- 输入
python scripts/combine_detect_obj_img_foler_opt.py打开识别图像程序，即可实现图像识别并抓取了。
树莓派版本：
- 使用 Ctrl+Alt+T 组合快捷键开启一个终端窗口，输入以下命令启动master节点：
roscore
- 在命令终端中键入 Ctrl+Shift+T 开启同目录下的另一个终端窗口，查看设备名称：
# 查看机械臂设备名称 ls /dev/ttyAMA* - 赋予机械臂权限：
# 默认设备名称为/dev/ttyAMA0，若设备名称不是默认值则需修改
sudo chmod 777 /dev/ttyAMA0
- 输入以下命令启动launch文件。
# 设备名称不是默认值时需修改port值 roslaunch ai_mycobot_280 vision_pi.launch port:=/dev/ttyAMA0 baud:=1000000 - 使用 Ctrl+Alt+T 快捷键开启另一个终端窗口，进入机械臂相对应的目录：
cd ~/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/ - 输入
python scripts/combine_detect_obj_img_foler_opt.py打开识别图像程序，即可实现图像识别并抓取了。
注意事项
- 当摄像头没有正确自动框出识别区域需要关闭程序，调整摄像头的位置，可将摄像头向左向右移动等操作。
- 若命令终端没有出现ok，且无法识别图片时，需要将摄像头稍微向后或向前移动一下，当命令终端出现ok时程序即可正常运行。
- 由于更换了识别的方式，请先确保在代码路径下有存放图片的文件夹以及需要进行识别的图片，如果没有的话，可以使用
python scripts/add_img.py添加图片。 - 由于物块中的图片物体较小，为了更好的识别图片需要将物块反放在识别区域。
- OpenCV图像识别会受环境的影响，若处在较为昏暗的环境下识别效果将大大降低。
5 代码讲解
本案例是基于OpenCV以及ROS通信控制机械臂实现的。首先，对摄像头进行校准，确保摄像头的准确性。通过识别抓取范围内的两个aruco码智能定位识别范围，并确定实际识别范围的中心点与视频像素点位之间的对应关系。使用OpenCV加载本地指定路径下的图片，通过对摄像头下的图片与指定图片进行匹配点判定，筛选并定位图片中的物体。根据物块在视频中的像素点与实际识别范围中心的视频像素点计算出物块相对于实际识别范围中心的坐标情况，之后根据实际识别范围中心与机械臂的相对坐标可计算出物块相对于机械臂的相对坐标。最后通过设计一系列动作抓取物块并放置到相应的桶中。
是否觉得跟上一个案例很相似？没错，我们只需将颜色识别模块代替为物体识别模块即可完成图片识别抓取物体的功能。
5.1 添加图像
在增加图像程序中，将图像卡片放置摄像头区域，按照提示操作，根据不同的路径文件选择，保存至对应文件夹。
def cut_photo():
path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/' # pi
path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/' # m5 "h"为自身电脑虚拟机系统用户名，可根据实际进行修改
# 判断路径选择
if os.path.exists(path1):
path = path1
elif os.path.exists(path2):
path = path2
else:
print("invalid file path! Please check whether the file path exists or modify it!")
# 获取文件夹路径及文件数量
path_red = path + '/res/red'
for i, j, k in os.walk(path_red):
file_len_red = len(k)
path_gray = path + '/res/gray'
for i, j, k in os.walk(path_gray):
file_len_gray = len(k)
path_green = path + '/res/green'
for i, j, k in os.walk(path_green):
file_len_green = len(k)
path_blue = path + '/res/blue'
for i, j, k in os.walk(path_blue):
file_len_blue = len(k)
print("请截取要识别的部分")
cut = cv2.imread(r"res/takephoto.jpeg")
cv2.imshow('original', cut)
# 选择ROI区域
roi = cv2.selectROI(windowName="original",
img=cut,
showCrosshair=False,
fromCenter=False)
x, y, w, h = roi
print(roi)
msg = """\
Image save location:
1 - Save to red folder
2 - Save to gray folder
3 - Save to green folder
4 - Save to blue folder
"""
print(msg)
kw = int(input("请输入保存图片文件夹数字编号:"))
# print(kw)
# 显示ROI并保存图片
if roi != (0, 0, 0, 0):
crop = cut[y:y + h, x:x + w]
cv2.imshow('crop', crop)
# 选择红桶文件夹
if kw == 1:
cv2.imwrite(path + '/res/red/goal{}.jpeg'.format(str(file_len_red + 1)),crop)
print('Saved')
# 选择灰桶文件夹
elif kw == 2:
cv2.imwrite(path + '/res/gray/goal{}.jpeg'.format(str(file_len_gray+1)),crop)
print('Saved')
# 选择绿桶文件夹
elif kw == 3:
cv2.imwrite(path + '/res/green/goal{}.jpeg'.format(str(file_len_green+1)),crop)
print('Saved')
# 选择蓝桶文件夹
elif kw == 4:
cv2.imwrite(path + '/res/blue/goal{}.jpeg'.format(str(file_len_blue+1)),crop)
print('Saved')
# 退出
cv2.waitKey(0)
cv2.destroyAllWindows()
5.2 物体识别
在自定义类的初始化中，通过对两张图片进行特征点匹配进行图像检测，处理检测结果并在图片中框出物块、给出相应信息。
def obj_detect(self, frame, goal):
count = 0
# 最小匹配点数
MIN_MATCH_COUNT = 10
# sift为实例化的sift函数
sift = cv2.xfeatures2d.SIFT_create()
# 找出图像中的特征点和描述
kp = []
des = []
for i in goal:
kp0, des0 = sift.detectAndCompute(i, None)
kp.append(kp0)
des.append(des0)
kp2, des2 = sift.detectAndCompute(img, None)
# FLANN 参数
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
search_params = dict(checks=50) # 传递空字典
flann = cv2.FlannBasedMatcher(index_params, search_params)
x, y = 0, 0
try:
for i in range(len(des)):
matches = flann.knnMatch(des[i], des2, k=2)
# 根据比率测试存储所有良好匹配项。
good = []
for m, n in matches:
if m.distance < 0.7*n.distance:
good.append(m)
# 当有足够的健壮匹配点对（至少个MIN_MATCH_COUNT）时
if len(good) > MIN_MATCH_COUNT:
# 获得关键点坐标
src_pts = np.float32(
[kp1[m.queryIdx].pt for m in good]).reshape(-1, 1, 2)
dst_pts = np.float32(
[kp2[m.trainIdx].pt for m in good]).reshape(-1, 1, 2)
# 利用匹配点找到CV2.RANSAC中的单应矩阵
M, mask = cv2.findHomography(
src_pts, dst_pts, cv2.RANSAC, 5.0)
matchesMask = mask.ravel().tolist()
# 获得原图像的高和宽
h, w, d = goal.shape
# 使用得到的变换矩阵对原图像的四个角进行变换,获得在目标图像上对应的坐标。
pts = np.float32(
[[0, 0], [0, h-1], [w-1, h-1], [w-1, 0]]).reshape(-1, 1, 2)
dst = cv2.perspectiveTransform(pts, M)
# 计算物体的中心点坐标
ccoord = (dst[0][0]+dst[1][0]+dst[2][0]+dst[3][0])/4.0
# 输出中心点坐标
cv2.putText(img, "{}".format(ccoord), (50, 60), fontFace=None,
fontScale=1, color=(0, 255, 0), lineType=1)
x = (dst[0][0][0]+dst[1][0][0] +
dst[2][0][0]+dst[3][0][0])/4.0
y = (dst[0][0][1]+dst[1][0][1] +
dst[2][0][1]+dst[3][0][1])/4.0
# 绘制边框
img = cv2.polylines(
img, [np.int32(dst)], True, 244, 3, cv2.LINE_AA)
else:
# 没有足够的匹配点时
if(len(good) < MIN_MATCH_COUNT):
# 输出没有匹配成功及匹配点数
count += 1
if(count % 10 == 0):
print("Not enough matches are found - %d/%d" %
(len(good), MIN_MATCH_COUNT))
matchesMask = None
except Exception as e:
pass
if x+y > 0:
return x, y
else:
return None
5.3 抓取实现模块
为机械臂的运动设计一系列的点，如机械臂的初始化点、待抓取点、蓝桶上方点、绿桶上方点等。为了能在rviz中模拟物块的移动，还为物块的运动设置了一系列点。由于rviz里面的模型坐标是以m为单位的，而机械臂坐标是以mm为单位的，所以在传递数据时需要除以1000。
# Grasping motion
def move(self, x, y, color):
# send Angle to move mycobot
print (color)
self.mc.send_angles(self.move_angles[1], 25)
time.sleep(3)
# 发送坐标移动机械臂 send coordinates to move mycobot
self.mc.send_coords([x, y, 190.6, 179.87, -3.78, -62.75], 25, 1) # usb :rx,ry,rz -173.3, -5.48, -57.9
time.sleep(3)
# self.mc.send_coords([x, y, 150, 179.87, -3.78, -62.75], 25, 0)
# time.sleep(3)
# self.mc.send_coords([x, y, 105, 179.87, -3.78, -62.75], 25, 0)
self.mc.send_coords([x, y, 60, 179.87, -3.78, -62.75], 25, 0)
time.sleep(3)
# 开启吸泵 open pump
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
self.pump_on()
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
self.gpio_status(True)
time.sleep(1.5)
tmp = []
while True:
if not tmp:
tmp = self.mc.get_angles()
else:
break
time.sleep(0.5)
# 发送角度移动机械臂
self.mc.send_angles([tmp[0], -0.71, -54.49, -23.02, -0.79, tmp[5]],25) # [18.8, -7.91, -54.49, -23.02, -0.79, -14.76]
time.sleep(5)
# 发送坐标移动机械臂至对应颜色桶上方
self.mc.send_coords(self.move_coords[color], 25, 1)
self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color]
[1]/1000.0, self.move_coords[color][2]/1000.0)
time.sleep(4)
# 关闭吸泵 close pump
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
self.pump_off()
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
self.gpio_status(False)
time.sleep(4)
self.mc.send_angles(self.move_angles[0], 25)
time.sleep(3)
5.4 吸取点位校准
当吸泵抓取木块不准时，可通过修改代码坐标进行校准，代码修改位置如下（位置1和位置2的坐标皆可修改，二选一即可）：
~/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/scripts/combine_detect_obj_img_folder_opt.py
class Object_detect(Movement):
# 位置1：调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
def __init__(self, camera_x = 160, camera_y = -5):
# inherit the parent class
super(Object_detect, self).__init__()
# get path of file
dir_path = os.path.dirname(__file__)
self.mc = None
# 移动角度
self.move_angles = [
[-7.11, -6.94, -55.01, -24.16, 0, -15], # 初始化点 init the point
[18.8, -7.91, -54.49, -23.02, -0.79, -14.76], # point to grab
]
# 移动坐标
self.move_coords = [
[120.8, -134.4, 258.0, -172.72, -5.31, -109.09], # 红桶上方点 above the red bucket
[219.8, -126.4, 249.7, -158.68, -7.93, -101.6], # 绿桶上方点 green
[124.7, 145.3, 250.4, -173.5, -2.23, -11.7], # 蓝桶上方点 blue
[14.6, 175.9, 250.4, -177.42, -0.08, 25.93], # 灰桶上方点 gray
]
# 判断连接设备:ttyUSB*为M5，ttyACM*为seeed
self.raspi = False
self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
# decide whether grab cube
def decide_move(self, x, y, color):
print(x, y, self.cache_x, self.cache_y)
# detect the cube status move or run
if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm
self.cache_x, self.cache_y = x, y
return
else:
self.cache_x = self.cache_y = 0
# 位置2：调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
self.move(x, y, color)
具体代码可直接查看程序源文件，查看代码中的注释。若对重点模块不太了解可参考上一个案例。