20221021 grasp best
This commit is contained in:
litian.zhuang
parent
441fd1107c
commit
4f8f589640
|
|
@ -0,0 +1,434 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- coding: utf-8 -*-
|
||||
|
||||
from copy import deepcopy
|
||||
import os
|
||||
import rospy
|
||||
import math
|
||||
import cmath
|
||||
import cv2
|
||||
import numpy as np
|
||||
from geometry_msgs.msg import Twist
|
||||
from std_msgs.msg import String
|
||||
from sensor_msgs.msg import Image,LaserScan
|
||||
from cv_bridge import CvBridge, CvBridgeError
|
||||
from spark_carry_object.msg import *
|
||||
from matplotlib import pyplot as plt
|
||||
|
||||
|
||||
class GraspObject():
|
||||
'''
|
||||
监听主控，用于物品抓取功能
|
||||
'''
|
||||
image_last_time = None
|
||||
|
||||
def __init__(self):
|
||||
'''
|
||||
初始化
|
||||
'''
|
||||
self.xc = 0
|
||||
self.yc = 0
|
||||
self.xc_prev = 0
|
||||
self.yc_prev = 0
|
||||
self.found_count = 0
|
||||
self.is_have_object = False
|
||||
self.is_found_object = False
|
||||
self.can_grasp = True
|
||||
self.def_object_union = {
|
||||
'p': [],
|
||||
'theta': [],
|
||||
'w': [],
|
||||
'h': [],
|
||||
'size': [],
|
||||
'x': [],
|
||||
'y': [],
|
||||
}
|
||||
self.object_union = deepcopy(self.def_object_union)
|
||||
|
||||
filename = os.environ['HOME'] + "/thefile.txt"
|
||||
with open(filename, 'r') as f:
|
||||
s = f.read()
|
||||
arr = s.split()
|
||||
self.x_kb = [float(arr[0]), float(arr[1])]
|
||||
self.y_kb = [float(arr[2]), float(arr[3])]
|
||||
rospy.logwarn('X axia k and b value: ' + str(self.x_kb))
|
||||
rospy.logwarn('X axia k and b value: ' + str(self.y_kb))
|
||||
|
||||
self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
|
||||
# 订阅机械臂抓取指令
|
||||
self.sub2 = rospy.Subscriber(
|
||||
'/grasp', String, self.grasp_cp, queue_size=1)
|
||||
# 发布机械臂位姿
|
||||
self.pub1 = rospy.Publisher(
|
||||
'position_write_topic', position, queue_size=10)
|
||||
# 发布机械臂吸盘
|
||||
self.pub2 = rospy.Publisher('pump_topic', status, queue_size=1)
|
||||
# 发布机械臂状态
|
||||
self.grasp_status_pub = rospy.Publisher(
|
||||
'grasp_status', String, queue_size=1)
|
||||
# 发布TWist消息控制机器人底盘
|
||||
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
|
||||
self.arm_to_home()
|
||||
|
||||
# CV检测部分，负责返回识别出的方块位置
|
||||
def image_cb(self, data):
|
||||
xc = -1
|
||||
yc = -1
|
||||
# change to opencv
|
||||
try:
|
||||
cv_image1 = CvBridge().imgmsg_to_cv2(data, "bgr8")
|
||||
except CvBridgeError as e:
|
||||
print('error')
|
||||
|
||||
# 转化成HSV格式
|
||||
cv_image2 = cv2.cvtColor(cv_image1, cv2.COLOR_BGR2HSV)
|
||||
# 蓝色物体颜色检测范围
|
||||
LowerBlue = np.array([95, 90, 80])
|
||||
UpperBlue = np.array([130, 255, 255])
|
||||
# 阈值处理，蓝色部分变为白色，其他部分变为黑色
|
||||
mask = cv2.inRange(cv_image2, LowerBlue, UpperBlue)
|
||||
# 与运算，将白色部分与原来图片的蓝色部分结合
|
||||
cv_image3 = cv2.bitwise_and(cv_image2, cv_image2, mask=mask)
|
||||
# 取三维数组的第一维所有的数据
|
||||
cv_image4 = cv_image3[:, :, 0]
|
||||
# 低通滤波器，9*9滤波
|
||||
blurred = cv2.blur(cv_image4, (9, 9))
|
||||
# 简单阈值函数，处理图像灰度值，大于90的都归为255，其他归0
|
||||
(_, thresh) = cv2.threshold(blurred, 90, 255, cv2.THRESH_BINARY)
|
||||
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
|
||||
cv_image5 = cv2.morphologyEx(thresh, cv2.MORPH_CLOSE, kernel)
|
||||
cv_image5 = cv2.erode(cv_image5, None, iterations=4)
|
||||
cv_image5 = cv2.dilate(cv_image5, None, iterations=4)
|
||||
|
||||
# 找出轮廓
|
||||
contours, hier = cv2.findContours(cv_image5, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
|
||||
# 将识别出的轮廓画出来，显示在原图中
|
||||
# cv2.drawContours(cv_image1, contours, 0, (0, 255, 0), 2)
|
||||
# cv2.imshow("test",cv_image1)
|
||||
self.object_union = deepcopy(self.def_object_union)
|
||||
# if find contours, pick the biggest box
|
||||
if len(contours) == 0:
|
||||
self.is_have_object = False
|
||||
self.is_found_object = False
|
||||
self.found_count = 0
|
||||
else:
|
||||
self.is_have_object = True
|
||||
index = -1
|
||||
p_min = 10000
|
||||
for i, c in enumerate(contours):
|
||||
rect = cv2.minAreaRect(c)
|
||||
box = cv2.boxPoints(rect)
|
||||
box = np.int0(box)
|
||||
cv2.drawContours(cv_image1, [box], 0, (0, 255, 0), 2)
|
||||
|
||||
x_mid = (box[0][0] + box[2][0] + box[1][0] + box[3][0]) / 4
|
||||
y_mid = (box[0][1] + box[2][1] + box[1][1] + box[3][1]) / 4
|
||||
|
||||
w = math.sqrt((box[0][0] - box[1][0]) ** 2 + (box[0][1] - box[1][1]) ** 2)
|
||||
h = math.sqrt((box[0][0] - box[3][0]) ** 2 + (box[0][1] - box[3][1]) ** 2)
|
||||
|
||||
size = w * h
|
||||
|
||||
p, theta = cmath.polar(complex(x_mid - 320, 480 - y_mid))
|
||||
cn = cmath.rect(p, theta)
|
||||
|
||||
if p > 350:
|
||||
continue
|
||||
|
||||
self.object_union['p'].append(p)
|
||||
self.object_union['theta'].append(theta)
|
||||
self.object_union['size'].append(size)
|
||||
self.object_union['w'].append(w)
|
||||
self.object_union['h'].append(h)
|
||||
self.object_union['x'].append(x_mid)
|
||||
self.object_union['y'].append(y_mid)
|
||||
|
||||
if p < p_min:
|
||||
index = index + 1
|
||||
p_min = p
|
||||
xc = x_mid
|
||||
yc = y_mid
|
||||
|
||||
# if box is not moving for 20 times
|
||||
# print found_count
|
||||
if self.found_count >= 30:
|
||||
self.found_count = 0
|
||||
self.is_found_object = True
|
||||
self.xc = xc
|
||||
self.yc = yc
|
||||
else:
|
||||
# if box is not moving
|
||||
if index == -1:
|
||||
# rospy.logwarn("No object eligible for grasp")
|
||||
self.found_count = 0
|
||||
self.is_found_object = False
|
||||
elif abs(xc - self.xc_prev) <= 4 and abs(yc - self.yc_prev) <= 4:
|
||||
# print(xc, " ", yc)
|
||||
cn = cmath.rect(self.object_union['p'][index], self.object_union['theta'][index])
|
||||
# cv2.line(cv_image1, (320, 480), (int(320 + cn.real), int(480 - cn.imag)), (255, 0, 0), 2)
|
||||
self.found_count = self.found_count + 1
|
||||
else:
|
||||
self.found_count = 0
|
||||
self.xc_prev = xc
|
||||
self.yc_prev = yc
|
||||
|
||||
cv2.imshow("source", cv_image1)
|
||||
# cv2.imshow("gray", cv_image5)
|
||||
cv2.waitKey(1)
|
||||
|
||||
self.image_last_time = rospy.get_rostime()
|
||||
|
||||
def grasp_cp(self, msg):
|
||||
# 判断收到的抓取或放的信息
|
||||
if msg.data == '0':
|
||||
print("收到抓取信号")
|
||||
times=0
|
||||
steps=0
|
||||
if self.is_found_object:
|
||||
if self.can_grasp:
|
||||
print("现在允许抓取")
|
||||
if self.grasp():
|
||||
print("已成功抓取")
|
||||
return
|
||||
else:
|
||||
# 抓取不成功，将允许重新抓取
|
||||
self.can_grasp = True
|
||||
return
|
||||
else:
|
||||
print("抓取执行中，不允许重复抓取")
|
||||
return
|
||||
|
||||
# 转一圈没有发现可抓取物体,退出抓取
|
||||
else:
|
||||
vel = Twist()
|
||||
vel.angular.z = 0.2
|
||||
rate = rospy.Rate(10)
|
||||
|
||||
# 判断视野范围内有无方块
|
||||
while not self.is_have_object:
|
||||
rate.sleep()
|
||||
times = times + 1
|
||||
if (times > math.pi * 2 / vel.angular.z * 10):
|
||||
rospy.logerr("can not found object!!! Waiting next clicked_point...")
|
||||
# self.grasp_status_pub.publish(String('0'))
|
||||
return
|
||||
self.cmd_vel_pub.publish(vel)
|
||||
|
||||
times = 0
|
||||
while not self.is_found_object:
|
||||
rate.sleep()
|
||||
times = times + 1
|
||||
if (times > 30):
|
||||
rospy.logerr("object founded but can not location it!!! Waiting next clicked_point...")
|
||||
# self.grasp_status_pub.publish(String('0'))
|
||||
return
|
||||
|
||||
# print("unregisting sub\n")
|
||||
# self.sub.unregister()
|
||||
|
||||
# 抓取检测到的物体
|
||||
if self.grasp():
|
||||
print("已成功抓取")
|
||||
else:
|
||||
# 抓取不成功，将允许重新抓取
|
||||
self.can_grasp = True
|
||||
|
||||
elif msg.data=='1':
|
||||
# 放下物体
|
||||
self.is_found_object = False
|
||||
self.release_object(1)
|
||||
elif msg.data=='2':
|
||||
# 放下物体
|
||||
self.is_found_object = False
|
||||
self.release_object(2)
|
||||
elif msg.data=='3':
|
||||
# 放下物体
|
||||
self.is_found_object = False
|
||||
self.release_object(3)
|
||||
|
||||
# 执行抓取
|
||||
def grasp(self):
|
||||
# 设置是否允许抓取值为False，避免抓取过程中重复收到抓取的信号
|
||||
self.can_grasp = False
|
||||
print("现在开始抓取\n")
|
||||
# 设置睡眠时间
|
||||
r1 = rospy.Rate(0.1)
|
||||
r2 = rospy.Rate(10)
|
||||
|
||||
pos = position()
|
||||
|
||||
# 物体所在坐标+标定误差
|
||||
# 先到目标位置上方
|
||||
pos.x = self.x_kb[0] * self.yc + self.x_kb[1]
|
||||
pos.y = self.y_kb[0] * self.xc + self.y_kb[1]
|
||||
pos.z = 20
|
||||
self.pub1.publish(pos)
|
||||
r2.sleep()
|
||||
|
||||
# 开始往下放
|
||||
pos.z = -50
|
||||
self.pub1.publish(pos)
|
||||
r2.sleep()
|
||||
|
||||
# 开始吸取物体，pub2 1为吸取，0为释放
|
||||
self.pub2.publish(1)
|
||||
r2.sleep()
|
||||
|
||||
# 提起物体到检查位置
|
||||
pos.x = 250
|
||||
pos.y = 0
|
||||
pos.z = 70
|
||||
self.pub1.publish(pos)
|
||||
r2.sleep()
|
||||
|
||||
# 利用雷达检查是否有方块
|
||||
if self.check_grasp_state():
|
||||
print("检测到抓取成功")
|
||||
pos.x = 220 #160
|
||||
pos.y = 0
|
||||
pos.z = 160
|
||||
self.pub1.publish(pos)
|
||||
r2.sleep()
|
||||
return True
|
||||
else:
|
||||
print("检测到抓取失败")
|
||||
self.arm_to_home()
|
||||
return False
|
||||
|
||||
|
||||
def arm_to_home(self):
|
||||
pos = position()
|
||||
|
||||
pos.x = 140
|
||||
pos.y = 0
|
||||
pos.z = 60
|
||||
self.pub1.publish(pos)
|
||||
rospy.sleep(1)
|
||||
|
||||
pos.x = 120
|
||||
pos.y = 0
|
||||
pos.z = 35
|
||||
self.pub1.publish(pos)
|
||||
rospy.sleep(2)
|
||||
|
||||
|
||||
# 检查是否成功抓取，成功返回True，反之返回False
|
||||
def check_grasp_state(self):
|
||||
|
||||
self.success_grasp_num = 0
|
||||
scan_num = 0
|
||||
# 读十次雷达数据，10hz
|
||||
while scan_num < 60:
|
||||
|
||||
scan_msg = rospy.wait_for_message("/scan",LaserScan,timeout=None)
|
||||
|
||||
# 计算激光束数量,num值为1817
|
||||
num = int((scan_msg.angle_max-scan_msg.angle_min)/scan_msg.angle_increment)
|
||||
|
||||
# 输入想要检测的角度与范围，机器人正后方为起始点
|
||||
expect_angle = 180
|
||||
ranges = 20
|
||||
test_direction = int(expect_angle/360*num)
|
||||
|
||||
detect_num = 0 # 已经检测的次数
|
||||
sum = 0 # 数据总和
|
||||
|
||||
# 计算检测区域的距离值
|
||||
for i in range(test_direction-ranges,test_direction+ranges):
|
||||
# 排除检测距离值有出现0值的情况
|
||||
if scan_msg.ranges[i] != 0:
|
||||
sum = scan_msg.ranges[i]+sum
|
||||
detect_num +=1
|
||||
# print("scan_msg.ranges[",i,"]=",scan_msg.ranges[i])
|
||||
|
||||
# 避免检测距离值出现均为0导致计算报错
|
||||
if detect_num == 0:
|
||||
continue
|
||||
|
||||
# 取距离值,抓取成功值约为0.26——0.32,单位为米
|
||||
averlaser = sum/detect_num
|
||||
print("averlaser=",averlaser)
|
||||
|
||||
# 判断是否成功抓取
|
||||
if averlaser < 0.35:
|
||||
self.success_grasp_num += 1
|
||||
# 判断成功抓取次数超过3次，返回true
|
||||
if self.success_grasp_num >= 3:
|
||||
self.success_grasp_num = 0
|
||||
return True
|
||||
print("scan_num=",scan_num)
|
||||
scan_num += 1
|
||||
return False
|
||||
|
||||
|
||||
|
||||
# 释放物体
|
||||
def release_object(self,height):
|
||||
r1 = rospy.Rate(1) # 1s
|
||||
pos = position()
|
||||
# go forward
|
||||
if height == 1:
|
||||
pos.x = 220
|
||||
pos.y = 0
|
||||
pos.z = -40 #-80
|
||||
self.pub1.publish(pos)
|
||||
r1.sleep()
|
||||
|
||||
if height == 2:
|
||||
pos.x = 220
|
||||
pos.y = 0
|
||||
pos.z = 80 #-80
|
||||
self.pub1.publish(pos)
|
||||
r1.sleep()
|
||||
|
||||
if height == 3:
|
||||
pos.x = 220
|
||||
pos.y = 0
|
||||
pos.z = 160 #-80
|
||||
self.pub1.publish(pos)
|
||||
r1.sleep()
|
||||
|
||||
# stop pump
|
||||
self.pub2.publish(0)
|
||||
rospy.sleep(1)
|
||||
|
||||
if height == 3:
|
||||
pos.x = 220
|
||||
pos.y = 80
|
||||
pos.z = 160
|
||||
print("避开第三层方块")
|
||||
self.pub1.publish(pos)
|
||||
rospy.sleep(2)
|
||||
|
||||
pos.x = 120
|
||||
pos.y = 150
|
||||
pos.z = 35
|
||||
self.pub1.publish(pos)
|
||||
rospy.sleep(1)
|
||||
|
||||
self.arm_to_home()
|
||||
# 释放后，修改成能够成功抓取
|
||||
self.can_grasp = True
|
||||
|
||||
return 'succeeded'
|
||||
|
||||
# 转动机器人到一定角度
|
||||
def turn_body(self):
|
||||
cmd_vel = Twist()
|
||||
cmd_vel.angular.z = 0.25
|
||||
rate = rospy.Rate(10)
|
||||
for i in range(40):
|
||||
self.cmd_vel_pub.publish(cmd_vel)
|
||||
rate.sleep()
|
||||
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
try:
|
||||
rospy.init_node('GraspObject', anonymous=False)
|
||||
rospy.loginfo("Init GraspObject main")
|
||||
GraspObject()
|
||||
rospy.spin()
|
||||
except rospy.ROSInterruptException:
|
||||
rospy.loginfo("End spark GraspObject main")
|
||||
|
||||
Loading…
Reference in New Issue