264 lines
8.0 KiB
Python
Executable File
264 lines
8.0 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
# -*- coding: utf-8 -*-
|
|
|
|
import os
|
|
import time
|
|
import random
|
|
import ctypes
|
|
import roslib
|
|
import rospy
|
|
import smach
|
|
import smach_ros
|
|
import threading
|
|
import string
|
|
import math
|
|
import cv2
|
|
import numpy as np
|
|
from geometry_msgs.msg import Twist
|
|
from std_msgs.msg import String
|
|
from geometry_msgs.msg import Pose, Point, Quaternion
|
|
from sensor_msgs.msg import Image
|
|
from cv_bridge import CvBridge, CvBridgeError
|
|
from spark_carry_object.msg import *
|
|
|
|
|
|
class GraspObject():
|
|
'''
|
|
监听主控，用于物品抓取功能
|
|
'''
|
|
|
|
def __init__(self):
|
|
'''
|
|
初始化
|
|
'''
|
|
|
|
global xc, yc, xc_prev, yc_prev, found_count
|
|
xc = 0
|
|
yc = 0
|
|
xc_prev = xc
|
|
yc_prev = yc
|
|
found_count = 0
|
|
self.is_found_object = False
|
|
# 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)
|
|
pos = position()
|
|
pos.x = 120
|
|
pos.y = 0
|
|
pos.z = 35
|
|
self.pub1.publish(pos)
|
|
|
|
def grasp_cp(self, msg):
|
|
if msg.data == '1':
|
|
# 订阅摄像头话题,对图像信息进行处理
|
|
self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
|
|
self.is_found_object = False
|
|
rate = rospy.Rate(10)
|
|
times=0
|
|
steps=0
|
|
while not self.is_found_object:
|
|
rate.sleep()
|
|
times+=1
|
|
# 转一圈没有发现可抓取物体,退出抓取
|
|
if steps>=5:
|
|
self.sub.unregister()
|
|
print("stop grasp\n")
|
|
status=String()
|
|
status.data='-1'
|
|
self.grasp_status_pub.publish(status)
|
|
return
|
|
# 旋转一定角度扫描是否有可供抓取的物体
|
|
if times>=30:
|
|
times=0
|
|
steps+=1
|
|
self.turn_body()
|
|
print("not found\n")
|
|
print("unregisting sub\n")
|
|
self.sub.unregister()
|
|
print("unregisted sub\n")
|
|
# 抓取检测到的物体
|
|
self.grasp()
|
|
status=String()
|
|
status.data='1'
|
|
self.grasp_status_pub.publish(status)
|
|
if msg.data=='0':
|
|
# 放下物体
|
|
self.is_found_object = False
|
|
self.release_object()
|
|
status=String()
|
|
status.data='0'
|
|
self.grasp_status_pub.publish(status)
|
|
|
|
# 执行抓取
|
|
def grasp(self):
|
|
print("start to grasp\n")
|
|
global xc, yc, found_count
|
|
# stop function
|
|
|
|
filename = os.environ['HOME'] + "/thefile.txt"
|
|
file_pix = open(filename, 'r')
|
|
s = file_pix.read()
|
|
file_pix.close()
|
|
print(s)
|
|
arr=s.split()
|
|
a1=arr[0]
|
|
a2=arr[1]
|
|
a3=arr[2]
|
|
a4=arr[3]
|
|
a = [0]*2
|
|
b = [0]*2
|
|
a[0]=float(a1)
|
|
a[1]=float(a2)
|
|
b[0]=float(a3)
|
|
b[1]=float(a4)
|
|
print('k and b value:',a[0],a[1],b[0],b[1])
|
|
r1 = rospy.Rate(0.095)
|
|
r2 = rospy.Rate(10)
|
|
pos = position()
|
|
# 物体所在坐标+标定误差
|
|
pos.x = a[0] * yc + a[1]
|
|
pos.y = b[0] * xc + b[1]
|
|
pos.z = 20
|
|
# pos.z = 20
|
|
print("z = 20\n")
|
|
self.pub1.publish(pos)
|
|
r2.sleep()
|
|
# go down -100
|
|
pos.z = -50
|
|
self.pub1.publish(pos)
|
|
print("z = -83\n")
|
|
r2.sleep()
|
|
|
|
# 开始吸取物体
|
|
self.pub2.publish(1)
|
|
r2.sleep()
|
|
|
|
# 提起物体
|
|
pos.x = 250 #160
|
|
pos.y = 0
|
|
pos.z = 150 #55
|
|
self.pub1.publish(pos)
|
|
r1.sleep()
|
|
# 使用CV检测物体
|
|
def image_cb(self, data):
|
|
global xc, yc, xc_prev, yc_prev, found_count
|
|
# change to opencv
|
|
try:
|
|
cv_image1 = CvBridge().imgmsg_to_cv2(data, "bgr8")
|
|
except CvBridgeError as e:
|
|
print('error')
|
|
|
|
# change rgb to hsv
|
|
cv_image2 = cv2.cvtColor(cv_image1, cv2.COLOR_BGR2HSV)
|
|
|
|
# 蓝色物体颜色检测范围
|
|
LowerBlue = np.array([95, 70, 60])
|
|
UpperBlue = np.array([140, 255, 255])
|
|
mask = cv2.inRange(cv_image2, LowerBlue, UpperBlue)
|
|
cv_image3 = cv2.bitwise_and(cv_image2, cv_image2, mask=mask)
|
|
|
|
# gray process
|
|
cv_image4 = cv_image3[:, :, 0]
|
|
|
|
# smooth and clean noise
|
|
blurred = cv2.blur(cv_image4, (9, 9))
|
|
(_, thresh) = cv2.threshold(blurred, 90, 255, cv2.THRESH_BINARY)
|
|
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (25, 25))
|
|
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)
|
|
|
|
# detect contour
|
|
# cv2.imshow("win1", cv_image3)
|
|
# cv2.imshow("win2", cv_image5)
|
|
cv2.waitKey(1)
|
|
contours, hier = cv2.findContours(cv_image5, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
|
|
|
|
# if find contours, pick the biggest box
|
|
if len(contours) > 0:
|
|
size = []
|
|
size_max = 0
|
|
for i, c in enumerate(contours):
|
|
rect = cv2.minAreaRect(c)
|
|
box = cv2.boxPoints(rect)
|
|
box = np.int0(box)
|
|
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.append(w * h)
|
|
if size[i] > size_max:
|
|
size_max = size[i]
|
|
index = i
|
|
xc = x_mid
|
|
yc = y_mid
|
|
# if box is not moving for 20 times
|
|
# print found_count
|
|
if found_count >= 30:
|
|
self.is_found_object = True
|
|
cmd_vel = Twist()
|
|
self.cmd_vel_pub.publish(cmd_vel)
|
|
else:
|
|
# if box is not moving
|
|
if abs(xc - xc_prev) <= 2 and abs(yc - yc_prev) <= 2:
|
|
found_count = found_count + 1
|
|
else:
|
|
found_count = 0
|
|
else:
|
|
found_count = 0
|
|
xc_prev = xc
|
|
yc_prev = yc
|
|
# 释放物体
|
|
def release_object(self):
|
|
r1 = rospy.Rate(0.15) # 5s
|
|
r2 = rospy.Rate(1) # 1s
|
|
pos = position()
|
|
# go forward
|
|
pos.x = 200
|
|
pos.y = 0
|
|
pos.z = -40 #-80
|
|
self.pub1.publish(pos)
|
|
r1.sleep()
|
|
|
|
# stop pump
|
|
self.pub2.publish(0)
|
|
r2.sleep()
|
|
#r1.sleep()
|
|
pos.x = 120
|
|
pos.y = 0
|
|
pos.z = 35
|
|
self.pub1.publish(pos)
|
|
r1.sleep()
|
|
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")
|
|
|