From 282ecb34ddfc0e5115939c09a72d2026224ced5b Mon Sep 17 00:00:00 2001 From: weijian Date: Fri, 2 Dec 2022 17:21:10 +0800 Subject: [PATCH] update mycobot_communication/scripts/mycobot_services.py --- .../ai_mecharm_270/scripts/color_test.py | 164 ------ .../scripts/advance_detect_obj_color.py | 480 --------------- .../ai_mycobot_280/scripts/aikit_img.py | 555 ----------------- .../ai_mycobot_280/scripts/encode_test.py | 248 -------- .../ai_mypalletizer_260/scripts/aikit_img.py | 557 ------------------ .../scripts/mycobot_services.py | 57 +- 6 files changed, 55 insertions(+), 2006 deletions(-) delete mode 100644 mycobot_ai/ai_mecharm_270/scripts/color_test.py delete mode 100644 mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_color.py delete mode 100644 mycobot_ai/ai_mycobot_280/scripts/aikit_img.py delete mode 100644 mycobot_ai/ai_mycobot_280/scripts/encode_test.py delete mode 100644 mycobot_ai/ai_mypalletizer_260/scripts/aikit_img.py diff --git a/mycobot_ai/ai_mecharm_270/scripts/color_test.py b/mycobot_ai/ai_mecharm_270/scripts/color_test.py deleted file mode 100644 index a21e333..0000000 --- a/mycobot_ai/ai_mecharm_270/scripts/color_test.py +++ /dev/null @@ -1,164 +0,0 @@ -# ecoding=utf-8 -import cv2 -import numpy as np -from imutils import contours -import math - -'''HSV中的颜色空间 -https://blog.csdn.net/wsp_1138886114/article/details/80660014 -''' -color_dist = {'red': {'Lower': np.array([0, 120, 120]), 'Upper': np.array([6, 255, 255])}, - # 'blue': {'Lower': np.array([100, 80, 46]), 'Upper': np.array([124, 255, 255])}, - # 'blue':{'Lower': np.array([100,43,46]),'Upper': np.array([124,255,255])}, - 'blue':{'Lower': np.array([100,43,46]),'Upper': np.array([124,255,255])}, - "cyan": {'Lower': np.array([78, 43, 46]),'Upper': np.array([99, 255, 255])}, - 'green': {'Lower': np.array([35, 43, 35]), 'Upper': np.array([90, 255, 255])}, - 'yellow': {"Lower": np.array([22, 93, 0]), "Upper": np.array([45, 255, 255])}, - # 'orange': {"Lower": np.array([0, 100, 45]), "Upper":np.array([255, 250, 255])}, - 'orange': {"Lower": np.array([11, 43, 46]), "Upper":np.array([25, 255, 255])}, - } - -cap = cv2.VideoCapture(0) -# cap = cv2.VideoCapture("src/test_1_2.mp4") -cap.set(3,420) -cap.set(4,360) - -cv2.namedWindow('camera', cv2.WINDOW_AUTOSIZE) -# 内核 -kernel = np.ones((5, 5), np.uint8) - -while cap.isOpened(): - ret, frame = cap.read() - if ret: - if frame is not None: - - gs_frame = cv2.GaussianBlur(frame, (5, 5), 0)# 高斯模糊 - hsv = cv2.cvtColor(gs_frame, cv2.COLOR_BGR2HSV) # 转化成HSV图像 - erode_hsv = cv2.erode(hsv, None, iterations=2) # 腐蚀 作用是粗的变细 - for _color in color_dist: - _font_x_pos = 0 - _font_y_pos = 0 - ## 颜色阈值识别 - inRange_hsv = cv2.inRange(erode_hsv, color_dist[_color]['Lower'], color_dist[_color]['Upper']) - # 膨胀操作 - dilation = cv2.dilate(inRange_hsv, kernel, iterations=1) - # 闭操作 - closing = cv2.morphologyEx(dilation, cv2.MORPH_CLOSE, kernel) - # 边缘检测 - edges = cv2.Canny(closing, 10, 20) - # 检测物体边框 - cnts, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) - - # 判断轮廓数量也就是判断是否寻找到轮廓，如果没有找到轮廓就不继续进行操作 - if len(cnts) > 0: - for cnt in cnts: - if cv2.contourArea(cnt) >1500: - peri = cv2.arcLength(cnt,True) - #print(peri) - # 用于获得轮廓的近似值，使用cv2.drawCountors进行画图操作 ， - # 参数说明：cnt为输入的轮廓值， epsilon为阈值T，通常使用轮廓的周长作为阈值，True表示的是轮廓是闭合的 - ''' - cv2.approxPolyDP - @:param - cnt: - epsilon :算法参数 - True：表示是否闭合 - ''' - approx = cv2.approxPolyDP(cnt,0.02*peri,True) - # print(len(approx)) - # 提取拐点 - ''' - 返回列表元素，列表中的元素代表一个边沿信息。 - ''' - objCor = len(approx) - - if objCor ==3: - '''https://blog.51cto.com/hiszm/5201991''' - objectType = "Triangle" - mm = cv2.moments(cnt) - cx = int(mm['m10']/mm['m00']) - cy = int(mm['m01']/mm['m00']) - _font_x_pos = cx - _font_y_pos = cy - cv2.circle(frame,(cx,cy),3,(0,0,255),-1) - cv2.drawContours(frame, [cnt], 0, (0, 0, 255), 3) - print("三角形的坐标",cx,cy) - - - # 检测出是矩形 - elif objCor == 4: - # 可旋转矩形，即最小的外包矩形 - rect= cv2.minAreaRect(cnt) - ''' - @:param - 函数 cv2.minAreaRect() 返回一个Box2D结构 rect：（最小外接矩形的中心（x，y），（宽度，高度），旋转角度）。 - 分别对应于返回值：(rect[0][0], rect[0][1]), (rect[1][0], rect[1][1]), rect[2] - ''' - # 中心点坐标 - pos_x = int(rect[0][0]) - pos_y = int(rect[0][1]) - #print("position",pos_x,pos_y) - # 旋转角度 - theta = np.round(cv2.minAreaRect(cnt)[2],2) - box = cv2.boxPoints(rect) - box = np.int0(box) - print("物体的坐标为",(pos_x,pos_y),"旋转角度为",theta) - # 判断长方形还是正方形 - _font_x_pos = box[0][0] - _font_y_pos = box[0][1] - _W = math.sqrt(math.pow((box[0][0] - box[1][0]), 2) + math.pow((box[0][1] - box[1][1]), 2)) - _H = math.sqrt(math.pow((box[0][0] - box[3][0]), 2) + math.pow((box[0][1] - box[3][1]), 2)) - # 长宽比 - aspRatio = _W/float(_H) - if aspRatio >0.98 and aspRatio <1.03: - - objectType= "Square"#正方形 - else: - objectType="Rectangle"#长方形 - _pos = (pos_x,pos_y) - cv2.circle(frame,_pos,1,(0,0,255),2)#绘制中心点 - #cv2.putText(frame, 'teheta = ' + str(theta), (int(_font_x_pos),int(_font_y_pos+20)), cv2.FONT_HERSHEY_COMPLEX_SMALL,0.8, (0, 255, 0) ) - cv2.drawContours(frame,[box],0,(0,0,255),2) - # 圆形 - elif objCor>4: - objectType= "Circles" - ''' - void minEnclosingCircle(InputArray points, Point2f& center, float& radius) - @:param - points：输入信息，可以为包含点的容器(vector)或是Mat。 - center：包覆圆形的圆心。 - radius：包覆圆形的半径。 - ''' - (x,y),radius = cv2.minEnclosingCircle(cnt) - center = (int(x),int(y)) - radius = int(radius) - _font_x_pos = center[0] - _font_y_pos = center[1] - print("物体的圆心为:",(_font_x_pos, _font_y_pos),"半径为",radius) - cv2.circle(frame,center,1,(255,0,0),2)#绘制中心点 - cv2.circle(frame,center,radius,(255,0,0),2) - - else: - objectType="None" - # objectType 物体形状 - print("颜色类别：",_color) - cv2.putText(frame, str(_color + objectType), (int(_font_x_pos),int(_font_y_pos)), cv2.FONT_HERSHEY_COMPLEX_SMALL,1, (0, 255, 0) ) - - cv2.imshow('camera', frame) - # 存储识别结果视频 - # out.write(frame) - - cv2.waitKey(1) - if cv2.waitKey(10) & 0xFF == 27: - break - else: - print("无画面") - break - else: - print("无法读取摄像头！") - break - -cap.release() -# out.release() -cv2.waitKey(0) -cv2.destroyAllWindows() diff --git a/mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_color.py b/mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_color.py deleted file mode 100644 index da893b8..0000000 --- a/mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_color.py +++ /dev/null @@ -1,480 +0,0 @@ -# encoding:utf-8 -#!/usr/bin/env python2 - -import cv2 -import numpy as np -import time -import os,sys -import rospy -from visualization_msgs.msg import Marker -from pymycobot.mycobot import MyCobot -from moving_utils import Movement - -IS_CV_4 = cv2.__version__[0] == '4' -__version__ = "1.0" -# Adaptive seeed - - -class Object_detect(Movement): - - def __init__(self, camera_x = 155, camera_y = 10): - # inherit the parent class - super(Object_detect, self).__init__() - # get path of file - dir_path = os.path.dirname(__file__) - self.mc = None - - # 移动角度 - self.move_angles = [ - [-7.11, -6.94, -55.01, -24.16, 0, -15], # init the point - [18.8, -7.91, -54.49, -23.02, -0.79, -14.76], # point to grab - ] - - # 移动坐标 - self.move_coords = [ - [132.2, -136.9, 200.8, -178.24, -3.72, -107.17], # above the red bucket - [238.8, -124.1, 204.3, -169.69, -5.52, -96.52], # green - [115.8, 177.3, 210.6, 178.06, -0.92, -6.11], # blue - [-6.9, 173.2, 201.5, 179.93, 0.63, 33.83], # gray - ] - # which robot: USB* is m5; ACM* is wio; AMA* is raspi - self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1] - self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1] - self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1] - self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1] - self.raspi = False - if "dev" in self.robot_m5: - self.Pin = [2, 5] - elif "dev" in self.robot_wio: - # self.Pin = [20, 21] - self.Pin = [2, 5] - - for i in self.move_coords: - i[2] -= 20 - elif "dev" in self.robot_raspi or "dev" in self.robot_jes: - import RPi.GPIO as GPIO - GPIO.setwarnings(False) - self.GPIO = GPIO - GPIO.setmode(GPIO.BCM) - GPIO.setup(20, GPIO.OUT) - GPIO.setup(21, GPIO.OUT) - GPIO.output(20, 1) - GPIO.output(21, 1) - self.raspi = True - if self.raspi: - self.gpio_status(False) - - # choose place to set cube - self.color = 0 - # parameters to calculate camera clipping parameters - self.x1 = self.x2 = self.y1 = self.y2 = 0 - # set cache of real coord - self.cache_x = self.cache_y = 0 - # set color HSV - self.HSV = { - "yellow": [np.array([11, 115, 70]), np.array([40, 255, 245])], - "red": [np.array([0, 43, 46]), np.array([8, 255, 255])], - "green": [np.array([35, 43, 35]), np.array([90, 255, 255])], # [77, 255, 255] - "blue": [np.array([100, 43, 46]), np.array([124, 255, 255])], - "cyan": [np.array([89, 43, 46]), np.array([99, 255, 255])], # np.array([78, 43, 46]), np.array([99, 255, 255]) - } - # 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 - self.camera_x, self.camera_y = camera_x, camera_y - # The coordinates of the cube relative to the mycobot - self.c_x, self.c_y = 0, 0 - # The ratio of pixels to actual values - self.ratio = 0 - # Get ArUco marker dict that can be detected. - self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250) - # Get ArUco marker params. - self.aruco_params = cv2.aruco.DetectorParameters_create() - - # init a node and a publisher - rospy.init_node("marker", anonymous=True) - self.pub = rospy.Publisher('/cube', Marker, queue_size=1) - # init a Marker - self.marker = Marker() - self.marker.header.frame_id = "/joint1" - self.marker.ns = "cube" - self.marker.type = self.marker.CUBE - self.marker.action = self.marker.ADD - self.marker.scale.x = 0.04 - self.marker.scale.y = 0.04 - self.marker.scale.z = 0.04 - self.marker.color.a = 1.0 - self.marker.color.g = 1.0 - self.marker.color.r = 1.0 - - # marker position initial - self.marker.pose.position.x = 0 - self.marker.pose.position.y = 0 - self.marker.pose.position.z = 0.03 - self.marker.pose.orientation.x = 0 - self.marker.pose.orientation.y = 0 - self.marker.pose.orientation.z = 0 - self.marker.pose.orientation.w = 1.0 - - # publish marker - def pub_marker(self, x, y, z=0.03): - self.marker.header.stamp = rospy.Time.now() - self.marker.pose.position.x = x - self.marker.pose.position.y = y - self.marker.pose.position.z = z - self.marker.color.g = self.color - self.pub.publish(self.marker) - - def gpio_status(self, flag): - if flag: - # self.GPIO.output(20, 0) - self.GPIO.output(21, 0) - else: - # self.GPIO.output(20, 1) - self.GPIO.output(21, 1) - - # 开启吸泵 m5 - def pump_on(self): - # 让2号位工作 - # self.mc.set_basic_output(2, 0) - # 让5号位工作 - self.mc.set_basic_output(5, 0) - - # 停止吸泵 m5 - def pump_off(self): - # 让2号位停止工作 - # self.mc.set_basic_output(2, 1) - # 让5号位停止工作 - self.mc.set_basic_output(5, 1) - # Grasping motion - def move(self, x, y, color): - # send Angle to move mycobot - print (color) - self.mc.send_angles(self.move_angles[1], 25) - time.sleep(3) - - # send coordinates to move mycobot - self.mc.send_coords([x, y, 190.6, 179.87, -3.78, -62.75], 25, 1) # usb :rx,ry,rz -173.3, -5.48, -57.9 - time.sleep(3) - - # self.mc.send_coords([x, y, 150, 179.87, -3.78, -62.75], 25, 0) - # time.sleep(3) - - self.mc.send_coords([x, y, 103, 179.87, -3.78, -62.75], 25, 0) - time.sleep(3) - - # open pump - if "dev" in self.robot_m5 or "dev" in self.robot_wio: - self.pump_on() - elif "dev" in self.robot_raspi or "dev" in self.robot_jes: - self.gpio_status(True) - time.sleep(1.5) - - tmp = [] - while True: - if not tmp: - tmp = self.mc.get_angles() - else: - break - time.sleep(0.5) - - # print(tmp) - self.mc.send_angles([tmp[0], -0.71, -54.49, -23.02, -0.79, tmp[5]],25) # [18.8, -7.91, -54.49, -23.02, -0.79, -14.76] - time.sleep(2.5) - - self.pub_marker( - self.move_coords[2][0]/1000.0, self.move_coords[2][1]/1000.0, self.move_coords[2][2]/1000.0) - - self.mc.send_coords(self.move_coords[color], 25, 1) - self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color] - [1]/1000.0, self.move_coords[color][2]/1000.0) - time.sleep(3) - - # close pump - if "dev" in self.robot_m5 or "dev" in self.robot_wio: - self.pump_off() - elif "dev" in self.robot_raspi or "dev" in self.robot_jes: - self.gpio_status(False) - time.sleep(4) - - if color == 1: - self.pub_marker( - self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02) - elif color == 0: - self.pub_marker( - self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0) - # self.pub_angles(self.move_angles[0], 20) - self.mc.send_angles(self.move_angles[0], 25) - time.sleep(3) - - # decide whether grab cube - def decide_move(self, x, y, color): - print(x, y, self.cache_x, self.cache_y) - # detect the cube status move or run - if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm - self.cache_x, self.cache_y = x, y - return - else: - self.cache_x = self.cache_y = 0 - # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动 - self.move(x, y, color) - - # init mycobot - def run(self): - if "dev" in self.robot_wio : - self.mc = MyCobot(self.robot_wio, 115200) - elif "dev" in self.robot_m5: - self.mc = MyCobot(self.robot_m5, 115200) - elif "dev" in self.robot_raspi: - self.mc = MyCobot(self.robot_raspi, 1000000) - if not self.raspi: - self.pub_pump(False, self.Pin) - self.mc.send_angles([-7.11, -6.94, -55.01, -24.16, 0, -15], 20) - time.sleep(3) - - # draw aruco - - def draw_marker(self, img, x, y): - # draw rectangle on img - cv2.rectangle( - img, - (x - 20, y - 20), - (x + 20, y + 20), - (0, 255, 0), - thickness=2, - lineType=cv2.FONT_HERSHEY_COMPLEX, - ) - # add text on rectangle - cv2.putText(img, "({},{})".format(x, y), (x, y), - cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (243, 0, 0), 2,) - - # get points of two aruco - def get_calculate_params(self, img): - # Convert the image to a gray image - gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) - # Detect ArUco marker. - corners, ids, rejectImaPoint = cv2.aruco.detectMarkers( - gray, self.aruco_dict, parameters=self.aruco_params - ) - - """ - Two Arucos must be present in the picture and in the same order. - There are two Arucos in the Corners, and each aruco contains the pixels of its four corners. - Determine the center of the aruco by the four corners of the aruco. - """ - if len(corners) > 0: - if ids is not None: - if len(corners) <= 1 or ids[0] == 1: - return None - x1 = x2 = y1 = y2 = 0 - point_11, point_21, point_31, point_41 = corners[0][0] - x1, y1 = int((point_11[0] + point_21[0] + point_31[0] + point_41[0]) / 4.0), int( - (point_11[1] + point_21[1] + point_31[1] + point_41[1]) / 4.0) - point_1, point_2, point_3, point_4 = corners[1][0] - x2, y2 = int((point_1[0] + point_2[0] + point_3[0] + point_4[0]) / 4.0), int( - (point_1[1] + point_2[1] + point_3[1] + point_4[1]) / 4.0) - return x1, x2, y1, y2 - return None - - # set camera clipping parameters - def set_cut_params(self, x1, y1, x2, y2): - self.x1 = int(x1) - self.y1 = int(y1) - self.x2 = int(x2) - self.y2 = int(y2) - print(self.x1, self.y1, self.x2, self.y2) - - # set parameters to calculate the coords between cube and mycobot - def set_params(self, c_x, c_y, ratio): - self.c_x = c_x - self.c_y = c_y - self.ratio = 220.0/ratio - - # calculate the coords between cube and mycobot - def get_position(self, x, y): - return ((y - self.c_y)*self.ratio + self.camera_x), ((x - self.c_x)*self.ratio + self.camera_y) - - """ - Calibrate the camera according to the calibration parameters. - Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times. - If two ARuco values have been calculated, clip the video. - """ - - def transform_frame(self, frame): - # enlarge the image by 1.5 times - fx = 1.5 - fy = 1.5 - frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy, - interpolation=cv2.INTER_CUBIC) - if self.x1 != self.x2: - # the cutting ratio here is adjusted according to the actual situation - frame = frame[int(self.y2*0.78):int(self.y1*1.1), - int(self.x1*0.86):int(self.x2*1.08)] - return frame - - # detect cube color - def color_detect(self, img): - # set the arrangement of color'HSV - x = y = 0 - gs_img = cv2.GaussianBlur(img, (3, 3), 0) # 高斯模糊 - # transfrom the img to model of gray - hsv = cv2.cvtColor(gs_img, cv2.COLOR_BGR2HSV) - - for mycolor, item in self.HSV.items(): - redLower = np.array(item[0]) - redUpper = np.array(item[1]) - # wipe off all color expect color in range - mask = cv2.inRange(hsv, item[0], item[1]) - # a etching operation on a picture to remove edge roughness - erosion = cv2.erode(mask, np.ones((1, 1), np.uint8), iterations=2) - # the image for expansion operation, its role is to deepen the color depth in the picture - dilation = cv2.dilate(erosion, np.ones( - (1, 1), np.uint8), iterations=2) - - - # adds pixels to the image - target = cv2.bitwise_and(img, img, mask=dilation) - # the filtered image is transformed into a binary image and placed in binary - ret, binary = cv2.threshold(dilation, 127, 255, cv2.THRESH_BINARY) - # get the contour coordinates of the image, where contours is the coordinate value, here only the contour is detected - contours, hierarchy = cv2.findContours( - dilation, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - - if len(contours) > 0: - # do something about misidentification - boxes = [ - box - for box in [cv2.boundingRect(c) for c in contours] - if min(img.shape[0], img.shape[1]) / 10 - < min(box[2], box[3]) - < min(img.shape[0], img.shape[1]) / 1 - ] - if boxes: - for box in boxes: - x, y, w, h = box - # find the largest object that fits the requirements - c = max(contours, key=cv2.contourArea) - # get the lower left and upper right points of the positioning object - x, y, w, h = cv2.boundingRect(c) - # locate the target by drawing rectangle - cv2.rectangle(img, (x, y), (x+w, y+h), (153, 153, 0), 2) - # calculate the rectangle center - x, y = (x*2+w)/2, (y*2+h)/2 - # calculate the real coordinates of mycobot relative to the target - if mycolor == "red": - self.color = 0 - - elif mycolor == "green": - self.color = 1 - - elif mycolor == "cyan" or mycolor == "blue": - self.color = 2 - - else: - self.color = 3 - - - if abs(x) + abs(y) > 0: - return x, y - else: - return None - - -if __name__ == "__main__": - # open the camera - cap_num = 0 - cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L) - - if not cap.isOpened(): - cap.open() - # init a class of Object_detect - detect = Object_detect() - # init mycobot - detect.run() - - _init_ = 20 # - init_num = 0 - nparams = 0 - num = 0 - real_sx = real_sy = 0 - while cv2.waitKey(1) < 0: - # read camera - _, frame = cap.read() - # deal img - frame = detect.transform_frame(frame) - if _init_ > 0: - _init_ -= 1 - continue - # calculate the parameters of camera clipping - if init_num < 20: - if detect.get_calculate_params(frame) is None: - cv2.imshow("figure", frame) - continue - else: - x1, x2, y1, y2 = detect.get_calculate_params(frame) - detect.draw_marker(frame, x1, y1) - detect.draw_marker(frame, x2, y2) - detect.sum_x1 += x1 - detect.sum_x2 += x2 - detect.sum_y1 += y1 - detect.sum_y2 += y2 - init_num += 1 - continue - elif init_num == 20: - detect.set_cut_params( - (detect.sum_x1)/20.0, - (detect.sum_y1)/20.0, - (detect.sum_x2)/20.0, - (detect.sum_y2)/20.0, - ) - detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0 - init_num += 1 - continue - - # calculate params of the coords between cube and mycobot - if nparams < 10: - if detect.get_calculate_params(frame) is None: - cv2.imshow("figure", frame) - continue - else: - x1, x2, y1, y2 = detect.get_calculate_params(frame) - detect.draw_marker(frame, x1, y1) - detect.draw_marker(frame, x2, y2) - detect.sum_x1 += x1 - detect.sum_x2 += x2 - detect.sum_y1 += y1 - detect.sum_y2 += y2 - nparams += 1 - continue - elif nparams == 10: - nparams += 1 - # calculate and set params of calculating real coord between cube and mycobot - detect.set_params( - (detect.sum_x1+detect.sum_x2)/20.0, - (detect.sum_y1+detect.sum_y2)/20.0, - abs(detect.sum_x1-detect.sum_x2)/10.0 + - abs(detect.sum_y1-detect.sum_y2)/10.0 - ) - print ("ok") - continue - - # get detect result - detect_result = detect.color_detect(frame) - if detect_result is None: - cv2.imshow("figure", frame) - continue - else: - x, y = detect_result - # calculate real coord between cube and mycobot - real_x, real_y = detect.get_position(x, y) - if num == 20: - detect.pub_marker(real_sx/20.0/1000.0, real_sy/20.0/1000.0) - detect.decide_move(real_sx/20.0, real_sy/20.0, detect.color) - num = real_sx = real_sy = 0 - - else: - num += 1 - real_sy += real_y - real_sx += real_x - - cv2.imshow("figure", frame) diff --git a/mycobot_ai/ai_mycobot_280/scripts/aikit_img.py b/mycobot_ai/ai_mycobot_280/scripts/aikit_img.py deleted file mode 100644 index f071a8a..0000000 --- a/mycobot_ai/ai_mycobot_280/scripts/aikit_img.py +++ /dev/null @@ -1,555 +0,0 @@ -# encoding=utf-8 -from multiprocessing import Process, Pipe -from cgi import parse -from difflib import restore -# import queue -from sys import path -from tokenize import Pointfloat -from turtle import color -# from typing_extensions import Self -import cv2 -import numpy as np -import time -import json -import os,sys - -from PIL import Image -from threading import Thread -from pymycobot.mypalletizer import MyPalletizer - -IS_CV_4 = cv2.__version__[0] == '4' -__version__ = "1.0" # Adaptive seeed - - -class Object_detect(): - - def __init__(self, camera_x = 160, camera_y = 10): - # inherit the parent class - super(Object_detect, self).__init__() - - # declare mypal260 - self.mc = None - # 移动角度 - self.move_angles = [ - [0, 0, 0, 0], # init the point - [-29.0, 5.88, -4.92, -76.28], # point to grab - [17.4, -10.1, -87.27, 5.8, -2.02, 15], # point to grab - ] - - # 移动坐标 - self.move_coords = [ - [132.6, -155.6, 211.8, -20.9], # above the red bucket - [232.5, -134.1, 197.7, -45.26], # above the green bucket - [111.6, 159, 221.5, -120], # above the blue bucket - [-15.9, 164.6, 217.5, -119.35], # above the gray bucket - ] - - # choose place to set cube - self.color = 0 - # parameters to calculate camera clipping parameters - self.x1 = self.x2 = self.y1 = self.y2 = 0 - # set cache of real coord - self.cache_x = self.cache_y = 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 - self.camera_x, self.camera_y = camera_x, camera_y - # The coordinates of the cube relative to the mycobot - self.c_x, self.c_y = 0, 0 - # The ratio of pixels to actual values - self.ratio = 0 - # Get ArUco marker dict that can be detected. - self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250) - # Get ArUco marker params. - self.aruco_params = cv2.aruco.DetectorParameters_create() - - # 开启吸泵 m5 - def pump_on(self): - # 让2号位工作 - # self.mc.set_basic_output(2, 0) - # 让5号位工作 - self.mc.set_basic_output(5, 0) - - # 停止吸泵 m5 - def pump_off(self): - # 让2号位停止工作 - # self.mc.set_basic_output(2, 1) - # 让5号位停止工作 - self.mc.set_basic_output(5, 1) - - # Grasping motion - def move(self, x, y, color): - # send Angle to move mypal260 - self.mc.send_angles(self.move_angles[0], 20) - time.sleep(3) - - # send coordinates to move mypal260 根据不同底板机械臂，调整吸泵高度 - self.mc.send_coords([x, y, 160, 0], 20, 0) - time.sleep(1.5) - self.mc.send_coords([x, y, 110, 0], 20, 0) - time.sleep(1.5) - - # open pump - self.pump_on() - time.sleep(1.5) - - self.mc.send_angle(2, 0, 20) - time.sleep(0.3) - self.mc.send_angle(3, -18, 20) - time.sleep(2) - - self.mc.send_coords(self.move_coords[color], 20, 1) - - time.sleep(3) - - # close pump - - self.pump_off() - time.sleep(6) - - self.mc.send_angles(self.move_angles[1], 20) - time.sleep(1.5) - self.mc.send_angles([-30, 0, 0, 0], 20) - time.sleep(1.5) - - # decide whether grab cube - def decide_move(self, x, y, color): - print(x, y, self.cache_x, self.cache_y) - # detect the cube status move or run - if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm - self.cache_x, self.cache_y = x, y - return - else: - self.cache_x = self.cache_y = 0 - # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动 - print('start...') - self.move(x, y, color) - print('end....') - - # init mypal260 - def run(self): - - self.mc = MyPalletizer("COM9", 115200) - - self.mc.send_angles([-29.0, 5.88, -4.92, -76.28], 20) - time.sleep(3) - - # draw aruco - def draw_marker(self, img, x, y): - # draw rectangle on img - cv2.rectangle( - img, - (x - 20, y - 20), - (x + 20, y + 20), - (0, 255, 0), - thickness=2, - lineType=cv2.FONT_HERSHEY_COMPLEX, - ) - # add text on rectangle - cv2.putText( - img, - "({},{})".format(x, y), - (x, y), - cv2.FONT_HERSHEY_COMPLEX_SMALL, - 1, - (243, 0, 0), - 2, - ) - - # get points of two aruco - def get_calculate_params(self, img): - # Convert the image to a gray image - gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) - # Detect ArUco marker. - corners, ids, rejectImaPoint = cv2.aruco.detectMarkers( - gray, self.aruco_dict, parameters=self.aruco_params) - - """ - Two Arucos must be present in the picture and in the same order. - There are two Arucos in the Corners, and each aruco contains the pixels of its four corners. - Determine the center of the aruco by the four corners of the aruco. - """ - if len(corners) > 0: - if ids is not None: - if len(corners) <= 1 or ids[0] == 1: - return None - x1 = x2 = y1 = y2 = 0 - point_11, point_21, point_31, point_41 = corners[0][0] - x1, y1 = int( - (point_11[0] + point_21[0] + point_31[0] + point_41[0]) / - 4.0), int( - (point_11[1] + point_21[1] + point_31[1] + point_41[1]) - / 4.0) - point_1, point_2, point_3, point_4 = corners[1][0] - x2, y2 = int( - (point_1[0] + point_2[0] + point_3[0] + point_4[0]) / - 4.0), int( - (point_1[1] + point_2[1] + point_3[1] + point_4[1]) / - 4.0) - return x1, x2, y1, y2 - return None - - # set camera clipping parameters - def set_cut_params(self, x1, y1, x2, y2): - self.x1 = int(x1) - self.y1 = int(y1) - self.x2 = int(x2) - self.y2 = int(y2) - print(self.x1, self.y1, self.x2, self.y2) - - # set parameters to calculate the coords between cube and mycobot - def set_params(self, c_x, c_y, ratio): - self.c_x = c_x - self.c_y = c_y - self.ratio = 220.0 / ratio - - # calculate the coords between cube and mycobot - def get_position(self, x, y): - return ((y - self.c_y) * self.ratio + - self.camera_x), ((x - self.c_x) * self.ratio + self.camera_y) - - """ - Calibrate the camera according to the calibration parameters. - Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times. - If two ARuco values have been calculated, clip the video. - """ - - def transform_frame(self, frame): - # enlarge the image by 1.5 times - fx = 1.5 - fy = 1.5 - frame = cv2.resize(frame, (0, 0), - fx=fx, - fy=fy, - interpolation=cv2.INTER_CUBIC) - if self.x1 != self.x2: - # the cutting ratio here is adjusted according to the actual situation - frame = frame[int(self.y2 * 0.2):int(self.y1 * 1.15), - int(self.x1 * 0.7):int(self.x2 * 1.15)] - return frame - - # according the class_id to get object name - def id_class_name(self, class_id): - for key, value in self.labels.items(): - if class_id == int(key): - return value - - # detect object - def obj_detect(self, img, goal, kp_img, desc_img, kp_list, desc_list, connection): - i = 0 - MIN_MATCH_COUNT = 5 - # sift = cv2.xfeatures2d.SIFT_create() - - # find the keypoints and descriptors with SIFT - # kp = [] - # des = [] - kp = kp_list - des = desc_list - - # for i in goal: - # kp0, des0 = sift.detectAndCompute(i, None) - # kp.append(kp0) - # des.append(des0) - - # kp1, des1 = sift.detectAndCompute(goal, None) - # kp2, des2 = sift.detectAndCompute(img, None) - kp2, des2 = kp_img, desc_img - - # FLANN parameters - FLANN_INDEX_KDTREE = 0 - index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5) - search_params = dict(checks=50) # or pass empty dictionary - flann = cv2.FlannBasedMatcher(index_params, search_params) - - x, y = 0, 0 - try: - for i in range(len(des)): - matches = flann.knnMatch(des[i], des2, k=2) - # store all the good matches as per Lowe's ratio test. 根据Lowe比率测试存储所有良好匹配项。 - good = [] - for m, n in matches: - if m.distance < 0.7 * n.distance: - good.append(m) - - # When there are enough robust matching point pairs 当有足够的健壮匹配点对（至少个MIN_MATCH_COUNT）时 - if len(good) > MIN_MATCH_COUNT: - - # extract corresponding point pairs from matching 从匹配中提取出对应点对 - # query index of small objects, training index of scenarios 小对象的查询索引，场景的训练索引 - src_pts = np.float32([kp[i][m.queryIdx].pt - for m in good]).reshape(-1, 1, 2) - dst_pts = np.float32([kp2[m.trainIdx].pt - for m in good]).reshape(-1, 1, 2) - - # Using matching points to find homography matrix in cv2.ransac 利用匹配点找到CV2.RANSAC中的单应矩阵 - M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, - 5.0) - matchesMask = mask.ravel().tolist() - # Calculate the distortion of image, that is the corresponding position in frame 计算图1的畸变，也就是在图2中的对应的位置 - h, w, d = goal[i].shape - pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1], - [w - 1, 0]]).reshape(-1, 1, 2) - dst = cv2.perspectiveTransform(pts, M) - coord = (dst[0][0] + dst[1][0] + dst[2][0] + - dst[3][0]) / 4.0 - connection.send((DRAW_COORDS, coord)) - # cv2.putText(img, "{}".format(coord), (50, 60), - # fontFace=None, fontScale=1, - # color=(0, 255, 0), lineType=1) - print(format(dst[0][0][0])) - x = (dst[0][0][0] + dst[1][0][0] + dst[2][0][0] + - dst[3][0][0]) / 4.0 - y = (dst[0][0][1] + dst[1][0][1] + dst[2][0][1] + - dst[3][0][1]) / 4.0 - - # bound box 绘制边框 - # img = cv2.polylines(img, [np.int32(dst)], True, 244, 3, cv2.LINE_AA) - connection.send((DRAW_RECT, dst)) - # cv2.polylines(mixture, [np.int32(dst)], True, (0, 255, 0), 2, cv2.LINE_AA) - except Exception as e: - pass - - if x + y > 0: - return x, y - else: - return None - -# The path to save the image folder -def parse_folder(folder): - restore = [] - path1 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mypalletizer_260/' + folder - path2 = r'D:/BaiduSyncdisk/PythonProject/OpenCV' + folder - - # if os.path.exists(path1): - # path = path1 - # elif os.path.exists(path2): - path = path1 - - for i, j, k in os.walk(path): - for l in k: - restore.append(cv2.imread(folder + '/{}'.format(l))) - return restore - -def compute_keypoints_and_descriptors(sift, images_lists): - kp_list = [] - desc_list = [] - for images in images_lists: - kp_tmp = [] - desc_tmp = [] - for img in images: - kp, desc = sift.detectAndCompute(img, None) - kp_tmp.append(kp) - desc_tmp.append(desc) - kp_list.append(kp_tmp) - desc_list.append(desc_tmp) - - return kp_list, desc_list - -GET_FRAME = 1 -STOP_PROCESSING = 2 -DRAW_COORDS = 3 -DRAW_RECT = 4 -CLEAR_DRAW = 5 -CROP_FRAME = 6 - -def get_frame(connection): - connection.send(GET_FRAME) - frame = connection.recv() - return frame - -def process_transform_frame(frame, x1, y1, x2, y2): - # enlarge the image by 1.5 times - fx = 1.5 - fy = 1.5 - frame = cv2.resize(frame, (0, 0), - fx=fx, - fy=fy, - interpolation=cv2.INTER_CUBIC) -# if x1 != x2: - # the cutting ratio here is adjusted according to the actual situation -# frame = frame[int(y2 * 0.2):int(y1 * 1.15), -# int(x1 * 0.7):int(x2 * 1.15)] - return frame - -def process_display_frame(connection): - cap_num = 1 - coord = None - dst = None - x1 = 0 - y1 = 0 - x2 = 0 - y2 = 0 - cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L) - if not cap.isOpened(): - cap.open(1) - while cv2.waitKey(1) < 0: - _, frame = cap.read() - frame = process_transform_frame(frame, x1, y1, x2, y2) - if connection.poll(): - request = connection.recv() - if request == GET_FRAME: - connection.send(frame) - elif request == CLEAR_DRAW: - coord = None - dst = None - elif type(request) is tuple: - if request[0] == DRAW_COORDS: - coord = request[1] - elif request[0] == DRAW_RECT: - dst = request[1] - elif request[0] == CROP_FRAME: - x1 = request[1] - y1 = request[2] - x2 = request[3] - y2 = request[4] - - if not coord is None: - cv2.putText(frame, "{}".format(coord), (50, 60), fontFace=None, - fontScale=1, color=(0, 255, 0), lineType=1) - if not dst is None: - frame = cv2.polylines(frame, [np.int32(dst)], True, 244, 3, cv2.LINE_AA) - cv2.imshow("figure", frame) - time.sleep(0.04) - connection.send(STOP_PROCESSING) - -def run(): - parent_conn, child_conn = Pipe() - child = Process(target = process_display_frame, args=(child_conn,)) - child.start() - - res_queue = [[], [], [], []] - res_queue[0] = parse_folder('res/red') - res_queue[1] = parse_folder('res/green') - res_queue[2] = parse_folder('res/blue') - res_queue[3] = parse_folder('res/gray') - - sift = cv2.xfeatures2d.SIFT_create() - kp_list, desc_list = compute_keypoints_and_descriptors(sift, res_queue) - - # init a class of Object_detect - detect = Object_detect() - - # init mycobot - detect.run() - - # _init_ = 20 # - init_num = 0 - nparams = 0 - # num = 0 - # real_sx = real_sy = 0 - while True: - start_time = time.time() - if parent_conn.poll(): - data = parent_conn.recv() - if data == STOP_PROCESSING: - break - # read camera - frame = get_frame(parent_conn) - # deal img - #frame = detect.transform_frame(frame) - - # if _init_ > 0: - # _init_ -= 1 - # continue - # calculate the parameters of camera clipping - if init_num < 20: - if detect.get_calculate_params(frame) is None: - # cv2.imshow("figure", frame) - continue - else: - x1, x2, y1, y2 = detect.get_calculate_params(frame) - detect.draw_marker(frame, x1, y1) - detect.draw_marker(frame, x2, y2) - detect.sum_x1 += x1 - detect.sum_x2 += x2 - detect.sum_y1 += y1 - detect.sum_y2 += y2 - init_num += 1 - continue - elif init_num == 20: - detect.set_cut_params( - (detect.sum_x1) / 20.0, - (detect.sum_y1) / 20.0, - (detect.sum_x2) / 20.0, - (detect.sum_y2) / 20.0, - ) - parent_conn.send((CROP_FRAME, - (detect.sum_x1) / 20.0, - (detect.sum_y1) / 20.0, - (detect.sum_x2) / 20.0, - (detect.sum_y2) / 20.0)) - detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0 - init_num += 1 - continue - - # calculate params of the coords between cube and mycobot - if nparams < 10: - if detect.get_calculate_params(frame) is None: - # cv2.imshow("figure", frame) - continue - else: - x1, x2, y1, y2 = detect.get_calculate_params(frame) - detect.draw_marker(frame, x1, y1) - detect.draw_marker(frame, x2, y2) - detect.sum_x1 += x1 - detect.sum_x2 += x2 - detect.sum_y1 += y1 - detect.sum_y2 += y2 - nparams += 1 - print ("ok") - continue - elif nparams == 10: - nparams += 1 - # calculate and set params of calculating real coord between cube and mycobot - detect.set_params((detect.sum_x1 + detect.sum_x2) / 20.0, - (detect.sum_y1 + detect.sum_y2) / 20.0, - abs(detect.sum_x1 - detect.sum_x2) / 10.0 + - abs(detect.sum_y1 - detect.sum_y2) / 10.0) - print("ok") - continue - - # get detect result - kp_img, desc_img = sift.detectAndCompute(frame, None) - frame = get_frame(parent_conn) - for i, v in enumerate(res_queue): - # HACK: to update frame every time - detect_result = detect.obj_detect(frame, v, kp_img, desc_img, kp_list[i], desc_list[i], parent_conn) - if detect_result: - x, y = detect_result - # calculate real coord between cube and mycobot - real_x, real_y = detect.get_position(x, y) - detect.color = i - detect.pub_marker(real_x / 1000.0, real_y / 1000.0) - detect.decide_move(real_x, real_y, detect.color) - # if num == 5: - # detect.color = i - # detect.pub_marker(real_sx / 5.0 / 1000.0, - # real_sy / 5.0 / 1000.0) - # detect.decide_move(real_sx / 5.0, real_sy / 5.0, - # detect.color) - # num = real_sx = real_sy = 0 - # else: - # num += 1 - # real_sy += real_y - # real_sx += real_x - parent_conn.send(CLEAR_DRAW) - - # cv2.imshow("figure", frame) - time.sleep(0.05) - end_time = time.time() - # print("loop_time = ", end_time - start_time) - - # close the window - if cv2.waitKey(1) & 0xFF == ord('q'): - # cap.release() - cv2.destroyAllWindows() - sys.exit() - - child.join() - - -if __name__ == "__main__": - run() - # Object_detect().take_photo() - # Object_detect().cut_photo() \ No newline at end of file diff --git a/mycobot_ai/ai_mycobot_280/scripts/encode_test.py b/mycobot_ai/ai_mycobot_280/scripts/encode_test.py deleted file mode 100644 index dfd5c47..0000000 --- a/mycobot_ai/ai_mycobot_280/scripts/encode_test.py +++ /dev/null @@ -1,248 +0,0 @@ -# encoding: UTF-8 -#!/usr/bin/env python2 -import cv2 as cv -import os -import numpy as np -import time -import rospy -from visualization_msgs.msg import Marker -from moving_utils import Movement - -# y轴偏移量 -pump_y = -55 -# x轴偏移量 -pump_x = 15 - - -class Detect_marker(Movement): - def __init__(self): - super(Detect_marker, self).__init__() - # set cache of real coord - self.cache_x = self.cache_y = 0 - - # which robot - self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1] - self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1] - self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1] - self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1] - self.raspi = False - if "dev" in self.robot_m5: - self.Pin = [2, 5] - elif "dev" in self.robot_wio: - self.Pin = [20, 21] - # for i in self.move_coords: - # i[2] -= 20 - elif "dev" in self.robot_raspi or "dev" in self.robot_jes: - import RPi.GPIO as GPIO - self.GPIO = GPIO - GPIO.setwarnings(False) - GPIO.setmode(GPIO.BCM) - GPIO.setup(20, GPIO.OUT) - GPIO.setup(21, GPIO.OUT) - self.raspi = True - if self.raspi: - self.gpio_status(False) - else: - self.pub_pump(False, self.Pin) - # Creating a Camera Object - cap_num = 0 - self.cap = cv.VideoCapture(cap_num, cv.CAP_V4L) - # Get ArUco marker dict that can be detected. - self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) - # Get ArUco marker params. - self.aruco_params = cv.aruco.DetectorParameters_create() - self.calibrationParams = cv.FileStorage( - "calibrationFileName.xml", cv.FILE_STORAGE_READ) - # Get distance coefficient. - self.dist_coeffs = self.calibrationParams.getNode("distCoeffs").mat() - - height = self.cap.get(4) - focal_length = width = self.cap.get(3) - center = [width / 2, height / 2] - # Calculate the camera matrix. - self.camera_matrix = np.array( - [ - [focal_length, 0, center[0]], - [0, focal_length, center[1]], - [0, 0, 1], - ], - dtype=np.float32, - ) - # init a node and a publisher - rospy.init_node("encode_marker", anonymous=True) - self.pub = rospy.Publisher('/cube', Marker, queue_size=1) - - self.marker = Marker() - self.marker.header.frame_id = "/joint1" - self.marker.ns = "cube" - self.marker.type = self.marker.CUBE - self.marker.action = self.marker.ADD - self.marker.scale.x = 0.04 - self.marker.scale.y = 0.04 - self.marker.scale.z = 0.04 - self.marker.color.a = 1 - self.marker.color.r = 0.3 - self.marker.color.g = 0.3 - self.marker.color.b = 0.3 - - # marker position initial - self.marker.pose.position.x = 0 - self.marker.pose.position.y = 0 - self.marker.pose.position.z = 0.03 - self.marker.pose.orientation.x = 0 - self.marker.pose.orientation.y = 0 - self.marker.pose.orientation.z = 0 - self.marker.pose.orientation.w = 1.0 - - # Grasping motion - def move(self, x, y): - if self.raspi: - coords = [ - [145.6, -64.9, 285.2, 179.88, 7.67, 179], - [130.1, -155.6, 243.9, 178.99, 5.38, -179.9] - ] - else: - coords = [ - [135.0, -65.5, 280.1, 178.99, 5.38, -179.9], - [136.1, -141.6, 243.9, 178.99, 5.38, -179.9] - ] - - # publish marker - self.marker.header.stamp = rospy.Time.now() - self.marker.pose.position.x = (coords[0][0]-x)/1000.0 - self.marker.pose.position.y = (coords[0][1]-y)/1000.0 - self.pub.publish(self.marker) - - # send coordinates to move mycobot - self.pub_coords(coords[0], 30, 1) - time.sleep(2) - self.pub_coords([coords[0][0]-x, coords[0][1]-y, - 240, 178.99, 5.38, -179.9], 25, 1) - time.sleep(2) - self.pub_coords([coords[0][0]-x, coords[0][1]-y, - 200, 178.99, 5.38, -179.9], 25, 1) - time.sleep(2) - if "dev" in self.robot_m5 or self.raspi: - self.pub_coords([coords[0][0]-x, coords[0][1]-y, - 90, 178.99, 5.38, -179.9], 25, 1) - elif "dev" in self.robot_wio: - self.pub_coords([coords[0][0]-x+20, coords[0][1] - - y-10, 70, 178.99, 5.38, -179.9], 25, 1) - time.sleep(2) - if self.raspi: - self.gpio_status(True) - else: - self.pub_pump(True, self.Pin) - time.sleep(1) - self.pub_coords(coords[0], 30, 1) - time.sleep(3) - self.pub_coords(coords[1], 30, 1) - time.sleep(2) - if self.raspi: - self.gpio_status(False) - else: - self.pub_pump(False, self.Pin) - # publish marker - time.sleep(1) - self.marker.header.stamp = rospy.Time.now() - self.marker.pose.position.x = coords[1][0]/1000.0 - self.marker.pose.position.y = coords[1][1]/1000.0 - self.pub.publish(self.marker) - - self.pub_coords(coords[0], 30, 1) - time.sleep(2) - - def gpio_status(self, flag): - if flag: - self.GPIO.output(20, 0) - self.GPIO.output(21, 0) - else: - self.GPIO.output(20, 1) - self.GPIO.output(21, 1) - - # decide whether grab cube - def decide_move(self, x, y): - - print(x, y) - # detect the cube status move or run - if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm - self.cache_x, self.cache_y = x, y - return - else: - self.cache_x = self.cache_y = 0 - if "dev" in self.robot_jes: - if x > -20: - y += 10 - if y > -25: - x -= 5 - x += 10 - - self.move(x, y) - - # init mycobot - def init_mycobot(self): - - for _ in range(5): - print(_) - self.pub_coords([145.6, -64.9, 285.2, 179.88, 7.67, 179], 20, 1) - time.sleep(0.5) - - def run(self): - global pump_y, pump_x - self.init_mycobot() - num = sum_x = sum_y = 0 - while cv.waitKey(1) < 0: - success, img = self.cap.read() - if not success: - print("It seems that the image cannot be acquired correctly.") - break - - # transfrom the img to model of gray - gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) - # Detect ArUco marker. - corners, ids, rejectImaPoint = cv.aruco.detectMarkers( - gray, self.aruco_dict, parameters=self.aruco_params - ) - font = cv.FONT_HERSHEY_SIMPLEX - if len(corners) > 0: - if ids is not None: - # get informations of aruco - ret = cv.aruco.estimatePoseSingleMarkers( - corners, 0.03, self.camera_matrix, self.dist_coeffs - ) - # rvec:rotation offset,tvec:translation deviator - (rvec, tvec) = (ret[0], ret[1]) - (rvec - tvec).any() - xyz = tvec[0, 0, :] - # calculate the coordinates of the aruco relative to the pump - xyz = [round(xyz[0]*1000+pump_y, 2), round(xyz[1] - * 1000+pump_x, 2), round(xyz[2]*1000, 2)] - cv.putText(img, "Id: " + str(ids[0][0]), (0, 40), font, 0.6, (0, 255, 0), 2, cv.LINE_AA) - for i in range(rvec.shape[0]): - # draw the aruco on img - cv.aruco.drawDetectedMarkers(img, corners) - cv.aruco.drawAxis( - img, - self.camera_matrix, - self.dist_coeffs, - rvec[i, :, :], - tvec[i, :, :], - 0.03, - ) - - if num < 40: - if self.raspi: - sum_x -= 30 - sum_x += xyz[1] - sum_y += xyz[0] - num += 1 - elif num == 40: - self.decide_move(sum_x/40.0, sum_y/40.0) - num = sum_x = sum_y = 0 - - cv.imshow("encode_image", img) - - -if __name__ == "__main__": - detect = Detect_marker() - detect.run() diff --git a/mycobot_ai/ai_mypalletizer_260/scripts/aikit_img.py b/mycobot_ai/ai_mypalletizer_260/scripts/aikit_img.py deleted file mode 100644 index 770ee11..0000000 --- a/mycobot_ai/ai_mypalletizer_260/scripts/aikit_img.py +++ /dev/null @@ -1,557 +0,0 @@ -# encoding=utf-8 -#!/usr/bin/env python2 -from multiprocessing import Process, Pipe -from cgi import parse -from difflib import restore -# import queue -from sys import path -from tokenize import Pointfloat -from turtle import color -# from typing_extensions import Self -import cv2 -import numpy as np -import time -import json -import os,sys - -from PIL import Image -from threading import Thread -from pymycobot.mypalletizer import MyPalletizer - -IS_CV_4 = cv2.__version__[0] == '4' -__version__ = "1.0" # Adaptive seeed - -__metaclass__ = type - -class Object_detect(): - - def __init__(self, camera_x = 160, camera_y = 10): - # inherit the parent class - super(Object_detect, self).__init__() - - # declare mypal260 - self.mc = None - # 移动角度 - self.move_angles = [ - [0, 0, 0, 0], # init the point - [-29.0, 5.88, -4.92, -76.28], # point to grab - [17.4, -10.1, -87.27, 5.8, -2.02, 15], # point to grab - ] - - # 移动坐标 - self.move_coords = [ - [132.6, -155.6, 211.8, -20.9], # above the red bucket - [232.5, -134.1, 197.7, -45.26], # above the green bucket - [111.6, 159, 221.5, -120], # above the blue bucket - [-15.9, 164.6, 217.5, -119.35], # above the gray bucket - ] - - # choose place to set cube - self.color = 0 - # parameters to calculate camera clipping parameters - self.x1 = self.x2 = self.y1 = self.y2 = 0 - # set cache of real coord - self.cache_x = self.cache_y = 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 - self.camera_x, self.camera_y = camera_x, camera_y - # The coordinates of the cube relative to the mycobot - self.c_x, self.c_y = 0, 0 - # The ratio of pixels to actual values - self.ratio = 0 - # Get ArUco marker dict that can be detected. - self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250) - # Get ArUco marker params. - self.aruco_params = cv2.aruco.DetectorParameters_create() - - # 开启吸泵 m5 - def pump_on(self): - # 让2号位工作 - # self.mc.set_basic_output(2, 0) - # 让5号位工作 - self.mc.set_basic_output(5, 0) - - # 停止吸泵 m5 - def pump_off(self): - # 让2号位停止工作 - # self.mc.set_basic_output(2, 1) - # 让5号位停止工作 - self.mc.set_basic_output(5, 1) - - # Grasping motion - def move(self, x, y, color): - # send Angle to move mypal260 - self.mc.send_angles(self.move_angles[0], 20) - time.sleep(3) - - # send coordinates to move mypal260 根据不同底板机械臂，调整吸泵高度 - self.mc.send_coords([x, y, 160, 0], 20, 0) - time.sleep(1.5) - self.mc.send_coords([x, y, 110, 0], 20, 0) - time.sleep(1.5) - - # open pump - self.pump_on() - time.sleep(1.5) - - self.mc.send_angle(2, 0, 20) - time.sleep(0.3) - self.mc.send_angle(3, -18, 20) - time.sleep(2) - - self.mc.send_coords(self.move_coords[color], 20, 1) - - time.sleep(3) - - # close pump - - self.pump_off() - time.sleep(6) - - self.mc.send_angles(self.move_angles[1], 20) - time.sleep(1.5) - self.mc.send_angles([-30, 0, 0, 0], 20) - time.sleep(1.5) - - # decide whether grab cube - def decide_move(self, x, y, color): - print(x, y, self.cache_x, self.cache_y) - # detect the cube status move or run - if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm - self.cache_x, self.cache_y = x, y - return - else: - self.cache_x = self.cache_y = 0 - # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动 - print('start...') - self.move(x, y, color) - print('end....') - - # init mypal260 - def run(self): - - self.mc = MyPalletizer("/dev/ttyUSB0", 115200) - - self.mc.send_angles([-29.0, 5.88, -4.92, -76.28], 20) - time.sleep(3) - - # draw aruco - def draw_marker(self, img, x, y): - # draw rectangle on img - cv2.rectangle( - img, - (x - 20, y - 20), - (x + 20, y + 20), - (0, 255, 0), - thickness=2, - lineType=cv2.FONT_HERSHEY_COMPLEX, - ) - # add text on rectangle - cv2.putText( - img, - "({},{})".format(x, y), - (x, y), - cv2.FONT_HERSHEY_COMPLEX_SMALL, - 1, - (243, 0, 0), - 2, - ) - - # get points of two aruco - def get_calculate_params(self, img): - # Convert the image to a gray image - gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) - # Detect ArUco marker. - corners, ids, rejectImaPoint = cv2.aruco.detectMarkers( - gray, self.aruco_dict, parameters=self.aruco_params) - - """ - Two Arucos must be present in the picture and in the same order. - There are two Arucos in the Corners, and each aruco contains the pixels of its four corners. - Determine the center of the aruco by the four corners of the aruco. - """ - if len(corners) > 0: - if ids is not None: - if len(corners) <= 1 or ids[0] == 1: - return None - x1 = x2 = y1 = y2 = 0 - point_11, point_21, point_31, point_41 = corners[0][0] - x1, y1 = int( - (point_11[0] + point_21[0] + point_31[0] + point_41[0]) / - 4.0), int( - (point_11[1] + point_21[1] + point_31[1] + point_41[1]) - / 4.0) - point_1, point_2, point_3, point_4 = corners[1][0] - x2, y2 = int( - (point_1[0] + point_2[0] + point_3[0] + point_4[0]) / - 4.0), int( - (point_1[1] + point_2[1] + point_3[1] + point_4[1]) / - 4.0) - return x1, x2, y1, y2 - return None - - # set camera clipping parameters - def set_cut_params(self, x1, y1, x2, y2): - self.x1 = int(x1) - self.y1 = int(y1) - self.x2 = int(x2) - self.y2 = int(y2) - print(self.x1, self.y1, self.x2, self.y2) - - # set parameters to calculate the coords between cube and mycobot - def set_params(self, c_x, c_y, ratio): - self.c_x = c_x - self.c_y = c_y - self.ratio = 220.0 / ratio - - # calculate the coords between cube and mycobot - def get_position(self, x, y): - return ((y - self.c_y) * self.ratio + - self.camera_x), ((x - self.c_x) * self.ratio + self.camera_y) - - """ - Calibrate the camera according to the calibration parameters. - Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times. - If two ARuco values have been calculated, clip the video. - """ - - def transform_frame(self, frame): - # enlarge the image by 1.5 times - fx = 1.5 - fy = 1.5 - frame = cv2.resize(frame, (0, 0), - fx=fx, - fy=fy, - interpolation=cv2.INTER_CUBIC) - if self.x1 != self.x2: - # the cutting ratio here is adjusted according to the actual situation - frame = frame[int(self.y2 * 0.2):int(self.y1 * 1.15), - int(self.x1 * 0.7):int(self.x2 * 1.15)] - return frame - - # according the class_id to get object name - def id_class_name(self, class_id): - for key, value in self.labels.items(): - if class_id == int(key): - return value - - # detect object - def obj_detect(self, img, goal, kp_img, desc_img, kp_list, desc_list, connection): - i = 0 - MIN_MATCH_COUNT = 5 - # sift = cv2.xfeatures2d.SIFT_create() - - # find the keypoints and descriptors with SIFT - # kp = [] - # des = [] - kp = kp_list - des = desc_list - - # for i in goal: - # kp0, des0 = sift.detectAndCompute(i, None) - # kp.append(kp0) - # des.append(des0) - - # kp1, des1 = sift.detectAndCompute(goal, None) - # kp2, des2 = sift.detectAndCompute(img, None) - kp2, des2 = kp_img, desc_img - - # FLANN parameters - FLANN_INDEX_KDTREE = 0 - index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5) - search_params = dict(checks=50) # or pass empty dictionary - flann = cv2.FlannBasedMatcher(index_params, search_params) - - x, y = 0, 0 - try: - for i in range(len(des)): - matches = flann.knnMatch(des[i], des2, k=2) - # store all the good matches as per Lowe's ratio test. 根据Lowe比率测试存储所有良好匹配项。 - good = [] - for m, n in matches: - if m.distance < 0.7 * n.distance: - good.append(m) - - # When there are enough robust matching point pairs 当有足够的健壮匹配点对（至少个MIN_MATCH_COUNT）时 - if len(good) > MIN_MATCH_COUNT: - - # extract corresponding point pairs from matching 从匹配中提取出对应点对 - # query index of small objects, training index of scenarios 小对象的查询索引，场景的训练索引 - src_pts = np.float32([kp[i][m.queryIdx].pt - for m in good]).reshape(-1, 1, 2) - dst_pts = np.float32([kp2[m.trainIdx].pt - for m in good]).reshape(-1, 1, 2) - - # Using matching points to find homography matrix in cv2.ransac 利用匹配点找到CV2.RANSAC中的单应矩阵 - M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, - 5.0) - matchesMask = mask.ravel().tolist() - # Calculate the distortion of image, that is the corresponding position in frame 计算图1的畸变，也就是在图2中的对应的位置 - h, w, d = goal[i].shape - pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1], - [w - 1, 0]]).reshape(-1, 1, 2) - dst = cv2.perspectiveTransform(pts, M) - coord = (dst[0][0] + dst[1][0] + dst[2][0] + - dst[3][0]) / 4.0 - connection.send((DRAW_COORDS, coord)) - # cv2.putText(img, "{}".format(coord), (50, 60), - # fontFace=None, fontScale=1, - # color=(0, 255, 0), lineType=1) - print(format(dst[0][0][0])) - x = (dst[0][0][0] + dst[1][0][0] + dst[2][0][0] + - dst[3][0][0]) / 4.0 - y = (dst[0][0][1] + dst[1][0][1] + dst[2][0][1] + - dst[3][0][1]) / 4.0 - - # bound box 绘制边框 - # img = cv2.polylines(img, [np.int32(dst)], True, 244, 3, cv2.LINE_AA) - connection.send((DRAW_RECT, dst)) - # cv2.polylines(mixture, [np.int32(dst)], True, (0, 255, 0), 2, cv2.LINE_AA) - except Exception as e: - pass - - if x + y > 0: - return x, y - else: - return None - -# The path to save the image folder -def parse_folder(folder): - restore = [] - path1 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mypalletizer_260/' + folder - path2 = r'D:/BaiduSyncdisk/PythonProject/OpenCV' + folder - - # if os.path.exists(path1): - # path = path1 - # elif os.path.exists(path2): - path = path1 - - for i, j, k in os.walk(path): - for l in k: - restore.append(cv2.imread(folder + '/{}'.format(l))) - return restore - -def compute_keypoints_and_descriptors(sift, images_lists): - kp_list = [] - desc_list = [] - for images in images_lists: - kp_tmp = [] - desc_tmp = [] - for img in images: - kp, desc = sift.detectAndCompute(img, None) - kp_tmp.append(kp) - desc_tmp.append(desc) - kp_list.append(kp_tmp) - desc_list.append(desc_tmp) - - return kp_list, desc_list - -GET_FRAME = 1 -STOP_PROCESSING = 2 -DRAW_COORDS = 3 -DRAW_RECT = 4 -CLEAR_DRAW = 5 -CROP_FRAME = 6 - -def get_frame(connection): - connection.send(GET_FRAME) - frame = connection.recv() - return frame - -def process_transform_frame(frame, x1, y1, x2, y2): - # enlarge the image by 1.5 times - fx = 1.5 - fy = 1.5 - frame = cv2.resize(frame, (0, 0), - fx=fx, - fy=fy, - interpolation=cv2.INTER_CUBIC) -# if x1 != x2: - # the cutting ratio here is adjusted according to the actual situation -# frame = frame[int(y2 * 0.2):int(y1 * 1.15), -# int(x1 * 0.7):int(x2 * 1.15)] - return frame - -def process_display_frame(connection): - cap_num = 0 - coord = None - dst = None - x1 = 0 - y1 = 0 - x2 = 0 - y2 = 0 - cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L) - if not cap.isOpened(): - cap.open() - while cv2.waitKey(1) < 0: - _, frame = cap.read() - frame = process_transform_frame(frame, x1, y1, x2, y2) - if connection.poll(): - request = connection.recv() - if request == GET_FRAME: - connection.send(frame) - elif request == CLEAR_DRAW: - coord = None - dst = None - elif type(request) is tuple: - if request[0] == DRAW_COORDS: - coord = request[1] - elif request[0] == DRAW_RECT: - dst = request[1] - elif request[0] == CROP_FRAME: - x1 = request[1] - y1 = request[2] - x2 = request[3] - y2 = request[4] - - if not coord is None: - cv2.putText(frame, "{}".format(coord), (50, 60), fontFace=None, - fontScale=1, color=(0, 255, 0), lineType=1) - if not dst is None: - frame = cv2.polylines(frame, [np.int32(dst)], True, 244, 3, cv2.LINE_AA) - cv2.imshow("figure", frame) - time.sleep(0.04) - connection.send(STOP_PROCESSING) - -def run(): - parent_conn, child_conn = Pipe() - child = Process(target = process_display_frame, args=(child_conn,)) - child.start() - - res_queue = [[], [], [], []] - res_queue[0] = parse_folder('res/red') - res_queue[1] = parse_folder('res/green') - res_queue[2] = parse_folder('res/blue') - res_queue[3] = parse_folder('res/gray') - - sift = cv2.xfeatures2d.SIFT_create() - kp_list, desc_list = compute_keypoints_and_descriptors(sift, res_queue) - - # init a class of Object_detect - detect = Object_detect() - - # init mycobot - detect.run() - - # _init_ = 20 # - init_num = 0 - nparams = 0 - # num = 0 - # real_sx = real_sy = 0 - while True: - start_time = time.time() - if parent_conn.poll(): - data = parent_conn.recv() - if data == STOP_PROCESSING: - break - # read camera - frame = get_frame(parent_conn) - # deal img - #frame = detect.transform_frame(frame) - - # if _init_ > 0: - # _init_ -= 1 - # continue - # calculate the parameters of camera clipping - if init_num < 20: - if detect.get_calculate_params(frame) is None: - # cv2.imshow("figure", frame) - continue - else: - x1, x2, y1, y2 = detect.get_calculate_params(frame) - detect.draw_marker(frame, x1, y1) - detect.draw_marker(frame, x2, y2) - detect.sum_x1 += x1 - detect.sum_x2 += x2 - detect.sum_y1 += y1 - detect.sum_y2 += y2 - init_num += 1 - continue - elif init_num == 20: - detect.set_cut_params( - (detect.sum_x1) / 20.0, - (detect.sum_y1) / 20.0, - (detect.sum_x2) / 20.0, - (detect.sum_y2) / 20.0, - ) - parent_conn.send((CROP_FRAME, - (detect.sum_x1) / 20.0, - (detect.sum_y1) / 20.0, - (detect.sum_x2) / 20.0, - (detect.sum_y2) / 20.0)) - detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0 - init_num += 1 - continue - - # calculate params of the coords between cube and mycobot - if nparams < 10: - if detect.get_calculate_params(frame) is None: - # cv2.imshow("figure", frame) - continue - else: - x1, x2, y1, y2 = detect.get_calculate_params(frame) - detect.draw_marker(frame, x1, y1) - detect.draw_marker(frame, x2, y2) - detect.sum_x1 += x1 - detect.sum_x2 += x2 - detect.sum_y1 += y1 - detect.sum_y2 += y2 - nparams += 1 - print ("ok") - continue - elif nparams == 10: - nparams += 1 - # calculate and set params of calculating real coord between cube and mycobot - detect.set_params((detect.sum_x1 + detect.sum_x2) / 20.0, - (detect.sum_y1 + detect.sum_y2) / 20.0, - abs(detect.sum_x1 - detect.sum_x2) / 10.0 + - abs(detect.sum_y1 - detect.sum_y2) / 10.0) - print("ok") - continue - - # get detect result - kp_img, desc_img = sift.detectAndCompute(frame, None) - frame = get_frame(parent_conn) - for i, v in enumerate(res_queue): - # HACK: to update frame every time - detect_result = detect.obj_detect(frame, v, kp_img, desc_img, kp_list[i], desc_list[i], parent_conn) - if detect_result: - x, y = detect_result - # calculate real coord between cube and mycobot - real_x, real_y = detect.get_position(x, y) - detect.color = i - # detect.pub_marker(real_x / 1000.0, real_y / 1000.0) - detect.decide_move(real_x, real_y, detect.color) - # if num == 5: - # detect.color = i - # detect.pub_marker(real_sx / 5.0 / 1000.0, - # real_sy / 5.0 / 1000.0) - # detect.decide_move(real_sx / 5.0, real_sy / 5.0, - # detect.color) - # num = real_sx = real_sy = 0 - # else: - # num += 1 - # real_sy += real_y - # real_sx += real_x - parent_conn.send(CLEAR_DRAW) - - # cv2.imshow("figure", frame) - time.sleep(0.05) - end_time = time.time() - # print("loop_time = ", end_time - start_time) - - # close the window - if cv2.waitKey(1) & 0xFF == ord('q'): - # cap.release() - cv2.destroyAllWindows() - sys.exit() - - child.join() - - -if __name__ == "__main__": - run() - # Object_detect().take_photo() - # Object_detect().cut_photo() \ No newline at end of file diff --git a/mycobot_communication/scripts/mycobot_services.py b/mycobot_communication/scripts/mycobot_services.py index 1f5c89a..ce2dc4d 100755 --- a/mycobot_communication/scripts/mycobot_services.py +++ b/mycobot_communication/scripts/mycobot_services.py @@ -2,12 +2,51 @@ # -*- coding: utf-8 -* import time import rospy +import os +import fcntl from mycobot_communication.srv import * from pymycobot.mycobot import MyCobot mc = None +# Avoid serial port conflicts and need to be locked +def acquire(lock_file): + open_mode = os.O_RDWR | os.O_CREAT | os.O_TRUNC + fd = os.open(lock_file, open_mode) + + pid = os.getpid() + lock_file_fd = None + + timeout = 50.0 + start_time = current_time = time.time() + while current_time < start_time + timeout: + try: + # The LOCK_EX means that only one process can hold the lock + # The LOCK_NB means that the fcntl.flock() is not blocking + # and we are able to implement termination of while loop, + # when timeout is reached. + fcntl.flock(fd, fcntl.LOCK_EX | fcntl.LOCK_NB) + except (IOError, OSError): + pass + else: + lock_file_fd = fd + break + print('pid waiting for lock:%d'% pid) + time.sleep(1.0) + current_time = time.time() + if lock_file_fd is None: + os.close(fd) + return lock_file_fd + + +def release(lock_file_fd): + # Do not remove the lockfile: + fcntl.flock(lock_file_fd, fcntl.LOCK_UN) + os.close(lock_file_fd) + return None + + def create_handle(): global mc @@ -43,7 +82,9 @@ def set_angles(req): sp = req.speed print('angles1:',angles) if mc: + lock = acquire("/tmp/mycobot_lock") mc.send_angles(angles, sp) + release(lock) return SetAnglesResponse(True) @@ -51,8 +92,9 @@ def set_angles(req): def get_angles(req): """get angles,获取角度""" if mc: + lock = acquire("/tmp/mycobot_lock") angles = mc.get_angles() - print('angles2:',angles) + release(lock) return GetAnglesResponse(*angles) @@ -69,15 +111,18 @@ def set_coords(req): mod = req.model if mc: + lock = acquire("/tmp/mycobot_lock") mc.send_coords(coords, sp, mod) + release(lock) return SetCoordsResponse(True) def get_coords(req): if mc: + lock = acquire("/tmp/mycobot_lock") coords = mc.get_coords() - print('coords:',coords) + release(lock) return GetCoordsResponse(*coords) @@ -85,22 +130,26 @@ def switch_status(req): """Gripper switch status""" """夹爪开关状态""" if mc: + lock = acquire("/tmp/mycobot_lock") if req.Status: mc.set_gripper_state(0, 80) else: mc.set_gripper_state(1, 80) + release(lock) return GripperStatusResponse(True) def toggle_pump(req): if mc: + lock = acquire("/tmp/mycobot_lock") if req.Status: mc.set_basic_output(req.Pin1, 0) mc.set_basic_output(req.Pin2, 0) else: mc.set_basic_output(req.Pin1, 1) mc.set_basic_output(req.Pin2, 1) + release(lock) return PumpStatusResponse(True) @@ -133,11 +182,15 @@ def output_robot_message(): atom_version = "unknown" if mc: + lock = acquire("/tmp/mycobot_lock") cn = mc.is_controller_connected() + release(lock) if cn == 1: connect_status = True time.sleep(0.1) + lock = acquire("/tmp/mycobot_lock") si = mc.is_all_servo_enable() + release(lock) if si == 1: servo_infomation = "all connected"