#encoding: UTF-8
#!/usr/bin/env python2
import cv2
import os
import numpy as np
import time
import rospy
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import Twist
from actionlib.action_client import GoalManager
from pymycobot.mycobot import MyCobot
from opencv_yolo import yolo
from VideoCapture import FastVideoCapture
import math
import basic
import argparse
class GrabParams(object):
x_bias = 0 # 在右侧抓取时调整的起始值
y_bias = 0 # 在右侧抓取时调整的起始值
height_bias = 155 # 控制高度偏移
grab_direct = "front"
coords_ready = [200, -30, 240, -175, 0, -134]
GRAB_MOVE_SPEED = 20 # 抓取移动的速度
debug = True # 调试开关
IMG_SIZE = 640 # 图像大小
done = False # 完成状态
cap_num = 2 # 捕捉编号
usb_dev = "/dev/arm" # USB设备路径
baudrate = 115200 # 通信波特率
# 创建 GrabParams 类的实例
grabParams = GrabParams()
rospy.init_node('my_node_name')
parser = argparse.ArgumentParser(description='manual to this script')
parser.add_argument("--debug", type=bool, default="True")
args = parser.parse_args()
height_bias = grabParams.height_bias
coords = [6.0, -175.7, 278.5, 88.52, 50.23, 2.81]
done = grabParams.done
CLASSES = ("apple", "clock", "banana","cat ","bird ") #[0,1,2,3,4][43.7, -233.3, 313.9, -99.55, -48.07, -170.41]
class Detect_marker(object):
def __init__(self):
super(Detect_marker, self).__init__()
self.mc = MyCobot(grabParams.usb_dev, grabParams.baudrate)
self.mc.power_on()
self.yolo = yolo()
# parameters to calculate camera clipping parameters
self.x1 = self.x2 = self.y1 = self.y2 = 0
# use to calculate coord between cube and mycobot
self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
# The coordinates of the grab center point relative to the mycobot
# The coordinates of the cube relative to the mycobot
self.c_x, self.c_y = grabParams.IMG_SIZE/2, grabParams.IMG_SIZE/2
# The ratio of pixels to actual values
self.ratio = 0.21
# Grasping motion
def move_1(self):
mc = MyCobot("/dev/arm",115200)
mc.power_on()
mc.set_color(0,0,255)#blue, arm is busy
# 抓取位置
mc.send_angles([-89.81, -47.19, 48.07, -1.23, 5.44, 130.95],40)
time.sleep(2)
mc.send_angles([-85.07, -79.01, 103.0, -27.15, -2.46, 130.78],25)
time.sleep(2)
mc.set_gripper_value(1,35)
time.sleep(1)
mc.send_angles([-90.43, -78.22,120, -15.64, 7.99, 130.78],40)
time.sleep(1)
mc.send_angles([0, -78.22,120, -15.64, 7.99, 130.78],40)
time.sleep(1)
mc.send_angles([39.99, -89.82, 55, -55.89, -4.57, -104.32],40)
done = True
print("Done")
self.mc.set_color(0,255,0)#green, arm is free
def move_2(self):
mc = MyCobot("/dev/arm",115200)
mc.power_on()
mc.set_color(0,0,255)#blue, arm is busy
mc.send_angles([-89.81, -47.19, 48.07, -1.23, 5.44, 130.95],40)
time.sleep(2)
mc.send_angles([-85.07, -79.01, 103.0, -27.15, -2.46, 130.78],25)
time.sleep(2)
mc.set_gripper_value(1,35)
time.sleep(1)
mc.send_angles([-90.43, -78.22,120, -15.64, 7.99, 130.78],40)
time.sleep(1)
mc.send_angles([0, -78.22,120, -15.64, 7.99, 130.78],40)
time.sleep(1)
mc.send_angles([-5, -93.69, 54.66, -51.5, 14.23, -104.32],40)
# 放置位置
self.mc.set_color(0,255,0)#green, arm is free
# init mycobot
def init_mycobot(self):
angles = [0, -78.22,120, -15.64, 7.99, 130.78]
self.mc.send_angles(angles,30)
basic.grap(False)
self.mc.send_angles([-89.12, -65.56, 140.36, -78.31, 4.74, 131.66],30)
def get_position(self, x, y):
# print "self.ratio: ", self.ratio
# return (-(x - self.c_x)*self.ratio), (-(y - self.c_y)*self.ratio)
wx = wy = 0
if grabParams.grab_direct == "front":
wx = (self.c_x - x) * self.ratio
wy = (self.c_y - y) * self.ratio
return wx, wy
def transform_frame(self, frame):
frame, ratio, (dw, dh) = self.yolo.letterbox(frame, (grabParams.IMG_SIZE, grabParams.IMG_SIZE))
return frame
#图像处理,适配物体识别
def transform_frame_128(self, frame):
frame, ratio, (dw, dh) = self.yolo.letterbox(frame, (128, 128))
return frame
# detect object
def obj_detect(self, img):
x=y=0
img_ori = img
img_ori = self.transform_frame(img)
img = self.transform_frame_128(img)
ima = cv2.flip(cv2.transpose(img),0)
#加载模型
net = cv2.dnn.readNetFromONNX("/home/robuster/beetle_ai/scripts/beetle_obj.onnx")
t1 = time.time()
#输入数据处理
blob = cv2.dnn.blobFromImage(img, 1 / 255.0, (128, 128), [0, 0, 0], swapRB=True, crop=False)
net.setInput(blob)
#推理
outputs = net.forward(net.getUnconnectedOutLayersNames())[0]
#获得识别结果
boxes, classes, scores = self.yolo.yolov5_post_process_simple(outputs)
t2 = time.time()
# img_0 = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
if boxes is not None:
class_index = int(classes[0]) # 假设检测到的类索引
label_detected = CLASSES[class_index].strip()
boxes = boxes*5
self.yolo.draw_single(img_ori, boxes[0], scores[0], classes[0])
left, top, right, bottom = boxes[0]
x = int((left+right)/2)
y = int((top+bottom)/2)
subt=left-right
print('x='+str(x), 'y='+str(y))
print('left-right='+str(left-right))
self.show_image(img_ori)
# Print time (inference-only)
# print("time: " + str(t2-t1) + "s")
if x+y > 0:
return x, y,label_detected,subt
else:
return None
def run(self):
self.mc.set_color(0,0,255)#blue, arm is busy
self.init_mycobot()
def show_image(self, img):
if grabParams.debug and args.debug:
cv2.imshow("figure", img)
cv2.waitKey(50)
if __name__ == "__main__":
detect = Detect_marker()
detect.run()
time.sleep(2)
cap = FastVideoCapture(2)
init_num = 0
nparams = 0
num = 0
miss = 0
counts = 20
rate = rospy.Rate(counts) # 20hz
while counts > 0 and True:
move_cmd = Twist()
pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
move_cmd.linear.x = 0.02
time.sleep(0.1)
pub.publish(move_cmd)
rate.sleep()
frame = cap.read()
time.sleep(0.1)
frame = cv2.flip(cv2.transpose(frame),0)
# counts -= 1
detect_result = detect.obj_detect(frame)
# if cv2.waitKey(1)==ord('q'):
# break
if detect_result is None:
continue
else:
x, y ,label,subt= detect_result
if abs(320-x)>30:
continue
if label == "clock" :
detect.move_1()
else:
detect.move_2()
cap.close()
break
为我解释每一行意思