任意位置积木分拣代码
dofbot_garbage_yolov5功能包包含了训练好的yolov5模型及功能脚本。主要为通过yolov5进行色块的识别与定位，根据位置控制机械臂进行积木分类的代码。
“dofbot_garbage_yolov5/dofbot_garbage_yolov5/data_collect.py”为摄像头采集数据的逻辑代码。
# 打开摄像头
capture = cv.VideoCapture(0)
# 当摄像头正常打开的情况下循环执行
while capture.isOpened():
# 读取相机的每一帧
_, img = capture.read()
# 统一图像大小
img = cv.resize(img, (640, 480))
try:
write_XYT(XYT_path, [93,130], 140)
except Exception:
print("File XYT_config Error !!!")
dp = np.array([[86, 31], [18, 426], [609, 427], [537, 31]])
img = calibration.Perspective_transform(dp, img)
# ---------- Get Image Index --------------
img_idx = -1
with open("/home/HwHiAiUser/RobotArmProject/ros2_dofbot_formal_ws/src/dofbot_garbage_yolov5/dofbot_garbage_yolov5/config/data_collect_idx.txt", "r") as f:
contents = f.readline()
# print(type(contents))
img_idx = int(contents)
stored_path = "/home/HwHiAiUser/RobotArmProject/ros2_dofbot_formal_ws/src/dofbot_garbage_yolov5/dofbot_garbage_yolov5/data/" + str(img_idx) + ".png"
cv.imwrite(stored_path, img)
with open("/home/HwHiAiUser/RobotArmProject/ros2_dofbot_formal_ws/src/dofbot_garbage_yolov5/dofbot_garbage_yolov5/config/data_collect_idx.txt", "w") as f:
img_idx += 1
f.write(str(img_idx))
# ---------- Done --------------------------
break
“dofbot_garbage_yolov5/dofbot_garbage_yolov5/dofbot_config.py”为图像检测与机器视觉逻辑代码。
def calibration_map(self, image,xy=None, threshold_num=130):
'''
放置方块区域检测函数
:param image:输入图像
:return:轮廓区域边点,处理后的图像
'''
if xy!=None:
self.xy=xy
# 机械臂初始位置角度
joints_init = [self.xy[0], self.xy[1], 0, 0, 90, 0] # REVISE
# 将机械臂移动到标定方框的状态
self.arm.Arm_serial_servo_write6_array(joints_init, 500)
self.image = image
self.threshold_num = threshold_num
# 创建边点容器
dp = []
h, w = self.image.shape[:2]
# print("h",h)
# 获取轮廓点集(坐标)
contours = self.Morphological_processing()
# 遍历点集
for i, c in enumerate(contours):
# 计算轮廓区域。
area = cv.contourArea(c)
low_bound = None
high_bound = None
if self.use_other_cam:
low_bound = h * w / 2 - 40000
else:
low_bound = h * w / 2
high_bound = h * w
# 设置轮廓区域范围
if low_bound < area < high_bound:
# 计算多边形的矩
mm = cv.moments(c)
if mm['m00'] == 0:
continue
cx = mm['m10'] / mm['m00']
cy = mm['m01'] / mm['m00']
# 绘制轮廓区域
cv.drawContours(self.image, contours, i, (255, 255, 0), 2)
# 获取轮廓区域边点
dp = np.squeeze(cv.approxPolyDP(c, 100, True))
# 绘制中心
if not self.data_collect:
cv.circle(self.image, (np.int_(cx), np.int_(cy)), 5, (0, 0, 255), -1)
return dp, self.image
def Morphological_processing(self):
'''
形态学及去噪处理,并获取轮廓点集
'''
# 将图像转为灰度图
gray = cv.cvtColor(self.image, cv.COLOR_BGR2GRAY)
# 使用高斯滤镜模糊图像。
gray = cv.GaussianBlur(gray, (5, 5), 1)
# 图像二值化操作
ref, threshold = cv.threshold(gray, self.threshold_num, 255, cv.THRESH_BINARY)
# 获取不同形状的结构元素
kernel = np.ones((3, 3), np.uint8)
# 形态学开操作
if self.use_other_cam:
blur = cv.morphologyEx(threshold, cv.MORPH_OPEN, kernel, iterations=8)
else:
blur = cv.morphologyEx(threshold, cv.MORPH_OPEN, kernel, iterations=4)
# 提取模式
mode = cv.RETR_EXTERNAL
# 提取方法
method = cv.CHAIN_APPROX_NONE
# 获取轮廓点集(坐标) python2和python3在此处略有不同
# 层级关系 参数一：输入的二值图，参数二：提取模式，参数三：提取方法。
contours, hierarchy = cv.findContours(blur, mode, method)
return contours
def Perspective_transform(self, dp, image):
'''
透视变换
:param dp: 方框边点(左上,左下,右下,右上)
:param image: 原始图像
:return: 透视变换后图像
'''
if len(dp) != 4:
return
upper_left = []
lower_left = []
lower_right = []
upper_right = []
for i in range(len(dp)):
if dp[i][0]<320 and dp[i][1]<240:
upper_left=dp[i]
if dp[i][0]<320 and dp[i][1]>240:
lower_left=dp[i]
if dp[i][0]>320 and dp[i][1]>240:
lower_right=dp[i]
if dp[i][0]>320 and dp[i][1]<240:
upper_right=dp[i]
# 原图中的四个顶点
pts1 = np.float32([upper_left, lower_left, lower_right, upper_right])
# 变换后的四个顶点
pts2 = np.float32([[0, 0], [0, 480], [640, 480], [640, 0]])
# 根据四对对应点计算透视变换。
M = cv.getPerspectiveTransform(pts1, pts2)
# 将透视变换应用于图像。
Transform_img = cv.warpPerspective(image, M, (640, 480))
return Transform_img
“dofbot_garbage_yolov5/dofbot_garbage_yolov5/garbage_grap.py”为机械臂抓取与移动分拣的逻辑代码。
def move(self, joints, joints_down):
'''
移动过程
:param joints: 移动到物体位置的各关节角度
:param joints_down: 机械臂抬起各关节角度
'''
joints_uu = [90, 80, 50, 50, 265, self.grap_joint]
# 抬起
joints_up = [joints_down[0], 80, 50, 50, 265, 30]
# 移动至物体位置上方
self.arm.Arm_serial_servo_write6_array(joints_uu, 1000)
sleep(1)
# 开合夹爪
# for i in range(5):
# self.arm.Arm_serial_servo_write(6, 180, 100)
# sleep(0.08)
# self.arm.Arm_serial_servo_write(6, 30, 100)
# sleep(0.08)
# 松开夹爪
self.arm.Arm_serial_servo_write(6, 0, 500)
sleep(0.5)
# 移动至物体位置
self.arm.Arm_serial_servo_write6_array(joints, 500)
sleep(0.5)
# 进行抓取,夹紧夹爪
self.arm.Arm_serial_servo_write(6, self.grap_joint, 500)
sleep(0.5)
# 架起
self.arm.Arm_serial_servo_write6_array(joints_uu, 1000)
sleep(1)
# 抬起至对应位置上方
self.arm.Arm_serial_servo_write(1, joints_down[0], 500)
sleep(0.5)
# 抬起至对应位置
self.arm.Arm_serial_servo_write6_array(joints_down, 1000)
sleep(1)
# 释放物体,松开夹爪
self.arm.Arm_serial_servo_write(6, 30, 500)
sleep(0.5)
# 抬起
self.arm.Arm_serial_servo_write6_array(joints_up, 1000)
sleep(1)
def arm_run(self, name, joints):
'''
机械臂移动函数
:param name:识别的垃圾名称
:param joints: 反解求得的各关节角度
'''
# 有害垃圾--红色
if name == "Syringe" or name == "Used_batteries" or name == "Expired_cosmetics" or name == "Expired_tablets" and self.move_status == True:
# 此处设置,需执行完本次操作,才能向下运行
self.move_status = False
# print("有害垃圾")
# print(joints[0], joints[1], joints[2], joints[3], joints[4])
# 获得目标关节角
joints = [joints[0], joints[1], joints[2], joints[3], 265, 30]
# 移动到垃圾桶前对应姿态
# joints_down = [45, 80, 35, 40, 265, self.grap_joint]
# 移动到垃圾桶位置放下对应姿态
joints_down = [45, 50, 20, 60, 265, self.grap_joint]
# 移动
self.move(joints, joints_down)
# 移动完毕
self.move_status = True
# 可回收垃圾--蓝色
if name == "Zip_top_can" or name == "Newspaper" or name == "Old_school_bag" or name == "Book" and self.move_status == True:
self.move_status = False
# print("可回收垃圾")
# print(joints[0], joints[1], joints[2], joints[3], joints[4])
joints = [joints[0], joints[1], joints[2], joints[3], 265, 30]
# joints_down = [27, 110, 0, 40, 265, self.grap_joint]
joints_down = [27, 75, 0, 50, 265, self.grap_joint]
self.move(joints, joints_down)
self.move_status = True
# 厨余垃圾--绿色
if name == "Fish_bone" or name == "Watermelon_rind" or name == "Apple_core" or name == "Egg_shell" and self.move_status == True:
self.move_status = False
# print("厨余垃圾")
# print(joints[0], joints[1], joints[2], joints[3], joints[4])
joints = [joints[0], joints[1], joints[2], joints[3], 265, 30]
# joints_down = [152, 110, 0, 40, 265, self.grap_joint]
joints_down = [147, 75, 0, 50, 265, self.grap_joint]
self.move(joints, joints_down)
self.move_status = True
# 其他垃圾--灰色
if name == "Yellow" or name == "Cigarette_butts" or name == "Toilet_paper" or name == "Peach_pit" or name == "Disposable_chopsticks" and self.move_status == True:
self.move_status = False
# print("其他垃圾")
# print(joints[0], joints[1], joints[2], joints[3], joints[4])
joints = [joints[0], joints[1], joints[2], joints[3], 265, 30]
# joints_down = [137, 80, 35, 40, 265, self.grap_joint]
joints_down = [133, 50, 20, 60, 265, self.grap_joint]
self.move(joints, joints_down)
self.move_status = True
父主题： 代码实现