optimize image recoginition of /scripts/detect_obj_img_folder_opt.py

mycobot_ai/launch/Readme.md

@@ -1,37 +0,0 @@
-### Tips：
-
-1. 请使用与机械臂相同型号的文件名文件。
-
-2. 请先执行以下操作:
-   + 1. 打开一个新的终端
-   + 2. 输入命令
-```bash
-chmod +x /home/h/catkin_mycobot/src/mycobot_ros/mycobot_communication/scripts/xxx.py
-							# 此处为各个新增文件的文件名
-```
-3. jetson nano的文件还没有使用机械臂进行过测试，可能存在问题。
-
-4. 数莓派版本的使用:
-   + 1. 打开VScode,新建一个文件，复制以下内容（请确保电脑与数莓派机械臂已经连接）并运行
-	```bash
-	from pymycobot import MyCobotSocket
-
-	mc = MyCobotSocket("192.168.10.10","9000")
-	mc.connect()
-	```
-
-   + 2. 打开“网络与internet”设置
-	更改适配器选项
-	右键打开数莓派的以太网属性
-	打开 “internet协议版本4” 的属性
-	选择 “使用下面的IP地址”
-	IP地址为 ：  192.168.10.100 （最后一位非10都可）
-	子网掩码为： 255.255.255.0
-	确认
-
-
-
-
-
-
-

mycobot_ai/launch/pi.launch

@@ -1,28 +0,0 @@
-<launch>
-	<arg name="port" default="192.168.10.10" />
-	<arg name="baud" default="9000" />
-
-    <!-- urdf文件，模型文件的路径 -->
-	<arg name="model" default="$(find mycobot_description)/urdf/jetsonNano/mycobot_urdf.urdf"/>
-	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
-	<!-- <arg name="gui" default="false" /> -->
-
-	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
-
-	<!-- Combinejoin values to TF -->
-	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" >
-		<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
-	</node>
-
-	<!-- mycobot-topics -->
-    <include file="$(find mycobot_communication)/launch/communication_topic_pi.launch">
-		<arg name="port" value="$(arg port)" />	
-		<arg name="baud" value="$(arg baud)" />	
-    </include>
-
-    <!-- listen and pub the real angles -->
-	<node name="real_listener" pkg="mycobot_280" type="listen_real_of_topic.py" />
-
-	<!-- Show in Rviz -->
-	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
-</launch>

mycobot_ai/launch/seeed.launch

@@ -1,27 +0,0 @@
-<launch>
-<arg name="port" default="/dev/ttyACM0" />
-	<arg name="baud" default="115200" />
-
-	<arg name="model" default="$(find mycobot_description)/urdf/seeed/mycobot_urdf.urdf"/>
-	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
-	<!-- <arg name="gui" default="false" /> -->
-
-	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
-
-	<!-- Combinejoin values to TF -->
-	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" >
-		<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
-	</node>
-
-	<!-- mycobot-topics -->
-    <include file="$(find mycobot_communication)/launch/communication_seeed.launch">
-		<arg name="port" value="$(arg port)" />	
-		<arg name="baud" value="$(arg baud)" />	
-    </include>
-
-    <!-- listen and pub the real angles -->
-	<node name="real_listener" pkg="mycobot_280" type="listen_real_of_topic.py" />
-
-	<!-- Show in Rviz -->
-	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
-</launch>

mycobot_ai/launch/vision.launch

@@ -1,6 +1,6 @@
-<launch>
+<launch>
-	<arg name="port" default="/dev/ttyACM0" />
+<arg name="port" default="/dev/ttyAMA0" />
-	<arg name="baud" default="115200" />
+	<arg name="baud" default="1000000" />
 
 	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_vision.urdf"/>
 	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />

mycobot_ai/launch/vision_wio.launch

@@ -1,8 +1,8 @@
 <launch>
-	<arg name="port" default="/dev/ttyTHS1" />
+	<arg name="port" default="/dev/ttyACM0" />
 	<arg name="baud" default="115200" />
 
-	<arg name="model" default="$(find mycobot_description)/urdf/jetsonNano/mycobot_urdf.urdf"/>
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_vision.urdf"/>
 	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
 	<!-- <arg name="gui" default="false" /> -->
 
@@ -14,7 +14,7 @@
 	</node>
 
 	<!-- mycobot-topics -->
-    <include file="$(find mycobot_communication)/launch/communication_jsnn.launch">
+    <include file="$(find mycobot_communication)/launch/communication_topic.launch">
 		<arg name="port" value="$(arg port)" />	
 		<arg name="baud" value="$(arg baud)" />	
     </include>

mycobot_ai/scripts/add_img.py

@@ -1,5 +1,4 @@
 # coding:utf-8
-from ast import keyword
 from fileinput import filename
 import os, cv2, sys
 
@@ -50,19 +49,19 @@ def take_photo():
 
 def cut_photo():
     
-    path_red = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/res/red'
+    path_red = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/red'
     for i, j, k in os.walk(path_red):
         file_len_red = len(k)
 
-    path_gray = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/res/gray'
+    path_gray = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/gray'
     for i, j, k in os.walk(path_gray):
         file_len_gray = len(k)
 
-    path_green = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/res/green'
+    path_green = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/green'
     for i, j, k in os.walk(path_green):
         file_len_green = len(k)
 
-    path_blue = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/res/blue'
+    path_blue = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/blue'
     for i, j, k in os.walk(path_blue):
         file_len_blue = len(k)
     print("请截取要识别的部分")
@@ -96,19 +95,19 @@ def cut_photo():
         cv2.imshow('crop', crop)
         # 选择红桶文件夹
         if kw == 'red':
-            cv2.imwrite('/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/res/red/goal{}.jpeg'.format(str(file_len_red + 1)),crop)
+            cv2.imwrite('/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/red/goal{}.jpeg'.format(str(file_len_red + 1)),crop)
             print('Saved')
         # 选择灰桶文件夹
         elif kw == 'gray':
-            cv2.imwrite('/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/res/gray/goal{}.jpeg'.format(str(file_len_gray+1)),crop)
+            cv2.imwrite('/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/gray/goal{}.jpeg'.format(str(file_len_gray+1)),crop)
             print('Saved')
         # 选择绿桶文件夹
         elif kw == 'green':
-            cv2.imwrite('/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/res/green/goal{}.jpeg'.format(str(file_len_green+1)),crop)
+            cv2.imwrite('/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/green/goal{}.jpeg'.format(str(file_len_green+1)),crop)
             print('Saved')
         # 选择蓝桶文件夹
         elif kw == 'blue':
-            cv2.imwrite('/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/res/blue/goal{}.jpeg'.format(str(file_len_blue+1)),crop)
+            cv2.imwrite('/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/blue/goal{}.jpeg'.format(str(file_len_blue+1)),crop)
             print('Saved')
 
     # 退出

mycobot_ai/scripts/ai_windows.py

@@ -1,182 +0,0 @@
-#!/usr/bin/env python3
-# encoding:utf-8
-
-from tkinter import ttk
-from tkinter import *
-import os
-import time
-
-import threading
-from multiprocessing import Process
-
-
-class Application(object):
-    def __init__(self):
-        self.win = Tk()
-        # 窗口置顶
-        self.win.wm_attributes('-topmost', 1)
-        self.ros = False
-        # 运行的文件
-        self.run_py = ""
-        # 判断通信口并给权限
-        try:
-            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_wio:
-                self.set_file(self.robot_wio)
-            elif "dev" in self.robot_m5:
-               self.set_file(self.robot_m5)
-            elif "dev" in self.robot_raspi:
-                self.change_file(self.robot_raspi)
-            elif "dev" in self.robot_jes:
-                self.change_file(self.robot_jes)
-        except Exception as e:
-            pass
-
-        # 设置标题
-        self.win.title("aikit启动工具")
-        self.win.geometry(
-            "550x350+100+100")  # 290 160为窗口大小，+10 +10 定义窗口弹出时的默认展示位置
-
-        self.chanse_code = Label(self.win, text="选择程序:", width=10)
-        self.chanse_code.grid(row=1)
-
-        self.myComboList = [u"颜色识别", u"物体识别", u"二维码识别"]
-        self.myCombox = ttk.Combobox(self.win, values=self.myComboList)
-        self.myCombox.grid(row=1, column=1)
-
-        self.add_btn = Button(self.win, text="添加新的物体图像", command=self.add_img)
-        self.add_btn.grid(row=1, column=2)
-
-        self.tips = "1、等待打开ros,大概需要15s\n2、选择所要运行的程序点击运行即可,开启大概需要10秒,可以通过查看终端查看开启情况。\n\n添加新的图像：\n1、点击按钮，等待开启摄像头\n2、选中图像框，按z键拍照\n3、使用鼠标框出需要识别的图像区域\n4、按Enter键提取图像\n5、再次按Enter键保存即可"
-
-        self.btn = Button(self.win, text="运行", command=self.start_run)
-        self.btn.grid(row=5)
-
-        self.close = Button(self.win, text="close", command=self.close_py)
-        self.close.grid(row=5, column=1)
-
-        self.t2 = None
-        self.log_data = Text(self.win, width=66, height=10)
-        self.log_data.grid(row=16, column=0, columnspan=10)
-        self.log_data.insert(END, self.tips)
-
-        self.open_ros()
-        self.win.protocol('WM_DELETE_WINDOW', self.close_rviz)
-
-    def close_rviz(self):
-        os.system(
-            "ps -ef | grep -E mycobot.rviz | grep -v 'grep' | awk '{print $2}' | xargs kill -9")
-        sys.exit(0)
-        
-    def set_file(self,port):
-        self.command = '<arg name="port" default="{}" />'.format(
-            port)
-        # 根据通信口修改ros启动文件
-        os.system(
-            "sed -i '2c {}' ~/catkin_ws/src/mycobot_ros/mycobot_ai/launch/vision.launch"
-            .format(self.command))
-
-    def change_file(self, port):
-        command1 = '<arg name="port" default="{}" />'.format(port)
-        command2 = '<arg name="baud" default="{}" />'.format(1000000)
-        # 根据通信口修改ros启动文件
-        os.system(
-            "sed -i '2c {}' ~/catkin_ws/src/mycobot_ros/mycobot_ai/launch/vision.launch".format(command1))
-        os.system(
-            "sed -i '3c {}' ~/catkin_ws/src/mycobot_ros/mycobot_ai/launch/vision.launch".format(command2))
-
-    def start_run(self):
-        try:
-            print(u"开始运行")
-            one = self.myCombox.get()
-            if one == u"颜色识别":
-                self.run_py = "detect_obj_color.py"
-                t2 = threading.Thread(target=self.open_py1)
-                t2.setDaemon(True)
-                t2.start()
-            elif one == u"物体识别":
-                self.run_py = "detect_obj_img.py"
-                t3 = threading.Thread(target=self.open_py)
-                t3.setDaemon(True)
-                t3.start()
-            elif one == u"二维码识别":
-                self.run_py = "detect_encode.py"
-                t3 = threading.Thread(target=self.open_py2)
-                t3.setDaemon(True)
-                t3.start()
-        except Exception as e:
-            self.tips = str(e)
-            self.log_data.insert(END, self.tips)
-
-    def open_py(self):
-        os.system(
-            "python ~/catkin_ws/src/mycobot_ros/mycobot_ai/scripts/detect_obj_img.py"
-        )
-
-    def open_py1(self):
-        os.system(
-            "python ~/catkin_ws/src/mycobot_ros/mycobot_ai/scripts/detect_obj_color.py"
-        )
-
-    def open_py2(self):
-        os.system(
-            "python ~/catkin_ws/src/mycobot_ros/mycobot_ai/scripts/detect_encode.py"
-        )
-
-    def add_img(self):
-        os.system(
-            "python ~/catkin_ws/src/mycobot_ros/mycobot_ai/scripts/add_img.py"
-        )
-
-    def open_ros(self):
-        t1 = threading.Thread(target=self.ross)
-        t1.setDaemon(True)
-        t1.start()
-        self.ros = True
-
-    def ross(self):
-        os.system(
-            "roslaunch ~/catkin_ws/src/mycobot_ros/mycobot_ai/launch/vision.launch"
-        )
-
-    def close_py(self):
-        t1 = threading.Thread(target=self.close_p)
-        t1.setDaemon(True)
-        t1.start()
-
-    def close_p(self):
-        # 关闭ai程序
-        os.system("ps -ef | grep -E " + self.run_py +
-                  " | grep -v 'grep' | awk '{print $2}' | xargs kill -9")
-
-    def get_current_time(self):
-        # 日志时间
-        """Get current time with format."""
-        current_time = time.strftime("%Y-%m-%d %H:%M:%S",
-                                     time.localtime(time.time()))
-        return current_time
-
-    def write_log_to_Text(self, logmsg):
-        # 设置日志函数
-        global LOG_NUM
-        current_time = self.get_current_time()
-        logmsg_in = str(current_time) + " " + str(logmsg) + "\n"  # 换行
-
-        if LOG_NUM <= 18:
-            self.log_data_Text.insert(END, logmsg_in)
-            LOG_NUM += len(logmsg_in.split("\n"))
-            # print(LOG_NUM)
-        else:
-            self.log_data_Text.insert(END, logmsg_in)
-            self.log_data_Text.yview("end")
-
-    def run(self):
-        self.win.mainloop()
-
-
-if __name__ == "__main__":
-    mc = Application()
-    mc.run()

mycobot_ai/scripts/detect_obj_color.py

@@ -64,6 +64,7 @@ class Object_detect(Movement):
             self.raspi = True
         if self.raspi:
             self.gpio_status(False)
+        self.Pin = [2, 5]
 
         # choose place to set cube
         self.color = 0
@@ -147,20 +148,10 @@ class Object_detect(Movement):
         # send coordinates to move mycobot
         self.pub_coords([x, y, 165,  -178.9, -1.57, -25.95], 20, 1)
         time.sleep(1.5)
-        if "dev" in self.robot_m5 or self.raspi:
-            self.pub_coords([x, y, 90,  -178.9, -1.57, -25.95], 20, 1)
-        elif "dev" in self.robot_wio:
-            h = 0
         
-            if 165 < x < 180:
+        self.pub_coords([x, y, 80,  -178.9, -1.57, -25.95], 20, 1)
-                h = 10
-            elif x > 180:
-                h = 20
-            elif x < 135:
-                h = -20
-            self.pub_coords([x, y, 31.9+h,  -178.9, -1, -25.95], 20, 1)
         
-        time.sleep(1.5)
+        time.sleep(1)
         # open pump
         if self.raspi:
             self.gpio_status(True)
@@ -213,34 +204,9 @@ class Object_detect(Movement):
         else:
             self.cache_x = self.cache_y = 0
             # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
-            if "dev" in self.robot_wio:
             
-                if (y < -30 and x > 140) or (x > 150 and y < -10):
-                    x -= 10
-                    y += 10
-                elif y > -10:
-                    y += 10
-                elif x > 170:
-                    x -= 10
-                    y += 10
-            elif "dev" in self.robot_raspi:
-                if x > 160:
-                    y += 10
-                elif y < -20:
-                    x -= 10
-                    y += 10
-            elif "dev" in self.robot_jes:
-                y += 13
-                x += 4
             
-            elif "dev" in self.robot_m5:
+            self.move(x,y, color)
-                x -= 10
-                if y < 0:
-                    y += 10
-                if y < -30:
-                    y += 7
-            print x, y
-            self.move(x, y, color)
 
     # init mycobot
     def run(self):

mycobot_ai/scripts/detect_obj_color_wio.py

@@ -0,0 +1,393 @@
+#encoding:utf-8
+
+from tokenize import Pointfloat
+import cv2
+import numpy as np
+import time
+import json
+import os
+import rospy
+from visualization_msgs.msg import Marker
+
+from moving_utils import Movement
+
+IS_CV_4 = cv2.__version__[0] == '4'
+
+class Object_detect(Movement):
+
+    def __init__(self, camera_x=150, camera_y=-10):
+        # inherit the parent class
+        super(Object_detect, self).__init__()
+        # get path of file
+        dir_path = os.path.dirname(__file__)
+        # 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])],
+		}
+        # 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)
+
+	    
+
+    # Grasping motion
+    def move(self, x,y,color):
+        angles = [
+           [-7.11, -6.94, -55.01, -24.16, 0, -38.84],  # init the point
+            [-1.14, -10.63, -87.8, 9.05, -3.07, -37.7],  # point to grab
+            [17.4, -10.1, -87.27, 5.8, -2.02, -37.7],  # point to grab
+        ]
+
+        coords = [
+        [106.1, -141.6, 240.9, -173.34, -8.15, -83.11],  # above the red bucket
+        [208.2, -127.8, 246.9, -157.51, -17.5, -71.18],  # above the green bucket
+        [209.7, -18.6, 230.4, -168.48, -9.86, -39.38],
+        [196.9, -64.7, 232.6, -166.66, -9.44, -52.47],
+        [126.6, -118.1, 305.0, -157.57, -13.72, -75.3],
+
+        ]
+
+        # send Angle to move mycobot
+        self.pub_angles(angles[0], 20)
+        time.sleep(1.5)
+        self.pub_angles(angles[1], 20)
+        time.sleep(1.5)
+        self.pub_angles(angles[2], 20)
+        time.sleep(1.5)
+        # send coordinates to move mycobot
+        self.pub_coords([x, y, 165,  -178.9, -1.57, -25.95], 20, 1)
+        time.sleep(1.5)
+        self.pub_coords([x, y, 40,  -178.9, -1.57, -25.95], 20, 1)
+        time.sleep(1.5)
+        # open pump
+        self.pub_pump(True,[20,21])
+        time.sleep(0.5)
+        self.pub_angles(angles[2], 20)
+        time.sleep(3)
+        self.pub_marker(coords[2][0]/1000.0, coords[2][1]/1000.0, coords[2][2]/1000.0)
+        
+        self.pub_angles(angles[1], 20)
+        time.sleep(1.5)
+        self.pub_marker(coords[3][0]/1000.0, coords[3][1]/1000.0, coords[3][2]/1000.0)
+        
+        self.pub_angles(angles[0], 20)
+        time.sleep(1.5)
+        self.pub_marker(coords[4][0]/1000.0, coords[4][1]/1000.0, coords[4][2]/1000.0)
+        
+        self.pub_coords(coords[color], 20, 1)
+        self.pub_marker(coords[color][0]/1000.0, coords[color][1]/1000.0, coords[color][2]/1000.0)
+        time.sleep(2)
+        # close pump
+        self.pub_pump(False,[20,21])
+        if color==1:
+           self.pub_marker(coords[color][0]/1000.0+0.04, coords[color][1]/1000.0-0.02)
+        elif color==0:
+           self.pub_marker(coords[color][0]/1000.0+0.03, coords[color][1]/1000.0)
+        self.pub_angles(angles[0], 20)
+        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
+            self.move(x-10,y+10,color)
+
+    # init mycobot
+    def run(self):
+        
+        for _ in range(10):
+            self.pub_angles([-7.11, -6.94, -55.01, -24.16, 0, -38.84], 20)
+            print(_)
+            time.sleep(0.5)
+        self.pub_pump(False,[20,21])
+
+    # 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
+   
+    # detect cube color
+    def color_detect(self, img):
+        # set the arrangement of color'HSV
+        x = y = 0
+        for mycolor, item in self.HSV.items():
+            redLower = np.array(item[0])
+            redUpper = np.array(item[1])
+            # transfrom the img to model of gray
+            hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
+            # 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 == "yellow":
+                        self.color = 1
+                    elif mycolor == "red":
+                        self.color = 0
+                    
+        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)
+    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)
+    
+
+        

mycobot_ai/scripts/detect_obj_img.py

@@ -25,6 +25,8 @@ class Object_detect(Movement):
         super(Object_detect, self).__init__()
         # get path of file
         dir_path = os.path.dirname(__file__)
+        
+        	
         # 移动角度
         self.move_angles = [
             [-7.11, -6.94, -55.01, -24.16, 0, 15],  # init the point
@@ -64,7 +66,7 @@ class Object_detect(Movement):
             self.raspi = True
         if self.raspi:
             self.gpio_status(False)
-
+        self.Pin = [2, 5]
         # choose place to set cube
         self.color = 0
         # parameters to calculate camera clipping parameters
@@ -77,6 +79,10 @@ class Object_detect(Movement):
         # 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
@@ -121,6 +127,8 @@ class Object_detect(Movement):
         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):
@@ -152,8 +160,8 @@ class Object_detect(Movement):
         self.pub_coords([x, y, 165,  -178.9, -1.57, -66], 20, 1)
         time.sleep(1.5)
         # 根据不同底板机械臂，调整吸泵高度
-        if "dev" in self.robot_m5 or "dev" in self.robot_raspi:
+        if "dev" in self.robot_m5:
-            # m5 and raspi
+            # m5 and jetson nano
             self.pub_coords([x, y, 90,  -178.9, -1.57, -66], 25, 1)
         elif "dev" in self.robot_wio:
             h = 0
@@ -166,8 +174,8 @@ class Object_detect(Movement):
             self.pub_coords([x, y, 31.9+h,  -178.9, -1, -66], 20, 1)
         elif "dev" in self.robot_jes:
             h = 0
-            if x < 130:
+            if x<130:
-                h = 15
+                h=15
             self.pub_coords([x, y, 90-h,  -178.9, -1.57, -66], 25, 1)
         time.sleep(1.5)
         # open pump
@@ -237,11 +245,11 @@ class Object_detect(Movement):
                     y += 5
                 # print x,y
             elif "dev" in self.robot_jes:
-                if y < 0:
+                if y<0:
-                    x += 5
+                    x+=5
-                    y += 3
+                    y+=3
-                y += 10
+                y+=10
-            print x, y
+            print x,y
             self.move(x, y, color)
 
     # init mycobot
@@ -476,6 +484,7 @@ def run():
     for i, j, k in os.walk(path):
         for l in k:
             goal.append(cv2.imread('local_photo/img/{}'.format(l)))
+    
     cap_num = 0
     cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
     if not cap.isOpened():

mycobot_ai/scripts/detect_obj_img_folder.py

@@ -315,7 +315,7 @@ class Object_detect(Movement):
     # detect object
     def obj_detect(self, img, goal):
         i = 0
-        MIN_MATCH_COUNT = 10
+        MIN_MATCH_COUNT = 5
         sift = cv2.xfeatures2d.SIFT_create()
 
         # find the keypoints and descriptors with SIFT
@@ -430,20 +430,21 @@ def run():
     # init mycobot
     detect.run()
 
-    _init_ = 20  #
+#    _init_ = 20  #
     init_num = 0
     nparams = 0
     num = 0
     real_sx = real_sy = 0
     while cv2.waitKey(1) < 0:
+        start_time = time.time()
         # read camera
         _, frame = cap.read()
         # deal img
         frame = detect.transform_frame(frame)
 
-        if _init_ > 0:
+#        if _init_ > 0:
-            _init_ -= 1
+#            _init_ -= 1
-            continue
+#            continue
         # calculate the parameters of camera clipping
         if init_num < 20:
             if detect.get_calculate_params(frame) is None:
@@ -519,6 +520,9 @@ def run():
 
             cv2.imshow("figure", frame)
 
+        end_time = time.time()
+        print("loop_time = ", end_time - start_time)
+
 
 if __name__ == "__main__":
     run()

mycobot_ai/scripts/detect_obj_img_folder_opt.py

@@ -0,0 +1,648 @@
+# 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
+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
+
+IS_CV_4 = cv2.__version__[0] == '4'
+__version__ = "1.0"  # Adaptive seeed
+
+
+class Object_detect(Movement):
+
+    def __init__(self, camera_x=150, camera_y=-10):
+        # inherit the parent class
+        super(Object_detect, self).__init__()
+        # get path of file
+        dir_path = os.path.dirname(__file__)
+
+        # 移动角度
+        self.move_angles = [
+            [-26.11, -6.94, -55.01, -24.16, 0, 15],  # init the point
+            [-1.14, 0.63, -87.8, 9.05, -3.07, 15],  # point to grab
+            [17.4, -10.1, -87.27, 5.8, -2.02, 15],  # point to grab
+        ]
+
+        # 移动坐标
+        self.move_coords = [
+            [120.1, -141.6, 240.9, -173.34, -8.15, -110.11],  # above the red bucket
+            # above the yello bucket
+            #[208.2, -127.8, 260.9, -157.51, -17.5, -71.18],
+            [205.6, -130.5, 263.0, -150.99, -0.07, -107.35],  # above the green bucket
+            [-20.0, 176.7, 242.6, -166.66, -9.44, -52.47],  # above the gray bucket
+            [104.9, 176.7, 242.6, -166.66, -9.44, -52.47],  # above the blue bucket
+            [126.6, -118.1, 305.0, -157.57, -13.72, -75.3],  
+        ]
+
+        # 判断连接设备: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]
+        # 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)
+
+    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)
+
+    # Grasping motion
+    def move(self, x, y, color):
+        # send Angle to move mycobot
+        self.pub_angles(self.move_angles[0], 20)
+        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
+        self.pub_coords([x, y, 165, -178.9, -1.57, -66], 20, 1)
+        time.sleep(1.5)
+        # 根据不同底板机械臂，调整吸泵高度
+        self.pub_coords([x, y, 90, -178.9, -1.57, -66], 25, 1)
+        time.sleep(1.5)
+        # open pump
+        self.gpio_status(True)
+        
+        time.sleep(0.5)
+        self.pub_angles(self.move_angles[2], 20)
+        time.sleep(3)
+        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.pub_angles(self.move_angles[1], 20)
+        time.sleep(1.5)
+        self.pub_marker(self.move_coords[3][0] / 1000.0,
+                        self.move_coords[3][1] / 1000.0,
+                        self.move_coords[3][2] / 1000.0)
+
+        self.pub_angles(self.move_angles[0], 20)
+        time.sleep(1.5)
+        self.pub_marker(self.move_coords[4][0] / 1000.0,
+                        self.move_coords[4][1] / 1000.0,
+                        self.move_coords[4][2] / 1000.0)
+
+        print(self.move_coords[color])
+        self.pub_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)
+        # close pump
+        self.gpio_status(False)
+        
+        time.sleep(1)
+        self.pub_marker(self.move_coords[color][0] / 1000.0 + 0.04,
+                        self.move_coords[color][1] / 1000.0 - 0.02)
+        self.pub_angles(self.move_angles[0], 20)
+        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减小,向后方移动
+            print(x, y)
+            self.move(x, y, color)
+
+    # init mycobot
+    def run(self):
+        for _ in range(5):
+            self.pub_angles([-26.11, -6.94, -55.01, -24.16, 0, -15], 20)
+            print(_)
+            time.sleep(0.5)
+
+    # 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
+
+        # else:
+        #     if(len(good) < MIN_MATCH_COUNT):
+
+        #         i += 1
+        #         if(i % 10 == 0):
+        #             print("Not enough matches are found - %d/%d" %
+        #                   (len(good), MIN_MATCH_COUNT))
+
+        #     matchesMask = None
+        if x + y > 0:
+            return x, y
+        else:
+            return None
+
+# The path to save the image folder
+def parse_folder(folder):
+    restore = []
+    path = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/' + folder
+    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/gray')
+    res_queue[3] = parse_folder('res/blue')
+
+    # 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)
+
+    child.join()
+
+
+if __name__ == "__main__":
+    run()
+    # Object_detect().take_photo()
+    # Object_detect().cut_photo()

mycobot_ai/scripts/detect_obj_img_wio.py

@@ -0,0 +1,406 @@
+#encoding:utf-8
+
+from tokenize import Pointfloat
+import cv2
+import numpy as np
+import time
+import json
+import os
+import rospy
+from visualization_msgs.msg import Marker
+
+from moving_utils import Movement
+
+IS_CV_4 = cv2.__version__[0] == '4'
+
+class Object_detect(Movement):
+
+    def __init__(self, camera_x=150, camera_y=-10):
+        # inherit the parent class
+        super(Object_detect, self).__init__()
+        # get path of file
+        dir_path = os.path.dirname(__file__)
+        # 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)
+
+    # Grasping motion
+    def move(self, x,y,color):
+        angles = [
+           [-7.11, -6.94, -55.01, -24.16, 0, -38.84],  # init the point
+            [-1.14, -10.63, -87.8, 9.05, -3.07, -37.7],  # point to grab
+            [17.4, -10.1, -87.27, 5.8, -2.02, -37.7],  # point to grab
+        ]
+
+        coords = [
+        [106.1, -141.6, 240.9, -173.34, -8.15, -83.11],  # above the red bucket
+        [208.2, -127.8, 246.9, -157.51, -17.5, -71.18],  # above the green bucket
+        [209.7, -18.6, 230.4, -168.48, -9.86, -39.38],
+        [196.9, -64.7, 232.6, -166.66, -9.44, -52.47],
+        [126.6, -118.1, 305.0, -157.57, -13.72, -75.3],
+
+        ]
+
+        # send Angle to move mycobot
+        self.pub_angles(angles[0], 20)
+        time.sleep(1.5)
+        self.pub_angles(angles[1], 20)
+        time.sleep(1.5)
+        self.pub_angles(angles[2], 20)
+        time.sleep(1.5)
+        # send coordinates to move mycobot
+        self.pub_coords([x, y, 165,  -178.9, -1.57, -25.95], 20, 1)
+        time.sleep(1.5)
+        self.pub_coords([x, y, 40,  -178.9, -1.57, -25.95], 20, 1)
+        time.sleep(1.5)
+        # open pump
+        self.pub_pump(True)
+        time.sleep(0.5)
+        self.pub_angles(angles[2], 20)
+        time.sleep(3)
+        self.pub_marker(coords[2][0]/1000.0, coords[2][1]/1000.0, coords[2][2]/1000.0)
+        
+        self.pub_angles(angles[1], 20)
+        time.sleep(1.5)
+        self.pub_marker(coords[3][0]/1000.0, coords[3][1]/1000.0, coords[3][2]/1000.0)
+        
+        self.pub_angles(angles[0], 20)
+        time.sleep(1.5)
+        self.pub_marker(coords[4][0]/1000.0, coords[4][1]/1000.0, coords[4][2]/1000.0)
+        
+        self.pub_coords(coords[color], 20, 1)
+        self.pub_marker(coords[color][0]/1000.0, coords[color][1]/1000.0, coords[color][2]/1000.0)
+        time.sleep(2)
+        # close pump
+        self.pub_pump(False)
+        if color==1:
+           self.pub_marker(coords[color][0]/1000.0+0.04, coords[color][1]/1000.0-0.02)
+        elif color==0:
+           self.pub_marker(coords[color][0]/1000.0+0.03, coords[color][1]/1000.0)
+        self.pub_angles(angles[0], 20)
+        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
+            self.move(x,y,color)
+
+    # init mycobot
+    def run(self):
+        
+        for _ in range(10):
+            self.pub_angles([-7.11, -6.94, -55.01, -24.16, 0, -38.84], 20)
+            print(_)
+            time.sleep(0.5)
+        self.pub_pump(False)
+
+    # 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, frame):
+        rows, cols = frame.shape[:-1]
+
+        # Resize image and swap BGR to RGB.
+        blob = cv2.dnn.blobFromImage(
+            frame,
+            size=(300, 300),
+            mean=(0, 0, 0),
+            swapRB=True,
+            crop=False,
+        )
+
+        # Detecting.
+        self.net.setInput(blob)
+        out = self.net.forward()
+        x, y = 0, 0
+
+        # Processing result.
+        for detection in out[0, 0, :, :]:
+            score = float(detection[2])
+            if score > 0.3:
+                class_id = detection[1]
+                left = detection[3] * cols
+                top = detection[4] * rows
+                right = detection[5] * cols
+                bottom = detection[6] * rows
+                if abs(right + bottom - left - top) > 380:
+                    continue
+                x, y = (left + right) / 2.0, (top + bottom) / 2.0
+                cv2.rectangle(
+                    frame,
+                    (int(left), int(top)),
+                    (int(right), int(bottom)),
+                    (0, 230, 0),
+                    thickness=2,
+                )
+                cv2.putText(
+                    frame,
+                    "{}: {}%".format(self.id_class_name(class_id),round(score * 100, 2)),
+                    (int(left), int(top) - 10),
+                    cv2.FONT_HERSHEY_COMPLEX_SMALL,
+                    1,
+                    (243, 0, 0),
+                    2,
+                )
+
+        if x+y > 0:
+            return x, y
+        else:
+            return None
+
+    
+
+
+
+
+if __name__ == "__main__":
+    # open the camera
+    cap_num = 0
+    cap = cv2.VideoCapture(cap_num)
+    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.obj_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 == 5:
+                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
+
+        cv2.imshow("figure",frame)
+    
+
+        
+

mycobot_ai/scripts/pump.py

@@ -5,34 +5,19 @@ import rospy
 import time
 from moving_utils import Movement
 
-
 class Pump(Movement):
 
-    def __init__(self):
+	def __init__(self):
-        super(Pump, self).__init__()
+		super(Pump, self).__init__()
-        rospy.init_node("pump", anonymous=True)
+		rospy.init_node("pump", anonymous=True)
-
-    def run(self):
-        self.pub_pump(False)
-        time.sleep(1)
-        self.pub_pump(True)
-        time.sleep(5)
-        self.pub_pump(False)
-
-    def gpiod(self):
-        import RPi.GPIO as GPIO
-        GPIO.setwarnings(False)
-        GPIO.setmode(GPIO.BCM)
-        GPIO.setup(20, GPIO.OUT)
-        GPIO.setup(21, GPIO.OUT)
-        GPIO.output(20, 0)
-        GPIO.output(21, 0)
-        time.sleep(3)
-        print "close"
-        GPIO.output(20, 1)
-        GPIO.output(21, 1)
 
+	def run(self):
+		self.pub_pump(False)
+		time.sleep(1)
+		self.pub_pump(True)
+		time.sleep(5)
+		self.pub_pump(False)
 
 if __name__ == "__main__":
-    pump = Pump()
+	pump = Pump()
-    pump.gpiod()
+	pump.run()

mycobot_ai/scripts/test.py

@@ -0,0 +1,104 @@
+# -*- coding: utf-8 -*-
+
+# from fileinput import filename
+# from genericpath import isfile
+# import os
+# from sys import path
+# import cv2
+# from PIL import Image
+
+
+# # #count=0
+# for file in dirs:
+#    pic_dir=os.path.join(path,file)  # res中子文件夹的路径
+#    print(pic_dir)
+#    for i in os.listdir(pic_dir):
+#       imgdir=os.path.join(pic_dir,i)
+#       print(imgdir)
+#    for i in os.listdir(pic_dir):
+#       image_dir=os.path.join(pic_dir,i)  #res中每个子文件夹中图片的路径
+#       img1 = cv2.imread(image_dir)  # 读取res中每个子文件夹中的图片
+#       #count+=1
+#       print(image_dir)#输出图片的路径
+#print(img1)#输出图片
+#print(count)#图片个数
+# dir_path = os.path.dirname(__file__)
+# print(dir_path)
+# # path1 = os.path.split(os.path.realpath(__file__))[0]
+# # print(path1)
+# path3=os.path.join(os.path.split(dir_path)[0]+'/res/')
+
+# print(path3)
+# #for file in os.listdir(path3):
+# pic_dir=os.path.join(path3+'red')
+# # print(pic_dir)
+# for i in os.listdir(pic_dir):
+#    img=os.path.join(pic_dir,i)
+#    print(img)
+# print(os.getcwd()+'/mycobot_ai/res/red')
+
+# list1 = [[1, 2, 3], [4, 5, 6], [7, 8, 9], [10, 11, 12]]
+# print(list1[0][0])
+# for i, v in enumerate(list1):
+#     print(i)
+
+# res = []
+# for i in list1:
+#     res.append(i)
+# print(res)
+
+# # for i in list1:
+# #    print(i)
+
+#    color=i
+#    print(color)
+# for color in range(0,4):
+#    print(color)
+
+from pymycobot.mycobot import MyCobot
+from pymycobot.genre import Angle
+from pymycobot import PI_PORT, PI_BAUD  # 当使用树莓派版本的mycobot时，可以引用这两个变量进行MyCobot初始化
+import time
+
+# mc = MyCobot("/dev/ttyACM0", 115200)
+
+mc = MyCobot("/dev/ttyAMA0", 1000000)
+# mc.send_angles([0,0,0,0,0,0], 20)
+
+time.sleep(2)
+# move_coords = [
+#         [120.1, -141.6, 240.9, -173.34, -8.15, -110.11],  # above the red bucket
+#         # above the green bucket
+#         #[208.2, -127.8, 260.9, -157.51, -17.5, -71.18],
+#         [205.6, -130.5, 263.0, -150.99, -0.07, -107.35],
+#         [209.7, -18.6, 230.4, -168.48, -9.86, -39.38],
+#         [196.9, -64.7, 232.6, -166.66, -9.44, -52.47],
+#         [126.6, -118.1, 305.0, -157.57, -13.72, -75.3],
+#     ]
+
+# mc.send_coords([126.6, -118.1, 305.0, -157.57, -13.72, -75.3],20,1)
+# time.sleep(2)
+# mc.send_coords([104.9, 176.7, 242.6, -166.66, -9.44, -52.47],20,1) # above the blue bucket
+# time.sleep(2)
+# mc.send_coords([-20.0, 176.7, 242.6, -166.66, -9.44, -52.47],20,1)  # abobe the gray bucket
+# time.sleep(2)
+# mc.send_coords([120.1,151.6,250.0,-173.34,-8.15,-110.11],20,1)
+# time.sleep(2)
+# mc.send_coords([104.9, 176.7, 242.6, -166.66, -9.44, -52.47],20,1)
+
+mc.send_angles([-26.11, -6.94, -55.01, -24.16, 0, 15],20)
+# mc.release_all_servos()
+time.sleep(3)
+print(mc.get_angles())
+# mc.send_angles([-1.14, 3.63, -87.8, 9.05, -3.07, 15],20)
+time.sleep(2)
+# print(mc.get_angles())
+# mc.send_angles([17.4, -10.1, -87.27, 5.8, -2.02, 15],20)
+# time.sleep(2)
+# print(mc.get_angles())
+
+# mc.release_servo(6)
+mc.release_all_servos()
+# mc.set_servo_calibration(6)
+# if os.listdir(path,filename='blue'):
+#     pass

mycobot_ai/scripts/tools.py

@@ -1,11 +1,13 @@
 from pymycobot.mycobot import MyCobot
-
+import os
 # name of device
-port = "/dev/ttyUSB0"
+# port = "/dev/ttyUSB0"
-mc = MyCobot(port)
+# mc = MyCobot(port)
 
 # release mycobot
 # mc.release_all_servos()
 
 # calibrate the sixth servo
-mc.set_servo_calibration(6)
+# mc.set_servo_calibration(6)
+
+print(os.getcwd())