435 lines
14 KiB
Python
Executable File
435 lines
14 KiB
Python
Executable File
#!/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")
|
||
|