任意位置积木堆叠代码
dofbot_color_stacking功能包包含了训练好的yolov5模型及功能脚本。主要为通过yolov5进行色块的识别与定位，根据位置控制机械臂进行积木堆叠的代码。
“dofbot_color_stacking/scripts/data_collect.py”为摄像头采集积木图像数据和处理的逻辑代码。
def get_pos(self):
'''
获取识别信息
:return: 名称,位置
'''
# 复制原始图像,避免处理过程中干扰
img = self.image.copy()
pred, names, drawed_res = infer_image(img, model, labels_dict, cfg)
self.frame = drawed_res
if len(pred) >= 1:
cv.putText(self.frame, "Fix Infer Result, Waiting for Robot Arm Finish..", (30, 50), cv.FONT_HERSHEY_SIMPLEX, 0.8, (0,0,255), 2)
data = self.bridge.cv2_to_imgmsg(drawed_res, encoding="bgr8")
self.image_pub.publish(data)
pred = [pred]
msg = {}
gn = torch.tensor([640, 480, 640, 480])
if pred:
# Process detections
cgz_ctr = 0
for i, det in enumerate(pred): # detections per image
for *xyxy, conf, cls in reversed(det):
prediction_status = True
xywh = (xyxy2xywh(torch.tensor(xyxy).view(1, 4)) / gn).view(-1).tolist() # normalized xywh
label = '%s %.2f' % (names[int(cls)], conf)
# get name
name = names[int(cls)]
name = name + str(cgz_ctr)
if prediction_status:
point_x = np.int_(xywh[0] * 640)
point_y = np.int_(xywh[1] * 480)
cv.circle(self.image, (point_x, point_y), 5, (0, 0, 255), -1)
# plot_one_box(xyxy, self.image, label=label, color=colors[int(cls)], line_thickness=2)
# 计算方块在图像中的位置
(a, b) = (round(((point_x - 320) / 4000), 5), round(((480 - point_y) / 3000) * 0.8+0.19, 5))
msg[name] = (a, b)
cgz_ctr += 1
return msg
def get_Sqaure(self, color_hsv):
'''
颜色识别,获得方块的坐标
'''
(lowerb, upperb) = color_hsv
# 复制原始图像,避免处理过程中干扰
mask = self.image.copy()
# 将图像转换为HSV。
hsv = cv.cvtColor(self.image, cv.COLOR_BGR2HSV)
# 筛选出位于两个数组之间的元素。
img = cv.inRange(hsv, lowerb, upperb)
# 设置非掩码检测部分全为黑色
mask[img == 0] = [0, 0, 0]
# 获取不同形状的结构元素
kernel = cv.getStructuringElement(cv.MORPH_RECT, (5, 5))
# 形态学闭操作
dst_img = cv.morphologyEx(mask, cv.MORPH_CLOSE, kernel)
# 将图像转为灰度图
dst_img = cv.cvtColor(dst_img, cv.COLOR_RGB2GRAY)
# 图像二值化操作
ret, binary = cv.threshold(dst_img, 10, 255, cv.THRESH_BINARY)
# 获取轮廓点集(坐标) python2和python3在此处略有不同
contours, heriachy = cv.findContours(binary, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE) # 获取轮廓点集(坐标)
for i, cnt in enumerate(contours):
# boundingRect函数计算边框值，x，y是坐标值，w，h是矩形的宽和高
x, y, w, h = cv.boundingRect(cnt)
# 计算轮廓的⾯积
area = cv.contourArea(cnt)
# ⾯积限制
if area > 1000:
# 中心坐标
point_x = float(x + w / 2)
point_y = float(y + h / 2)
# 在img图像画出矩形，(x, y), (x + w, y + h)是矩形坐标，(0, 255, 0)设置通道颜色，2是设置线条粗度
cv.rectangle(self.image, (x, y), (x + w, y + h), (0, 255, 0), 2)
# 绘制矩形中心
cv.circle(self.image, (int(point_x), int(point_y)), 5, (0, 0, 255), -1)
# # 在图片中绘制结果
cv.putText(self.image, self.color_name, (int(x - 15), int(y - 15)),
cv.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 255), 2)
# 计算方块在图像中的位置
(a, b) = (round(((point_x - 320) / 4000), 5), round(((480 - point_y) / 3000) * 0.8+0.19, 5))
# print("------------------ identify up -------------------")
# print(a, b)
# print("------------------ identify down -------------------")
return (a, b)
def server_joint(self, posxy):
'''
发布位置请求,获取关节旋转角度
:param posxy: 位置点x,y坐标
:return: 每个关节旋转角度
'''
# 等待server端启动
self.client.wait_for_service()
# 创建消息包
# request = kinemaricsRequest()
request = Kinemarics.Request()
request.tar_x = posxy[0]
request.tar_y = posxy[1] + self.offset
request.kin_name = "ik"
try:
# response = self.client.call(request)
self.future = self.client.call_async(request)
rclpy.spin_until_future_complete(self.node, self.future)
response = self.future.result()
if response:
# 获得反解响应结果
joints = [0.0, 0.0, 0.0, 0.0, 0.0]
joints[0] = response.joint1
joints[1] = response.joint2
joints[2] = response.joint3
joints[3] = response.joint4
joints[4] = response.joint5
# 当逆解越界,出现负值时,适当调节.
if joints[2] < 0:
joints[1] += joints[2] * 3 / 5
joints[3] += joints[2] * 3 / 5
joints[2] = 0
# print joints
return joints
except Exception:
# rospy.loginfo("arg error")
print("arg error")
“dofbot_color_stacking/scripts/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_color_stacking/scripts/garbage_grap.py”为机械臂抓取与移动堆叠的逻辑代码。
def move(self, joints, joints_down):
'''
移动过程
:param joints: 移动到物体位置的各关节角度
:param joints_down: 机械臂堆叠各关节角度
'''
# 架起角度
joints_00 = [90, 80, 50, 50, 265, self.grap_joint]
# 抬起
joints_up = [135, 80, 50, 50, 265, 30]
# 架起
self.arm.Arm_serial_servo_write6_array(joints_00, 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, 1000)
sleep(1)
# 夹紧夹爪
self.arm.Arm_serial_servo_write(6, self.grap_joint, 500)
sleep(0.5)
# 架起
self.arm.Arm_serial_servo_write6_array(joints_00, 1000)
sleep(1)
# 移动至对应位置上方
self.arm.Arm_serial_servo_write(1, joints_down[0], 1000)
sleep(1)
# 移动至目标位置
self.arm.Arm_serial_servo_write6_array(joints_down, 1000)
sleep(1.5)
# 松开夹爪
self.arm.Arm_serial_servo_write(6, 0, 500)
sleep(0.5)
# 抬起
self.arm.Arm_serial_servo_write6_array(joints_up, 1000)
sleep(1)
def arm_run(self, move_num, joints):
'''
机械臂移动函数
:param move_num: 抓取次数
:param joints: 反解求得的各关节角度
'''
# a=[132, 50, 20, 60, 265, 100]
# b=[132, 55, 38, 38, 265, 100]
# c=[132, 60, 45, 30, 265, 100]
# d=[132, 65, 55, 20, 265, 100]
if move_num == '1' and self.move_status == True:
# 此处设置,需执行完本次操作,才能向下运行
self.move_status = False # 将设置机械臂的状态,以保证运行完此过程在执行其它
# print(joints[0], joints[1], joints[2], joints[3], joints[4])
# 获得目标关节角
joints = [joints[0], joints[1], joints[2], joints[3], 265, 30]
# 移动到目标前的姿态
joints_down = [135, 50, 20, 60, 265, self.grap_joint]
# 调用移动函数
self.move(joints, joints_down)
# 执行完此过程
self.move_status = True
if move_num == '2' and self.move_status == True:
self.move_status = False
# print joints[0], joints[1], joints[2], joints[3], joints[4]
joints = [joints[0], joints[1], joints[2], joints[3], 265, 30]
joints_down = [135, 55, 38, 38, 265, self.grap_joint]
self.move(joints, joints_down)
self.move_status = True
if move_num == '3' and self.move_status == True:
self.move_status = False
# print joints[0], joints[1], joints[2], joints[3], joints[4]
joints = [joints[0], joints[1], joints[2], joints[3], 265, 30]
joints_down = [135, 60, 45, 30, 265, self.grap_joint]
self.move(joints, joints_down)
self.move_status = True
if move_num == '4' and self.move_status == True:
self.move_status = False
# print joints[0], joints[1], joints[2], joints[3], joints[4]
joints = [joints[0], joints[1], joints[2], joints[3], 265, 30]
joints_down = [135, 65, 55, 20, 265, self.grap_joint]
self.move(joints, joints_down)
self.move_status = True
父主题： 代码实现