add no.20
This commit is contained in:
litian.zhuang
@@ -51,7 +51,7 @@
|
||||
</node>
|
||||
</group>
|
||||
<group if="$(eval arg('camera_type_tel')=='astrapro')">
|
||||
<node pkg="move2grasp" type="grasp_pro.py" name="grasp" />
|
||||
<node pkg="move2grasp" type="grasp_pro_new.py" name="grasp" />
|
||||
</group>
|
||||
|
||||
</launch>
|
||||
|
||||
@@ -15,6 +15,7 @@
|
||||
|
||||
<arg name="camera_type_tel" value="$(arg camera_type_tel)"/>
|
||||
<arg name="lidar_type_tel" value="$(arg lidar_type_tel)"/>
|
||||
<arg name="start_camera" value="true"/>
|
||||
</include>
|
||||
|
||||
|
||||
@@ -53,7 +54,7 @@
|
||||
</node>
|
||||
</group>
|
||||
<group if="$(eval arg('camera_type_tel')=='astrapro')">
|
||||
<node pkg="move2grasp" type="grasp_pro.py" name="grasp" />
|
||||
<node pkg="move2grasp" type="grasp_pro_new.py" name="grasp" output="screen"/>
|
||||
</group>
|
||||
|
||||
</launch>
|
||||
|
||||
263
src/3rd_app/move2grasp/scripts/grasp_pro_new.py
Executable file
263
src/3rd_app/move2grasp/scripts/grasp_pro_new.py
Executable file
@@ -0,0 +1,263 @@
|
||||
#!/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")
|
||||
|
||||
@@ -33,7 +33,7 @@ def keyboardLoop():
|
||||
|
||||
#速度变量
|
||||
# 慢速
|
||||
walk_vel_ = rospy.get_param('walk_vel', 0.3)
|
||||
walk_vel_ = rospy.get_param('walk_vel', 0.2)
|
||||
# 快速
|
||||
run_vel_ = rospy.get_param('run_vel', 1.0)
|
||||
yaw_rate_ = rospy.get_param('yaw_rate', 1.0)
|
||||
@@ -72,12 +72,16 @@ def keyboardLoop():
|
||||
msg.data='1'
|
||||
grasp_pub.publish(msg)
|
||||
can_grasp=False
|
||||
speed = 0
|
||||
turn = 0
|
||||
elif ch == 'h':
|
||||
if can_release:
|
||||
msg=String()
|
||||
msg.data='0'
|
||||
grasp_pub.publish(msg)
|
||||
can_release=False
|
||||
speed = 0
|
||||
turn = 0
|
||||
elif ch == 'w':
|
||||
max_tv = walk_vel_
|
||||
speed = 1
|
||||
@@ -124,7 +128,7 @@ def keyboardLoop():
|
||||
pub.publish(cmd)
|
||||
rate.sleep()
|
||||
#停止机器人
|
||||
stop_robot()
|
||||
#stop_robot()
|
||||
|
||||
def stop_robot():
|
||||
cmd.linear.x = 0.0
|
||||
|
||||
Reference in New Issue
Block a user