add mira aikit

mycobot_ai/ai_mira/CMakeLists.txt

@@ -0,0 +1,204 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(ai_mira)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  mira
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES mycobot_ai
+#  CATKIN_DEPENDS mycobot_280
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/mycobot_ai.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/mycobot_ai_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_mycobot_ai.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

mycobot_ai/ai_mira/launch/vision_m5.launch

@@ -0,0 +1,27 @@
+<launch>
+	<arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" />
+
+	<arg name="model" default="$(find mycobot_description)/urdf/260_urdf/mypal_260_aikit.urdf"/>
+	<arg name="rvizconfig" default="$(find mypalletizer_260)/config/mypal_260.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>
+
+	<!-- mypalletizer-topics -->
+    <include file="$(find mypalletizer_communication)/launch/communication_topic.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="mypalletizer_260" 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/ai_mira/launch/vision_pi.launch

@@ -0,0 +1,27 @@
+<launch>
+<arg name="port" default="/dev/ttyAMA0" />
+	<arg name="baud" default="1000000" />
+
+	<arg name="model" default="$(find mycobot_description)/urdf/260_pi/mypal_260_pi_aikit.urdf"/>
+	<arg name="rvizconfig" default="$(find mypalletizer_260_pi)/config/mypal_260_pi.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>
+
+	<!-- mypalletizer-topics -->
+    <include file="$(find mypalletizer_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="mypalletizer_260_pi" 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/ai_mira/launch/vision_wio.launch

@@ -0,0 +1,27 @@
+<launch>
+	<arg name="port" default="/dev/ttyACM0" />
+	<arg name="baud" default="115200" />
+
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_vision.urdf"/>
+	<arg name="rvizconfig" default="$(find mypalletizer_260)/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.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="mypalletizer_260" 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/ai_mira/package.xml

@@ -0,0 +1,62 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ai_mira</name>
+  <version>0.0.0</version>
+  <description>The ai_mira package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="huang@todo.todo">huang</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/mycobot_ai</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>mira</build_depend>
+  <build_export_depend>mira</build_export_depend>
+  <exec_depend>mira</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

mycobot_ai/ai_mira/scripts/add_img.py

@@ -0,0 +1,139 @@
+# coding:utf-8
+from fileinput import filename
+import os, cv2, sys
+
+
+def take_photo():
+    # 提醒用户操作字典
+    print("*********************************************")
+    print("*  热键(请在摄像头的窗口使用)：             *")
+    print("*  z: 拍摄图片                              *")
+    print("*  q: 退出                                  *")
+    print("*********************************************")
+
+    # 创建/使用local_photo文件夹
+    class_name = "res"
+    if (os.path.exists("res")):
+        pass
+    else:
+        os.mkdir(class_name)
+
+    # 设置特定值
+
+    index = 'takephoto'
+    cap = cv2.VideoCapture(0)
+
+    while True:
+        # 读入每一帧
+        ret, frame = cap.read()
+
+        cv2.imshow("capture", frame)
+
+        # 存储
+        input = cv2.waitKey(1) & 0xFF
+        # 拍照
+        if input == ord('z'):
+            cv2.imwrite(
+                "%s/%s.jpeg" % (class_name, index),
+                cv2.resize(frame, (600, 480), interpolation=cv2.INTER_AREA))
+            break
+
+        # 退出
+        if input == ord('q'):
+
+            # 关闭窗口
+            cap.release()
+            cv2.destroyAllWindows()
+            sys.exit()
+
+
+def cut_photo():
+    
+    path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mypalletizer_260/'    # pi
+    path2 = '/home/u20/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mira/'         # m5
+
+    if os.path.exists(path1):
+        path = path1
+    elif os.path.exists(path2):
+        path = path2
+    else:
+        print("invalid file path! Please check whether the file path exists or modify it!")
+
+
+    path_red = path + '/res/red'
+    for i, j, k in os.walk(path_red):
+        file_len_red = len(k)
+
+    path_gray = path + '/res/gray'
+    for i, j, k in os.walk(path_gray):
+        file_len_gray = len(k)
+
+    path_green = path + '/res/green'
+    for i, j, k in os.walk(path_green):
+        file_len_green = len(k)
+
+    path_blue = path + '/res/blue'
+    for i, j, k in os.walk(path_blue):
+        file_len_blue = len(k)
+    print("请截取要识别的部分")
+    # root = tk.Tk()
+    # root.withdraw()
+    # temp1=filedialog.askopenfilename(parent=root)   #rgb
+    # temp2=Image.open(temp1,mode='r')
+    # temp2= cv.cvtColor(np.asarray(temp2),cv.COLOR_RGB2BGR)
+    # cut = np.array(temp2)
+
+    cut = cv2.imread(r"res/takephoto.jpeg")
+
+    cv2.imshow('original', cut)
+    # C:\Users\Elephant\Desktop\pymycobot+opencv\local_photo/takephoto.jpeg
+
+    # 选择ROI
+    roi = cv2.selectROI(windowName="original",
+                        img=cut,
+                        showCrosshair=False,
+                        fromCenter=False)
+    x, y, w, h = roi
+    print(roi)
+
+    msg = """\
+    Image save location:
+        1 - Save to red folder
+        2 - Save to gray folder
+        3 - Save to green folder
+        4 - Save to blue folder
+        """
+    print(msg)
+    kw = int(input("请输入保存图片文件夹数字编号:"))
+    # print(kw)
+
+    # 显示ROI并保存图片
+    if roi != (0, 0, 0, 0):
+        
+        crop = cut[y:y + h, x:x + w]
+        cv2.imshow('crop', crop)
+        # 选择红桶文件夹
+        if kw == 1:
+            cv2.imwrite(path + '/res/red/goal{}.jpeg'.format(str(file_len_red + 1)),crop)
+            print('Saved')
+        # 选择灰桶文件夹
+        elif kw == 2:
+            cv2.imwrite(path + '/res/gray/goal{}.jpeg'.format(str(file_len_gray+1)),crop)
+            print('Saved')
+        # 选择绿桶文件夹
+        elif kw == 3:
+            cv2.imwrite(path + '/res/green/goal{}.jpeg'.format(str(file_len_green+1)),crop)
+            print('Saved')
+        # 选择蓝桶文件夹
+        elif kw == 4:
+            cv2.imwrite(path + '/res/blue/goal{}.jpeg'.format(str(file_len_blue+1)),crop)
+            print('Saved')
+
+    # 退出
+    cv2.waitKey(0)
+    cv2.destroyAllWindows()
+
+
+if __name__ == "__main__":
+    take_photo()
+    cut_photo()

mycobot_ai/ai_mira/scripts/combine_detect_obj_color.py

@@ -0,0 +1,442 @@
+#!/usr/bin/env python3
+# -*- coding:utf-8 -*- 
+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.mira import Mira
+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__)
+
+        # declare mypal260
+        self.mc = None
+
+        # 移动角度
+        self.move_angles = [
+            [0.0, 0.0, 0.0],  # init the point
+            [19.48, 0.0, 0.0],  # point to grab
+            # [17.4, -10.1, -87.27, 5.8, -2.02, 15],  # point to grab
+        ]
+
+        # 移动坐标
+        self.move_coords = [
+            [-6.91, 175.86, 120.0],  # above the red bucket
+            [136.45, 149.22, 117.11], # above the green bucket
+            [107.54, -171.23, 117.11], # above the blue bucket
+            [-6.91, -175.86, 120.0], # 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.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])],
+            # "yellow": [np.array([26, 43, 46]), np.array([34, 255, 255])],
+            "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):
+        self.mc.set_gpio_state(0)
+
+    # 停止吸泵 m5
+    def pump_off(self):
+        self.mc.set_gpio_state(1)
+
+    # Grasping motion
+    def move(self, x, y, color):
+        # send Angle to move mira
+        print('color-->', color)
+        self.mc.set_angles(self.move_angles[0], 20)
+        time.sleep(3)
+
+        # send coordinates to move mypal260
+        self.mc.set_coords([x, -y, 58.84], 20)
+        time.sleep(1.5)
+        self.mc.set_coords([x, -y, 21.8], 20)
+        time.sleep(2)
+
+        # open pump
+        self.pump_on()
+        time.sleep(1.5)
+
+        self.mc.set_angle(2, 0, 30)
+        time.sleep(0.3)
+        self.mc.set_angle(3, 0, 30)
+        time.sleep(1)
+
+        self.mc.set_coords(self.move_coords[color], 30)
+        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(7)
+       
+        # close pump
+        
+        self.pump_off()
+        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.set_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):
+        self.mc = Mira(self.robot_m5, 115200)
+        self.mc.go_zero()
+        self.mc.set_angles([19.48, 0.0, 0.0], 40)
+        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
+                    print('mira_mycolor:', mycolor)
+                    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/ai_mira/scripts/combine_detect_obj_img_folder_opt.py

@@ -0,0 +1,596 @@
+#!/usr/bin/env python3
+# 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.mira import Mira
+
+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__)
+
+        # declare mypal260
+        self.mc = None
+        # 移动角度
+        self.move_angles = [
+            [0.0, 0.0, 0.0],  # init the point
+            [19.48, 0.0, 0.0],  # point to grab
+            # [17.4, -10.1, -87.27, 5.8, -2.02, 15],  # point to grab
+        ]
+
+        # 移动坐标
+        self.move_coords = [
+            [-6.91, 175.86, 120.0],  # above the red bucket
+            [136.45, 149.22, 117.11], # above the green bucket
+            [107.54, -171.23, 117.11], # above the blue bucket
+            [-6.91, -175.86, 120.0], # above the gray bucket     
+        ]
+
+        # 判断连接设备:ttyUSB*为M5，ttyACM*为seeed       
+        self.raspi = False
+        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]
+    
+        # choose place to set cube
+        self.color = 0
+        # parameters to calculate camera clipping parameters
+        self.x1 = self.x2 = self.y1 = self.y2 = 0
+        # set cache of real coord
+        self.cache_x = self.cache_y = 0
+
+        # use to calculate coord between cube and mycobot
+        self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
+        # The coordinates of the grab center point relative to the mycobot
+        self.camera_x, self.camera_y = camera_x, camera_y
+        # The coordinates of the cube relative to the mycobot
+        self.c_x, self.c_y = 0, 0
+        # The ratio of pixels to actual values
+        self.ratio = 0
+        # Get ArUco marker dict that can be detected.
+        self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
+        # Get ArUco marker params.
+        self.aruco_params = cv2.aruco.DetectorParameters_create()
+
+
+        # 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)
+    
+    # 开启吸泵 m5
+    def pump_on(self):
+        self.mc.set_gpio_state(0)
+
+    # 停止吸泵 m5
+    def pump_off(self):
+        self.mc.set_gpio_state(1)
+
+    # Grasping motion
+    def move(self, x, y, color):
+        # send Angle to move mypal260
+        self.mc.set_angles(self.move_angles[0], 20)
+        time.sleep(3)
+        
+        # send coordinates to move mypal260 根据不同底板机械臂，调整吸泵高度
+        self.mc.set_coords([x, -y, 58.84], 20)
+        time.sleep(1.5)
+        self.mc.set_coords([x, -y, 21.8], 20)
+        time.sleep(2)
+
+        # open pump
+        self.pump_on()
+        time.sleep(1.5)
+
+        self.mc.set_angle(2, 0, 30)
+        time.sleep(0.3)
+        self.mc.set_angle(3, 0, 30)
+        time.sleep(2)
+
+        self.mc.set_coords(self.move_coords[color], 30)
+        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(7)
+
+        # close pump
+        self.pump_off()
+        time.sleep(6)
+
+        self.mc.set_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):
+        self.mc = Mira(self.robot_m5, 115200) 
+        self.mc.go_zero()
+        self.mc.set_angles([19.48, 0.0, 0.0], 40)
+        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/ai_mira/' + folder
+    path2 = '/home/u20/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mira/' + 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()
+
+    res_queue = [[], [], [], []]
+    res_queue[0] = parse_folder('res/red')
+    res_queue[1] = parse_folder('res/green')
+    res_queue[2] = parse_folder('res/blue')
+    res_queue[3] = parse_folder('res/gray')
+
+    sift = cv2.xfeatures2d.SIFT_create()
+    kp_list, desc_list = compute_keypoints_and_descriptors(sift, res_queue)
+
+    # init a class of Object_detect
+    detect = Object_detect()
+
+    # init mycobot
+    detect.run()
+
+    # _init_ = 20  #
+    init_num = 0
+    nparams = 0
+    # num = 0
+    # real_sx = real_sy = 0
+    while True:
+        start_time = time.time()
+        if parent_conn.poll():
+            data = parent_conn.recv()
+            if data == STOP_PROCESSING:
+                break
+        # read camera
+        frame = get_frame(parent_conn)
+        # deal img
+        #frame = detect.transform_frame(frame)
+
+        # if _init_ > 0:
+        #     _init_ -= 1
+        #     continue
+        # calculate the parameters of camera clipping
+        if init_num < 20:
+            if detect.get_calculate_params(frame) is None:
+                # cv2.imshow("figure", frame)
+                continue
+            else:
+                x1, x2, y1, y2 = detect.get_calculate_params(frame)
+                detect.draw_marker(frame, x1, y1)
+                detect.draw_marker(frame, x2, y2)
+                detect.sum_x1 += x1
+                detect.sum_x2 += x2
+                detect.sum_y1 += y1
+                detect.sum_y2 += y2
+                init_num += 1
+                continue
+        elif init_num == 20:
+            detect.set_cut_params(
+                (detect.sum_x1) / 20.0,
+                (detect.sum_y1) / 20.0,
+                (detect.sum_x2) / 20.0,
+                (detect.sum_y2) / 20.0,
+            )
+            parent_conn.send((CROP_FRAME,
+                (detect.sum_x1) / 20.0,
+                (detect.sum_y1) / 20.0,
+                (detect.sum_x2) / 20.0,
+                (detect.sum_y2) / 20.0))
+            detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0
+            init_num += 1
+            continue
+
+        # calculate params of the coords between cube and mycobot
+        if nparams < 10:
+            if detect.get_calculate_params(frame) is None:
+                # cv2.imshow("figure", frame)
+                continue
+            else:
+                x1, x2, y1, y2 = detect.get_calculate_params(frame)
+                detect.draw_marker(frame, x1, y1)
+                detect.draw_marker(frame, x2, y2)
+                detect.sum_x1 += x1
+                detect.sum_x2 += x2
+                detect.sum_y1 += y1
+                detect.sum_y2 += y2
+                nparams += 1
+                print ("ok")
+                continue
+        elif nparams == 10:
+            nparams += 1
+            # calculate and set params of calculating real coord between cube and mycobot
+            detect.set_params((detect.sum_x1 + detect.sum_x2) / 20.0,
+                              (detect.sum_y1 + detect.sum_y2) / 20.0,
+                              abs(detect.sum_x1 - detect.sum_x2) / 10.0 +
+                              abs(detect.sum_y1 - detect.sum_y2) / 10.0)
+            print("ok")
+            continue
+
+        # get detect result
+        kp_img, desc_img = sift.detectAndCompute(frame, None)
+        frame = get_frame(parent_conn)
+        for i, v in enumerate(res_queue):
+            # HACK: to update frame every time
+            detect_result = detect.obj_detect(frame, v, kp_img, desc_img, kp_list[i], desc_list[i], parent_conn)
+            if detect_result:
+                x, y = detect_result
+                # calculate real coord between cube and mycobot
+                real_x, real_y = detect.get_position(x, y)
+                detect.color = i
+                detect.pub_marker(real_x / 1000.0, real_y / 1000.0)
+                detect.decide_move(real_x, real_y, detect.color)
+                # if num == 5:
+                #     detect.color = i
+                #     detect.pub_marker(real_sx / 5.0 / 1000.0,
+                #                       real_sy / 5.0 / 1000.0)
+                #     detect.decide_move(real_sx / 5.0, real_sy / 5.0,
+                #                        detect.color)
+                #     num = real_sx = real_sy = 0
+                # else:
+                #     num += 1
+                #     real_sy += real_y
+                #     real_sx += real_x
+                parent_conn.send(CLEAR_DRAW)
+
+        # cv2.imshow("figure", frame)
+        time.sleep(0.05)
+        end_time = time.time()
+        print("loop_time = ", end_time - start_time)
+
+        # close the window
+        if cv2.waitKey(1) & 0xFF == ord('q'):
+            # cap.release()
+            cv2.destroyAllWindows()
+            sys.exit()
+
+    child.join()
+    
+
+if __name__ == "__main__":
+    run()
+    # Object_detect().take_photo()
+    # Object_detect().cut_photo()

mycobot_ai/ai_mira/scripts/detect_encode.py

@@ -0,0 +1,248 @@
+# encoding: UTF-8
+#!/usr/bin/env python2
+import cv2 as cv
+import os
+import numpy as np
+import time
+import rospy
+from visualization_msgs.msg import Marker
+from moving_utils import Movement
+
+# y轴偏移量
+pump_y = -55
+# x轴偏移量
+pump_x = 15
+
+
+class Detect_marker(Movement):
+    def __init__(self):
+        super(Detect_marker, self).__init__()
+        # set cache of real coord
+        self.cache_x = self.cache_y = 0
+
+        # which robot
+        self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
+        self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
+        self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
+        self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
+        self.raspi = False
+        if "dev" in self.robot_m5:
+            self.Pin = [2, 5]
+        elif "dev" in self.robot_wio:
+            self.Pin = [20, 21]
+            for i in self.move_coords:
+                i[2] -= 20
+        elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+            import RPi.GPIO as GPIO
+            self.GPIO = GPIO
+            GPIO.setwarnings(False)
+            GPIO.setmode(GPIO.BCM)
+            GPIO.setup(20, GPIO.OUT)
+            GPIO.setup(21, GPIO.OUT)
+            self.raspi = True
+        if self.raspi:
+            self.gpio_status(False)
+        else:
+            self.pub_pump(False, self.Pin)
+        # Creating a Camera Object
+        cap_num = 0
+        self.cap = cv.VideoCapture(cap_num, cv.CAP_V4L)
+        # Get ArUco marker dict that can be detected.
+        self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
+        # Get ArUco marker params.
+        self.aruco_params = cv.aruco.DetectorParameters_create()
+        self.calibrationParams = cv.FileStorage(
+            "calibrationFileName.xml", cv.FILE_STORAGE_READ)
+        # Get distance coefficient.
+        self.dist_coeffs = self.calibrationParams.getNode("distCoeffs").mat()
+
+        height = self.cap.get(4)
+        focal_length = width = self.cap.get(3)
+        center = [width / 2, height / 2]
+        # Calculate the camera matrix.
+        self.camera_matrix = np.array(
+            [
+                [focal_length, 0, center[0]],
+                [0, focal_length, center[1]],
+                [0, 0, 1],
+            ],
+            dtype=np.float32,
+        )
+        # init a node and a publisher
+        rospy.init_node("encode_marker", anonymous=True)
+        self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
+
+        self.marker = Marker()
+        self.marker.header.frame_id = "/joint1"
+        self.marker.ns = "cube"
+        self.marker.type = self.marker.CUBE
+        self.marker.action = self.marker.ADD
+        self.marker.scale.x = 0.04
+        self.marker.scale.y = 0.04
+        self.marker.scale.z = 0.04
+        self.marker.color.a = 1
+        self.marker.color.r = 0.3
+        self.marker.color.g = 0.3
+        self.marker.color.b = 0.3
+
+        # marker position initial
+        self.marker.pose.position.x = 0
+        self.marker.pose.position.y = 0
+        self.marker.pose.position.z = 0.03
+        self.marker.pose.orientation.x = 0
+        self.marker.pose.orientation.y = 0
+        self.marker.pose.orientation.z = 0
+        self.marker.pose.orientation.w = 1.0
+
+    # Grasping motion
+    def move(self, x, y):
+        if self.raspi:
+            coords = [
+                [145.6, -64.9, 285.2, 179.88, 7.67, 179],
+                [130.1, -155.6, 243.9, 178.99, 5.38, -179.9]
+            ]
+        else:
+            coords = [
+                [135.0, -65.5, 280.1, 178.99, 5.38, -179.9],
+                [136.1, -141.6, 243.9, 178.99, 5.38, -179.9]
+            ]
+
+        # publish marker
+        self.marker.header.stamp = rospy.Time.now()
+        self.marker.pose.position.x = (coords[0][0]-x)/1000.0
+        self.marker.pose.position.y = (coords[0][1]-y)/1000.0
+        self.pub.publish(self.marker)
+
+        # send coordinates to move mycobot
+        self.pub_coords(coords[0], 30, 1)
+        time.sleep(2)
+        self.pub_coords([coords[0][0]-x, coords[0][1]-y,
+                        240, 178.99, 5.38, -179.9], 25, 1)
+        time.sleep(2)
+        self.pub_coords([coords[0][0]-x, coords[0][1]-y,
+                        200, 178.99, 5.38, -179.9], 25, 1)
+        time.sleep(2)
+        if "dev" in self.robot_m5 or self.raspi:
+            self.pub_coords([coords[0][0]-x, coords[0][1]-y,
+                            90, 178.99, 5.38, -179.9], 25, 1)
+        elif "dev" in self.robot_wio:
+            self.pub_coords([coords[0][0]-x+20, coords[0][1] -
+                            y-10, 70, 178.99, 5.38, -179.9], 25, 1)
+        time.sleep(2)
+        if self.raspi:
+            self.gpio_status(True)
+        else:
+            self.pub_pump(True, self.Pin)
+        time.sleep(1)
+        self.pub_coords(coords[0], 30, 1)
+        time.sleep(3)
+        self.pub_coords(coords[1], 30, 1)
+        time.sleep(2)
+        if self.raspi:
+            self.gpio_status(False)
+        else:
+            self.pub_pump(False, self.Pin)
+        # publish marker
+        time.sleep(1)
+        self.marker.header.stamp = rospy.Time.now()
+        self.marker.pose.position.x = coords[1][0]/1000.0
+        self.marker.pose.position.y = coords[1][1]/1000.0
+        self.pub.publish(self.marker)
+
+        self.pub_coords(coords[0], 30, 1)
+        time.sleep(2)
+
+    def gpio_status(self, flag):
+        if flag:
+            self.GPIO.output(20, 0)
+            self.GPIO.output(21, 0)
+        else:
+            self.GPIO.output(20, 1)
+            self.GPIO.output(21, 1)
+
+    # decide whether grab cube
+    def decide_move(self, x, y):
+
+        print(x, y)
+        # detect the cube status move or run
+        if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5:  # mm
+            self.cache_x, self.cache_y = x, y
+            return
+        else:
+            self.cache_x = self.cache_y = 0
+            if "dev" in self.robot_jes:
+                if x > -20:
+                    y += 10
+                if y > -25:
+                    x -= 5
+                x += 10
+
+            self.move(x, y)
+
+    # init mycobot
+    def init_mycobot(self):
+
+        for _ in range(5):
+            print _
+            self.pub_coords([145.6, -64.9, 285.2, 179.88, 7.67, 179], 20, 1)
+            time.sleep(0.5)
+
+    def run(self):
+        global pump_y, pump_x
+        self.init_mycobot()
+        num = sum_x = sum_y = 0
+        while cv.waitKey(1) < 0:
+            success, img = self.cap.read()
+            if not success:
+                print("It seems that the image cannot be acquired correctly.")
+                break
+
+            # transfrom the img to model of gray
+            gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
+            # Detect ArUco marker.
+            corners, ids, rejectImaPoint = cv.aruco.detectMarkers(
+                gray, self.aruco_dict, parameters=self.aruco_params
+            )
+
+            if len(corners) > 0:
+                if ids is not None:
+                    # get informations of aruco
+                    ret = cv.aruco.estimatePoseSingleMarkers(
+                        corners, 0.03, self.camera_matrix, self.dist_coeffs
+                    )
+                    # rvec:rotation offset,tvec:translation deviator
+                    (rvec, tvec) = (ret[0], ret[1])
+                    (rvec - tvec).any()
+                    xyz = tvec[0, 0, :]
+                    # calculate the coordinates of the aruco relative to the pump
+                    xyz = [round(xyz[0]*1000+pump_y, 2), round(xyz[1]
+                                                               * 1000+pump_x, 2), round(xyz[2]*1000, 2)]
+
+                    for i in range(rvec.shape[0]):
+                        # draw the aruco on img
+                        cv.aruco.drawDetectedMarkers(img, corners)
+                        cv.aruco.drawAxis(
+                            img,
+                            self.camera_matrix,
+                            self.dist_coeffs,
+                            rvec[i, :, :],
+                            tvec[i, :, :],
+                            0.03,
+                        )
+
+                        if num < 40:
+                            if self.raspi:
+                                sum_x -= 30
+                            sum_x += xyz[1]
+                            sum_y += xyz[0]
+                            num += 1
+                        elif num == 40:
+                            self.decide_move(sum_x/40.0, sum_y/40.0)
+                            num = sum_x = sum_y = 0
+
+            cv.imshow("encode_image", img)
+
+
+if __name__ == "__main__":
+    detect = Detect_marker()
+    detect.run()

mycobot_ai/ai_mira/scripts/detect_obj_color.py

@@ -0,0 +1,461 @@
+# -*- 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)
+        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
+        # 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)
+
+    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 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 self.raspi:
+            self.gpio_status(True)
+        else:
+            self.pub_pump(True, self.Pin)
+        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 self.raspi:
+            self.gpio_status(False)
+        else:
+            self.pub_pump(False, self.Pin)
+        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.pub_angles(self.move_angles[0], 20)
+        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):
+        self.mc = MyPalletizer("/dev/ttyAMA0",1000000) # ok
+        if not self.raspi:
+            self.pub_pump(False, self.Pin)
+        self.mc.send_angles([-29.0, 5.88, -4.92, -76.28], 20) # ok
+        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/ai_mira/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/ai_mira/scripts/detect_obj_img.py

@@ -0,0 +1,588 @@
+# encoding:utf-8
+#!/usr/bin/env python2
+
+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 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 = [
+            [-7.11, -6.94, -55.01, -24.16, 0, 15],  # init the point
+            [-1.14, -10.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],
+            [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],
+        ]
+        # 判断连接设备:ttyUSB*为M5，ttyACM*为seeed
+        self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
+        self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
+        self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
+        self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
+        self.raspi = False
+        if "dev" in self.robot_m5:
+            self.Pin = [2, 5]
+        elif "dev" in self.robot_wio:
+            self.Pin = [20, 21]
+            for i in self.move_coords:
+                i[2] -= 20
+        elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+            import RPi.GPIO as GPIO
+            self.GPIO = GPIO
+            GPIO.setwarnings(False)
+            GPIO.setmode(GPIO.BCM)
+            GPIO.setup(20, GPIO.OUT)
+            GPIO.setup(21, GPIO.OUT)
+
+            self.raspi = True
+        if self.raspi:
+            self.gpio_status(False)
+        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)
+        # 根据不同底板机械臂，调整吸泵高度
+        if "dev" in self.robot_m5:
+            # 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
+            if 165 < x < 180:
+                h = 10
+            elif x > 180:
+                h = 20
+            elif x < 135:
+                h = -20
+            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:
+                h=15
+            self.pub_coords([x, y, 90-h,  -178.9, -1.57, -66], 25, 1)
+        time.sleep(1.5)
+        # open pump
+        if self.raspi:
+            self.gpio_status(True)
+        else:
+            self.pub_pump(True, self.Pin)
+        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(2)
+        # close pump
+        if self.raspi:
+            self.gpio_status(False)
+        else:
+            self.pub_pump(False, self.Pin)
+        time.sleep(1)
+        if color == 1:
+            self.pub_marker(
+                self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02)
+        elif color == 0:
+            self.pub_marker(
+                self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0)
+        self.pub_angles(self.move_angles[0], 20)
+        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减小,向后方移动
+            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_m5:
+                y += 10
+                x -= 15
+                if y < -20:
+                    y += 5
+                # print x,y
+            elif "dev" in self.robot_jes:
+                if y<0:
+                    x+=5
+                    y+=3
+                y+=10
+            print x,y
+            self.move(x, y, color)
+
+    # init mycobot
+    def run(self):
+        if not self.raspi:
+            self.pub_pump(False, self.Pin)
+        for _ in range(5):
+            self.pub_angles([-7.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):
+        # 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,
+        #         )
+        i = 0
+        MIN_MATCH_COUNT = 10
+        sift = cv2.xfeatures2d.SIFT_create()
+
+        # find the keypoints and descriptors with SIFT
+        kp = []
+        des = []
+
+        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)
+
+        # 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)
+                    ccoord = (dst[0][0]+dst[1][0]+dst[2][0]+dst[3][0])/4.0
+                    cv2.putText(img, "{}".format(ccoord), (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)
+                    # 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
+
+
+def run():
+
+    # Object_detect().take_photo()
+    # Object_detect().cut_photo()
+    # goal = Object_detect().distinguist()
+    goal = []
+    path = os.getcwd()+'/local_photo/img'
+
+    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():
+        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, goal)
+        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)
+
+
+if __name__ == "__main__":
+    run()
+    # Object_detect().take_photo()
+    # Object_detect().cut_photo()

mycobot_ai/ai_mira/scripts/detect_obj_img_folder.py

@@ -0,0 +1,530 @@
+# encoding:utf-8
+#!/usr/bin/env python2
+
+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 = [
+            [-7.11, -6.94, -55.01, -24.16, 0, 15],  # init the point
+            [-1.14, -10.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([-7.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):
+        i = 0
+        MIN_MATCH_COUNT = 5
+        sift = cv2.xfeatures2d.SIFT_create()
+
+        # find the keypoints and descriptors with SIFT
+        kp = []
+        des = []
+
+        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)
+
+        # 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)
+                    ccoord = (dst[0][0] + dst[1][0] + dst[2][0] +
+                              dst[3][0]) / 4.0
+                    cv2.putText(img,
+                                "{}".format(ccoord), (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)
+                    # 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 run():
+
+    # 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')
+
+    cap_num = 0
+    cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
+    if not cap.isOpened():
+        cap.open()
+    # init a class of Object_detect
+    detect = Object_detect()
+    # init mycobot
+    detect.run()
+
+#    _init_ = 20  #
+    init_num = 0
+    nparams = 0
+    num = 0
+    real_sx = real_sy = 0
+    while cv2.waitKey(1) < 0:
+        start_time = time.time()
+        # 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
+                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
+        for i, v in enumerate(res_queue):
+            detect_result = detect.obj_detect(frame, v)
+            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.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
+
+            cv2.imshow("figure", frame)
+
+        end_time = time.time()
+        print("loop_time = ", end_time - start_time)
+
+
+if __name__ == "__main__":
+    run()
+    # Object_detect().take_photo()
+    # Object_detect().cut_photo()

mycobot_ai/ai_mira/scripts/detect_obj_img_folder_opt.py

@@ -0,0 +1,624 @@
+# 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
+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]
+        # 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 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
+        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)
+
+        print(self.move_coords[color])
+
+        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
+        self.gpio_status(False)
+        time.sleep(3)
+
+        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减小,向后方移动
+            print(x, y)
+            self.move(x, y, color)
+
+    # init mycobot
+    def run(self):
+        self.mc = MyPalletizer("/dev/ttyAMA0",1000000) # ok
+        if not self.raspi:
+            self.pub_pump(False, self.Pin)
+
+        self.mc.send_angles([-29.0, 5.88, -4.92, -76.28], 20) # ok
+        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 = []
+    path = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/myPalletizer_260/' + 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/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/ai_mira/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/ai_mira/scripts/labels.json

@@ -0,0 +1,82 @@
+{
+  "1": "person",
+  "2": "bicycle",
+  "3": "car",
+  "4": "motorcycle",
+  "5": "airplane",
+  "6": "bus",
+  "7": "train",
+  "8": "truck",
+  "9": "boat",
+  "10": "traffic light",
+  "11": "fire hydrant",
+  "13": "stop sign",
+  "14": "parking meter",
+  "15": "bench",
+  "16": "bird",
+  "17": "cat",
+  "18": "dog",
+  "19": "horse",
+  "20": "sheep",
+  "21": "cow",
+  "22": "elephant",
+  "23": "bear",
+  "24": "zebra",
+  "25": "giraffe",
+  "27": "backpack",
+  "28": "umbrella",
+  "31": "handbag",
+  "32": "tie",
+  "33": "suitcase",
+  "34": "frisbee",
+  "35": "skis",
+  "36": "snowboard",
+  "37": "sports ball",
+  "38": "kite",
+  "39": "baseball bat",
+  "40": "baseball glove",
+  "41": "skateboard",
+  "42": "surfboard",
+  "43": "tennis racket",
+  "44": "bottle",
+  "46": "wine glass",
+  "47": "cup",
+  "48": "fork",
+  "49": "knife",
+  "50": "spoon",
+  "51": "bowl",
+  "52": "banana",
+  "53": "apple",
+  "54": "sandwich",
+  "55": "orange",
+  "56": "broccoli",
+  "57": "carrot",
+  "58": "hot dog",
+  "59": "pizza",
+  "60": "donut",
+  "61": "cake",
+  "62": "chair",
+  "63": "couch",
+  "64": "potted plant",
+  "65": "bed",
+  "67": "dining table",
+  "70": "toilet",
+  "72": "tv",
+  "73": "laptop",
+  "74": "mouse",
+  "75": "remote",
+  "76": "keyboard",
+  "77": "cell phone",
+  "78": "microwave",
+  "79": "oven",
+  "80": "toaster",
+  "81": "sink",
+  "82": "refrigerator",
+  "84": "book",
+  "85": "clock",
+  "86": "vase",
+  "87": "scissors",
+  "88": "teddy bear",
+  "89": "hair drier",
+  "90": "toothbrush"
+}

mycobot_ai/ai_mira/scripts/moving_utils.py

@@ -0,0 +1,49 @@
+#encoding: UTF-8
+#!/usr/bin/env python2
+import rospy
+import time,os
+
+from mira_communication.msg import MiraSetAngles, MiraSetCoords, MiraPumpStatus
+
+
+class Movement(object):
+	"""Tools class: Communication with mycobot."""
+	def __init__(self):
+		super(Movement, self).__init__()
+		self.angle_pub = rospy.Publisher("mycobot/angles_goal", MiraSetAngles, queue_size=5)
+		self.coord_pub = rospy.Publisher("mycobot/coords_goal", MiraSetCoords, queue_size=5)
+
+		self.pump_pub = rospy.Publisher("mycobot/pump_status", MiraPumpStatus, queue_size=10)
+
+		self.angles = MiraSetAngles()
+		self.coords = MiraSetCoords()
+		self.pump = MiraPumpStatus()
+
+	def pub_coords(self, item, sp=20):
+		self.coords.x = item[0]
+		self.coords.y = item[1]
+		self.coords.z = item[2]
+		# self.coords.rx = item[3]
+		# self.coords.ry = item[4]
+		# self.coords.rz = item[5]
+		self.coords.speed = sp
+		# self.coords.model = m
+		self.coord_pub.publish(self.coords)
+
+
+	def pub_angles(self, item, sp):
+		self.angles.joint_1 = item[0]
+		self.angles.joint_2 = item[1]
+		self.angles.joint_3 = item[2]
+		# self.angles.joint_4 = item[3]
+		# self.angles.joint_5 = item[4]
+		# self.angles.joint_6 = item[5]
+		self.angles.speed = sp
+		self.angle_pub.publish(self.angles)
+
+
+	def pub_pump(self, flag):
+		self.pump.Status= flag
+		# self.pump.Pin1 = Pin[0]
+		# self.pump.Pin2 = Pin[1]
+		self.pump_pub.publish(self.pump)

mycobot_ai/ai_mira/scripts/openVideo.py

@@ -0,0 +1,8 @@
+import cv2 as cv
+
+if __name__ == "__main__":
+    cap_num = 0
+    cap = cv.VideoCapture(cap_num)
+    while cv.waitKey(1)<0:
+        _, img = cap.read()
+        cv.imshow("", img)

mycobot_ai/ai_mira/scripts/pump.py

@@ -0,0 +1,23 @@
+#encoding: UTF-8
+#!/usr/bin/env python2
+
+import rospy
+import time
+from moving_utils import Movement
+
+class Pump(Movement):
+
+	def __init__(self):
+		super(Pump, self).__init__()
+		rospy.init_node("pump", anonymous=True)
+
+	def run(self):
+		# self.pub_pump(True)
+		# time.sleep(1)
+		self.pub_pump(True)
+		time.sleep(5)
+		self.pub_pump(False)
+
+if __name__ == "__main__":
+	pump = Pump()
+	pump.run()

mycobot_ai/ai_mira/scripts/send_maker.py

@@ -0,0 +1,72 @@
+# encoding: utf-8
+
+import rospy
+import time
+from visualization_msgs.msg import Marker
+
+class Send_marker(object):
+    def __init__(self):
+        # 继承object类对象
+        super(Send_marker, self).__init__()
+        # 初始化一个节点，如果没有创建节点会导致无法发布信息
+        rospy.init_node("send_marker", anonymous=True)
+        # 创建一个发布者，用来发布marker
+        self.pub = rospy.Publisher("/test_marker", Marker, queue_size=1)
+        # 创建一个marker用来创建方块模型
+        self.marker = Marker()
+        # 配置其所属关系，其坐标均是相对于/joint1而言的。
+        # /joint1在模型中代表机械臂的底部
+        self.marker.header.frame_id = "/joint1"
+        # 设置marker的名称
+        self.marker.ns = "test_marker"
+        # 设置marker的类型是方块
+        self.marker.type = self.marker.CUBE
+        # 设置marker的动作为添加（没有这个名称的marker就为其添加一个）
+        self.marker.action = self.marker.ADD
+        # 设置marker的实际大小情况，单位为m
+        self.marker.scale.x = 0.04
+        self.marker.scale.y = 0.04
+        self.marker.scale.z = 0.04
+        # 设置marker的颜色，1.0表示255（这表示着一种比率换算）
+        self.marker.color.a = 1.0
+        self.marker.color.g = 1.0
+        self.marker.color.r = 1.0
+        # 初始化marker的位置以及其四维姿态
+        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
+
+    # 修改坐标并发布marker
+    def pub_marker(self, x, y, z=0.03):
+        # 设置marker的时间戳
+        self.marker.header.stamp = rospy.Time.now()
+        # 设置marker的空间坐标
+        self.marker.pose.position.x = x
+        self.marker.pose.position.y = y
+        self.marker.pose.position.z = z
+        # 发布marker
+        self.pub.publish(self.marker)
+
+    # 让marker发生位移效果
+    def run(self):
+        time.sleep(1)
+        self.pub_marker(0.2, 0)
+        time.sleep(1)
+        self.pub_marker(0.15, -0.05)
+        time.sleep(1)
+        self.pub_marker(0.15, 0.05)
+        time.sleep(1)
+        self.pub_marker(0.1, 0)
+        time.sleep(1)
+        self.pub_marker(0.136, -0.141)
+        time.sleep(1)
+        self.pub_marker(0.238, -0.147)
+        time.sleep(1)
+
+if __name__ == '__main__':
+    marker = Send_marker()
+    marker.run()

mycobot_ai/ai_mira/scripts/test.py

@@ -0,0 +1,41 @@
+# -*- coding: utf-8 -*-
+from pymycobot.mypalletizer import MyPalletizer
+from pymycobot.genre import Angle
+from pymycobot import PI_PORT, PI_BAUD  # 当使用树莓派版本的mycobot时，可以引用这两个变量进行MyCobot初始化
+import time,os
+
+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]
+# time.sleep(1.5)
+
+# 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,-15,0],25)
+time.sleep(2)
+
+# mc.send_coords([141.2, -142.0, 206.2, -26.8],25,1) # above the red bucket
+# time.sleep(2)
+# mc.send_coords([234.3, -120, 210, -48.77],25,1) # above the green bucket
+# time.sleep(2)
+# mc.send_coords([100.9, 159.3, 248.6, -124.27],20,1) # above the blue bucket
+# time.sleep(3)
+# mc.send_coords([-17.6, 161.6, 238.4, -152.31],20,1) # above the gray bucket
+# time.sleep(3)
+
+# mc.send_angle(3,0,25)
+# print(mc.get_angles())
+# print(mc.get_coords())
+
+# while True:
+#     print("angles:%s"%mc.get_angles())
+#     print("coords:%s"%mc.get_coords())
+#     print("\n")
+
+# mc.release_all_servos()
+# mc.set_servo_calibration(1)
+# mc.set_servo_calibration(2)
+# mc.set_servo_calibration(3)
+# mc.set_servo_calibration(4) 

mycobot_ai/ai_mira/scripts/tools.py

@@ -0,0 +1,101 @@
+from pymycobot.mira import Mira
+import os
+import time
+port = "/dev/ttyUSB0"
+mc = Mira(port)
+mc.go_zero()
+
+
+radians_chushi= [0.34, 0, 0]
+chushi_angles = [19.48, 0.0, 0.0]
+chishi_coords = [165.93, 58.69, 120.0]
+
+radians_pre = [0.0, 0.09, 0.53]
+pre_angles = [0.0, 5.16, 30.37]
+pre_coords = [170.33, 0.0, 58.84]
+
+radians_zhuaqu = [0.0, 0.33, 0.87]
+zhuaqu_angles = [0.0, 18.91, 49.85]
+zhuaqu_coords = [172.26, 0.0, 21.8]
+
+radians_qilai = [0.0, 0.0, 0.0]
+qilai_angles = [0.0, 0.0, 0.0]
+qilai_coords = [176.0, 0.0, 120.0]
+
+
+move_blue = [-1.01, 0.22, 0.0]
+blue_angles = [-57.87, 12.61, 0.0]
+blue_coords = [107.54, -171.23, 117.11]
+
+move_gray = [-1.61, 0.0, 0.0]
+gray_angles = [-92.25, 0.0, 0.0]
+gray_coords = [-6.91, -175.86, 120.0]
+
+move_green = [0.83, 0.22, 0]
+green_angles = [47.56, 12.61, 0.0]
+green_coords = [136.45, 149.22, 117.11]
+
+move_red = [1.61, 0.0, 0.0]
+red_angles = [92.25, 0.0, 0.0]
+red_coords = [-6.91, 175.86, 120.0]
+
+
+radians_chushi= [0.34, 0, 0]
+chushi_angles = [19.48, 0.0, 0.0]
+chishi_coords = [165.93, 58.69, 120.0]
+
+# mc.set_radians(radians_chushi, 50)
+# time.sleep(3)
+# print('chushi_angles:', mc.get_angles_info())
+# print('chishi_coords:', mc.get_coords_info())
+
+# mc.set_radians(radians_pre, 50)
+# time.sleep(3)
+# print('pre_angles:', mc.get_angles_info())
+# print('pre_coords:', mc.get_coords_info())
+
+# mc.set_radians(radians_zhuaqu, 50)
+# time.sleep(3)
+# print('zhuaqu_angles:', mc.get_angles_info())
+# print('zhuaqu_coords:', mc.get_coords_info())
+# # mc.set_gpio_state(0)
+
+# mc.set_radians(radians_qilai, 50)
+# time.sleep(3)
+# print('qilai_angles:', mc.get_angles_info())
+# print('qilai_coords:', mc.get_coords_info())
+
+mc.set_radians(move_blue, 50)
+time.sleep(4)
+print('blue_angles:', mc.get_angles_info())
+print('blue_coords:', mc.get_coords_info())
+# mc.set_gpio_state(1)
+time.sleep(2)
+
+mc.set_radians(radians_chushi, 50)
+time.sleep(5)
+print('chushi_angles:', mc.get_angles_info())
+print('chishi_coords:', mc.get_coords_info())
+
+# mc.set_radians(move_gray, 50)
+# time.sleep(6)
+# print('gray_angles:', mc.get_angles_info())
+# print('gray_coords:', mc.get_coords_info())
+
+# mc.set_radians(radians_chushi, 50)
+# time.sleep(2)
+
+# mc.set_radians(move_green, 50)
+# time.sleep(4)
+# print('green_angles:', mc.get_angles_info())
+# print('green_coords:', mc.get_coords_info())
+
+# mc.set_radians(radians_chushi, 50)
+# time.sleep(2)
+
+# mc.set_radians(move_red, 50)
+# time.sleep(4)
+# print('red_angles:', mc.get_angles_info())
+# print('red_coords:', mc.get_coords_info())
+
+mc.set_radians(radians_chushi, 50)

mycobot_description/urdf/mira_urdf/mira_aiki.urdf

@@ -0,0 +1,108 @@
+<?xml version="1.0"?>
+<robot  xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter" >
+	
+<xacro:property name="width" value=".2" />
+
+   <link name="env">
+    <visual>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mira_urdf/suit_env.dae" scale="1.2 1.2 1.2"/>
+      </geometry>
+      <origin xyz = "0 0 -0.02 " rpy = "1.5708 0 -1.5708"/>
+    </visual>
+  </link>
+
+  <link name="base">
+    <visual>
+      <geometry>
+	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
+       <mesh filename="package://mycobot_description/urdf/mira_urdf/Mira-base.dae"/>
+      </geometry>
+      <origin xyz = " 0 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/mira_urdf/Mira-base.dae"/>
+      </geometry>
+      <origin xyz = " 0 0 0.0 " rpy = " 0 0 0"/>
+    </collision>
+
+  </link>
+
+  <link name="link1">
+    <visual>
+      <geometry>
+       <mesh filename="package://mycobot_description/urdf/mira_urdf/Mira-1.dae"/>
+      </geometry>
+      <origin xyz = "0 0 0.122 " rpy = " 3.1415926 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mira_urdf/Mira-1.dae"/>
+      </geometry>
+      <origin xyz = "0 0  0.122 " rpy = " 3.1415926 0 0"/>
+    </collision>
+  </link>
+
+  <link name="link2">
+    <visual>
+      <geometry>    
+       <mesh filename="package://mycobot_description/urdf/mira_urdf/Mira-2.dae"/>
+      </geometry>
+      <origin xyz = "-0.03  0.112 0.0  " rpy = " 1.5708 0 0"/>
+    </visual>
+    <collision>
+      <geometry>    
+       <mesh filename="package://mycobot_description/urdf/mira_urdf/Mira-2.dae"/>
+      </geometry>
+      <origin xyz = "-0.03  0.112 0.0  " rpy = " 1.5708 0 0"/>
+    </collision>     
+
+  </link>
+
+  <link name="link3">
+    <visual>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mira_urdf/Mira-3.dae"/>
+      </geometry>
+      <origin xyz = "-0.03 0.23  0.0 " rpy = "1.5708 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+       <!--- 0.0 0 -0.04 -->
+       <mesh filename="package://mycobot_description/urdf/mira_urdf/Mira-3.dae"/>
+      </geometry>
+      <origin xyz = "-0.03 0.23  0.0 " rpy = "1.5708 0 0"/>
+    </collision>
+  </link>
+
+
+  <joint name="joint1_to_base" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-2.36" upper = "2.967" velocity = "0"/>
+    <parent link="base"/>
+    <child link="link1"/>
+    <origin xyz= "0 0 0.12" rpy = "3.1415926 0 0"/>  
+  </joint>
+
+
+  <joint name="joint2_to_joint1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "0" upper = "1.14" velocity = "0"/>
+    <parent link="link1"/>
+    <child link="link2"/>
+    <origin xyz= "0.03 0 0.01" rpy = " -1.5707963 3.1415926 3.1415926 "/>  
+  </joint>
+
+
+  <joint name="joint3_to_joint2" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "0" upper = "1.0472" velocity = "0"/>
+    <parent link="link2"/>
+    <child link="link3"/>
+    <origin xyz= "0.00 -0.12 0   " rpy = "0 0 0 "/>  
+  </joint>
+
+</robot>