optimize

mycobot_ai/myPalletizer_260/launch/vision_m5.launch

@@ -1,5 +1,5 @@
 <launch>
-<arg name="port" default="/dev/ttyUSB0" />
+	<arg name="port" default="/dev/ttyUSB1" />
 	<arg name="baud" default="115200" />
 
 	<arg name="model" default="$(find mycobot_description)/urdf/260_urdf/mypal_260_aikit.urdf"/>

mycobot_ai/myPalletizer_260/scripts/combine_detect_obj_color.py

@@ -0,0 +1,476 @@
+# -*- coding:utf-8 -*- 
+#!/usr/bin/env python2
+from operator import imod
+from tokenize import Pointfloat
+import cv2
+import numpy as np
+import time
+import json
+import os,sys
+import rospy
+from visualization_msgs.msg import Marker   
+from pymycobot.mypalletizer import MyPalletizer
+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 = 160, camera_y = 10):
+        # inherit the parent class
+        super(Object_detect, self).__init__()
+        # get path of file
+        dir_path = os.path.dirname(__file__)
+
+        # 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 = [
+            [141.2, -142.0, 210, -26.8],  # above the red bucket
+            [234.3, -120, 210, -48.77], # above the green bucket
+            [100.9, 159.3, 248.6, -124.27], # above the blue bucket
+            [-17.6, 161.6, 238.4, -152.31], # above the gray bucket         
+        ]
+
+        # 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]
+            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, 46]), np.array([77, 255, 255])],
+            "blue": [np.array([100, 43, 46]), np.array([124, 255, 255])],
+            "cyan": [np.array([78, 43, 46]), np.array([99, 255, 255])],
+        }
+        # use to calculate coord between cube and mypal260
+        self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
+        # The coordinates of the grab center point relative to the mypal260
+        self.camera_x, self.camera_y = camera_x, camera_y
+        # The coordinates of the cube relative to the mypal260
+        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)
+
+    # pump_control pi
+    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 mypal260
+        print(color)
+        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, 90, 0], 20, 0)
+        time.sleep(1.5)
+
+        # open pump
+        if "dev" in self.robot_m5:
+            self.pump_on()
+        elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+            self.gpio_status(True)
+        time.sleep(1.5)
+
+        self.mc.send_angle(2, 0, 20)
+        time.sleep(0.3)
+        self.mc.send_angle(3, -15, 20)
+        time.sleep(2)
+
+        self.mc.send_coords(self.move_coords[color], 20, 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:
+           self.pump_off()
+        elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+            self.gpio_status(False)
+        time.sleep(6)
+
+        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.mc.send_angles(self.move_angles[1], 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减小,向后方移动
+            self.move(x, y, color)
+
+    # init mypal260
+    def run(self):
+        if "dev" in self.robot_m5:
+            self.mc = MyPalletizer(self.robot_m5, 115200) 
+        elif "dev" in self.robot_raspi:
+            self.mc = MyPalletizer(self.robot_raspi, 1000000)
+        if not self.raspi:
+            self.pub_pump(False, self.Pin)
+        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 mypal260
+    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 mypal260
+    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
+
+    # detect cube color
+    def color_detect(self, img):
+        # set the arrangement of color'HSV
+        x = y = 0
+        for mycolor, item in self.HSV.items():
+            # print("mycolor:",mycolor)
+            redLower = np.array(item[0])
+            redUpper = np.array(item[1])
+
+            # transfrom the img to model of gray
+            hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
+            # print("hsv",hsv)
+
+            # 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 mypal260 relative to the target
+                    
+                    if mycolor == "red":
+                        self.color = 0
+                    elif mycolor == "green":
+                        self.color = 1
+                    elif mycolor == "cyan":
+                        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 mypal260
+    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 mypal260
+        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 mypal260
+            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 mypal260
+            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)
+
+        # close the window
+        if cv2.waitKey(1) & 0xFF == ord('q'):
+            cap.release()
+            cv2.destroyAllWindows()
+            sys.exit()

mycobot_ai/myPalletizer_260/scripts/combine_detect_obj_img_folder_opt.py

@@ -0,0 +1,667 @@
+#!/usr/bin/env python2
+# 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
+import rospy
+from visualization_msgs.msg import Marker
+from PIL import Image
+from threading import Thread
+import tkFileDialog as filedialog
+import Tkinter as tk
+from moving_utils import Movement
+from pymycobot.mypalletizer import MyPalletizer
+
+IS_CV_4 = cv2.__version__[0] == '4'
+__version__ = "1.0"  # Adaptive seeed
+
+
+class Object_detect(Movement):
+
+    def __init__(self, camera_x = 160, camera_y = 10):
+        # inherit the parent class
+        super(Object_detect, self).__init__()
+        # get path of file
+        dir_path = os.path.dirname(__file__)
+
+        # 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 = [
+            [141.2, -142.0, 210, -26.8],  # above the red bucket
+            [234.3, -120, 210, -48.77], # above the green bucket
+            [100.9, 159.3, 248.6, -124.27], # above the blue bucket
+            [-17.6, 161.6, 238.4, -152.31], # above the gray bucket  
+        ]
+
+        # 判断连接设备:ttyUSB*为M5，ttyACM*为seeed       
+        self.raspi = False
+        # 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.gpio_status(False)
+        # self.Pin = [2, 5]
+        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]
+        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
+            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
+        # load model of img recognition
+        # self.model_path = os.path.join(dir_path, "frozen_inference_graph.pb")
+        # self.pbtxt_path = os.path.join(dir_path, "graph.pbtxt")
+        # self.label_path = os.path.join(dir_path, "labels.json")
+        # # load class labels
+        # self.labels = json.load(open(self.label_path))
+
+        # 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()
+
+        # if IS_CV_4:
+        #     self.net = cv2.dnn.readNetFromTensorflow(self.model_path, self.pbtxt_path)
+        # else:
+        #     print('Load tensorflow model need the version of opencv is 4.')
+        #     exit(0)
+
+        # 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
+
+        self.cache_x = self.cache_y = 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)
+    # pump_control pi
+    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 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, 90, 0], 20, 0)
+        time.sleep(1.5)
+
+        # open pump
+        if "dev" in self.robot_m5:
+           self.pump_on()
+        elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+            self.gpio_status(True)
+        time.sleep(1.5)
+
+        self.mc.send_angle(2, 0, 20)
+        time.sleep(0.3)
+        self.mc.send_angle(3, -15, 20)
+        time.sleep(2)
+
+        self.mc.send_coords(self.move_coords[color], 20, 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:
+           self.pump_off()
+        elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+            self.gpio_status(False)
+        time.sleep(6)
+
+        self.mc.send_angles(self.move_angles[1], 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减小,向后方移动
+            self.move(x, y, color)
+
+    # init mypal260
+    def run(self):
+        if "dev" in self.robot_m5:
+            self.mc = MyPalletizer(self.robot_m5, 115200) 
+        elif "dev" in self.robot_raspi:
+            self.mc = MyPalletizer(self.robot_raspi, 1000000) 
+        if not self.raspi:
+            self.pub_pump(False, self.Pin)
+        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/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/myPalletizer_260/' + folder
+    path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/myPalletizer_260/' + folder
+
+    if os.path.exists(path1):
+        path = path1
+    elif os.path.exists(path2):
+        path = path2
+
+    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()
+
+    # Object_detect().take_photo()
+    # Object_detect().cut_photo()
+    # goal = Object_detect().distinguist()
+    
+    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')
+
+    # res_queue = []
+    # res_queue.extend(parse_folder('res/red'))
+    # res_queue.extend(parse_folder('res/green'))
+    # res_queue.extend(parse_folder('res/gray'))
+    # res_queue.extend(parse_folder('res/blue'))
+
+    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()

mycobot_ai/myPalletizer_260/scripts/pi_detect_obj_color.py

@@ -8,7 +8,7 @@ import time
 import json
 import os,sys
 import rospy
-from visualization_msgs.msg import Marker
+from visualization_msgs.msg import Marker   
 from pymycobot.mypalletizer import MyPalletizer
 from moving_utils import Movement
 
@@ -147,7 +147,7 @@ class Object_detect(Movement):
         time.sleep(3)
 
         # send coordinates to move mypal260
-        self.mc.send_coords([x, y, 160, 0], 20, 0) # -82.17
+        self.mc.send_coords([x, y, 160, 0], 20, 0)
         time.sleep(1.5)
         self.mc.send_coords([x, y, 90, 0], 20, 0)
         time.sleep(1.5)
@@ -167,14 +167,14 @@ class Object_detect(Movement):
         self.mc.send_coords(self.move_coords[color], 20, 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(4.5)
+        time.sleep(3)
-
+       
         # close pump
         if self.raspi:
             self.gpio_status(False)
         else:
             self.pub_pump(False, self.Pin)
-        time.sleep(4)
+        time.sleep(6)
 
         if color == 1:
             self.pub_marker(
@@ -183,6 +183,7 @@ class Object_detect(Movement):
             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[1], 20)
         time.sleep(1.5)
 
@@ -200,13 +201,10 @@ class Object_detect(Movement):
 
     # init mypal260
     def run(self):
-        if "dev" in self.robot_m5:
+        self.mc = MyPalletizer("/dev/ttyAMA0",1000000) # ok
-            self.mc = MyPalletizer(self.robot_m5,115200) 
-        if "dev" in self.robot_raspi:
-            self.mc = MyPalletizer(self.robot_raspi,1000000) 
         if not self.raspi:
             self.pub_pump(False, self.Pin)
-        self.mc.send_angles([-29.0, 5.88, -4.92, -76.28], 20) 
+        self.mc.send_angles([-29.0, 5.88, -4.92, -76.28], 20) # ok
         time.sleep(3)
 
     # draw aruco

mycobot_ai/myPalletizer_260/scripts/pi_detect_obj_img_folder_opt.py

@@ -1,6 +1,8 @@
 # encoding:utf-8
 #!/usr/bin/env python2
+
 from multiprocessing import Process, Pipe
+
 from cgi import parse
 from difflib import restore
 # import queue
@@ -28,7 +30,7 @@ __version__ = "1.0"  # Adaptive seeed
 
 class Object_detect(Movement):
 
-    def __init__(self, camera_x= 160, camera_y = 10):
+    def __init__(self, camera_x = 160, camera_y = 10):
         # inherit the parent class
         super(Object_detect, self).__init__()
         # get path of file
@@ -142,34 +144,26 @@ class Object_detect(Movement):
 
     # Grasping motion
     def move(self, x, y, color):
-        # send Angle to move mycobot
+        # send Angle to move mypal260
         self.mc.send_angles(self.move_angles[0], 20)
-        # self.pub_angles(self.move_angles[0], 20)
+        time.sleep(3)
-        # time.sleep(1.5)
-        # self.pub_angles(self.move_angles[1], 20)
-        # time.sleep(1.5)
-        # self.pub_angles(self.move_angles[2], 20)
-        # time.sleep(1.5)
         
-        # send coordinates to move mycobot 根据不同底板机械臂，调整吸泵高度
+        # send coordinates to move mypal260 根据不同底板机械臂，调整吸泵高度
-        # self.pub_coords([x, y, 165, -178.9, -1.57, -66], 20, 1)
+        self.mc.send_coords([x, y, 160, 0], 20, 0)
-        self.mc.send_coords([x, y, 160, -82.17], 20, 2)
+        time.sleep(1.5)
-        time.sleep(2.5)
+        self.mc.send_coords([x, y, 90, 0], 20, 0)
-        # self.pub_coords([x, y, 90, -178.9, -1.57, -66], 25, 1)
-        self.mc.send_coords([x, y, 90, -82.17], 20, 1)
         time.sleep(1.5)
 
         # open pump
         self.gpio_status(True)
-        time.sleep(0.5)
+        time.sleep(1.5)
 
         self.mc.send_angle(2, 0, 20)
         time.sleep(0.3)
-        self.mc.send_angle(3, -18, 20)
+        self.mc.send_angle(3, -15, 20)
         time.sleep(2)
 
         print(self.move_coords[color])
-        # self.pub_coords(self.move_coords[color], 20, 1)
 
         self.mc.send_coords(self.move_coords[color], 20, 1)
         self.pub_marker(self.move_coords[color][0] / 1000.0,
@@ -179,10 +173,10 @@ class Object_detect(Movement):
 
         # close pump
         self.gpio_status(False)
-        time.sleep(4)
+        time.sleep(3)
 
         self.mc.send_angles(self.move_angles[1], 20)
-        time.sleep(3)
+        time.sleep(1.5)
 
     # decide whether grab cube
     def decide_move(self, x, y, color):
@@ -490,8 +484,8 @@ def run():
     res_queue = [[], [], [], []]
     res_queue[0] = parse_folder('res/red')
     res_queue[1] = parse_folder('res/green')
-    res_queue[2] = parse_folder('res/gray')
+    res_queue[2] = parse_folder('res/blue')
-    res_queue[3] = parse_folder('res/blue')
+    res_queue[3] = parse_folder('res/gray')
 
     # res_queue = []
     # res_queue.extend(parse_folder('res/red'))

mycobot_ai/myPalletizer_260/scripts/test.py

@@ -2,9 +2,9 @@
 from pymycobot.mypalletizer import MyPalletizer
 from pymycobot.genre import Angle
 from pymycobot import PI_PORT, PI_BAUD  # 当使用树莓派版本的mycobot时，可以引用这两个变量进行MyCobot初始化
-import time
+import time,os
 
-mc = MyPalletizer("/dev/ttyUSB0", 115200)
+mc = MyPalletizer(os.popen("ls /dev/ttyUSB*").readline()[:-1], 115200)
 
 # mc = MyPalletizer("/dev/ttyAMA0", 1000000)
 # mc.send_angles([-29.0, 5.88, -4.92, -76.28],25) # init the point coords:[155.3, -86.1, 218.4, -47.28]
@@ -13,8 +13,8 @@ mc = MyPalletizer("/dev/ttyUSB0", 115200)
 # mc.send_angles([-47.1, 10.19, -10.1, -76.37],25) # above the red bucket; coords:[127.3, -137.1, 219.2, -29.26]
 # time.sleep(1.5)
 
-# mc.send_angles([0,0,0,0],25)
+mc.send_angles([0,0,-15,0],25)
-# time.sleep(2)
+time.sleep(2)
 
 # mc.send_coords([141.2, -142.0, 206.2, -26.8],25,1) # above the red bucket
 # time.sleep(2)
@@ -26,8 +26,8 @@ mc = MyPalletizer("/dev/ttyUSB0", 115200)
 # time.sleep(3)
 
 # mc.send_angle(3,0,25)
-print(mc.get_angles())
+# print(mc.get_angles())
-print(mc.get_coords())
+# print(mc.get_coords())
 
 # while True:
 #     print("angles:%s"%mc.get_angles())
@@ -38,4 +38,4 @@ print(mc.get_coords())
 # mc.set_servo_calibration(1)
 # mc.set_servo_calibration(2)
 # mc.set_servo_calibration(3)
-# mc.set_servo_calibration(4)
+# mc.set_servo_calibration(4) 

mycobot_description/urdf/260_urdf/mypal_260_aikit.urdf

@@ -3,7 +3,7 @@
 	
 <xacro:property name="width" value=".2" />
 
-   <link name="env">
+ <link name="env">
     <visual>
       <geometry>
         <mesh filename="package://mycobot_description/urdf/260_urdf/suit_env.dae" scale="1.2 1.2 1.2"/>
@@ -18,14 +18,16 @@
 	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
        <mesh filename="package://mycobot_description/urdf/260_urdf/base.dae"/>
       </geometry>
-      <origin xyz = " 0 -0.28 0.051 " rpy = " 1.5707963 0 -1.5707963"/>
+      <origin xyz = " 0 -0.28 0.051" rpy = " 1.5707963   0 -1.5707963"/>
+      <!-- <origin xyz = " 0 0 0 " rpy = " 0 0 0"/> -->
     </visual>
     <collision>
       <geometry>
 	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
         <mesh filename="package://mycobot_description/urdf/260_urdf/base.dae"/>
       </geometry>
-      <origin xyz = " 0 -0.28 0.051 " rpy = " 1.5707963 0 -1.5707963"/>
+      <!-- <origin xyz = " 0 -0.28 0.051 " rpy = " 1.5707963 0 1.5707963"/> -->
+      <!-- <origin xyz = " 0 0 0 " rpy = " 0 0 3.1415926"/> -->
     </collision>
 
   </link>
@@ -100,6 +102,7 @@
       <geometry>
        <!--- 0.0 0 -0.04 -->
        <cylinder length="0.015" radius="0.0152"/>
+        <!-- <mesh filename="package://mycobot_description/urdf/260_urdf/link5.dae"/> -->
       </geometry>
       <material name = "grey">
         <color rgba = "0.5 0.5 0.5 1"/>
@@ -123,7 +126,6 @@
     <origin xyz= "0 0 0.12" rpy = "0 0 0"/>  
   </joint>
 
-
   <joint name="joint2_to_joint1" type="revolute">
     <axis xyz="0 0 1"/>
     <limit effort = "1000.0" lower = "-0.014" upper = "1.49" velocity = "0"/>
@@ -132,18 +134,17 @@
     <origin xyz= "0 0 0.0" rpy = " -1.5707963 0 0 "/>  
   </joint>
 
-
   <joint name="joint3_to_joint2" type="revolute">
     <axis xyz=" 0 0 1"/>
-    <limit effort = "1000.0" lower = "-0.213" upper = "1.106" velocity = "0"/>
+    <limit effort = "1000.0" lower = "-0.213" upper = "0.872665" velocity = "0"/>
     <parent link="link2"/>
     <child link="link3"/>
     <origin xyz= "0.0 -0.14 0   " rpy = "0 0 0 "/>  
   </joint>
 
   <joint name="joint4_to_joint3" type="fixed">
-    <axis xyz=" 0 0 1"/>
+    <!-- <axis xyz=" 0 0 1"/> -->
-    <limit effort = "1000.0" lower = "0" upper = "1.5707963" velocity = "0"/>
+    <!-- <limit effort = "1000.0" lower = "0" upper = "1.5707963" velocity = "0"/> -->
     <parent link="link3"/>
     <child link="link4"/>
     <origin xyz= "0.134 -0.005   0.0" rpy = "3.1415926 0 0 "/>  
@@ -165,5 +166,4 @@
     <child link="env"/>
     <origin xyz= "0 0 0" rpy = "0 0 0"/>
   </joint>
-
 </robot>

mypalletizer_260/mypalletizer_260/config/mypal_260.rviz

@@ -9,7 +9,7 @@ Panels:
         - /RobotModel1
         - /TF1
       Splitter Ratio: 0.5
-    Tree Height: 775
+    Tree Height: 601
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -18,7 +18,7 @@ Panels:
       - /2D Nav Goal1
       - /Publish Point1
     Name: Tool Properties
-    Splitter Ratio: 0.588679016
+    Splitter Ratio: 0.5886790156364441
   - Class: rviz/Views
     Expanded:
       - /Current View1
@@ -29,6 +29,8 @@ Panels:
     Name: Time
     SyncMode: 0
     SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
 Toolbars:
   toolButtonStyle: 2
 Visualization Manager:
@@ -40,7 +42,7 @@ Visualization Manager:
       Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.0299999993
+        Line Width: 0.029999999329447746
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -58,7 +60,7 @@ Visualization Manager:
       Enabled: true
       Links:
         All Links Enabled: true
-        Expand link Details: false
+        Expand Joint Details: false
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
@@ -92,7 +94,6 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-       
       Name: RobotModel
       Robot Description: robot_description
       TF Prefix: ""
@@ -116,8 +117,7 @@ Visualization Manager:
           Value: true
         link5:
           Value: true
-        
+      Marker Scale: 0.30000001192092896
-      Marker Scale: 0.300000012
       Name: TF
       Show Arrows: true
       Show Axes: true
@@ -147,7 +147,10 @@ Visualization Manager:
     - Class: rviz/FocusCamera
     - Class: rviz/Measure
     - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
       Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
     - Class: rviz/SetGoal
       Topic: /move_base_simple/goal
     - Class: rviz/PublishPoint
@@ -157,33 +160,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.20289087
+      Distance: 1.2028908729553223
       Enable Stereo Rendering:
-        Stereo Eye Separation: 0.0599999987
+        Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
         X: 0
         Y: 0
-        Z: 0.107583851
+        Z: 0.10758385062217712
       Focal Shape Fixed Size: true
-      Focal Shape Size: 0.0500000007
+      Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.00999999978
+      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.440398335
+      Pitch: 0.42039862275123596
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.430389732
+      Yaw: 1.1754094362258911
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1056
+  Height: 892
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002e2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001a4000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002e2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006200000003efc0100000002fb0000000800540069006d00650100000000000006200000024400fffffffb0000000800540069006d0065010000000000000450000000000000000000000306000002e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -192,6 +195,204 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1855
+  Width: 1568
-  X: 65
+  X: 144
-  Y: 24
+  Y: 41
+
+# Panels:
+#   - Class: rviz/Displays
+#     Help Height: 78
+#     Name: Displays
+#     Property Tree Widget:
+#       Expanded:
+#         - /Global Options1
+#         - /Status1
+#         - /RobotModel1
+#         - /TF1
+#       Splitter Ratio: 0.5
+#     Tree Height: 775
+#   - Class: rviz/Selection
+#     Name: Selection
+#   - Class: rviz/Tool Properties
+#     Expanded:
+#       - /2D Pose Estimate1
+#       - /2D Nav Goal1
+#       - /Publish Point1
+#     Name: Tool Properties
+#     Splitter Ratio: 0.588679016
+#   - Class: rviz/Views
+#     Expanded:
+#       - /Current View1
+#     Name: Views
+#     Splitter Ratio: 0.5
+#   - Class: rviz/Time
+#     Experimental: false
+#     Name: Time
+#     SyncMode: 0
+#     SyncSource: ""
+# Toolbars:
+#   toolButtonStyle: 2
+# Visualization Manager:
+#   Class: ""
+#   Displays:
+#     - Alpha: 0.5
+#       Cell Size: 1
+#       Class: rviz/Grid
+#       Color: 160; 160; 164
+#       Enabled: true
+#       Line Style:
+#         Line Width: 0.0299999993
+#         Value: Lines
+#       Name: Grid
+#       Normal Cell Count: 0
+#       Offset:
+#         X: 0
+#         Y: 0
+#         Z: 0
+#       Plane: XY
+#       Plane Cell Count: 10
+#       Reference Frame: <Fixed Frame>
+#       Value: true
+#     - Alpha: 1
+#       Class: rviz/RobotModel
+#       Collision Enabled: false
+#       Enabled: true
+#       Links:
+#         All Links Enabled: true
+#         Expand link Details: false
+#         Expand Link Details: false
+#         Expand Tree: false
+#         Link Tree Style: Links in Alphabetic Order
+#         base:
+#           Alpha: 1
+#           Show Axes: false
+#           Show Trail: false
+#           Value: true
+#         link1:
+#           Alpha: 1
+#           Show Axes: false
+#           Show Trail: false
+#           Value: true
+#         link2:
+#           Alpha: 1
+#           Show Axes: false
+#           Show Trail: false
+#           Value: true
+#         link3:
+#           Alpha: 1
+#           Show Axes: false
+#           Show Trail: false
+#           Value: true
+#         link4:
+#           Alpha: 1
+#           Show Axes: false
+#           Show Trail: false
+#           Value: true
+#         link5:
+#           Alpha: 1
+#           Show Axes: false
+#           Show Trail: false
+#           Value: true
+       
+#       Name: RobotModel
+#       Robot Description: robot_description
+#       TF Prefix: ""
+#       Update Interval: 0
+#       Value: true
+#       Visual Enabled: true
+#     - Class: rviz/TF
+#       Enabled: true
+#       Frame Timeout: 15
+#       Frames:
+#         All Enabled: true
+#         base:
+#           Value: true
+#         link1:
+#           Value: true
+#         link2:
+#           Value: true
+#         link3:
+#           Value: true
+#         link4:
+#           Value: true
+#         link5:
+#           Value: true
+        
+#       Marker Scale: 0.300000012
+#       Name: TF
+#       Show Arrows: true
+#       Show Axes: true
+#       Show Names: true
+#       Tree:
+#         base:
+#           link1:
+#             link2:
+#               link3:
+#                 link4:
+#                   link5:
+#                     {}
+#       Update Interval: 0
+#       Value: true
+#   Enabled: true
+#   Global Options:
+#     Background Color: 48; 48; 48
+#     Default Light: true
+#     Fixed Frame: base
+#     Frame Rate: 30
+#   Name: root
+#   Tools:
+#     - Class: rviz/Interact
+#       Hide Inactive Objects: true
+#     - Class: rviz/MoveCamera
+#     - Class: rviz/Select
+#     - Class: rviz/FocusCamera
+#     - Class: rviz/Measure
+#     - Class: rviz/SetInitialPose
+#       Topic: /initialpose
+#     - Class: rviz/SetGoal
+#       Topic: /move_base_simple/goal
+#     - Class: rviz/PublishPoint
+#       Single click: true
+#       Topic: /clicked_point
+#   Value: true
+#   Views:
+#     Current:
+#       Class: rviz/Orbit
+#       Distance: 1.20289087
+#       Enable Stereo Rendering:
+#         Stereo Eye Separation: 0.0599999987
+#         Stereo Focal Distance: 1
+#         Swap Stereo Eyes: false
+#         Value: false
+#       Focal Point:
+#         X: 0
+#         Y: 0
+#         Z: 0.107583851
+#       Focal Shape Fixed Size: true
+#       Focal Shape Size: 0.0500000007
+#       Invert Z Axis: false
+#       Name: Current View
+#       Near Clip Distance: 0.00999999978
+#       Pitch: 0.440398335
+#       Target Frame: <Fixed Frame>
+#       Value: Orbit (rviz)
+#       Yaw: 0.430389732
+#     Saved: ~
+# Window Geometry:
+#   Displays:
+#     collapsed: false
+#   Height: 1056
+#   Hide Left Dock: false
+#   Hide Right Dock: false
+#   QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+#   Selection:
+#     collapsed: false
+#   Time:
+#     collapsed: false
+#   Tool Properties:
+#     collapsed: false
+#   Views:
+#     collapsed: false
+#   Width: 1855
+#   X: 65
+#   Y: 24

mypalletizer_260/mypalletizer_260/scripts/listen_real_of_topic.py

@@ -23,7 +23,7 @@ class Listener(object):
         """`mypal260/angles_real` subscriber callback method.
 
         Args:
-            data (MypalAngles): callback argument.
+            data (mypalAngles): callback argument.
         """
         # ini publisher object.
         #  初始化发布者对象

mypalletizer_260/mypalletizer_communication/launch/communication_topic.launch

@@ -1,9 +1,8 @@
 <launch>
-	<arg name="port" default="/dev/ttyUSB0" />
+	<arg name="port" default="/dev/ttyUSB1" />
 	<arg name="baud" default="115200" />
 
 	<!-- Open communication service -->
-	<!-- <node name="mycobot_services" pkg="mycobot_communication" type="mycobot_topics.py" output="screen"> -->
 	<node name="mypalletizer_services" pkg="mypalletizer_communication" type="mypal_topics.py" output="screen">
 	
 		<param name="port" type="string" value="$(arg port)" />

mypalletizer_260/mypalletizer_communication/scripts/mypal_topics.py

@@ -1,5 +1,5 @@
 #!/usr/bin/env python2
-# -*- coding: utf-8 -*-
+# -*- coding:utf-8 -*-
 import time
 import os
 import sys
@@ -7,7 +7,6 @@ import signal
 import threading
 import rospy
 from pymycobot.mypalletizer import MyPalletizer
-
 from mypalletizer_communication.msg import(
     MypalAngles,
     MypalCoords,
@@ -66,9 +65,10 @@ class MypalTopics(object):
     def __init__(self):
         super(MypalTopics, self).__init__()
 
-        rospy.init_node("Mypal_topics")
+        rospy.init_node("mypal_topics")
         rospy.loginfo("start ...")
-        port = rospy.get_param("~port", "/dev/ttyUSB0")
+        port = rospy.get_param("~port", os.popen("ls /dev/ttyUSB*").readline()[:-1])
+        print(port)
         baud = rospy.get_param("~baud", 115200)
         rospy.loginfo("%s,%s" % (port, baud))
         self.mc = MyPalletizer(port, baud)
@@ -105,8 +105,7 @@ class MypalTopics(object):
     def pub_real_angles(self):
         """Publish real angle"""
         """发布真实角度"""
-        pub = rospy.Publisher("Mypal/angles_real",
+        pub = rospy.Publisher("mypal/angles_real", MypalAngles, queue_size=5)
-                              MypalAngles, queue_size=5)
         ma = MypalAngles()
         while not rospy.is_shutdown():
             self.lock.acquire()
@@ -125,8 +124,7 @@ class MypalTopics(object):
     def pub_real_coords(self):
         """publish real coordinates"""
         """发布真实坐标"""
-        pub = rospy.Publisher("Mypal/coords_real",
+        pub = rospy.Publisher("mypal/coords_real", MypalCoords, queue_size=5)
-                              MypalCoords, queue_size=5)
         ma = MypalCoords()
         while not rospy.is_shutdown():
             self.lock.acquire()
@@ -158,7 +156,7 @@ class MypalTopics(object):
             self.mc.send_angles(angles, sp)
 
         sub = rospy.Subscriber(
-            "Mypal/angles_goal", MypalSetAngles, callback=callback
+            "mypal/angles_goal", MypalSetAngles, callback=callback
         )
         rospy.spin()
 
@@ -171,7 +169,7 @@ class MypalTopics(object):
             self.mc.send_coords(angles, sp, model)
 
         sub = rospy.Subscriber(
-            "Mypal/coords_goal", MypalSetCoords, callback=callback
+            "mypal/coords_goal", MypalSetCoords, callback=callback
         )
         rospy.spin()
 
@@ -185,7 +183,7 @@ class MypalTopics(object):
                 self.mc.set_gripper_state(1, 80)
 
         sub = rospy.Subscriber(
-            "Mypal/gripper_status", MypalGripperStatus, callback=callback
+            "mypal/gripper_status", MypalGripperStatus, callback=callback
         )
         rospy.spin()
 
@@ -199,11 +197,10 @@ class MypalTopics(object):
                 self.mc.set_basic_output(data.Pin2, 1)
 
         sub = rospy.Subscriber(
-            "Mypal/pump_status", MypalPumpStatus, callback=callback
+            "mypal/pump_status", MypalPumpStatus, callback=callback
         )
         rospy.spin()
 
-
 if __name__ == "__main__":
     Watcher()
     mc_topics = MypalTopics()

mypalletizer_260/mypalletizer_communication/scripts/mypal_topics_pi.py

@@ -16,8 +16,6 @@ from mypalletizer_communication.msg import(
     MypalPumpStatus,
 )
 
-# from pymycobot import MyCobotSocket
-
 
 class Watcher:
     """this class solves two problems with multithreaded
@@ -171,7 +169,6 @@ class MypalTopics(object):
         def callback(data):
             # angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz]
             angles = [data.x, data.y, data.z, data.rx]
-
             sp = int(data.speed)
             model = int(data.model)
             self.mc.send_coords(angles, sp, model)