diff --git a/mecharm/mecharm/scripts/simple_gui.py b/mecharm/mecharm/scripts/simple_gui.py
index 67784ec..387718d 100755
--- a/mecharm/mecharm/scripts/simple_gui.py
+++ b/mecharm/mecharm/scripts/simple_gui.py
@@ -33,7 +33,7 @@ class Window:
# 计算 Tk 根窗口的 x 和 y 坐标
x = (self.ws / 2) - 190
y = (self.hs / 2) - 250
- self.win.geometry("430x400+{}+{}".format(x, y))
+ self.win.geometry("430x400+{}+{}".format(int(x), int(y)))
# layout,布局
self.set_layout()
# input section,输入部分
diff --git a/mecharm/mecharm/scripts/teleop_keyboard.py b/mecharm/mecharm/scripts/teleop_keyboard.py
index 3d2b971..b7d1033 100755
--- a/mecharm/mecharm/scripts/teleop_keyboard.py
+++ b/mecharm/mecharm/scripts/teleop_keyboard.py
@@ -84,7 +84,7 @@ def teleop_keyboard():
init_pose = [0, 0, 0, 0, 0, 0, speed]
- home_pose = [0, 8, -127, 40, 0, 0, speed]
+ home_pose = [0, 30, 30, 0, 30, 0, speed]
# rsp = set_angles(*init_pose)
diff --git a/mecharm/mecharm_communication/scripts/mecharm_services.py b/mecharm/mecharm_communication/scripts/mecharm_services.py
index 322127c..aa1e026 100755
--- a/mecharm/mecharm_communication/scripts/mecharm_services.py
+++ b/mecharm/mecharm_communication/scripts/mecharm_services.py
@@ -1,17 +1,55 @@
-#!/usr/bin/env python2
- # -*- coding: UTF-8 -*-
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
import time
import rospy
+import os
+import fcntl
from mycobot_communication.srv import *
from pymycobot.mycobot import MyCobot
mc = None
+# Avoid serial port conflicts and need to be locked
+def acquire(lock_file):
+ open_mode = os.O_RDWR | os.O_CREAT | os.O_TRUNC
+ fd = os.open(lock_file, open_mode)
+
+ pid = os.getpid()
+ lock_file_fd = None
+
+ timeout = 50.0
+ start_time = current_time = time.time()
+ while current_time < start_time + timeout:
+ try:
+ # The LOCK_EX means that only one process can hold the lock
+ # The LOCK_NB means that the fcntl.flock() is not blocking
+ # and we are able to implement termination of while loop,
+ # when timeout is reached.
+ fcntl.flock(fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
+ except (IOError, OSError):
+ pass
+ else:
+ lock_file_fd = fd
+ break
+ # print('pid waiting for lock:%d'% pid)
+ time.sleep(1.0)
+ current_time = time.time()
+ if lock_file_fd is None:
+ os.close(fd)
+ return lock_file_fd
+
+
+def release(lock_file_fd):
+ # Do not remove the lockfile:
+ fcntl.flock(lock_file_fd, fcntl.LOCK_UN)
+ os.close(lock_file_fd)
+ return None
+
def create_handle():
global mc
- rospy.init_node("mecharm_services")
+ rospy.init_node("mycobot_services")
rospy.loginfo("start ...")
port = rospy.get_param("~port")
baud = rospy.get_param("~baud")
@@ -43,7 +81,9 @@ def set_angles(req):
sp = req.speed
if mc:
+ lock = acquire("/tmp/mycobot_lock")
mc.send_angles(angles, sp)
+ release(lock)
return SetAnglesResponse(True)
@@ -51,7 +91,9 @@ def set_angles(req):
def get_angles(req):
"""get angles,获取角度"""
if mc:
+ lock = acquire("/tmp/mycobot_lock")
angles = mc.get_angles()
+ release(lock)
return GetAnglesResponse(*angles)
@@ -68,14 +110,18 @@ def set_coords(req):
mod = req.model
if mc:
+ lock = acquire("/tmp/mycobot_lock")
mc.send_coords(coords, sp, mod)
+ release(lock)
return SetCoordsResponse(True)
def get_coords(req):
if mc:
+ lock = acquire("/tmp/mycobot_lock")
coords = mc.get_coords()
+ release(lock)
return GetCoordsResponse(*coords)
@@ -83,23 +129,26 @@ def switch_status(req):
"""Gripper switch status"""
"""夹爪开关状态"""
if mc:
+ lock = acquire("/tmp/mycobot_lock")
if req.Status:
mc.set_gripper_state(0, 80)
else:
mc.set_gripper_state(1, 80)
+ release(lock)
return GripperStatusResponse(True)
def toggle_pump(req):
if mc:
+ lock = acquire("/tmp/mycobot_lock")
if req.Status:
mc.set_basic_output(req.Pin1, 0)
mc.set_basic_output(req.Pin2, 0)
else:
mc.set_basic_output(req.Pin1, 1)
mc.set_basic_output(req.Pin2, 1)
-
+ release(lock)
return PumpStatusResponse(True)
@@ -107,12 +156,12 @@ robot_msg = """
MyCobot Status
--------------------------------
Joint Limit:
- joint 1: -160 ~ +160
- joint 2: -90 ~ +90
+ joint 1: -160 ~ +170
+ joint 2: -85 ~ +90
joint 3: -180 ~ +45
joint 4: -160 ~ +160
joint 5: -100 ~ +100
- joint 6: -180 ~ +180
+ joint 6: -∞ ~ +∞
Connect Status: %s
@@ -131,11 +180,15 @@ def output_robot_message():
atom_version = "unknown"
if mc:
+ lock = acquire("/tmp/mycobot_lock")
cn = mc.is_controller_connected()
+ release(lock)
if cn == 1:
connect_status = True
time.sleep(0.1)
+ lock = acquire("/tmp/mycobot_lock")
si = mc.is_all_servo_enable()
+ release(lock)
if si == 1:
servo_infomation = "all connected"
diff --git a/mecharm/mecharm_communication/scripts/mecharm_topics.py b/mecharm/mecharm_communication/scripts/mecharm_topics.py
index 02c3934..5039b0d 100755
--- a/mecharm/mecharm_communication/scripts/mecharm_topics.py
+++ b/mecharm/mecharm_communication/scripts/mecharm_topics.py
@@ -71,6 +71,8 @@ class MycobotTopics(object):
rospy.init_node("mycobot_topics")
rospy.loginfo("start ...")
port = rospy.get_param("~port", os.popen("ls /dev/ttyUSB*").readline()[:-1])
+ if not port:
+ port = rospy.get_param("~port", os.popen("ls /dev/ttyACM*").readline()[:-1])
baud = rospy.get_param("~baud", 115200)
rospy.loginfo("%s,%s" % (port, baud))
self.mc = MyCobot(port, baud)
diff --git a/mecharm/mecharm_pi/scripts/simple_gui.py b/mecharm/mecharm_pi/scripts/simple_gui.py
index 67784ec..c7e76cf 100644
--- a/mecharm/mecharm_pi/scripts/simple_gui.py
+++ b/mecharm/mecharm_pi/scripts/simple_gui.py
@@ -1,6 +1,7 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
# -*- coding: utf-8 -*-
-import Tkinter as tk
+# import Tkinter as tk # python2
+import tkinter as tk
from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
import rospy
import time
@@ -33,7 +34,7 @@ class Window:
# 计算 Tk 根窗口的 x 和 y 坐标
x = (self.ws / 2) - 190
y = (self.hs / 2) - 250
- self.win.geometry("430x400+{}+{}".format(x, y))
+ self.win.geometry("430x400+{}+{}".format(int(x), int(y)))
# layout,布局
self.set_layout()
# input section,输入部分
diff --git a/mecharm/mecharm_pi/scripts/teleop_keyboard.py b/mecharm/mecharm_pi/scripts/teleop_keyboard.py
index 3d2b971..6cfa0f3 100644
--- a/mecharm/mecharm_pi/scripts/teleop_keyboard.py
+++ b/mecharm/mecharm_pi/scripts/teleop_keyboard.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
# -*- coding:utf-8 -*-
from __future__ import print_function
from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
@@ -84,7 +84,7 @@ def teleop_keyboard():
init_pose = [0, 0, 0, 0, 0, 0, speed]
- home_pose = [0, 8, -127, 40, 0, 0, speed]
+ home_pose = [0, 30, 30, 0, 30, 0, speed]
# rsp = set_angles(*init_pose)
diff --git a/mycobot_280/mycobot_280/config/mycobot.rviz b/mycobot_280/mycobot_280/config/mycobot.rviz
index 68ab389..6fe1018 100644
--- a/mycobot_280/mycobot_280/config/mycobot.rviz
+++ b/mycobot_280/mycobot_280/config/mycobot.rviz
@@ -8,6 +8,7 @@ Panels:
- /Status1
- /RobotModel1
- /TF1
+ - /Marker1
Splitter Ratio: 0.5
Tree Height: 607
- Class: rviz/Selection
@@ -147,6 +148,14 @@ Visualization Manager:
{}
Update Interval: 0
Value: true
+ - Class: rviz/Marker
+ Enabled: true
+ Marker Topic: /cube
+ Name: Marker
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
diff --git a/mycobot_280/mycobot_280/scripts/1245.jpg b/mycobot_280/mycobot_280/scripts/1245.jpg
deleted file mode 100644
index 62c9920..0000000
Binary files a/mycobot_280/mycobot_280/scripts/1245.jpg and /dev/null differ
diff --git a/mycobot_280/mycobot_280/scripts/3a4.bmp b/mycobot_280/mycobot_280/scripts/3a4.bmp
deleted file mode 100644
index 8995b93..0000000
Binary files a/mycobot_280/mycobot_280/scripts/3a4.bmp and /dev/null differ
diff --git a/mycobot_280/mycobot_280/scripts/calibration_camera.py b/mycobot_280/mycobot_280/scripts/calibration_camera.py
deleted file mode 100644
index 84843b4..0000000
--- a/mycobot_280/mycobot_280/scripts/calibration_camera.py
+++ /dev/null
@@ -1,199 +0,0 @@
-#!/usr/bin/python3
-# -*- coding: UTF-8 -*-
-import os
-import numpy as np
-import cv2 as cv
-
-import cv2
-import matplotlib.pyplot as plt
-
-# 生成一张630*890的全黑图片
-# img = np.zeros((630,890,3),np.uint8)
-# plt.imshow(img[:,:,::-1])
-
-# while True:
-# # plt.show()
-# cv2.imshow('img', img)
-
-# key = cv2.waitKey(0)
-# if key == ord('q'):
-# break
-# elif key == ord('s'):
-# cv2.imwrite('/home/h/catkin_ws/src/mycobot_ros/mycobot_280/mycobot_280/scripts/123.png', img)
-# print('saved')
-
-# cv2.destroyAllWindows()
-
-path = os.path.join(os.path.dirname(__file__), "3a4.bmp")
-print(path)
-
-frame = cv2.imread(path)
-row, col, nc = frame.shape
-
-width_of_roi = 90
-# 这里是对全黑图片做处理，将图片以黑白间隔的形式zh
-for j in range(row):
- data = frame[j]
- for i in range(col):
- f = int(i / width_of_roi) % 2 ^ int(j / width_of_roi) % 2
- if f:
- frame[j][i][0] = 255
- frame[j][i][1] = 255
- frame[j][i][2] = 255
-cv2.imshow("", frame)
-cv2.waitKey(0) & 0xFF == ord("q")
-cv2.imwrite(os.path.join(os.path.dirname(__file__), "1245.jpg"), frame)
-
-
-# import os
-# import cv2
-# import threading
-
-# if_save = False
-# # 设置摄像头编号（由于电脑型号不同，分配给USB摄像头的编号也可能不同，一般为0或1）
-# cap_num = int(input("Input the camare number:"))
-# # 设置所存储的图片名称，设置为1，即表示从1开始累加存储。如：1.jpg,2.jpg,3.jpg......
-# name = int(input("Input start name, use number:"))
-
-# cap = cv2.VideoCapture(cap_num)
-# dir_path = os.path.dirname(__file__)
-
-# def save():
-# global if_save
-# while True:
-# input("Input any to save a image:")
-# if_save = True
-
-# # 开启线程进行摄像头拍摄
-# t = threading.Thread(target=save)
-# # 设置为异步运行
-# t.setDaemon(True)
-# t.start()
-
-# while cv2.waitKey(1) != ord("q"):
-# _, frame = cap.read()
-# if if_save:
-# # 设置名称为当前路径下，否则会因为运行环境的原因使得存储位置发生变化
-# img_name = os.path.join(dir_path,str(name)+".jpg")
-# # 存储图片
-# cv2.imwrite(img_name, frame)
-# print("Save {} successful.".format(img_name))
-# name += 1
-# if_save = False
-# cv2.imshow("", frame)
-
-
-# import os
-# import glob
-# import numpy as np
-# import cv2 as cv
-# from pprint import pprint
-
-# obj_points = [] # 3d点在现实世界的位置。
-# img_points = [] # 2d点在图片中的位置。
-
-# gray = None
-
-# def calibration_camera(row, col, path=None, cap_num=None, saving=False):
-# """校准摄像头
-
-# 参数说明:
-# row (int): 网格中的行数。
-# col (int): 网格中的列数。
-# path (string): 存放校准图片的位置。
-# cap_num (int): 表示摄像头的编号，一般0或1
-# saving (bool): 是否存放相机矩阵和失真系数(.npz).
-# """
-
-# # 终止准则/失效准则
-# criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
-# # 准备物体点, 比如 (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
-# obj_p = np.zeros((row * col, 3), np.float32)
-# obj_p[:, :2] = np.mgrid[0:row, 0:col].T.reshape(-1, 2)
-# # 组用于存储来自所有图像的对象点和图像点。
-
-
-# def _find_grid(img):
-# # 使用函数外的参数
-# global gray, obj_points, img_points
-# # 将图片转换为灰色度图片
-# gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
-# # 寻找棋盘的角落
-# ret, corners = cv.findChessboardCorners(gray, (row, col), None)
-# # 如果找到，则添加处理后的2d点和3d点
-# if ret == True:
-# obj_points.append(obj_p)
-# corners2 = cv.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
-# img_points.append(corners)
-# # 在图片中绘制并展示所寻找到的角
-# cv.drawChessboardCorners(img, (row, col), corners2, ret)
-
-# # 要求必须选择使用图片校准或者摄像头实时捕获校准中的一种
-# if path and cap_num:
-# raise Exception("The parameter `path` and `cap_num` only need one.")
-# # 图片校准
-# if path:
-# # 获取当前路径中的所有图片
-# images = glob.glob(os.path.join(path, "*.jpg"))
-# pprint(images)
-# # 对获取的每张图片进行处理
-# for f_name in images:
-# # 读取图片
-# img = cv.imread(f_name)
-# _find_grid(img)
-# # 展示图片
-# cv.imshow("img", img)
-# # 图片展示等待0.5s
-# cv.waitKey(500)
-# # 摄像头实时捕获校准
-# if cap_num:
-# # 开启摄像头
-# cap = cv.VideoCapture(cap_num)
-# while True:
-# # 读取摄像头开启后的每帧图片
-# _, img = cap.read()
-# _find_grid(img)
-# cv.imshow("img", img)
-# cv.waitKey(500)
-# print(len(obj_points))
-# if len(obj_points) > 14:
-# break
-# # 销毁展示窗口
-# cv.destroyAllWindows()
-# # 通过计算获取的3d点和2d点得出相机矩阵和失真系数
-# ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(
-# obj_points, img_points, gray.shape[::-1], None, None
-# )
-# print("ret: {}".format(ret))
-# print("matrix:")
-# pprint(mtx)
-# print("distortion: {}".format(dist))
-# # 决定是否存储所计算出的参数
-# if saving:
-# np.savez(os.path.join(os.path.dirname(__file__), "camera_mtx_dist.npz"), mtx=mtx, dist=dist)
-
-# mean_error = 0
-# for i in range(len(obj_points)):
-# img_points_2, _ = cv.projectPoints(obj_points[i], rvecs[i], tvecs[i], mtx, dist)
-# error = cv.norm(img_points[i], img_points_2, cv.NORM_L2) / len(img_points_2)
-# mean_error += error
-# print("total error: {}".format(mean_error / len(obj_points)))
-
-# return mtx, dist
-
-# if __name__ == "__main__":
-# path = os.path.dirname(__file__)
-# mtx, dist = calibration_camera(8, 6, path, saving=True)
-# # 设置是否需要测试计算出的参数
-# if_test = input("If testing the result (default: no), [yes/no]:")
-# if if_test not in ["y", "Y", "yes", "Yes"]:
-# exit(0)
-
-# cap_num = int(input("Input camera number:"))
-# cap = cv.VideoCapture(cap_num)
-# while cv.waitKey(1) != ord("q"):
-# _, img = cap.read()
-# h, w = img.shape[:2]
-# # 相机校准
-# dst = cv.undistort(img, mtx, dist)
-# cv.imshow("", dst)
\ No newline at end of file
diff --git a/mycobot_280/mycobot_280/scripts/mtx_dist.npz b/mycobot_280/mycobot_280/scripts/mtx_dist.npz
deleted file mode 100644
index 88f1cee..0000000
Binary files a/mycobot_280/mycobot_280/scripts/mtx_dist.npz and /dev/null differ
diff --git a/mycobot_280/mycobot_280/scripts/test.py b/mycobot_280/mycobot_280/scripts/test.py
deleted file mode 100755
index a6d3a01..0000000
--- a/mycobot_280/mycobot_280/scripts/test.py
+++ /dev/null
@@ -1,68 +0,0 @@
-#!/usr/bin/python
-# -*- coding: UTF-8 -*-
-
-from pymycobot.mycobot import MyCobot
-import time
-import threading
-import os
-
-print(os.path.join(os.path.dirname(__file__)))
-
-mc = MyCobot('/dev/ttyUSB0', 115200)
-mc.set_tool_reference([-50,0,0,0,0,0])
-mc.set_end_type(1)
-
-init_angles = [0, 0.52, -85.69, 0.0, 89.82, 0.08]
-start_angles = [18.64, 0.52, -85.69, 0.0, 89.82, 0.08]
-end_angles = [-18.56, 0.52, -85.69, 0.0, 89.82, 0.08]
-
-ang = []
-
-def wait_time(t):
- global ang
- for i in range(t*10+1):
- time.sleep(0.1)
- ang1 = mc.get_angles()
- ang.append(ang1)
- coord = mc.get_coords()
- # print('ange-------->', ang)
- print('coord-------->', coord)
-
-
-def get_ang():
- i = 0
-
- print('55555555555')
- ang_list = None
- while True:
- for j in range(i,len(ang)):
- i+=1
- # print('1111111111111')
- # if ang_list != ang:
- # ang_list = ang
-
- print('----------', ang[j])
-
-t = threading.Thread(target=get_ang)
-t.setDaemon(True)
-t.start()
-
-def move():
- try:
- mc.send_angles(init_angles, 5)
- time.sleep(0.1)
-
- # for _ in range(50):
-
- # mc.send_angles(start_angles, 5)
- # wait_time(6)
- # mc.send_angles(end_angles, 5)
- # wait_time(6)
- except Exception as e:
- print(e)
-
-while True:
- t = wait_time(1)
- print('dddd', t)
- move()
-
diff --git a/mycobot_280/mycobot_280pi/scripts/simple_gui.py b/mycobot_280/mycobot_280pi/scripts/simple_gui.py
index d2757cb..41f9258 100755
--- a/mycobot_280/mycobot_280pi/scripts/simple_gui.py
+++ b/mycobot_280/mycobot_280pi/scripts/simple_gui.py
@@ -1,6 +1,7 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
# -*- coding: utf-8 -*-
-import Tkinter as tk
+# import Tkinter as tk
+import tkinter as tk
from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
import rospy
import time
@@ -33,7 +34,7 @@ class Window:
# 计算 Tk 根窗口的 x 和 y 坐标
x = (self.ws / 2) - 190
y = (self.hs / 2) - 250
- self.win.geometry("430x400+{}+{}".format(x, y))
+ self.win.geometry("430x400+{}+{}".format(int(x), int(y)))
# layout,布局
self.set_layout()
# input section,输入部分
diff --git a/mycobot_280/mycobot_280pi/scripts/teleop_keyboard.py b/mycobot_280/mycobot_280pi/scripts/teleop_keyboard.py
index e5ee716..110f8a4 100755
--- a/mycobot_280/mycobot_280pi/scripts/teleop_keyboard.py
+++ b/mycobot_280/mycobot_280pi/scripts/teleop_keyboard.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
# encoding=utf-8
from __future__ import print_function
from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
diff --git a/mycobot_ai/ai_mecharm_270/launch/vision_wio.launch b/mycobot_ai/ai_mecharm_270/launch/vision_wio.launch
index 51eb78b..6b6f132 100644
--- a/mycobot_ai/ai_mecharm_270/launch/vision_wio.launch
+++ b/mycobot_ai/ai_mecharm_270/launch/vision_wio.launch
@@ -2,8 +2,8 @@
-
-
+
+
@@ -14,13 +14,13 @@
-
+
-
+
diff --git a/mycobot_ai/ai_mecharm_270/res/blue/goal9.jpeg b/mycobot_ai/ai_mecharm_270/res/blue/goal9.jpeg
new file mode 100644
index 0000000..ef91257
Binary files /dev/null and b/mycobot_ai/ai_mecharm_270/res/blue/goal9.jpeg differ
diff --git a/mycobot_ai/ai_mecharm_270/res/gray/goal9.jpeg b/mycobot_ai/ai_mecharm_270/res/gray/goal9.jpeg
new file mode 100644
index 0000000..e92138f
Binary files /dev/null and b/mycobot_ai/ai_mecharm_270/res/gray/goal9.jpeg differ
diff --git a/mycobot_ai/ai_mecharm_270/res/green/goal1.jpeg b/mycobot_ai/ai_mecharm_270/res/green/goal1.jpeg
deleted file mode 100644
index ab8991c..0000000
Binary files a/mycobot_ai/ai_mecharm_270/res/green/goal1.jpeg and /dev/null differ
diff --git a/mycobot_ai/ai_mecharm_270/res/green/goal11.jpeg b/mycobot_ai/ai_mecharm_270/res/green/goal11.jpeg
index b1242b0..e4315ee 100644
Binary files a/mycobot_ai/ai_mecharm_270/res/green/goal11.jpeg and b/mycobot_ai/ai_mecharm_270/res/green/goal11.jpeg differ
diff --git a/mycobot_ai/ai_mecharm_270/res/red/goal8.jpeg b/mycobot_ai/ai_mecharm_270/res/red/goal8.jpeg
new file mode 100644
index 0000000..0eaceae
Binary files /dev/null and b/mycobot_ai/ai_mecharm_270/res/red/goal8.jpeg differ
diff --git a/mycobot_ai/ai_mecharm_270/res/takephoto.jpeg b/mycobot_ai/ai_mecharm_270/res/takephoto.jpeg
index 1798785..54c25d7 100644
Binary files a/mycobot_ai/ai_mecharm_270/res/takephoto.jpeg and b/mycobot_ai/ai_mecharm_270/res/takephoto.jpeg differ
diff --git a/mycobot_ai/ai_mecharm_270/scripts/advance_detect_obj_color.py b/mycobot_ai/ai_mecharm_270/scripts/advance_detect_obj_color.py
new file mode 100644
index 0000000..04d3b0a
--- /dev/null
+++ b/mycobot_ai/ai_mecharm_270/scripts/advance_detect_obj_color.py
@@ -0,0 +1,493 @@
+#!/usr/bin/env python2
+# -*- coding:utf-8 -*-
+import cv2
+import numpy as np
+import time
+import os,sys
+import rospy
+from visualization_msgs.msg import Marker
+from pymycobot.mycobot import MyCobot
+from moving_utils import Movement
+
+
+IS_CV_4 = cv2.__version__[0] == '4'
+__version__ = "1.0"
+# Adaptive seeed
+
+
+class Object_detect(Movement):
+
+ def __init__(self, camera_x = 148, camera_y = 5): # m5
+ # def __init__(self, camera_x = 148, camera_y = -5): # pi
+ # inherit the parent class
+ super(Object_detect, self).__init__()
+ # get path of file
+ dir_path = os.path.dirname(__file__)
+
+ # declare 270
+ self.mc = None
+
+ # 移动角度
+ self.move_angles = [
+ [0, 0, 0, 0, 90, 0], # point to grab
+ [-33.31, 2.02, -10.72, -0.08, 95, -54.84], # init the point
+ ]
+
+ # 移动坐标
+ self.move_coords = [
+ [109.1, -118.8, 164.9, -179.02, 11.07, 132.93], # above the red bucket
+ [178.4, -98.5, 172.7, -175.8, 41.25, 159.41], # above the green bucket
+ [97.9, 139.9, 170.7, 163.54, 2.03, 156.04], # above the blue bucket
+ [-1.8, 143.8, 172.4, 170.69, -4.62, 161.79], # 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]
+ # self.Pin = [5]
+ elif "dev" in self.robot_wio:
+ # self.Pin = [20, 21]
+ self.Pin = [2, 5]
+ for i in self.move_coords:
+ i[2] -= 20
+ elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+ import RPi.GPIO as GPIO
+ GPIO.setwarnings(False)
+ self.GPIO = GPIO
+ GPIO.setmode(GPIO.BCM)
+ GPIO.setup(20, GPIO.OUT)
+ GPIO.setup(21, GPIO.OUT)
+ GPIO.output(20, 1)
+ GPIO.output(21, 1)
+ self.raspi = True
+ if self.raspi:
+ self.gpio_status(False)
+ # choose place to set cube
+ self.color = 0
+ # parameters to calculate camera clipping parameters
+ self.x1 = self.x2 = self.y1 = self.y2 = 0
+ # set cache of real coord
+ self.cache_x = self.cache_y = 0
+ # set color HSV
+ self.HSV = {
+ "yellow": [np.array([11, 115, 70]), np.array([40, 255, 245])],
+ "red": [np.array([0, 43, 46]), np.array([8, 255, 255])],
+ "green": [np.array([35, 43, 35]), np.array([90, 255, 255])],
+ "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 270
+ self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
+ # The coordinates of the grab center point relative to the 270
+ self.camera_x, self.camera_y = camera_x, camera_y
+ # The coordinates of the cube relative to the 270
+ self.c_x, self.c_y = 0, 0
+ # The ratio of pixels to actual values
+ self.ratio = 0
+ # Get ArUco marker dict that can be detected.
+ self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
+ # Get ArUco marker params.
+ self.aruco_params = cv2.aruco.DetectorParameters_create()
+
+ # init a node and a publisher
+ rospy.init_node("marker", anonymous=True)
+ self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
+ # init a Marker
+ self.marker = Marker()
+ self.marker.header.frame_id = "/joint1"
+ self.marker.ns = "cube"
+ self.marker.type = self.marker.CUBE
+ self.marker.action = self.marker.ADD
+ self.marker.scale.x = 0.04
+ self.marker.scale.y = 0.04
+ self.marker.scale.z = 0.04
+ self.marker.color.a = 1.0
+ self.marker.color.g = 1.0
+ self.marker.color.r = 1.0
+
+ # marker position initial
+ self.marker.pose.position.x = 0
+ self.marker.pose.position.y = 0
+ self.marker.pose.position.z = 0.03
+ self.marker.pose.orientation.x = 0
+ self.marker.pose.orientation.y = 0
+ self.marker.pose.orientation.z = 0
+ self.marker.pose.orientation.w = 1.0
+
+ # publish marker
+ def pub_marker(self, x, y, z=0.03):
+ self.marker.header.stamp = rospy.Time.now()
+ self.marker.pose.position.x = x
+ self.marker.pose.position.y = y
+ self.marker.pose.position.z = z
+ self.marker.color.g = self.color
+ self.pub.publish(self.marker)
+
+ # pump_control pi
+ def gpio_status(self, flag):
+ if flag:
+ self.GPIO.output(20, 0)
+ self.GPIO.output(21, 0)
+ else:
+ self.GPIO.output(20, 1)
+ self.GPIO.output(21, 1)
+
+ # 开启吸泵 m5
+ def pump_on(self):
+ # 让2号位工作
+ self.mc.set_basic_output(2, 0)
+ # 让5号位工作
+ self.mc.set_basic_output(5, 0)
+
+ # 停止吸泵 m5
+ def pump_off(self):
+ # 让2号位停止工作
+ self.mc.set_basic_output(2, 1)
+ # 让5号位停止工作
+ self.mc.set_basic_output(5, 1)
+
+ # Grasping motion
+ def move(self, x, y, color):
+ # send Angle to move 270
+ print(color)
+ self.mc.send_angles(self.move_angles[0], 30)
+ time.sleep(4)
+
+ # send coordinates to move 270
+ self.mc.send_coords([x, y, 140, 179.12, -0.18, 179.46], 30, 0)
+ time.sleep(3)
+ self.pub_marker(x/1000.0, y/1000.0, 140/1000.0)
+
+
+ self.mc.send_coords([x, y, 96, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15 m5
+ # self.mc.send_coords([x, y, 103, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15 pi
+ time.sleep(3)
+ self.pub_marker(x/1000.0, y/1000.0, 96/1000.0)
+
+
+ # open pump
+ if "dev" in self.robot_m5 or "dev" in self.robot_wio:
+ self.pump_on()
+ elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+ self.gpio_status(True)
+ time.sleep(1.5)
+
+ tmp = []
+ while True:
+ if not tmp:
+ tmp = self.mc.get_angles()
+ else:
+ break
+ time.sleep(0.5)
+
+ # print(tmp)
+ self.mc.send_angles([tmp[0], 17.22, -32.51, tmp[3], 97, tmp[5]],30)
+ time.sleep(3)
+
+
+
+ self.mc.send_coords(self.move_coords[color], 30, 1)
+ self.pub_marker(self.move_coords[color][0]/1000.0,
+ self.move_coords[color][1]/1000.0,
+ self.move_coords[color][2]/1000.0)
+ time.sleep(3)
+
+ # close pump
+ if "dev" in self.robot_m5 or "dev" in self.robot_wio:
+ self.pump_off()
+ elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+ self.gpio_status(False)
+ time.sleep(6)
+
+ if color == 1:
+ self.pub_marker(
+ self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02)
+ elif color == 0:
+ self.pub_marker(
+ self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0)
+
+ self.mc.send_angles(self.move_angles[1], 30)
+ 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 270
+ def run(self):
+ if "dev" in self.robot_m5:
+ self.mc = MyCobot(self.robot_m5, 115200)
+ elif "dev" in self.robot_wio:
+ self.mc = MyCobot(self.robot_wio, 115200)
+ elif "dev" in self.robot_raspi:
+ self.mc = MyCobot(self.robot_raspi, 1000000)
+ if not self.raspi:
+ self.pub_pump(False, self.Pin)
+ self.mc.send_angles([-33.31, 2.02, -10.72, -0.08, 95, -54.84], 30)
+ 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 270
+ 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 270
+ def get_position(self, x, y):
+ return ((y - self.c_y)*self.ratio + self.camera_x), ((x - self.c_x)*self.ratio + self.camera_y)
+
+ """
+ Calibrate the camera according to the calibration parameters.
+ Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times.
+ If two ARuco values have been calculated, clip the video.
+ """
+ def transform_frame(self, frame):
+ # enlarge the image by 1.5 times
+ fx = 1.5
+ fy = 1.5
+ frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy,
+ interpolation=cv2.INTER_CUBIC)
+ if self.x1 != self.x2:
+ # the cutting ratio here is adjusted according to the actual situation
+ frame = frame[int(self.y2*0.78):int(self.y1*1.1),
+ int(self.x1*0.86):int(self.x2*1.08)]
+ return frame
+
+ # detect cube color
+ def color_detect(self, img):
+ # set the arrangement of color'HSV
+ x = y = 0
+ gs_img = cv2.GaussianBlur(img, (3, 3), 0) # 高斯模糊
+ # transfrom the img to model of gray
+ hsv = cv2.cvtColor(gs_img, cv2.COLOR_BGR2HSV)
+
+ for mycolor, item in self.HSV.items():
+ # print("mycolor:",mycolor)
+ redLower = np.array(item[0])
+ redUpper = np.array(item[1])
+
+ # wipe off all color expect color in range
+ mask = cv2.inRange(hsv, item[0], item[1])
+
+ # a etching operation on a picture to remove edge roughness
+ erosion = cv2.erode(mask, np.ones((1, 1), np.uint8), iterations=2)
+
+ # the image for expansion operation, its role is to deepen the color depth in the picture
+ dilation = cv2.dilate(erosion, np.ones(
+ (1, 1), np.uint8), iterations=2)
+
+ # adds pixels to the image
+ target = cv2.bitwise_and(img, img, mask=dilation)
+
+ # the filtered image is transformed into a binary image and placed in binary
+ ret, binary = cv2.threshold(dilation, 127, 255, cv2.THRESH_BINARY)
+
+ # get the contour coordinates of the image, where contours is the coordinate value, here only the contour is detected
+ contours, hierarchy = cv2.findContours(
+ dilation, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+
+ if len(contours) > 0:
+ # do something about misidentification
+ boxes = [
+ box
+ for box in [cv2.boundingRect(c) for c in contours]
+ if min(img.shape[0], img.shape[1]) / 10
+ < min(box[2], box[3])
+ < min(img.shape[0], img.shape[1]) / 1
+ ]
+ if boxes:
+ for box in boxes:
+ x, y, w, h = box
+ # find the largest object that fits the requirements
+ c = max(contours, key=cv2.contourArea)
+ # get the lower left and upper right points of the positioning object
+ x, y, w, h = cv2.boundingRect(c)
+ # locate the target by drawing rectangle
+ cv2.rectangle(img, (x, y), (x+w, y+h), (153, 153, 0), 2)
+ # calculate the rectangle center
+ x, y = (x*2+w)/2, (y*2+h)/2
+ # calculate the real coordinates of 270 relative to the target
+
+ if mycolor == "red":
+ self.color = 0
+ elif mycolor == "green":
+ self.color = 1
+ elif mycolor == "cyan" or mycolor == "blue":
+ self.color = 2
+ else:
+ self.color = 3
+
+ if abs(x) + abs(y) > 0:
+ return x, y
+ else:
+ return None
+
+if __name__ == "__main__":
+
+ # open the camera
+ cap_num = 0
+ cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
+ if not cap.isOpened():
+ cap.open()
+ # init a class of Object_detect
+ detect = Object_detect()
+ # init 270
+ 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 270
+ 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 270
+ 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 270
+ 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()
\ No newline at end of file
diff --git a/mycobot_ai/ai_mecharm_270/scripts/advance_detect_obj_img_folder.py b/mycobot_ai/ai_mecharm_270/scripts/advance_detect_obj_img_folder.py
new file mode 100644
index 0000000..7f95c4c
--- /dev/null
+++ b/mycobot_ai/ai_mecharm_270/scripts/advance_detect_obj_img_folder.py
@@ -0,0 +1,643 @@
+#!/usr/bin/env python2
+# encoding:utf-8
+from multiprocessing import Process, Pipe
+import cv2
+import numpy as np
+import time
+import os,sys
+import rospy
+from visualization_msgs.msg import Marker
+from moving_utils import Movement
+from pymycobot.mycobot import MyCobot
+
+IS_CV_4 = cv2.__version__[0] == '4'
+__version__ = "1.0" # Adaptive seeed
+
+
+class Object_detect(Movement):
+
+ def __init__(self, camera_x = 148, camera_y = 5):
+ # inherit the parent class
+ super(Object_detect, self).__init__()
+ # get path of file
+ dir_path = os.path.dirname(__file__)
+
+ # declare 270
+ self.mc = None
+ # 移动角度
+ self.move_angles = [
+ [0, 0, 0, 0, 90, 0], # point to grab
+ [-33.31, 2.02, -10.72, -0.08, 95, -54.84], # init the point
+ ]
+
+ # 移动坐标
+ self.move_coords = [
+ [109.1, -118.8, 164.9, -179.02, 11.07, 132.93], # above the red bucket
+ [178.4, -98.5, 172.7, -175.8, 41.25, 159.41], # above the green bucket
+ [97.9, 139.9, 170.7, 163.54, 2.03, 156.04], # above the blue bucket
+ [-1.8, 143.8, 172.4, 170.69, -4.62, 161.79], # 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]
+ if "dev" in self.robot_m5:
+ self.Pin = [2, 5]
+ elif "dev" in self.robot_wio:
+ # self.Pin = [20, 21]
+ self.Pin = [2, 5]
+ for i in self.move_coords:
+ i[2] -= 20
+ elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+ import RPi.GPIO as GPIO
+ GPIO.setwarnings(False)
+ self.GPIO = GPIO
+ GPIO.setmode(GPIO.BCM)
+ GPIO.setup(20, GPIO.OUT)
+ GPIO.setup(21, GPIO.OUT)
+ GPIO.output(20, 1)
+ GPIO.output(21, 1)
+ self.raspi = True
+ if self.raspi:
+ self.gpio_status(False)
+
+ # choose place to set cube
+ self.color = 0
+ # parameters to calculate camera clipping parameters
+ self.x1 = self.x2 = self.y1 = self.y2 = 0
+ # set cache of real coord
+ self.cache_x = self.cache_y = 0
+
+ # 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)
+
+ # pump_control pi
+ def gpio_status(self, flag):
+ if flag:
+ self.GPIO.output(20, 0)
+ self.GPIO.output(21, 0)
+ else:
+ self.GPIO.output(20, 1)
+ self.GPIO.output(21, 1)
+
+ # 开启吸泵 m5
+ def pump_on(self):
+ # 让2号位工作
+ self.mc.set_basic_output(2, 0)
+ # 让5号位工作
+ self.mc.set_basic_output(5, 0)
+
+ # 停止吸泵 m5
+ def pump_off(self):
+ # 让2号位停止工作
+ self.mc.set_basic_output(2, 1)
+ # 让5号位停止工作
+ self.mc.set_basic_output(5, 1)
+
+ # Grasping motion
+ def move(self, x, y, color):
+ # send Angle to move 270
+ self.mc.send_angles(self.move_angles[0], 30)
+ time.sleep(4)
+
+ print("x %s ,y %s" % (x,y))
+ # send coordinates to move 270 根据不同底板机械臂，调整吸泵高度
+ self.mc.send_coords([x, y, 140, 179.12, -0.18, 179.46], 30, 0)
+ time.sleep(3)
+
+ self.mc.send_coords([x, y, 96, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15 m5
+ # self.mc.send_coords([x, y, 90, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15 pi
+ # self.mc.send_coords([x, y, 92, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15
+ time.sleep(3)
+
+ # open pump
+ if "dev" in self.robot_m5 or "dev" in self.robot_wio:
+ self.pump_on()
+ elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+ self.gpio_status(True)
+ time.sleep(1.5)
+
+ tmp = []
+ while True:
+ if not tmp:
+ tmp = self.mc.get_angles()
+ else:
+ break
+ time.sleep(0.5)
+
+ # print(tmp)
+ self.mc.send_angles([tmp[0], 17.22, -32.51, tmp[3], 97, tmp[5]],30)
+ time.sleep(3)
+
+
+ self.mc.send_coords(self.move_coords[color], 30, 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(6)
+
+ # close pump
+ if "dev" in self.robot_m5 or "dev" in self.robot_wio:
+ self.pump_off()
+ elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+ self.gpio_status(False)
+ time.sleep(6)
+
+ self.mc.send_angles(self.move_angles[1], 30)
+ time.sleep(6)
+
+ # 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 270
+ def run(self):
+ if "dev" in self.robot_wio :
+ self.mc = MyCobot(self.robot_wio, 115200)
+ elif "dev" in self.robot_m5:
+ self.mc = MyCobot(self.robot_m5, 115200)
+ elif "dev" in self.robot_raspi:
+ self.mc = MyCobot(self.robot_raspi, 1000000)
+ if not self.raspi:
+ self.pub_pump(False, self.Pin)
+ self.mc.send_angles([-33.31, 2.02, -10.72, -0.08, 95, -54.84], 30)
+ 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 = ''
+ path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mecharm_270/' + folder
+ path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mecharm_270/' + folder
+
+ if os.path.exists(path1):
+ path = path1
+ elif os.path.exists(path2):
+ path = path2
+
+ # print("path:",path)
+ 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()
diff --git a/mycobot_ai/ai_mecharm_270/scripts/combine_detect_obj_color.py b/mycobot_ai/ai_mecharm_270/scripts/combine_detect_obj_color.py
index 3a8e940..92d39af 100644
--- a/mycobot_ai/ai_mecharm_270/scripts/combine_detect_obj_color.py
+++ b/mycobot_ai/ai_mecharm_270/scripts/combine_detect_obj_color.py
@@ -40,8 +40,8 @@ class Object_detect(Movement):
self.move_coords = [
[92.3, -104.9, 211.4, -179.6, 28.91, 131.29], # above the red bucket
[165.0, -93.6, 201.4, -173.43, 46.23, 160.65], # above the green bucket
- [88.1, 126.3, 193.4, 162.15, 2.23, 156.02], # above the blue bucket
- [-5.4, 120.6, 204.6, 162.66, -6.96, 159.93], # above the gray bucket
+ [84.3, 123.8, 205.0, 153.45, -3.67, 142.01], # above the blue bucket
+ [-15, 120.6, 204.6, 162.66, -6.96, 159.93], # above the gray bucket
]
# which robot: USB* is m5; ACM* is wio; AMA* is raspi
@@ -50,7 +50,7 @@ class Object_detect(Movement):
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:
+ if "dev" in self.robot_m5 or "dev" in self.robot_wio:
self.Pin = [2, 5]
# self.Pin = [5]
elif "dev" in self.robot_wio:
@@ -173,7 +173,7 @@ class Object_detect(Movement):
# open pump
- if "dev" in self.robot_m5:
+ if "dev" in self.robot_m5 or "dev" in self.robot_wio:
self.pump_on()
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
self.gpio_status(True)
@@ -188,7 +188,7 @@ class Object_detect(Movement):
time.sleep(0.5)
# print(tmp)
- self.mc.send_angles([tmp[0], 17.22, -32.51, tmp[3], 97, tmp[5]],30)
+ self.mc.send_angles([tmp[0], 17.22, -45, tmp[3], 97, tmp[5]],30)
time.sleep(3)
@@ -200,7 +200,7 @@ class Object_detect(Movement):
time.sleep(3)
# close pump
- if "dev" in self.robot_m5:
+ if "dev" in self.robot_m5 or "dev" in self.robot_wio:
self.pump_off()
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
self.gpio_status(False)
@@ -231,7 +231,9 @@ class Object_detect(Movement):
# init 270
def run(self):
if "dev" in self.robot_m5:
- self.mc = MyCobot(self.robot_m5, 115200)
+ self.mc = MyCobot(self.robot_m5, 115200)
+ elif "dev" in self.robot_wio:
+ self.mc = MyCobot(self.robot_wio, 115200)
elif "dev" in self.robot_raspi:
self.mc = MyCobot(self.robot_raspi, 1000000)
if not self.raspi:
diff --git a/mycobot_ai/ai_mecharm_270/scripts/combine_detect_obj_img_folder_opt.py b/mycobot_ai/ai_mecharm_270/scripts/combine_detect_obj_img_folder_opt.py
index d0f7873..5d891dd 100755
--- a/mycobot_ai/ai_mecharm_270/scripts/combine_detect_obj_img_folder_opt.py
+++ b/mycobot_ai/ai_mecharm_270/scripts/combine_detect_obj_img_folder_opt.py
@@ -46,8 +46,8 @@ class Object_detect(Movement):
self.move_coords = [
[92.3, -104.9, 211.4, -179.6, 28.91, 131.29], # above the red bucket
[165.0, -93.6, 201.4, -173.43, 46.23, 160.65], # above the green bucket
- [88.1, 126.3, 193.4, 162.15, 2.23, 156.02], # above the blue bucket
- [-5.4, 120.6, 204.6, 162.66, -6.96, 159.93], # above the gray bucket
+ [84.3, 123.8, 205.0, 153.45, -3.67, 142.01], # above the blue bucket
+ [-15, 120.6, 204.6, 162.66, -6.96, 159.93], # above the gray bucket
]
# 判断连接设备:ttyUSB*为M5，ttyACM*为seeed
@@ -56,7 +56,7 @@ class Object_detect(Movement):
self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
- if "dev" in self.robot_m5:
+ if "dev" in self.robot_m5 or "dev" in self.robot_wio:
self.Pin = [2, 5]
elif "dev" in self.robot_wio:
self.Pin = [20, 21]
@@ -158,21 +158,20 @@ class Object_detect(Movement):
def move(self, x, y, color):
# send Angle to move 270
self.mc.send_angles(self.move_angles[0], 30)
- time.sleep(7)
+ time.sleep(4)
print("x %s ,y %s" % (x,y))
# send coordinates to move 270 根据不同底板机械臂，调整吸泵高度
self.mc.send_coords([x, y, 140, 179.12, -0.18, 179.46], 30, 0)
- time.sleep(7)
- print("ntm")
+ time.sleep(3)
self.mc.send_coords([x, y, 95, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15 m5
# self.mc.send_coords([x, y, 90, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15 pi
# self.mc.send_coords([x, y, 92, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15
- time.sleep(6)
+ time.sleep(3)
# open pump
- if "dev" in self.robot_m5:
+ if "dev" in self.robot_m5 or "dev" in self.robot_wio:
self.pump_on()
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
self.gpio_status(True)
@@ -187,25 +186,25 @@ class Object_detect(Movement):
time.sleep(0.5)
# print(tmp)
- self.mc.send_angles([tmp[0], 17.22, -32.51, tmp[3], 97, tmp[5]],30)
- time.sleep(6)
+ self.mc.send_angles([tmp[0], 17.22, -45, tmp[3], 97, tmp[5]],30)
+ time.sleep(3)
self.mc.send_coords(self.move_coords[color], 30, 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(6)
+ time.sleep(3)
# close pump
- if "dev" in self.robot_m5:
+ if "dev" in self.robot_m5 or "dev" in self.robot_wio:
self.pump_off()
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
self.gpio_status(False)
time.sleep(6)
self.mc.send_angles(self.move_angles[1], 30)
- time.sleep(6)
+ time.sleep(2)
# decide whether grab cube
def decide_move(self, x, y, color):
diff --git a/mycobot_ai/ai_mycobot_280/res/blue/goal9.jpeg b/mycobot_ai/ai_mycobot_280/res/blue/goal9.jpeg
new file mode 100644
index 0000000..3eaf44f
Binary files /dev/null and b/mycobot_ai/ai_mycobot_280/res/blue/goal9.jpeg differ
diff --git a/mycobot_ai/ai_mycobot_280/res/gray/goal10.jpeg b/mycobot_ai/ai_mycobot_280/res/gray/goal10.jpeg
new file mode 100644
index 0000000..df419e2
Binary files /dev/null and b/mycobot_ai/ai_mycobot_280/res/gray/goal10.jpeg differ
diff --git a/mycobot_ai/ai_mycobot_280/res/green/goal12.jpeg b/mycobot_ai/ai_mycobot_280/res/green/goal12.jpeg
new file mode 100644
index 0000000..19f703f
Binary files /dev/null and b/mycobot_ai/ai_mycobot_280/res/green/goal12.jpeg differ
diff --git a/mycobot_ai/ai_mycobot_280/res/red/goal8.jpeg b/mycobot_ai/ai_mycobot_280/res/red/goal8.jpeg
new file mode 100644
index 0000000..e8dfcbb
Binary files /dev/null and b/mycobot_ai/ai_mycobot_280/res/red/goal8.jpeg differ
diff --git a/mycobot_ai/ai_mycobot_280/res/red/goal9.jpeg b/mycobot_ai/ai_mycobot_280/res/red/goal9.jpeg
new file mode 100644
index 0000000..4ef5940
Binary files /dev/null and b/mycobot_ai/ai_mycobot_280/res/red/goal9.jpeg differ
diff --git a/mycobot_ai/ai_mycobot_280/res/takephoto.jpeg b/mycobot_ai/ai_mycobot_280/res/takephoto.jpeg
index 4d761e6..348b385 100644
Binary files a/mycobot_ai/ai_mycobot_280/res/takephoto.jpeg and b/mycobot_ai/ai_mycobot_280/res/takephoto.jpeg differ
diff --git a/mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_color.py b/mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_color.py
new file mode 100755
index 0000000..7d7f562
--- /dev/null
+++ b/mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_color.py
@@ -0,0 +1,480 @@
+# encoding:utf-8
+#!/usr/bin/env python2
+
+import cv2
+import numpy as np
+import time
+import os,sys
+import rospy
+from visualization_msgs.msg import Marker
+from pymycobot.mycobot import MyCobot
+from moving_utils import Movement
+
+IS_CV_4 = cv2.__version__[0] == '4'
+__version__ = "1.0"
+# Adaptive seeed
+
+
+class Object_detect(Movement):
+
+ def __init__(self, camera_x = 155, camera_y = 10):
+ # inherit the parent class
+ super(Object_detect, self).__init__()
+ # get path of file
+ dir_path = os.path.dirname(__file__)
+ self.mc = None
+
+ # 移动角度
+ self.move_angles = [
+ [-7.11, -6.94, -55.01, -24.16, 0, -15], # init the point
+ [18.8, -7.91, -54.49, -23.02, -0.79, -14.76], # point to grab
+ ]
+
+ # 移动坐标
+ self.move_coords = [
+ [132.2, -136.9, 200.8, -178.24, -3.72, -107.17], # above the red bucket
+ [238.8, -124.1, 204.3, -169.69, -5.52, -96.52], # green
+ [115.8, 177.3, 210.6, 178.06, -0.92, -6.11], # blue
+ [-6.9, 173.2, 201.5, 179.93, 0.63, 33.83], # gray
+ ]
+ # which robot: USB* is m5; ACM* is wio; AMA* is raspi
+ self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
+ self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
+ self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
+ self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
+ self.raspi = False
+ if "dev" in self.robot_m5:
+ self.Pin = [2, 5]
+ elif "dev" in self.robot_wio:
+ # self.Pin = [20, 21]
+ self.Pin = [2, 5]
+
+ for i in self.move_coords:
+ i[2] -= 20
+ elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+ import RPi.GPIO as GPIO
+ GPIO.setwarnings(False)
+ self.GPIO = GPIO
+ GPIO.setmode(GPIO.BCM)
+ GPIO.setup(20, GPIO.OUT)
+ GPIO.setup(21, GPIO.OUT)
+ GPIO.output(20, 1)
+ GPIO.output(21, 1)
+ self.raspi = True
+ if self.raspi:
+ self.gpio_status(False)
+
+ # choose place to set cube
+ self.color = 0
+ # parameters to calculate camera clipping parameters
+ self.x1 = self.x2 = self.y1 = self.y2 = 0
+ # set cache of real coord
+ self.cache_x = self.cache_y = 0
+ # set color HSV
+ self.HSV = {
+ "yellow": [np.array([11, 115, 70]), np.array([40, 255, 245])],
+ "red": [np.array([0, 43, 46]), np.array([8, 255, 255])],
+ "green": [np.array([35, 43, 35]), np.array([90, 255, 255])], # [77, 255, 255]
+ "blue": [np.array([100, 43, 46]), np.array([124, 255, 255])],
+ "cyan": [np.array([89, 43, 46]), np.array([99, 255, 255])], # np.array([78, 43, 46]), np.array([99, 255, 255])
+ }
+ # use to calculate coord between cube and mycobot
+ self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
+ # The coordinates of the grab center point relative to the mycobot
+ self.camera_x, self.camera_y = camera_x, camera_y
+ # The coordinates of the cube relative to the mycobot
+ self.c_x, self.c_y = 0, 0
+ # The ratio of pixels to actual values
+ self.ratio = 0
+ # Get ArUco marker dict that can be detected.
+ self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
+ # Get ArUco marker params.
+ self.aruco_params = cv2.aruco.DetectorParameters_create()
+
+ # init a node and a publisher
+ rospy.init_node("marker", anonymous=True)
+ self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
+ # init a Marker
+ self.marker = Marker()
+ self.marker.header.frame_id = "/joint1"
+ self.marker.ns = "cube"
+ self.marker.type = self.marker.CUBE
+ self.marker.action = self.marker.ADD
+ self.marker.scale.x = 0.04
+ self.marker.scale.y = 0.04
+ self.marker.scale.z = 0.04
+ self.marker.color.a = 1.0
+ self.marker.color.g = 1.0
+ self.marker.color.r = 1.0
+
+ # marker position initial
+ self.marker.pose.position.x = 0
+ self.marker.pose.position.y = 0
+ self.marker.pose.position.z = 0.03
+ self.marker.pose.orientation.x = 0
+ self.marker.pose.orientation.y = 0
+ self.marker.pose.orientation.z = 0
+ self.marker.pose.orientation.w = 1.0
+
+ # publish marker
+ def pub_marker(self, x, y, z=0.03):
+ self.marker.header.stamp = rospy.Time.now()
+ self.marker.pose.position.x = x
+ self.marker.pose.position.y = y
+ self.marker.pose.position.z = z
+ self.marker.color.g = self.color
+ self.pub.publish(self.marker)
+
+ def gpio_status(self, flag):
+ if flag:
+ self.GPIO.output(20, 0)
+ self.GPIO.output(21, 0)
+ else:
+ self.GPIO.output(20, 1)
+ self.GPIO.output(21, 1)
+
+ # 开启吸泵 m5
+ def pump_on(self):
+ # 让2号位工作
+ self.mc.set_basic_output(2, 0)
+ # 让5号位工作
+ self.mc.set_basic_output(5, 0)
+
+ # 停止吸泵 m5
+ def pump_off(self):
+ # 让2号位停止工作
+ self.mc.set_basic_output(2, 1)
+ # 让5号位停止工作
+ self.mc.set_basic_output(5, 1)
+ # Grasping motion
+ def move(self, x, y, color):
+ # send Angle to move mycobot
+ print (color)
+ self.mc.send_angles(self.move_angles[1], 25)
+ time.sleep(3)
+
+ # send coordinates to move mycobot
+ self.mc.send_coords([x, y, 190.6, 179.87, -3.78, -62.75], 25, 1) # usb :rx,ry,rz -173.3, -5.48, -57.9
+ time.sleep(3)
+
+ # self.mc.send_coords([x, y, 150, 179.87, -3.78, -62.75], 25, 0)
+ # time.sleep(3)
+
+ self.mc.send_coords([x, y, 103, 179.87, -3.78, -62.75], 25, 0)
+ time.sleep(3)
+
+ # open pump
+ if "dev" in self.robot_m5 or "dev" in self.robot_wio:
+ self.pump_on()
+ elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+ self.gpio_status(True)
+ time.sleep(1.5)
+
+ tmp = []
+ while True:
+ if not tmp:
+ tmp = self.mc.get_angles()
+ else:
+ break
+ time.sleep(0.5)
+
+ # print(tmp)
+ self.mc.send_angles([tmp[0], -0.71, -54.49, -23.02, -0.79, tmp[5]],25) # [18.8, -7.91, -54.49, -23.02, -0.79, -14.76]
+ time.sleep(2.5)
+
+ self.pub_marker(
+ self.move_coords[2][0]/1000.0, self.move_coords[2][1]/1000.0, self.move_coords[2][2]/1000.0)
+
+ self.mc.send_coords(self.move_coords[color], 25, 1)
+ self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color]
+ [1]/1000.0, self.move_coords[color][2]/1000.0)
+ time.sleep(3)
+
+ # close pump
+ if "dev" in self.robot_m5 or "dev" in self.robot_wio:
+ self.pump_off()
+ elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+ self.gpio_status(False)
+ time.sleep(4)
+
+ if color == 1:
+ self.pub_marker(
+ self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02)
+ elif color == 0:
+ self.pub_marker(
+ self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0)
+ # self.pub_angles(self.move_angles[0], 20)
+ self.mc.send_angles(self.move_angles[0], 25)
+ time.sleep(3)
+
+ # decide whether grab cube
+ def decide_move(self, x, y, color):
+ print(x, y, self.cache_x, self.cache_y)
+ # detect the cube status move or run
+ if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm
+ self.cache_x, self.cache_y = x, y
+ return
+ else:
+ self.cache_x = self.cache_y = 0
+ # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
+ self.move(x, y, color)
+
+ # init mycobot
+ def run(self):
+ if "dev" in self.robot_wio :
+ self.mc = MyCobot(self.robot_wio, 115200)
+ elif "dev" in self.robot_m5:
+ self.mc = MyCobot(self.robot_m5, 115200)
+ elif "dev" in self.robot_raspi:
+ self.mc = MyCobot(self.robot_raspi, 1000000)
+ if not self.raspi:
+ self.pub_pump(False, self.Pin)
+ self.mc.send_angles([-7.11, -6.94, -55.01, -24.16, 0, -15], 20)
+ time.sleep(3)
+
+ # draw aruco
+
+ def draw_marker(self, img, x, y):
+ # draw rectangle on img
+ cv2.rectangle(
+ img,
+ (x - 20, y - 20),
+ (x + 20, y + 20),
+ (0, 255, 0),
+ thickness=2,
+ lineType=cv2.FONT_HERSHEY_COMPLEX,
+ )
+ # add text on rectangle
+ cv2.putText(img, "({},{})".format(x, y), (x, y),
+ cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (243, 0, 0), 2,)
+
+ # get points of two aruco
+ def get_calculate_params(self, img):
+ # Convert the image to a gray image
+ gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+ # Detect ArUco marker.
+ corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
+ gray, self.aruco_dict, parameters=self.aruco_params
+ )
+
+ """
+ Two Arucos must be present in the picture and in the same order.
+ There are two Arucos in the Corners, and each aruco contains the pixels of its four corners.
+ Determine the center of the aruco by the four corners of the aruco.
+ """
+ if len(corners) > 0:
+ if ids is not None:
+ if len(corners) <= 1 or ids[0] == 1:
+ return None
+ x1 = x2 = y1 = y2 = 0
+ point_11, point_21, point_31, point_41 = corners[0][0]
+ x1, y1 = int((point_11[0] + point_21[0] + point_31[0] + point_41[0]) / 4.0), int(
+ (point_11[1] + point_21[1] + point_31[1] + point_41[1]) / 4.0)
+ point_1, point_2, point_3, point_4 = corners[1][0]
+ x2, y2 = int((point_1[0] + point_2[0] + point_3[0] + point_4[0]) / 4.0), int(
+ (point_1[1] + point_2[1] + point_3[1] + point_4[1]) / 4.0)
+ return x1, x2, y1, y2
+ return None
+
+ # set camera clipping parameters
+ def set_cut_params(self, x1, y1, x2, y2):
+ self.x1 = int(x1)
+ self.y1 = int(y1)
+ self.x2 = int(x2)
+ self.y2 = int(y2)
+ print(self.x1, self.y1, self.x2, self.y2)
+
+ # set parameters to calculate the coords between cube and mycobot
+ def set_params(self, c_x, c_y, ratio):
+ self.c_x = c_x
+ self.c_y = c_y
+ self.ratio = 220.0/ratio
+
+ # calculate the coords between cube and mycobot
+ def get_position(self, x, y):
+ return ((y - self.c_y)*self.ratio + self.camera_x), ((x - self.c_x)*self.ratio + self.camera_y)
+
+ """
+ Calibrate the camera according to the calibration parameters.
+ Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times.
+ If two ARuco values have been calculated, clip the video.
+ """
+
+ def transform_frame(self, frame):
+ # enlarge the image by 1.5 times
+ fx = 1.5
+ fy = 1.5
+ frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy,
+ interpolation=cv2.INTER_CUBIC)
+ if self.x1 != self.x2:
+ # the cutting ratio here is adjusted according to the actual situation
+ frame = frame[int(self.y2*0.78):int(self.y1*1.1),
+ int(self.x1*0.86):int(self.x2*1.08)]
+ return frame
+
+ # detect cube color
+ def color_detect(self, img):
+ # set the arrangement of color'HSV
+ x = y = 0
+ gs_img = cv2.GaussianBlur(img, (3, 3), 0) # 高斯模糊
+ # transfrom the img to model of gray
+ hsv = cv2.cvtColor(gs_img, cv2.COLOR_BGR2HSV)
+
+ for mycolor, item in self.HSV.items():
+ redLower = np.array(item[0])
+ redUpper = np.array(item[1])
+ # wipe off all color expect color in range
+ mask = cv2.inRange(hsv, item[0], item[1])
+ # a etching operation on a picture to remove edge roughness
+ erosion = cv2.erode(mask, np.ones((1, 1), np.uint8), iterations=2)
+ # the image for expansion operation, its role is to deepen the color depth in the picture
+ dilation = cv2.dilate(erosion, np.ones(
+ (1, 1), np.uint8), iterations=2)
+
+
+ # adds pixels to the image
+ target = cv2.bitwise_and(img, img, mask=dilation)
+ # the filtered image is transformed into a binary image and placed in binary
+ ret, binary = cv2.threshold(dilation, 127, 255, cv2.THRESH_BINARY)
+ # get the contour coordinates of the image, where contours is the coordinate value, here only the contour is detected
+ contours, hierarchy = cv2.findContours(
+ dilation, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+
+ if len(contours) > 0:
+ # do something about misidentification
+ boxes = [
+ box
+ for box in [cv2.boundingRect(c) for c in contours]
+ if min(img.shape[0], img.shape[1]) / 10
+ < min(box[2], box[3])
+ < min(img.shape[0], img.shape[1]) / 1
+ ]
+ if boxes:
+ for box in boxes:
+ x, y, w, h = box
+ # find the largest object that fits the requirements
+ c = max(contours, key=cv2.contourArea)
+ # get the lower left and upper right points of the positioning object
+ x, y, w, h = cv2.boundingRect(c)
+ # locate the target by drawing rectangle
+ cv2.rectangle(img, (x, y), (x+w, y+h), (153, 153, 0), 2)
+ # calculate the rectangle center
+ x, y = (x*2+w)/2, (y*2+h)/2
+ # calculate the real coordinates of mycobot relative to the target
+ if mycolor == "red":
+ self.color = 0
+
+ elif mycolor == "green":
+ self.color = 1
+
+ elif mycolor == "cyan" or mycolor == "blue":
+ self.color = 2
+
+ else:
+ self.color = 3
+
+
+ if abs(x) + abs(y) > 0:
+ return x, y
+ else:
+ return None
+
+
+if __name__ == "__main__":
+ # open the camera
+ cap_num = 0
+ cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
+
+ if not cap.isOpened():
+ cap.open()
+ # init a class of Object_detect
+ detect = Object_detect()
+ # init mycobot
+ detect.run()
+
+ _init_ = 20 #
+ init_num = 0
+ nparams = 0
+ num = 0
+ real_sx = real_sy = 0
+ while cv2.waitKey(1) < 0:
+ # read camera
+ _, frame = cap.read()
+ # deal img
+ frame = detect.transform_frame(frame)
+ if _init_ > 0:
+ _init_ -= 1
+ continue
+ # calculate the parameters of camera clipping
+ if init_num < 20:
+ if detect.get_calculate_params(frame) is None:
+ cv2.imshow("figure", frame)
+ continue
+ else:
+ x1, x2, y1, y2 = detect.get_calculate_params(frame)
+ detect.draw_marker(frame, x1, y1)
+ detect.draw_marker(frame, x2, y2)
+ detect.sum_x1 += x1
+ detect.sum_x2 += x2
+ detect.sum_y1 += y1
+ detect.sum_y2 += y2
+ init_num += 1
+ continue
+ elif init_num == 20:
+ detect.set_cut_params(
+ (detect.sum_x1)/20.0,
+ (detect.sum_y1)/20.0,
+ (detect.sum_x2)/20.0,
+ (detect.sum_y2)/20.0,
+ )
+ detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0
+ init_num += 1
+ continue
+
+ # calculate params of the coords between cube and mycobot
+ if nparams < 10:
+ if detect.get_calculate_params(frame) is None:
+ cv2.imshow("figure", frame)
+ continue
+ else:
+ x1, x2, y1, y2 = detect.get_calculate_params(frame)
+ detect.draw_marker(frame, x1, y1)
+ detect.draw_marker(frame, x2, y2)
+ detect.sum_x1 += x1
+ detect.sum_x2 += x2
+ detect.sum_y1 += y1
+ detect.sum_y2 += y2
+ nparams += 1
+ continue
+ elif nparams == 10:
+ nparams += 1
+ # calculate and set params of calculating real coord between cube and mycobot
+ detect.set_params(
+ (detect.sum_x1+detect.sum_x2)/20.0,
+ (detect.sum_y1+detect.sum_y2)/20.0,
+ abs(detect.sum_x1-detect.sum_x2)/10.0 +
+ abs(detect.sum_y1-detect.sum_y2)/10.0
+ )
+ print ("ok")
+ continue
+
+ # get detect result
+ detect_result = detect.color_detect(frame)
+ if detect_result is None:
+ cv2.imshow("figure", frame)
+ continue
+ else:
+ x, y = detect_result
+ # calculate real coord between cube and mycobot
+ real_x, real_y = detect.get_position(x, y)
+ if num == 20:
+ detect.pub_marker(real_sx/20.0/1000.0, real_sy/20.0/1000.0)
+ detect.decide_move(real_sx/20.0, real_sy/20.0, detect.color)
+ num = real_sx = real_sy = 0
+
+ else:
+ num += 1
+ real_sy += real_y
+ real_sx += real_x
+
+ cv2.imshow("figure", frame)
diff --git a/mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_img_folder.py b/mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_img_folder.py
new file mode 100755
index 0000000..84e8b77
--- /dev/null
+++ b/mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_img_folder.py
@@ -0,0 +1,660 @@
+# encoding:utf-8
+#!/usr/bin/env python2
+
+from multiprocessing import Process, Pipe
+import cv2
+import numpy as np
+import time
+import os,sys
+import rospy
+from visualization_msgs.msg import Marker
+from pymycobot.mycobot import MyCobot
+from moving_utils import Movement
+
+IS_CV_4 = cv2.__version__[0] == '4'
+__version__ = "1.0" # Adaptive seeed
+
+
+class Object_detect(Movement):
+
+ def __init__(self, camera_x = 155, camera_y = 10):
+ # inherit the parent class
+ super(Object_detect, self).__init__()
+ # get path of file
+ dir_path = os.path.dirname(__file__)
+ self.mc = None
+
+ # 移动角度
+ self.move_angles = [
+ [-7.11, -6.94, -55.01, -24.16, 0, -15], # init the point
+ [18.8, -7.91, -54.49, -23.02, -0.79, -14.76], # point to grab
+ ]
+
+ # 移动坐标
+ self.move_coords = [
+ [132.2, -136.9, 200.8, -178.24, -3.72, -107.17], # above the red bucket
+ [238.8, -124.1, 204.3, -169.69, -5.52, -96.52], # green
+ [115.8, 177.3, 210.6, 178.06, -0.92, -6.11], # blue
+ [-6.9, 173.2, 201.5, 179.93, 0.63, 33.83], # gray
+ ]
+ # 判断连接设备: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]
+ if "dev" in self.robot_m5:
+ self.Pin = [2, 5]
+ elif "dev" in self.robot_wio:
+ self.Pin = [2, 5]
+ for i in self.move_coords:
+ i[2] -= 20
+ elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+ import RPi.GPIO as GPIO
+ GPIO.setwarnings(False)
+ self.GPIO = GPIO
+ GPIO.setmode(GPIO.BCM)
+ GPIO.setup(20, GPIO.OUT)
+ GPIO.setup(21, GPIO.OUT)
+ GPIO.output(20, 1)
+ GPIO.output(21, 1)
+ self.raspi = True
+ if self.raspi:
+ self.gpio_status(False)
+
+ # choose place to set cube
+ self.color = 0
+ # parameters to calculate camera clipping parameters
+ self.x1 = self.x2 = self.y1 = self.y2 = 0
+ # set cache of real coord
+ self.cache_x = self.cache_y = 0
+ # 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)
+
+ # 开启吸泵 m5
+ def pump_on(self):
+ # 让2号位工作
+ self.mc.set_basic_output(2, 0)
+ # 让5号位工作
+ self.mc.set_basic_output(5, 0)
+
+ # 停止吸泵 m5
+ def pump_off(self):
+ # 让2号位停止工作
+ self.mc.set_basic_output(2, 1)
+ # 让5号位停止工作
+ self.mc.set_basic_output(5, 1)
+
+ # Grasping motion
+ def move(self, x, y, color):
+ # send Angle to move mycobot
+ print (color)
+ self.mc.send_angles(self.move_angles[1], 25)
+ time.sleep(3)
+
+ # send coordinates to move mycobot
+ self.mc.send_coords([x, y, 190.6, 179.87, -3.78, -62.75], 25, 1) # usb :rx,ry,rz -173.3, -5.48, -57.9
+ time.sleep(3)
+
+ # self.mc.send_coords([x, y, 150, 179.87, -3.78, -62.75], 25, 0)
+ # time.sleep(3)
+
+ # self.mc.send_coords([x, y, 105, 179.87, -3.78, -62.75], 25, 0)
+ self.mc.send_coords([x, y, 103, 179.87, -3.78, -62.75], 25, 0)
+
+ time.sleep(3)
+
+ # open pump
+ if "dev" in self.robot_m5 or "dev" in self.robot_wio:
+ self.pump_on()
+ elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+ self.gpio_status(True)
+ time.sleep(1.5)
+
+ tmp = []
+ while True:
+ if not tmp:
+ tmp = self.mc.get_angles()
+ else:
+ break
+ time.sleep(0.5)
+
+ # print(tmp)
+ self.mc.send_angles([tmp[0], -0.71, -54.49, -23.02, -0.79, tmp[5]],25) # [18.8, -7.91, -54.49, -23.02, -0.79, -14.76]
+ time.sleep(5)
+
+
+
+ self.mc.send_coords(self.move_coords[color], 25, 1)
+ self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color]
+ [1]/1000.0, self.move_coords[color][2]/1000.0)
+ time.sleep(4)
+
+ # close pump
+ if "dev" in self.robot_m5 or "dev" in self.robot_wio:
+ self.pump_off()
+ elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+ self.gpio_status(False)
+ time.sleep(4)
+
+ self.mc.send_angles(self.move_angles[0], 25)
+ time.sleep(3)
+
+ # decide whether grab cube
+ def decide_move(self, x, y, color):
+ print(x, y, self.cache_x, self.cache_y)
+ # detect the cube status move or run
+ if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm
+ self.cache_x, self.cache_y = x, y
+ return
+ else:
+ self.cache_x = self.cache_y = 0
+ # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
+ self.move(x, y, color)
+
+ # init mycobot
+ def run(self):
+ if "dev" in self.robot_wio :
+ self.mc = MyCobot(self.robot_wio, 115200)
+ elif "dev" in self.robot_m5:
+ self.mc = MyCobot(self.robot_m5, 115200)
+ elif "dev" in self.robot_raspi:
+ self.mc = MyCobot(self.robot_raspi, 1000000)
+ if not self.raspi:
+ self.pub_pump(False, self.Pin)
+ self.mc.send_angles([-7.11, -6.94, -55.01, -24.16, 0, -15], 20)
+ time.sleep(3)
+
+ # draw aruco
+ def draw_marker(self, img, x, y):
+ # draw rectangle on img
+ cv2.rectangle(
+ img,
+ (x - 20, y - 20),
+ (x + 20, y + 20),
+ (0, 255, 0),
+ thickness=2,
+ lineType=cv2.FONT_HERSHEY_COMPLEX,
+ )
+ # add text on rectangle
+ cv2.putText(
+ img,
+ "({},{})".format(x, y),
+ (x, y),
+ cv2.FONT_HERSHEY_COMPLEX_SMALL,
+ 1,
+ (243, 0, 0),
+ 2,
+ )
+
+ # get points of two aruco
+ def get_calculate_params(self, img):
+ # Convert the image to a gray image
+ gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+ # Detect ArUco marker.
+ corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
+ gray, self.aruco_dict, parameters=self.aruco_params)
+ """
+ Two Arucos must be present in the picture and in the same order.
+ There are two Arucos in the Corners, and each aruco contains the pixels of its four corners.
+ Determine the center of the aruco by the four corners of the aruco.
+ """
+ if len(corners) > 0:
+ if ids is not None:
+ if len(corners) <= 1 or ids[0] == 1:
+ return None
+ x1 = x2 = y1 = y2 = 0
+ point_11, point_21, point_31, point_41 = corners[0][0]
+ x1, y1 = int(
+ (point_11[0] + point_21[0] + point_31[0] + point_41[0]) /
+ 4.0), int(
+ (point_11[1] + point_21[1] + point_31[1] + point_41[1])
+ / 4.0)
+ point_1, point_2, point_3, point_4 = corners[1][0]
+ x2, y2 = int(
+ (point_1[0] + point_2[0] + point_3[0] + point_4[0]) /
+ 4.0), int(
+ (point_1[1] + point_2[1] + point_3[1] + point_4[1]) /
+ 4.0)
+ return x1, x2, y1, y2
+ return None
+
+ # set camera clipping parameters
+ def set_cut_params(self, x1, y1, x2, y2):
+ self.x1 = int(x1)
+ self.y1 = int(y1)
+ self.x2 = int(x2)
+ self.y2 = int(y2)
+ print(self.x1, self.y1, self.x2, self.y2)
+
+ # set parameters to calculate the coords between cube and mycobot
+ def set_params(self, c_x, c_y, ratio):
+ self.c_x = c_x
+ self.c_y = c_y
+ self.ratio = 220.0 / ratio
+
+ # calculate the coords between cube and mycobot
+ def get_position(self, x, y):
+ return ((y - self.c_y) * self.ratio +
+ self.camera_x), ((x - self.c_x) * self.ratio + self.camera_y)
+
+ """
+ Calibrate the camera according to the calibration parameters.
+ Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times.
+ If two ARuco values have been calculated, clip the video.
+ """
+
+ def transform_frame(self, frame):
+ # enlarge the image by 1.5 times
+ fx = 1.5
+ fy = 1.5
+ frame = cv2.resize(frame, (0, 0),
+ fx=fx,
+ fy=fy,
+ interpolation=cv2.INTER_CUBIC)
+ if self.x1 != self.x2:
+ # the cutting ratio here is adjusted according to the actual situation
+ frame = frame[int(self.y2 * 0.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 = ''
+ path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/' + folder
+ path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/' + folder
+ if os.path.exists(path1):
+ path = path1
+ elif os.path.exists(path2):
+ path = path2
+
+ # print("path:",path)
+
+ 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()
diff --git a/mycobot_ai/ai_mycobot_280/scripts/ai_windows.py b/mycobot_ai/ai_mycobot_280/scripts/ai_windows.py
new file mode 100755
index 0000000..a0ebfe9
--- /dev/null
+++ b/mycobot_ai/ai_mycobot_280/scripts/ai_windows.py
@@ -0,0 +1,191 @@
+#!/usr/bin/env python3
+# encoding:utf-8
+
+from tkinter import ttk
+from tkinter import *
+import os,sys
+import time
+import subprocess
+
+import threading
+from multiprocessing import Process
+
+
+class Application(object):
+ def __init__(self):
+ self.win = Tk()
+ # 窗口置顶
+ self.win.wm_attributes('-topmost', 1)
+ self.ros = False
+ # 运行的文件
+ self.run_py = ""
+ # 判断通信口并给权限
+ try:
+ self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
+ self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
+ self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
+ self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
+ if "dev" in self.robot_wio:
+ self.set_file(self.robot_wio)
+ elif "dev" in self.robot_m5:
+ self.set_file(self.robot_m5)
+ elif "dev" in self.robot_raspi:
+ self.change_file(self.robot_raspi)
+ elif "dev" in self.robot_jes:
+ self.change_file(self.robot_jes)
+ except Exception as e:
+ pass
+
+ # 设置标题
+ self.win.title("aikit启动工具")
+ self.win.geometry(
+ "800x600+100+100") # 290 160为窗口大小，+10 +10 定义窗口弹出时的默认展示位置
+ # 打开ros按钮
+ self.btn = Button(self.win, text="open ROS", font=("Helvetica","13"), command=self.open_ros)
+ self.btn.grid(row=0)
+
+ self.chanse_code = Label(self.win, text="选择程序:", font=("Helvetica","13"), width=10)
+ self.chanse_code.grid(row=1)
+
+ self.myComboList = [u"颜色识别", u"物体识别", u"二维码识别"]
+ self.myCombox = ttk.Combobox(self.win, font=("Helvetica","13"), values=self.myComboList)
+ self.myCombox.grid(row=1, column=1)
+
+ self.add_btn = Button(self.win, text="添加新的物体图像", font=("Helvetica","13"), command=self.add_img)
+ self.add_btn.grid(row=1, column=2)
+
+ self.tips = "1、首先打开ros,大概需要等待5s\n2、选择所要运行的程序点击运行即可,开启大概需要10秒,可以通过查看终端查看开启情况。\n\n添加新的图像：\n1、点击按钮，等待开启摄像头\n2、选中图像框，按z键拍照\n3、使用鼠标框出需要识别的图像区域\n4、按Enter键提取图像\n5、根据终端提示，输入数字(1~4)保存到相对应图像的文件夹，按下Enter键即可保存至对应文件夹。"
+
+ self.btn = Button(self.win, text="运行", font=("Helvetica","13"), command=self.start_run)
+ self.btn.grid(row=5)
+
+ self.close = Button(self.win, text="close", font=("Helvetica","13"), command=self.close_py)
+ self.close.grid(row=5, column=1)
+
+ self.t2 = None
+ self.log_data = Text(self.win, width=74, height=20, font=("Helvetica","13"))
+ self.log_data.grid(row=16, column=0, columnspan=10)
+ self.log_data.insert(END, self.tips)
+
+ # self.open_ros()
+ self.win.protocol('WM_DELETE_WINDOW', self.close_rviz)
+
+ def close_rviz(self):
+ os.system(
+ "ps -ef | grep -E mycobot.rviz | grep -v 'grep' | awk '{print $2}' | xargs kill -9")
+ sys.exit(0)
+
+ def set_file(self,port):
+ self.command = ''.format(
+ port)
+ # 根据通信口修改ros启动文件
+ os.system(
+ "sed -i '2c {}' ~/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/launch/vision_wio.launch"
+ .format(self.command))
+
+ def change_file(self, port):
+ command1 = ''.format(port)
+ command2 = ''.format(1000000)
+ # 根据通信口修改ros启动文件
+ os.system(
+ "sed -i '2c {}' ~/catkin_ws/src/mycobot_ros/mycobot_ai/launch/vision.launch".format(command1))
+ os.system(
+ "sed -i '3c {}' ~/catkin_ws/src/mycobot_ros/mycobot_ai/launch/vision.launch".format(command2))
+
+ def start_run(self):
+ try:
+ print(u"开始运行")
+ one = self.myCombox.get()
+ if one == u"颜色识别":
+ self.run_py = "combine_detect_obj_color.py"
+ t2 = threading.Thread(target=self.open_py1)
+ t2.setDaemon(True)
+ t2.start()
+ elif one == u"物体识别":
+ self.run_py = "combine_detect_obj_img_folder_opt.py"
+ t3 = threading.Thread(target=self.open_py)
+ t3.setDaemon(True)
+ t3.start()
+ elif one == u"二维码识别":
+ self.run_py = "detect_encode.py"
+ t3 = threading.Thread(target=self.open_py2)
+ t3.setDaemon(True)
+ t3.start()
+ except Exception as e:
+ self.tips = str(e)
+ self.log_data.insert(END, self.tips)
+
+ def open_py(self):
+ os.system(
+ "cd /home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280 && python scripts/combine_detect_obj_img_folder_opt.py"
+ )
+
+ def open_py1(self):
+ os.system(
+ "python ~/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/scripts/combine_detect_obj_color.py"
+ )
+
+ def open_py2(self):
+ os.system(
+ "python ~/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/scripts/detect_encode.py"
+ )
+
+ def add_img(self):
+ os.system(
+ "python ~/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/scripts/add_img.py"
+ )
+
+ def open_ros(self):
+ if self.ros:
+ print("ros is opened")
+ return
+ # t1 = threading.Thread(target=self.ross)
+ # t1.setDaemon(True)
+ # t1.start()
+ self.ross()
+ self.ros = True
+
+ def ross(self):
+ # os.system(
+ # "roslaunch ~/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/launch/vision_wio.launch"
+ # )
+ p = subprocess.Popen(["roslaunch", "/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mycobot_280/launch/vision_wio.launch"],shell=False, stdout=subprocess.PIPE, stderr=subprocess.STDOUT)
+
+ def close_py(self):
+ t1 = threading.Thread(target=self.close_p)
+ t1.setDaemon(True)
+ t1.start()
+
+ def close_p(self):
+ # 关闭ai程序
+ os.system("ps -ef | grep -E " + self.run_py +
+ " | grep -v 'grep' | awk '{print $2}' | xargs kill -9")
+
+ def get_current_time(self):
+ # 日志时间
+ """Get current time with format."""
+ current_time = time.strftime("%Y-%m-%d %H:%M:%S",
+ time.localtime(time.time()))
+ return current_time
+
+ def write_log_to_Text(self, logmsg):
+ # 设置日志函数
+ global LOG_NUM
+ current_time = self.get_current_time()
+ logmsg_in = str(current_time) + " " + str(logmsg) + "\n" # 换行
+
+ if LOG_NUM <= 18:
+ self.log_data_Text.insert(END, logmsg_in)
+ LOG_NUM += len(logmsg_in.split("\n"))
+ # print(LOG_NUM)
+ else:
+ self.log_data_Text.insert(END, logmsg_in)
+ self.log_data_Text.yview("end")
+
+ def run(self):
+ self.win.mainloop()
+
+
+if __name__ == "__main__":
+ mc = Application()
+ mc.run()
diff --git a/mycobot_ai/ai_mycobot_280/scripts/combine_detect_obj_color.py b/mycobot_ai/ai_mycobot_280/scripts/combine_detect_obj_color.py
index f29c73f..389a29a 100755
--- a/mycobot_ai/ai_mycobot_280/scripts/combine_detect_obj_color.py
+++ b/mycobot_ai/ai_mycobot_280/scripts/combine_detect_obj_color.py
@@ -51,8 +51,8 @@ class Object_detect(Movement):
# self.Pin = [20, 21]
self.Pin = [2, 5]
- for i in self.move_coords:
- i[2] -= 20
+ # 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)
diff --git a/mycobot_ai/ai_mycobot_280/scripts/combine_detect_obj_img_folder_opt.py b/mycobot_ai/ai_mycobot_280/scripts/combine_detect_obj_img_folder_opt.py
old mode 100644
new mode 100755
index ee5ca7d..de5ca4a
--- a/mycobot_ai/ai_mycobot_280/scripts/combine_detect_obj_img_folder_opt.py
+++ b/mycobot_ai/ai_mycobot_280/scripts/combine_detect_obj_img_folder_opt.py
@@ -61,8 +61,8 @@ class Object_detect(Movement):
self.Pin = [2, 5]
elif "dev" in self.robot_wio:
self.Pin = [2, 5]
- for i in self.move_coords:
- i[2] -= 20
+ # 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)
diff --git a/mycobot_ai/ai_mycobot_280/scripts/detect_obj_img_folder_opt.py b/mycobot_ai/ai_mycobot_280/scripts/detect_obj_img_folder_opt.py
old mode 100644
new mode 100755
diff --git a/mycobot_ai/ai_mycobot_280/scripts/test.py b/mycobot_ai/ai_mycobot_280/scripts/test.py
index 816215a..0e4aead 100755
--- a/mycobot_ai/ai_mycobot_280/scripts/test.py
+++ b/mycobot_ai/ai_mycobot_280/scripts/test.py
@@ -1,14 +1,14 @@
# -*- coding: utf-8 -*-
from pymycobot.mycobot import MyCobot
from pymycobot.genre import Angle
-from pymycobot import PI_PORT, PI_BAUD # 当使用树莓派版本的mycobot时，可以引用这两个变量进行MyCobot初始化
+ # 当使用树莓派版本的mycobot时，可以引用这两个变量进行MyCobot初始化
import time
mc = MyCobot("/dev/ttyACM0", 115200)
# mc = MyCobot("/dev/ttyUSB0", 115200)
# mc = MyCobot("/dev/ttyAMA0", 1000000)
-mc.send_angles([0,0,0,0,0,0], 25)
+# mc.send_angles([0,0,0,0,0,0], 25)
# print(mc.get_angles())
# mc.send_angles([-7.11, -6.94, -55.01, -24.16, 0.0, -15], 30)
# time.sleep(4)
@@ -42,3 +42,16 @@ mc.send_angles([0,0,0,0,0,0], 25)
# print("angles:%s"% mc.get_angles())
# print("coords:%s"% mc.get_coords())
# print("\n")
+move_coords = [
+ [132.2, -136.9, 200.8, -178.24, -3.72, -107.17], # above the red bucket
+ [232.5, -124.6, 212.8, -169.94, -5.88, -97.63], # green
+ [115.8, 177.3, 210.6, 178.06, -0.92, -6.11], # blue
+ [-6.9, 173.2, 201.5, 179.93, 0.63, 33.83], # gray
+ ]
+
+# mc.send_coords(move_coords[1],20, 1)
+# mc.send_angles([-13.44, -57.3, 0.7, -22.76, -4.74, -6.76], 20)
+mc.send_coords([238.8, -124.1, 204.3, -169.69, -5.52, -96.52], 20, 1)
+time.sleep(3)
+print(mc.get_angles())
+print(mc.get_coords())
\ No newline at end of file
diff --git a/mycobot_ai/ai_mypalletizer_260/launch/vision_wio.launch b/mycobot_ai/ai_mypalletizer_260/launch/vision_wio.launch
index 51eb78b..5548425 100644
--- a/mycobot_ai/ai_mypalletizer_260/launch/vision_wio.launch
+++ b/mycobot_ai/ai_mypalletizer_260/launch/vision_wio.launch
@@ -2,8 +2,8 @@
-
-
+
+
@@ -14,7 +14,7 @@
-
+
diff --git a/mycobot_ai/ai_mypalletizer_260/res/blue/goal9.jpeg b/mycobot_ai/ai_mypalletizer_260/res/blue/goal9.jpeg
new file mode 100644
index 0000000..a370eb4
Binary files /dev/null and b/mycobot_ai/ai_mypalletizer_260/res/blue/goal9.jpeg differ
diff --git a/mycobot_ai/ai_mypalletizer_260/res/gray/goal9.jpeg b/mycobot_ai/ai_mypalletizer_260/res/gray/goal9.jpeg
new file mode 100644
index 0000000..8f63e91
Binary files /dev/null and b/mycobot_ai/ai_mypalletizer_260/res/gray/goal9.jpeg differ
diff --git a/mycobot_ai/ai_mypalletizer_260/res/green/goal12.jpeg b/mycobot_ai/ai_mypalletizer_260/res/green/goal12.jpeg
new file mode 100644
index 0000000..c2949f0
Binary files /dev/null and b/mycobot_ai/ai_mypalletizer_260/res/green/goal12.jpeg differ
diff --git a/mycobot_ai/ai_mypalletizer_260/res/green/goal14.jpeg b/mycobot_ai/ai_mypalletizer_260/res/green/goal14.jpeg
new file mode 100644
index 0000000..7778ecb
Binary files /dev/null and b/mycobot_ai/ai_mypalletizer_260/res/green/goal14.jpeg differ
diff --git a/mycobot_ai/ai_mypalletizer_260/res/red/goal8.jpeg b/mycobot_ai/ai_mypalletizer_260/res/red/goal8.jpeg
new file mode 100644
index 0000000..abb5652
Binary files /dev/null and b/mycobot_ai/ai_mypalletizer_260/res/red/goal8.jpeg differ
diff --git a/mycobot_ai/ai_mypalletizer_260/res/takephoto.jpeg b/mycobot_ai/ai_mypalletizer_260/res/takephoto.jpeg
index 1798785..e529be2 100644
Binary files a/mycobot_ai/ai_mypalletizer_260/res/takephoto.jpeg and b/mycobot_ai/ai_mypalletizer_260/res/takephoto.jpeg differ
diff --git a/mycobot_ai/ai_mypalletizer_260/scripts/advance_detect_obj_color.py b/mycobot_ai/ai_mypalletizer_260/scripts/advance_detect_obj_color.py
new file mode 100644
index 0000000..3c4293e
--- /dev/null
+++ b/mycobot_ai/ai_mypalletizer_260/scripts/advance_detect_obj_color.py
@@ -0,0 +1,486 @@
+# -*- coding:utf-8 -*-
+#!/usr/bin/env python2
+import cv2
+import numpy as np
+import time
+import os,sys
+import rospy
+from visualization_msgs.msg import Marker
+from pymycobot.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 = [
+ [132.6, -155.6, 211.8, -20.9], # above the red bucket
+ [232.5, -134.1, 197.7, -45.26], # above the green bucket
+ [111.6, 159, 221.5, -120], # above the blue bucket
+ [-15.9, 164.6, 217.5, -119.35], # above the gray bucket
+ ]
+
+ # which robot: USB* is m5; ACM* is wio; AMA* is raspi
+ self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
+ self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
+ self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
+ self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
+ self.raspi = False
+ if "dev" in self.robot_m5:
+ self.Pin = [2, 5]
+ elif "dev" in self.robot_wio:
+ # self.Pin = [20, 21]
+ self.Pin = [2, 5]
+ for i in self.move_coords:
+ i[2] -= 20
+ elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+ import RPi.GPIO as GPIO
+ GPIO.setwarnings(False)
+ self.GPIO = GPIO
+ GPIO.setmode(GPIO.BCM)
+ GPIO.setup(20, GPIO.OUT)
+ GPIO.setup(21, GPIO.OUT)
+ GPIO.output(20, 1)
+ GPIO.output(21, 1)
+ self.raspi = True
+ if self.raspi:
+ self.gpio_status(False)
+ # choose place to set cube
+ self.color = 0
+ # parameters to calculate camera clipping parameters
+ self.x1 = self.x2 = self.y1 = self.y2 = 0
+ # set cache of real coord
+ self.cache_x = self.cache_y = 0
+
+ # set color HSV
+ self.HSV = {
+ "yellow": [np.array([11, 115, 70]), np.array([40, 255, 245])],
+ "red": [np.array([0, 43, 46]), np.array([8, 255, 255])],
+ "green": [np.array([35, 43, 35]), np.array([90, 255, 255])], # [77, 255, 255]
+ "blue": [np.array([100, 43, 46]), np.array([124, 255, 255])],
+ "cyan": [np.array([78, 43, 46]), np.array([99, 255, 255])], # np.array([78, 43, 46]), np.array([99, 255, 255])
+ }
+
+ # use to calculate coord between cube and mypal260
+ self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
+ # The coordinates of the grab center point relative to the mypal260
+ self.camera_x, self.camera_y = camera_x, camera_y
+ # The coordinates of the cube relative to the mypal260
+ self.c_x, self.c_y = 0, 0
+ # The ratio of pixels to actual values
+ self.ratio = 0
+ # Get ArUco marker dict that can be detected.
+ self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
+ # Get ArUco marker params.
+ self.aruco_params = cv2.aruco.DetectorParameters_create()
+
+ # init a node and a publisher
+ rospy.init_node("marker", anonymous=True)
+ self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
+ # init a Marker
+ self.marker = Marker()
+ self.marker.header.frame_id = "/joint1"
+ self.marker.ns = "cube"
+ self.marker.type = self.marker.CUBE
+ self.marker.action = self.marker.ADD
+ self.marker.scale.x = 0.04
+ self.marker.scale.y = 0.04
+ self.marker.scale.z = 0.04
+ self.marker.color.a = 1.0
+ self.marker.color.g = 1.0
+ self.marker.color.r = 1.0
+
+ # marker position initial
+ self.marker.pose.position.x = 0
+ self.marker.pose.position.y = 0
+ self.marker.pose.position.z = 0.03
+ self.marker.pose.orientation.x = 0
+ self.marker.pose.orientation.y = 0
+ self.marker.pose.orientation.z = 0
+ self.marker.pose.orientation.w = 1.0
+
+ # publish marker
+ def pub_marker(self, x, y, z=0.03):
+ self.marker.header.stamp = rospy.Time.now()
+ self.marker.pose.position.x = x
+ self.marker.pose.position.y = y
+ self.marker.pose.position.z = z
+ self.marker.color.g = self.color
+ self.pub.publish(self.marker)
+
+ # pump_control pi
+ def gpio_status(self, flag):
+ if flag:
+ # self.GPIO.output(20, 0)
+ self.GPIO.output(21, 0)
+ else:
+ # self.GPIO.output(20, 1)
+ self.GPIO.output(21, 1)
+
+ # 开启吸泵 m5
+ def pump_on(self):
+ # 让2号位工作
+ # self.mc.set_basic_output(2, 0)
+ # 让5号位工作
+ self.mc.set_basic_output(5, 0)
+
+ # 停止吸泵 m5
+ def pump_off(self):
+ # 让2号位停止工作
+ # self.mc.set_basic_output(2, 1)
+ # 让5号位停止工作
+ self.mc.set_basic_output(5, 1)
+
+ # Grasping motion
+ def move(self, x, y, color):
+ # send Angle to move mypal260
+ print(color)
+ self.mc.send_angles(self.move_angles[0], 20)
+ time.sleep(3)
+
+ # send coordinates to move mypal260
+ self.mc.send_coords([x, y, 160, 0], 20, 0)
+ time.sleep(1.5)
+ self.mc.send_coords([x, y, 96, 0], 20, 0)
+ time.sleep(1.5)
+
+ # open pump
+ if "dev" in self.robot_m5 or "dev" in self.robot_wio:
+ self.pump_on()
+ elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+ self.gpio_status(True)
+ time.sleep(1.5)
+
+ self.mc.send_angle(2, 0, 20)
+ time.sleep(0.3)
+ self.mc.send_angle(3, -20, 20)
+ time.sleep(2)
+
+ self.mc.send_coords(self.move_coords[color], 20, 1)
+ self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color]
+ [1]/1000.0, self.move_coords[color][2]/1000.0)
+ time.sleep(3)
+
+ # close pump
+ if "dev" in self.robot_m5 or "dev" in self.robot_wio:
+ self.pump_off()
+ elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+ self.gpio_status(False)
+ time.sleep(6)
+
+ if color == 1:
+ self.pub_marker(
+ self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02)
+ elif color == 0:
+ self.pub_marker(
+ self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0)
+
+ self.mc.send_angles(self.move_angles[1], 20)
+ time.sleep(1.5)
+
+ # decide whether grab cube
+ def decide_move(self, x, y, color):
+ print(x, y, self.cache_x, self.cache_y)
+ # detect the cube status move or run
+ if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm
+ self.cache_x, self.cache_y = x, y
+ return
+ else:
+ self.cache_x = self.cache_y = 0
+ # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
+ self.move(x, y, color)
+
+ # init mypal260
+ def run(self):
+ if "dev" in self.robot_m5:
+ self.mc = MyPalletizer(self.robot_m5, 115200)
+ elif "dev" in self.robot_wio:
+ self.mc = MyPalletizer(self.robot_wio, 115200)
+ elif "dev" in self.robot_raspi:
+ self.mc = MyPalletizer(self.robot_raspi, 1000000)
+ if not self.raspi:
+ print('start pump')
+ self.pub_pump(False, self.Pin)
+ print('end')
+ self.mc.send_angles([-29.0, 5.88, -4.92, -76.28], 20)
+ time.sleep(3)
+
+ # draw aruco
+ def draw_marker(self, img, x, y):
+ # draw rectangle on img
+ cv2.rectangle(
+ img,
+ (x - 20, y - 20),
+ (x + 20, y + 20),
+ (0, 255, 0),
+ thickness=2,
+ lineType=cv2.FONT_HERSHEY_COMPLEX,
+ )
+ # add text on rectangle
+ cv2.putText(img, "({},{})".format(x, y), (x, y),
+ cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (243, 0, 0), 2,)
+
+ # get points of two aruco
+ def get_calculate_params(self, img):
+ # Convert the image to a gray image
+ gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+ # Detect ArUco marker.
+ corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
+ gray, self.aruco_dict, parameters=self.aruco_params
+ )
+
+ """
+ Two Arucos must be present in the picture and in the same order.
+ There are two Arucos in the Corners, and each aruco contains the pixels of its four corners.
+ Determine the center of the aruco by the four corners of the aruco.
+ """
+ if len(corners) > 0:
+ if ids is not None:
+ if len(corners) <= 1 or ids[0] == 1:
+ return None
+ x1 = x2 = y1 = y2 = 0
+ point_11, point_21, point_31, point_41 = corners[0][0]
+ x1, y1 = int((point_11[0] + point_21[0] + point_31[0] + point_41[0]) / 4.0), int(
+ (point_11[1] + point_21[1] + point_31[1] + point_41[1]) / 4.0)
+ point_1, point_2, point_3, point_4 = corners[1][0]
+ x2, y2 = int((point_1[0] + point_2[0] + point_3[0] + point_4[0]) / 4.0), int(
+ (point_1[1] + point_2[1] + point_3[1] + point_4[1]) / 4.0)
+ return x1, x2, y1, y2
+ return None
+
+ # set camera clipping parameters
+ def set_cut_params(self, x1, y1, x2, y2):
+ self.x1 = int(x1)
+ self.y1 = int(y1)
+ self.x2 = int(x2)
+ self.y2 = int(y2)
+ print(self.x1, self.y1, self.x2, self.y2)
+
+ # set parameters to calculate the coords between cube and mypal260
+ def set_params(self, c_x, c_y, ratio):
+ self.c_x = c_x
+ self.c_y = c_y
+ self.ratio = 220.0/ratio
+
+ # calculate the coords between cube and mypal260
+ def get_position(self, x, y):
+ return ((y - self.c_y)*self.ratio + self.camera_x), ((x - self.c_x)*self.ratio + self.camera_y)
+
+ """
+ Calibrate the camera according to the calibration parameters.
+ Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times.
+ If two ARuco values have been calculated, clip the video.
+ """
+ def transform_frame(self, frame):
+ # enlarge the image by 1.5 times
+ fx = 1.5
+ fy = 1.5
+ frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy,
+ interpolation=cv2.INTER_CUBIC)
+ if self.x1 != self.x2:
+ # the cutting ratio here is adjusted according to the actual situation
+ # frame = frame[int(self.y2*0.2):int(self.y1*1.15),
+ # int(self.x1*0.7):int(self.x2*1.15)]
+ frame = frame[int(self.y2*0.78):int(self.y1*1.1),
+ int(self.x1*0.86):int(self.x2*1.08)]
+ return frame
+
+ # detect cube color
+ def color_detect(self, img):
+ # set the arrangement of color'HSV
+ x = y = 0
+ gs_img = cv2.GaussianBlur(img, (3, 3), 0) # 高斯模糊
+ # transfrom the img to model of gray
+ hsv = cv2.cvtColor(gs_img, cv2.COLOR_BGR2HSV)
+
+ for mycolor, item in self.HSV.items():
+ # print("mycolor:",mycolor)
+ redLower = np.array(item[0])
+ redUpper = np.array(item[1])
+
+ # wipe off all color expect color in range
+ mask = cv2.inRange(hsv, item[0], item[1])
+
+ # a etching operation on a picture to remove edge roughness
+ erosion = cv2.erode(mask, np.ones((1, 1), np.uint8), iterations=2)
+
+ # the image for expansion operation, its role is to deepen the color depth in the picture
+ dilation = cv2.dilate(erosion, np.ones(
+ (1, 1), np.uint8), iterations=2)
+
+ # adds pixels to the image
+ target = cv2.bitwise_and(img, img, mask=dilation)
+
+ # the filtered image is transformed into a binary image and placed in binary
+ ret, binary = cv2.threshold(dilation, 127, 255, cv2.THRESH_BINARY)
+
+ # get the contour coordinates of the image, where contours is the coordinate value, here only the contour is detected
+ contours, hierarchy = cv2.findContours(
+ dilation, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+
+ if len(contours) > 0:
+ # do something about misidentification
+ boxes = [
+ box
+ for box in [cv2.boundingRect(c) for c in contours]
+ if min(img.shape[0], img.shape[1]) / 10
+ < min(box[2], box[3])
+ < min(img.shape[0], img.shape[1]) / 1
+ ]
+ if boxes:
+ for box in boxes:
+ x, y, w, h = box
+ # find the largest object that fits the requirements
+ c = max(contours, key=cv2.contourArea)
+ # get the lower left and upper right points of the positioning object
+ x, y, w, h = cv2.boundingRect(c)
+ # locate the target by drawing rectangle
+ cv2.rectangle(img, (x, y), (x+w, y+h), (153, 153, 0), 2)
+ # calculate the rectangle center
+ x, y = (x*2+w)/2, (y*2+h)/2
+ # calculate the real coordinates of mypal260 relative to the target
+ if mycolor == "red":
+ self.color = 0
+
+ elif mycolor == "green":
+ self.color = 1
+
+ elif mycolor == "cyan" or mycolor == "blue":
+ self.color = 2
+
+ else:
+ self.color = 3
+
+
+
+ if abs(x) + abs(y) > 0:
+ return x, y
+ else:
+ return None
+
+if __name__ == "__main__":
+
+ # open the camera
+ cap_num = 0
+ cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
+ if not cap.isOpened():
+ cap.open()
+ # init a class of Object_detect
+ detect = Object_detect()
+ # init 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()
\ No newline at end of file
diff --git a/mycobot_ai/ai_mypalletizer_260/scripts/advance_detect_obj_img_folder.py b/mycobot_ai/ai_mypalletizer_260/scripts/advance_detect_obj_img_folder.py
new file mode 100644
index 0000000..6ded356
--- /dev/null
+++ b/mycobot_ai/ai_mypalletizer_260/scripts/advance_detect_obj_img_folder.py
@@ -0,0 +1,628 @@
+#!/usr/bin/env python2
+# encoding:utf-8
+from multiprocessing import Process, Pipe
+import cv2
+import numpy as np
+import time
+import os,sys
+import rospy
+from visualization_msgs.msg import Marker
+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 = [
+ [132.6, -155.6, 211.8, -20.9], # above the red bucket
+ [232.5, -134.1, 197.7, -45.26], # above the green bucket
+ [111.6, 159, 221.5, -120], # above the blue bucket
+ [-15.9, 164.6, 217.5, -119.35], # above the gray bucket
+ ]
+ # 判断连接设备: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]
+ if "dev" in self.robot_m5:
+ self.Pin = [2, 5]
+ elif "dev" in self.robot_wio:
+ self.Pin = [2, 5]
+ for i in self.move_coords:
+ i[2] -= 20
+ elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+ import RPi.GPIO as GPIO
+ GPIO.setwarnings(False)
+ self.GPIO = GPIO
+ GPIO.setmode(GPIO.BCM)
+ GPIO.setup(20, GPIO.OUT)
+ GPIO.setup(21, GPIO.OUT)
+ GPIO.output(20, 1)
+ GPIO.output(21, 1)
+ self.raspi = True
+ if self.raspi:
+ self.gpio_status(False)
+ # choose place to set cube
+ self.color = 0
+ # parameters to calculate camera clipping parameters
+ self.x1 = self.x2 = self.y1 = self.y2 = 0
+ # set cache of real coord
+ self.cache_x = self.cache_y = 0
+
+ # 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)
+
+ # pump_control pi
+ def gpio_status(self, flag):
+ if flag:
+ self.GPIO.output(20, 0)
+ self.GPIO.output(21, 0)
+ else:
+ self.GPIO.output(20, 1)
+ self.GPIO.output(21, 1)
+
+ # 开启吸泵 m5
+ def pump_on(self):
+ # 让2号位工作
+ # self.mc.set_basic_output(2, 0)
+ # 让5号位工作
+ self.mc.set_basic_output(5, 0)
+
+ # 停止吸泵 m5
+ def pump_off(self):
+ # 让2号位停止工作
+ # self.mc.set_basic_output(2, 1)
+ # 让5号位停止工作
+ self.mc.set_basic_output(5, 1)
+
+ # Grasping motion
+ def move(self, x, y, color):
+ print(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, 96, 0], 20, 0)
+ time.sleep(1.5)
+
+ # open pump
+ if "dev" in self.robot_m5 or "dev" in self.robot_wio:
+ self.pump_on()
+ elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+ self.gpio_status(True)
+ time.sleep(1.5)
+
+ self.mc.send_angle(2, 0, 20)
+ time.sleep(0.3)
+ self.mc.send_angle(3, -18, 20)
+ time.sleep(2)
+
+ self.mc.send_coords(self.move_coords[color], 20, 1)
+ self.pub_marker(self.move_coords[color][0] / 1000.0,
+ self.move_coords[color][1] / 1000.0,
+ self.move_coords[color][2] / 1000.0)
+ time.sleep(3)
+
+ # close pump
+ if "dev" in self.robot_m5 or "dev" in self.robot_wio:
+ self.pump_off()
+ elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
+ self.gpio_status(False)
+ time.sleep(6)
+
+ self.mc.send_angles(self.move_angles[1], 20)
+ time.sleep(1.5)
+
+ # decide whether grab cube
+ def decide_move(self, x, y, color):
+ print(x, y, self.cache_x, self.cache_y)
+ # detect the cube status move or run
+ if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm
+ self.cache_x, self.cache_y = x, y
+ return
+ else:
+ self.cache_x = self.cache_y = 0
+ # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
+ self.move(x, y, color)
+
+ # init mypal260
+ def run(self):
+ if "dev" in self.robot_m5:
+ self.mc = MyPalletizer(self.robot_m5, 115200)
+ elif "dev" in self.robot_wio:
+ self.mc = MyPalletizer(self.robot_wio, 115200)
+ elif "dev" in self.robot_raspi:
+ self.mc = MyPalletizer(self.robot_raspi, 1000000)
+ if not self.raspi:
+ self.pub_pump(False, self.Pin)
+ self.mc.send_angles([-29.0, 5.88, -4.92, -76.28], 20)
+ time.sleep(3)
+
+ # draw aruco
+ def draw_marker(self, img, x, y):
+ # draw rectangle on img
+ cv2.rectangle(
+ img,
+ (x - 20, y - 20),
+ (x + 20, y + 20),
+ (0, 255, 0),
+ thickness=2,
+ lineType=cv2.FONT_HERSHEY_COMPLEX,
+ )
+ # add text on rectangle
+ cv2.putText(
+ img,
+ "({},{})".format(x, y),
+ (x, y),
+ cv2.FONT_HERSHEY_COMPLEX_SMALL,
+ 1,
+ (243, 0, 0),
+ 2,
+ )
+
+ # get points of two aruco
+ def get_calculate_params(self, img):
+ # Convert the image to a gray image
+ gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+ # Detect ArUco marker.
+ corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
+ gray, self.aruco_dict, parameters=self.aruco_params)
+
+ """
+ Two Arucos must be present in the picture and in the same order.
+ There are two Arucos in the Corners, and each aruco contains the pixels of its four corners.
+ Determine the center of the aruco by the four corners of the aruco.
+ """
+ if len(corners) > 0:
+ if ids is not None:
+ if len(corners) <= 1 or ids[0] == 1:
+ return None
+ x1 = x2 = y1 = y2 = 0
+ point_11, point_21, point_31, point_41 = corners[0][0]
+ x1, y1 = int(
+ (point_11[0] + point_21[0] + point_31[0] + point_41[0]) /
+ 4.0), int(
+ (point_11[1] + point_21[1] + point_31[1] + point_41[1])
+ / 4.0)
+ point_1, point_2, point_3, point_4 = corners[1][0]
+ x2, y2 = int(
+ (point_1[0] + point_2[0] + point_3[0] + point_4[0]) /
+ 4.0), int(
+ (point_1[1] + point_2[1] + point_3[1] + point_4[1]) /
+ 4.0)
+ return x1, x2, y1, y2
+ return None
+
+ # set camera clipping parameters
+ def set_cut_params(self, x1, y1, x2, y2):
+ self.x1 = int(x1)
+ self.y1 = int(y1)
+ self.x2 = int(x2)
+ self.y2 = int(y2)
+ print(self.x1, self.y1, self.x2, self.y2)
+
+ # set parameters to calculate the coords between cube and mycobot
+ def set_params(self, c_x, c_y, ratio):
+ self.c_x = c_x
+ self.c_y = c_y
+ self.ratio = 220.0 / ratio
+
+ # calculate the coords between cube and mycobot
+ def get_position(self, x, y):
+ return ((y - self.c_y) * self.ratio +
+ self.camera_x), ((x - self.c_x) * self.ratio + self.camera_y)
+
+ """
+ Calibrate the camera according to the calibration parameters.
+ Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times.
+ If two ARuco values have been calculated, clip the video.
+ """
+
+ def transform_frame(self, frame):
+ # enlarge the image by 1.5 times
+ fx = 1.5
+ fy = 1.5
+ frame = cv2.resize(frame, (0, 0),
+ fx=fx,
+ fy=fy,
+ interpolation=cv2.INTER_CUBIC)
+ if self.x1 != self.x2:
+ # the cutting ratio here is adjusted according to the actual situation
+ frame = frame[int(self.y2 * 0.2):int(self.y1 * 1.15),
+ int(self.x1 * 0.7):int(self.x2 * 1.15)]
+ return frame
+
+ # according the class_id to get object name
+ def id_class_name(self, class_id):
+ for key, value in self.labels.items():
+ if class_id == int(key):
+ return value
+
+ # detect object
+ def obj_detect(self, img, goal, kp_img, desc_img, kp_list, desc_list, connection):
+ i = 0
+ MIN_MATCH_COUNT = 5
+ # sift = cv2.xfeatures2d.SIFT_create()
+
+ # find the keypoints and descriptors with SIFT
+ # kp = []
+ # des = []
+ kp = kp_list
+ des = desc_list
+
+ # for i in goal:
+ # kp0, des0 = sift.detectAndCompute(i, None)
+ # kp.append(kp0)
+ # des.append(des0)
+
+ # kp1, des1 = sift.detectAndCompute(goal, None)
+ # kp2, des2 = sift.detectAndCompute(img, None)
+ kp2, des2 = kp_img, desc_img
+
+ # FLANN parameters
+ FLANN_INDEX_KDTREE = 0
+ index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
+ search_params = dict(checks=50) # or pass empty dictionary
+ flann = cv2.FlannBasedMatcher(index_params, search_params)
+
+ x, y = 0, 0
+ try:
+ for i in range(len(des)):
+ matches = flann.knnMatch(des[i], des2, k=2)
+ # store all the good matches as per Lowe's ratio test. 根据Lowe比率测试存储所有良好匹配项。
+ good = []
+ for m, n in matches:
+ if m.distance < 0.7 * n.distance:
+ good.append(m)
+
+ # When there are enough robust matching point pairs 当有足够的健壮匹配点对（至少个MIN_MATCH_COUNT）时
+ if len(good) > MIN_MATCH_COUNT:
+
+ # extract corresponding point pairs from matching 从匹配中提取出对应点对
+ # query index of small objects, training index of scenarios 小对象的查询索引，场景的训练索引
+ src_pts = np.float32([kp[i][m.queryIdx].pt
+ for m in good]).reshape(-1, 1, 2)
+ dst_pts = np.float32([kp2[m.trainIdx].pt
+ for m in good]).reshape(-1, 1, 2)
+
+ # Using matching points to find homography matrix in cv2.ransac 利用匹配点找到CV2.RANSAC中的单应矩阵
+ M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,
+ 5.0)
+ matchesMask = mask.ravel().tolist()
+ # Calculate the distortion of image, that is the corresponding position in frame 计算图1的畸变，也就是在图2中的对应的位置
+ h, w, d = goal[i].shape
+ pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1],
+ [w - 1, 0]]).reshape(-1, 1, 2)
+ dst = cv2.perspectiveTransform(pts, M)
+ coord = (dst[0][0] + dst[1][0] + dst[2][0] +
+ dst[3][0]) / 4.0
+ connection.send((DRAW_COORDS, coord))
+ # cv2.putText(img, "{}".format(coord), (50, 60),
+ # fontFace=None, fontScale=1,
+ # color=(0, 255, 0), lineType=1)
+ print(format(dst[0][0][0]))
+ x = (dst[0][0][0] + dst[1][0][0] + dst[2][0][0] +
+ dst[3][0][0]) / 4.0
+ y = (dst[0][0][1] + dst[1][0][1] + dst[2][0][1] +
+ dst[3][0][1]) / 4.0
+
+ # bound box 绘制边框
+ # img = cv2.polylines(img, [np.int32(dst)], True, 244, 3, cv2.LINE_AA)
+ connection.send((DRAW_RECT, dst))
+ # cv2.polylines(mixture, [np.int32(dst)], True, (0, 255, 0), 2, cv2.LINE_AA)
+ except Exception as e:
+ pass
+
+ if x + y > 0:
+ return x, y
+ else:
+ return None
+
+# The path to save the image folder
+def parse_folder(folder):
+ restore = []
+ path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mypalletizer_260/' + folder
+ path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mypalletizer_260/' + folder
+
+ if os.path.exists(path1):
+ path = path1
+ elif os.path.exists(path2):
+ path = path2
+
+ for i, j, k in os.walk(path):
+ for l in k:
+ restore.append(cv2.imread(folder + '/{}'.format(l)))
+ return restore
+
+def compute_keypoints_and_descriptors(sift, images_lists):
+ kp_list = []
+ desc_list = []
+ for images in images_lists:
+ kp_tmp = []
+ desc_tmp = []
+ for img in images:
+ kp, desc = sift.detectAndCompute(img, None)
+ kp_tmp.append(kp)
+ desc_tmp.append(desc)
+ kp_list.append(kp_tmp)
+ desc_list.append(desc_tmp)
+
+ return kp_list, desc_list
+
+GET_FRAME = 1
+STOP_PROCESSING = 2
+DRAW_COORDS = 3
+DRAW_RECT = 4
+CLEAR_DRAW = 5
+CROP_FRAME = 6
+
+def get_frame(connection):
+ connection.send(GET_FRAME)
+ frame = connection.recv()
+ return frame
+
+def process_transform_frame(frame, x1, y1, x2, y2):
+ # enlarge the image by 1.5 times
+ fx = 1.5
+ fy = 1.5
+ frame = cv2.resize(frame, (0, 0),
+ fx=fx,
+ fy=fy,
+ interpolation=cv2.INTER_CUBIC)
+# if x1 != x2:
+ # the cutting ratio here is adjusted according to the actual situation
+# frame = frame[int(y2 * 0.2):int(y1 * 1.15),
+# int(x1 * 0.7):int(x2 * 1.15)]
+ return frame
+
+def process_display_frame(connection):
+ cap_num = 0
+ coord = None
+ dst = None
+ x1 = 0
+ y1 = 0
+ x2 = 0
+ y2 = 0
+ cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
+ if not cap.isOpened():
+ cap.open()
+ while cv2.waitKey(1) < 0:
+ _, frame = cap.read()
+ frame = process_transform_frame(frame, x1, y1, x2, y2)
+ if connection.poll():
+ request = connection.recv()
+ if request == GET_FRAME:
+ connection.send(frame)
+ elif request == CLEAR_DRAW:
+ coord = None
+ dst = None
+ elif type(request) is tuple:
+ if request[0] == DRAW_COORDS:
+ coord = request[1]
+ elif request[0] == DRAW_RECT:
+ dst = request[1]
+ elif request[0] == CROP_FRAME:
+ x1 = request[1]
+ y1 = request[2]
+ x2 = request[3]
+ y2 = request[4]
+
+ if not coord is None:
+ cv2.putText(frame, "{}".format(coord), (50, 60), fontFace=None,
+ fontScale=1, color=(0, 255, 0), lineType=1)
+ if not dst is None:
+ frame = cv2.polylines(frame, [np.int32(dst)], True, 244, 3, cv2.LINE_AA)
+ cv2.imshow("figure", frame)
+ time.sleep(0.04)
+ connection.send(STOP_PROCESSING)
+
+def run():
+ parent_conn, child_conn = Pipe()
+ child = Process(target = process_display_frame, args=(child_conn,))
+ child.start()
+
+ 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()
diff --git a/mycobot_ai/ai_mypalletizer_260/scripts/test.py b/mycobot_ai/ai_mypalletizer_260/scripts/test.py
index d73de19..1d232e0 100755
--- a/mycobot_ai/ai_mypalletizer_260/scripts/test.py
+++ b/mycobot_ai/ai_mypalletizer_260/scripts/test.py
@@ -4,7 +4,7 @@ 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(os.popen("ls /dev/ttyUSB*").readline()[:-1], 115200)
# mc = MyPalletizer("/dev/ttyAMA0", 1000000)
# mc.send_angles([-29.0, 5.88, -4.92, -76.28],25) # init the point coords:[155.3, -86.1, 218.4, -47.28]
@@ -13,8 +13,8 @@ mc = MyPalletizer(os.popen("ls /dev/ttyUSB*").readline()[:-1], 115200)
# mc.send_angles([-47.1, 10.19, -10.1, -76.37],25) # above the red bucket; coords:[127.3, -137.1, 219.2, -29.26]
# time.sleep(1.5)
-mc.send_angles([0,0,-15,0],25)
-time.sleep(2)
+# 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)
@@ -38,4 +38,27 @@ time.sleep(2)
# mc.set_servo_calibration(1)
# mc.set_servo_calibration(2)
# mc.set_servo_calibration(3)
-# mc.set_servo_calibration(4)
\ No newline at end of file
+# mc.set_servo_calibration(4)
+
+import sys
+import numpy as np
+import cv2
+
+print("Please enter blue:")
+blue = input()
+print("Please enter green:")
+green = input()
+print("Please enter red:")
+red = input()
+
+color = np.uint8([[[blue, green, red]]])
+hsv_color = cv2.cvtColor(color, cv2.COLOR_BGR2HSV)
+
+hue = hsv_color[0][0][0]
+
+print("Lower bound is :")
+print("[" + str(hue - 10) + ", 100, 100]\n")
+
+print("Upper bound is :"),
+print("[" + str(hue + 10) + ", 255, 255]")
+
diff --git a/mycobot_communication/scripts/mycobot_services.py b/mycobot_communication/scripts/mycobot_services.py
index ce2dc4d..af4dbf7 100755
--- a/mycobot_communication/scripts/mycobot_services.py
+++ b/mycobot_communication/scripts/mycobot_services.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/usr/bin/env python2
# -*- coding: utf-8 -*
import time
import rospy
@@ -32,7 +32,10 @@ def acquire(lock_file):
else:
lock_file_fd = fd
break
- print('pid waiting for lock:%d'% pid)
+
+ # print('pid waiting for lock:%d'% pid)
+
+
time.sleep(1.0)
current_time = time.time()
if lock_file_fd is None:
@@ -46,8 +49,6 @@ def release(lock_file_fd):
os.close(lock_file_fd)
return None
-
-
def create_handle():
global mc
rospy.init_node("mycobot_services")
@@ -80,7 +81,7 @@ def set_angles(req):
req.joint_6,
]
sp = req.speed
- print('angles1:',angles)
+
if mc:
lock = acquire("/tmp/mycobot_lock")
mc.send_angles(angles, sp)
@@ -151,6 +152,7 @@ def toggle_pump(req):
mc.set_basic_output(req.Pin2, 1)
release(lock)
+
return PumpStatusResponse(True)
diff --git a/mypalletizer_260/mypalletizer_260/config/mypal_260.rviz b/mypalletizer_260/mypalletizer_260/config/mypal_260.rviz
index 4e1a0a5..2c84dd7 100644
--- a/mypalletizer_260/mypalletizer_260/config/mypal_260.rviz
+++ b/mypalletizer_260/mypalletizer_260/config/mypal_260.rviz
@@ -8,8 +8,9 @@ Panels:
- /Status1
- /RobotModel1
- /TF1
+ - /Marker1
Splitter Ratio: 0.5
- Tree Height: 601
+ Tree Height: 607
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@@ -18,7 +19,7 @@ Panels:
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
- Splitter Ratio: 0.5886790156364441
+ Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
@@ -29,8 +30,6 @@ Panels:
Name: Time
SyncMode: 0
SyncSource: ""
-Preferences:
- PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
@@ -42,7 +41,7 @@ Visualization Manager:
Color: 160; 160; 164
Enabled: true
Line Style:
- Line Width: 0.029999999329447746
+ Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
@@ -69,6 +68,11 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
+ env:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
link1:
Alpha: 1
Show Axes: false
@@ -107,6 +111,8 @@ Visualization Manager:
All Enabled: true
base:
Value: true
+ env:
+ Value: true
link1:
Value: true
link2:
@@ -117,13 +123,15 @@ Visualization Manager:
Value: true
link5:
Value: true
- Marker Scale: 0.30000001192092896
+ Marker Scale: 0.300000012
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
base:
+ env:
+ {}
link1:
link2:
link3:
@@ -132,6 +140,14 @@ Visualization Manager:
{}
Update Interval: 0
Value: true
+ - Class: rviz/Marker
+ Enabled: true
+ Marker Topic: /cube
+ Name: Marker
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
@@ -147,10 +163,7 @@ Visualization Manager:
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
- Theta std deviation: 0.2617993950843811
Topic: /initialpose
- X std deviation: 0.5
- Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
@@ -160,33 +173,33 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
- Distance: 1.2028908729553223
+ Distance: 1.20289087
Enable Stereo Rendering:
- Stereo Eye Separation: 0.05999999865889549
+ Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
- Z: 0.10758385062217712
+ Z: 0.107583851
Focal Shape Fixed Size: true
- Focal Shape Size: 0.05000000074505806
+ Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
- Near Clip Distance: 0.009999999776482582
- Pitch: 0.42039862275123596
+ Near Clip Distance: 0.00999999978
+ Pitch: 0.420398623
Target Frame:
Value: Orbit (rviz)
- Yaw: 1.1754094362258911
+ Yaw: 1.17540944
Saved: ~
Window Geometry:
Displays:
collapsed: false
- Height: 892
+ Height: 888
Hide Left Dock: false
Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd00000004000000000000016a000002e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002e2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001a4000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002e2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006200000003efc0100000002fb0000000800540069006d00650100000000000006200000024400fffffffb0000000800540069006d0065010000000000000450000000000000000000000306000002e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd00000004000000000000016a000002eefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002ee000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001a4000002eefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002ee000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006200000003efc0100000002fb0000000800540069006d00650100000000000006200000030000fffffffb0000000800540069006d0065010000000000000450000000000000000000000306000002ee00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -198,201 +211,3 @@ Window Geometry:
Width: 1568
X: 144
Y: 41
-
-# Panels:
-# - Class: rviz/Displays
-# Help Height: 78
-# Name: Displays
-# Property Tree Widget:
-# Expanded:
-# - /Global Options1
-# - /Status1
-# - /RobotModel1
-# - /TF1
-# Splitter Ratio: 0.5
-# Tree Height: 775
-# - Class: rviz/Selection
-# Name: Selection
-# - Class: rviz/Tool Properties
-# Expanded:
-# - /2D Pose Estimate1
-# - /2D Nav Goal1
-# - /Publish Point1
-# Name: Tool Properties
-# Splitter Ratio: 0.588679016
-# - Class: rviz/Views
-# Expanded:
-# - /Current View1
-# Name: Views
-# Splitter Ratio: 0.5
-# - Class: rviz/Time
-# Experimental: false
-# Name: Time
-# SyncMode: 0
-# SyncSource: ""
-# Toolbars:
-# toolButtonStyle: 2
-# Visualization Manager:
-# Class: ""
-# Displays:
-# - Alpha: 0.5
-# Cell Size: 1
-# Class: rviz/Grid
-# Color: 160; 160; 164
-# Enabled: true
-# Line Style:
-# Line Width: 0.0299999993
-# Value: Lines
-# Name: Grid
-# Normal Cell Count: 0
-# Offset:
-# X: 0
-# Y: 0
-# Z: 0
-# Plane: XY
-# Plane Cell Count: 10
-# Reference Frame:
-# Value: true
-# - Alpha: 1
-# Class: rviz/RobotModel
-# Collision Enabled: false
-# Enabled: true
-# Links:
-# All Links Enabled: true
-# Expand link Details: false
-# Expand Link Details: false
-# Expand Tree: false
-# Link Tree Style: Links in Alphabetic Order
-# base:
-# Alpha: 1
-# Show Axes: false
-# Show Trail: false
-# Value: true
-# link1:
-# Alpha: 1
-# Show Axes: false
-# Show Trail: false
-# Value: true
-# link2:
-# Alpha: 1
-# Show Axes: false
-# Show Trail: false
-# Value: true
-# link3:
-# Alpha: 1
-# Show Axes: false
-# Show Trail: false
-# Value: true
-# link4:
-# Alpha: 1
-# Show Axes: false
-# Show Trail: false
-# Value: true
-# link5:
-# Alpha: 1
-# Show Axes: false
-# Show Trail: false
-# Value: true
-
-# Name: RobotModel
-# Robot Description: robot_description
-# TF Prefix: ""
-# Update Interval: 0
-# Value: true
-# Visual Enabled: true
-# - Class: rviz/TF
-# Enabled: true
-# Frame Timeout: 15
-# Frames:
-# All Enabled: true
-# base:
-# Value: true
-# link1:
-# Value: true
-# link2:
-# Value: true
-# link3:
-# Value: true
-# link4:
-# Value: true
-# link5:
-# Value: true
-
-# Marker Scale: 0.300000012
-# Name: TF
-# Show Arrows: true
-# Show Axes: true
-# Show Names: true
-# Tree:
-# base:
-# link1:
-# link2:
-# link3:
-# link4:
-# link5:
-# {}
-# Update Interval: 0
-# Value: true
-# Enabled: true
-# Global Options:
-# Background Color: 48; 48; 48
-# Default Light: true
-# Fixed Frame: base
-# Frame Rate: 30
-# Name: root
-# Tools:
-# - Class: rviz/Interact
-# Hide Inactive Objects: true
-# - Class: rviz/MoveCamera
-# - Class: rviz/Select
-# - Class: rviz/FocusCamera
-# - Class: rviz/Measure
-# - Class: rviz/SetInitialPose
-# Topic: /initialpose
-# - Class: rviz/SetGoal
-# Topic: /move_base_simple/goal
-# - Class: rviz/PublishPoint
-# Single click: true
-# Topic: /clicked_point
-# Value: true
-# Views:
-# Current:
-# Class: rviz/Orbit
-# Distance: 1.20289087
-# Enable Stereo Rendering:
-# Stereo Eye Separation: 0.0599999987
-# Stereo Focal Distance: 1
-# Swap Stereo Eyes: false
-# Value: false
-# Focal Point:
-# X: 0
-# Y: 0
-# Z: 0.107583851
-# Focal Shape Fixed Size: true
-# Focal Shape Size: 0.0500000007
-# Invert Z Axis: false
-# Name: Current View
-# Near Clip Distance: 0.00999999978
-# Pitch: 0.440398335
-# Target Frame:
-# Value: Orbit (rviz)
-# Yaw: 0.430389732
-# Saved: ~
-# Window Geometry:
-# Displays:
-# collapsed: false
-# Height: 1056
-# Hide Left Dock: false
-# Hide Right Dock: false
-# QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-# Selection:
-# collapsed: false
-# Time:
-# collapsed: false
-# Tool Properties:
-# collapsed: false
-# Views:
-# collapsed: false
-# Width: 1855
-# X: 65
-# Y: 24
diff --git a/mypalletizer_260/mypalletizer_communication/launch/communication_topic.launch b/mypalletizer_260/mypalletizer_communication/launch/communication_topic.launch
index 6cf9d22..3966f37 100644
--- a/mypalletizer_260/mypalletizer_communication/launch/communication_topic.launch
+++ b/mypalletizer_260/mypalletizer_communication/launch/communication_topic.launch
@@ -1,5 +1,5 @@
-
+
diff --git a/mypalletizer_260/mypalletizer_communication/scripts/mypal_topics.py b/mypalletizer_260/mypalletizer_communication/scripts/mypal_topics.py
index 49deb6e..0655778 100755
--- a/mypalletizer_260/mypalletizer_communication/scripts/mypal_topics.py
+++ b/mypalletizer_260/mypalletizer_communication/scripts/mypal_topics.py
@@ -67,11 +67,17 @@ class MypalTopics(object):
rospy.init_node("mypal_topics")
rospy.loginfo("start ...")
- port = rospy.get_param("~port", os.popen("ls /dev/ttyUSB*").readline()[:-1])
- print(port)
- baud = rospy.get_param("~baud", 115200)
- rospy.loginfo("%s,%s" % (port, baud))
- self.mc = MyPalletizer(port, baud)
+ port_m5 = rospy.get_param("~port", os.popen("ls /dev/ttyUSB*").readline()[:-1])
+ # if not port:
+ port_wio = rospy.get_param("~port", os.popen("ls /dev/ttyACM*").readline()[:-1])
+ if "dev" in port_m5:
+ baud = rospy.get_param("~baud", 115200)
+ rospy.loginfo("%s,%s" % (port_m5, baud))
+ self.mc = MyPalletizer(port_m5, baud)
+ elif "dev" in port_wio:
+ baud = rospy.get_param("~baud", 115200)
+ rospy.loginfo("%s,%s" % (port_wio, baud))
+ self.mc = MyPalletizer(port_wio, baud)
self.lock = threading.Lock()
def start(self):