mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
optimize image recoginition of /scripts/detect_obj_img_folder_opt.py
This commit is contained in:
wangWking
parent
b86e8b4d68
commit
2e6482823b
45 changed files with 26085 additions and 3140 deletions
|
|
@ -1,37 +0,0 @@
|
|||
### Tips：
|
||||
|
||||
1. 请使用与机械臂相同型号的文件名文件。
|
||||
|
||||
2. 请先执行以下操作:
|
||||
+ 1. 打开一个新的终端
|
||||
+ 2. 输入命令
|
||||
```bash
|
||||
chmod +x /home/h/catkin_mycobot/src/mycobot_ros/mycobot_communication/scripts/xxx.py
|
||||
# 此处为各个新增文件的文件名
|
||||
```
|
||||
3. jetson nano的文件还没有使用机械臂进行过测试，可能存在问题。
|
||||
|
||||
4. 数莓派版本的使用:
|
||||
+ 1. 打开VScode,新建一个文件，复制以下内容（请确保电脑与数莓派机械臂已经连接）并运行
|
||||
```bash
|
||||
from pymycobot import MyCobotSocket
|
||||
|
||||
mc = MyCobotSocket("192.168.10.10","9000")
|
||||
mc.connect()
|
||||
```
|
||||
|
||||
+ 2. 打开“网络与internet”设置
|
||||
更改适配器选项
|
||||
右键打开数莓派的以太网属性
|
||||
打开 “internet协议版本4” 的属性
|
||||
选择 “使用下面的IP地址”
|
||||
IP地址为 ： 192.168.10.100 （最后一位非10都可）
|
||||
子网掩码为： 255.255.255.0
|
||||
确认
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -1,28 +0,0 @@
|
|||
<launch>
|
||||
<arg name="port" default="192.168.10.10" />
|
||||
<arg name="baud" default="9000" />
|
||||
|
||||
<!-- urdf文件，模型文件的路径 -->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/jetsonNano/mycobot_urdf.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
|
||||
<!-- <arg name="gui" default="false" /> -->
|
||||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||
|
||||
<!-- Combinejoin values to TF -->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" >
|
||||
<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
|
||||
</node>
|
||||
|
||||
<!-- mycobot-topics -->
|
||||
<include file="$(find mycobot_communication)/launch/communication_topic_pi.launch">
|
||||
<arg name="port" value="$(arg port)" />
|
||||
<arg name="baud" value="$(arg baud)" />
|
||||
</include>
|
||||
|
||||
<!-- listen and pub the real angles -->
|
||||
<node name="real_listener" pkg="mycobot_280" type="listen_real_of_topic.py" />
|
||||
|
||||
<!-- Show in Rviz -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
</launch>
|
||||
|
|
@ -1,27 +0,0 @@
|
|||
<launch>
|
||||
<arg name="port" default="/dev/ttyACM0" />
|
||||
<arg name="baud" default="115200" />
|
||||
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/seeed/mycobot_urdf.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
|
||||
<!-- <arg name="gui" default="false" /> -->
|
||||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||
|
||||
<!-- Combinejoin values to TF -->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" >
|
||||
<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
|
||||
</node>
|
||||
|
||||
<!-- mycobot-topics -->
|
||||
<include file="$(find mycobot_communication)/launch/communication_seeed.launch">
|
||||
<arg name="port" value="$(arg port)" />
|
||||
<arg name="baud" value="$(arg baud)" />
|
||||
</include>
|
||||
|
||||
<!-- listen and pub the real angles -->
|
||||
<node name="real_listener" pkg="mycobot_280" type="listen_real_of_topic.py" />
|
||||
|
||||
<!-- Show in Rviz -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
</launch>
|
||||
|
|
@ -1,6 +1,6 @@
|
|||
<launch>
|
||||
<arg name="port" default="/dev/ttyACM0" />
|
||||
<arg name="baud" default="115200" />
|
||||
<launch>
|
||||
<arg name="port" default="/dev/ttyAMA0" />
|
||||
<arg name="baud" default="1000000" />
|
||||
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_vision.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
|
||||
|
|
|
|||
|
|
@ -1,8 +1,8 @@
|
|||
<launch>
|
||||
<arg name="port" default="/dev/ttyTHS1" />
|
||||
<arg name="port" default="/dev/ttyACM0" />
|
||||
<arg name="baud" default="115200" />
|
||||
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/jetsonNano/mycobot_urdf.urdf"/>
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_vision.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
|
||||
<!-- <arg name="gui" default="false" /> -->
|
||||
|
||||
|
|
@ -14,7 +14,7 @@
|
|||
</node>
|
||||
|
||||
<!-- mycobot-topics -->
|
||||
<include file="$(find mycobot_communication)/launch/communication_jsnn.launch">
|
||||
<include file="$(find mycobot_communication)/launch/communication_topic.launch">
|
||||
<arg name="port" value="$(arg port)" />
|
||||
<arg name="baud" value="$(arg baud)" />
|
||||
</include>
|
||||
BIN
mycobot_ai/local_photo/goal5.jpeg
Normal file
BIN
mycobot_ai/local_photo/goal5.jpeg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 4.3 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 6.2 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 6.9 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 5 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 6.3 KiB After Width: | Height: | Size: 5 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 4.8 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 4.4 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 4.3 KiB |
BIN
mycobot_ai/local_photo/takephoto.jpeg
Normal file
BIN
mycobot_ai/local_photo/takephoto.jpeg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 50 KiB |
21703
mycobot_ai/prof.calltree
Normal file
21703
mycobot_ai/prof.calltree
Normal file
File diff suppressed because it is too large
Load diff
BIN
mycobot_ai/prof.out
Normal file
BIN
mycobot_ai/prof.out
Normal file
Binary file not shown.
Binary file not shown.
|
Before Width: | Height: | Size: 4.3 KiB After Width: | Height: | Size: 5.2 KiB |
BIN
mycobot_ai/res/blue/goal2.jpeg
Normal file
BIN
mycobot_ai/res/blue/goal2.jpeg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 6 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 6.2 KiB After Width: | Height: | Size: 5.3 KiB |
BIN
mycobot_ai/res/gray/goal2.jpeg
Normal file
BIN
mycobot_ai/res/gray/goal2.jpeg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 4.6 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 4.9 KiB After Width: | Height: | Size: 5.6 KiB |
BIN
mycobot_ai/res/green/goal2.jpeg
Normal file
BIN
mycobot_ai/res/green/goal2.jpeg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 5.1 KiB |
BIN
mycobot_ai/res/green/goal3.jpeg
Normal file
BIN
mycobot_ai/res/green/goal3.jpeg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 6 KiB |
BIN
mycobot_ai/res/green/goal4.jpeg
Normal file
BIN
mycobot_ai/res/green/goal4.jpeg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 4.5 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 4.7 KiB After Width: | Height: | Size: 6.9 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 47 KiB After Width: | Height: | Size: 43 KiB |
17
mycobot_ai/scripts/add_img.py
Normal file → Executable file
17
mycobot_ai/scripts/add_img.py
Normal file → Executable file
|
|
@ -1,5 +1,4 @@
|
|||
# coding:utf-8
|
||||
from ast import keyword
|
||||
from fileinput import filename
|
||||
import os, cv2, sys
|
||||
|
||||
|
|
@ -50,19 +49,19 @@ def take_photo():
|
|||
|
||||
def cut_photo():
|
||||
|
||||
path_red = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/res/red'
|
||||
path_red = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/red'
|
||||
for i, j, k in os.walk(path_red):
|
||||
file_len_red = len(k)
|
||||
|
||||
path_gray = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/res/gray'
|
||||
path_gray = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/gray'
|
||||
for i, j, k in os.walk(path_gray):
|
||||
file_len_gray = len(k)
|
||||
|
||||
path_green = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/res/green'
|
||||
path_green = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/green'
|
||||
for i, j, k in os.walk(path_green):
|
||||
file_len_green = len(k)
|
||||
|
||||
path_blue = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/res/blue'
|
||||
path_blue = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/blue'
|
||||
for i, j, k in os.walk(path_blue):
|
||||
file_len_blue = len(k)
|
||||
print("请截取要识别的部分")
|
||||
|
|
@ -96,19 +95,19 @@ def cut_photo():
|
|||
cv2.imshow('crop', crop)
|
||||
# 选择红桶文件夹
|
||||
if kw == 'red':
|
||||
cv2.imwrite('/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/res/red/goal{}.jpeg'.format(str(file_len_red + 1)),crop)
|
||||
cv2.imwrite('/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/red/goal{}.jpeg'.format(str(file_len_red + 1)),crop)
|
||||
print('Saved')
|
||||
# 选择灰桶文件夹
|
||||
elif kw == 'gray':
|
||||
cv2.imwrite('/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/res/gray/goal{}.jpeg'.format(str(file_len_gray+1)),crop)
|
||||
cv2.imwrite('/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/gray/goal{}.jpeg'.format(str(file_len_gray+1)),crop)
|
||||
print('Saved')
|
||||
# 选择绿桶文件夹
|
||||
elif kw == 'green':
|
||||
cv2.imwrite('/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/res/green/goal{}.jpeg'.format(str(file_len_green+1)),crop)
|
||||
cv2.imwrite('/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/green/goal{}.jpeg'.format(str(file_len_green+1)),crop)
|
||||
print('Saved')
|
||||
# 选择蓝桶文件夹
|
||||
elif kw == 'blue':
|
||||
cv2.imwrite('/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/res/blue/goal{}.jpeg'.format(str(file_len_blue+1)),crop)
|
||||
cv2.imwrite('/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/res/blue/goal{}.jpeg'.format(str(file_len_blue+1)),crop)
|
||||
print('Saved')
|
||||
|
||||
# 退出
|
||||
|
|
|
|||
|
|
@ -1,182 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
# encoding:utf-8
|
||||
|
||||
from tkinter import ttk
|
||||
from tkinter import *
|
||||
import os
|
||||
import time
|
||||
|
||||
import threading
|
||||
from multiprocessing import Process
|
||||
|
||||
|
||||
class Application(object):
|
||||
def __init__(self):
|
||||
self.win = Tk()
|
||||
# 窗口置顶
|
||||
self.win.wm_attributes('-topmost', 1)
|
||||
self.ros = False
|
||||
# 运行的文件
|
||||
self.run_py = ""
|
||||
# 判断通信口并给权限
|
||||
try:
|
||||
self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
|
||||
self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
|
||||
self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
|
||||
self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
|
||||
if "dev" in self.robot_wio:
|
||||
self.set_file(self.robot_wio)
|
||||
elif "dev" in self.robot_m5:
|
||||
self.set_file(self.robot_m5)
|
||||
elif "dev" in self.robot_raspi:
|
||||
self.change_file(self.robot_raspi)
|
||||
elif "dev" in self.robot_jes:
|
||||
self.change_file(self.robot_jes)
|
||||
except Exception as e:
|
||||
pass
|
||||
|
||||
# 设置标题
|
||||
self.win.title("aikit启动工具")
|
||||
self.win.geometry(
|
||||
"550x350+100+100") # 290 160为窗口大小，+10 +10 定义窗口弹出时的默认展示位置
|
||||
|
||||
self.chanse_code = Label(self.win, text="选择程序:", width=10)
|
||||
self.chanse_code.grid(row=1)
|
||||
|
||||
self.myComboList = [u"颜色识别", u"物体识别", u"二维码识别"]
|
||||
self.myCombox = ttk.Combobox(self.win, values=self.myComboList)
|
||||
self.myCombox.grid(row=1, column=1)
|
||||
|
||||
self.add_btn = Button(self.win, text="添加新的物体图像", command=self.add_img)
|
||||
self.add_btn.grid(row=1, column=2)
|
||||
|
||||
self.tips = "1、等待打开ros,大概需要15s\n2、选择所要运行的程序点击运行即可,开启大概需要10秒,可以通过查看终端查看开启情况。\n\n添加新的图像：\n1、点击按钮，等待开启摄像头\n2、选中图像框，按z键拍照\n3、使用鼠标框出需要识别的图像区域\n4、按Enter键提取图像\n5、再次按Enter键保存即可"
|
||||
|
||||
self.btn = Button(self.win, text="运行", command=self.start_run)
|
||||
self.btn.grid(row=5)
|
||||
|
||||
self.close = Button(self.win, text="close", command=self.close_py)
|
||||
self.close.grid(row=5, column=1)
|
||||
|
||||
self.t2 = None
|
||||
self.log_data = Text(self.win, width=66, height=10)
|
||||
self.log_data.grid(row=16, column=0, columnspan=10)
|
||||
self.log_data.insert(END, self.tips)
|
||||
|
||||
self.open_ros()
|
||||
self.win.protocol('WM_DELETE_WINDOW', self.close_rviz)
|
||||
|
||||
def close_rviz(self):
|
||||
os.system(
|
||||
"ps -ef | grep -E mycobot.rviz | grep -v 'grep' | awk '{print $2}' | xargs kill -9")
|
||||
sys.exit(0)
|
||||
|
||||
def set_file(self,port):
|
||||
self.command = '<arg name="port" default="{}" />'.format(
|
||||
port)
|
||||
# 根据通信口修改ros启动文件
|
||||
os.system(
|
||||
"sed -i '2c {}' ~/catkin_ws/src/mycobot_ros/mycobot_ai/launch/vision.launch"
|
||||
.format(self.command))
|
||||
|
||||
def change_file(self, port):
|
||||
command1 = '<arg name="port" default="{}" />'.format(port)
|
||||
command2 = '<arg name="baud" default="{}" />'.format(1000000)
|
||||
# 根据通信口修改ros启动文件
|
||||
os.system(
|
||||
"sed -i '2c {}' ~/catkin_ws/src/mycobot_ros/mycobot_ai/launch/vision.launch".format(command1))
|
||||
os.system(
|
||||
"sed -i '3c {}' ~/catkin_ws/src/mycobot_ros/mycobot_ai/launch/vision.launch".format(command2))
|
||||
|
||||
def start_run(self):
|
||||
try:
|
||||
print(u"开始运行")
|
||||
one = self.myCombox.get()
|
||||
if one == u"颜色识别":
|
||||
self.run_py = "detect_obj_color.py"
|
||||
t2 = threading.Thread(target=self.open_py1)
|
||||
t2.setDaemon(True)
|
||||
t2.start()
|
||||
elif one == u"物体识别":
|
||||
self.run_py = "detect_obj_img.py"
|
||||
t3 = threading.Thread(target=self.open_py)
|
||||
t3.setDaemon(True)
|
||||
t3.start()
|
||||
elif one == u"二维码识别":
|
||||
self.run_py = "detect_encode.py"
|
||||
t3 = threading.Thread(target=self.open_py2)
|
||||
t3.setDaemon(True)
|
||||
t3.start()
|
||||
except Exception as e:
|
||||
self.tips = str(e)
|
||||
self.log_data.insert(END, self.tips)
|
||||
|
||||
def open_py(self):
|
||||
os.system(
|
||||
"python ~/catkin_ws/src/mycobot_ros/mycobot_ai/scripts/detect_obj_img.py"
|
||||
)
|
||||
|
||||
def open_py1(self):
|
||||
os.system(
|
||||
"python ~/catkin_ws/src/mycobot_ros/mycobot_ai/scripts/detect_obj_color.py"
|
||||
)
|
||||
|
||||
def open_py2(self):
|
||||
os.system(
|
||||
"python ~/catkin_ws/src/mycobot_ros/mycobot_ai/scripts/detect_encode.py"
|
||||
)
|
||||
|
||||
def add_img(self):
|
||||
os.system(
|
||||
"python ~/catkin_ws/src/mycobot_ros/mycobot_ai/scripts/add_img.py"
|
||||
)
|
||||
|
||||
def open_ros(self):
|
||||
t1 = threading.Thread(target=self.ross)
|
||||
t1.setDaemon(True)
|
||||
t1.start()
|
||||
self.ros = True
|
||||
|
||||
def ross(self):
|
||||
os.system(
|
||||
"roslaunch ~/catkin_ws/src/mycobot_ros/mycobot_ai/launch/vision.launch"
|
||||
)
|
||||
|
||||
def close_py(self):
|
||||
t1 = threading.Thread(target=self.close_p)
|
||||
t1.setDaemon(True)
|
||||
t1.start()
|
||||
|
||||
def close_p(self):
|
||||
# 关闭ai程序
|
||||
os.system("ps -ef | grep -E " + self.run_py +
|
||||
" | grep -v 'grep' | awk '{print $2}' | xargs kill -9")
|
||||
|
||||
def get_current_time(self):
|
||||
# 日志时间
|
||||
"""Get current time with format."""
|
||||
current_time = time.strftime("%Y-%m-%d %H:%M:%S",
|
||||
time.localtime(time.time()))
|
||||
return current_time
|
||||
|
||||
def write_log_to_Text(self, logmsg):
|
||||
# 设置日志函数
|
||||
global LOG_NUM
|
||||
current_time = self.get_current_time()
|
||||
logmsg_in = str(current_time) + " " + str(logmsg) + "\n" # 换行
|
||||
|
||||
if LOG_NUM <= 18:
|
||||
self.log_data_Text.insert(END, logmsg_in)
|
||||
LOG_NUM += len(logmsg_in.split("\n"))
|
||||
# print(LOG_NUM)
|
||||
else:
|
||||
self.log_data_Text.insert(END, logmsg_in)
|
||||
self.log_data_Text.yview("end")
|
||||
|
||||
def run(self):
|
||||
self.win.mainloop()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
mc = Application()
|
||||
mc.run()
|
||||
0
mycobot_ai/scripts/detect_encode.py
Normal file → Executable file
0
mycobot_ai/scripts/detect_encode.py
Normal file → Executable file
50
mycobot_ai/scripts/detect_obj_color.py
Normal file → Executable file
50
mycobot_ai/scripts/detect_obj_color.py
Normal file → Executable file
|
|
@ -64,6 +64,7 @@ class Object_detect(Movement):
|
|||
self.raspi = True
|
||||
if self.raspi:
|
||||
self.gpio_status(False)
|
||||
self.Pin = [2, 5]
|
||||
|
||||
# choose place to set cube
|
||||
self.color = 0
|
||||
|
|
@ -147,20 +148,10 @@ class Object_detect(Movement):
|
|||
# send coordinates to move mycobot
|
||||
self.pub_coords([x, y, 165, -178.9, -1.57, -25.95], 20, 1)
|
||||
time.sleep(1.5)
|
||||
if "dev" in self.robot_m5 or self.raspi:
|
||||
self.pub_coords([x, y, 90, -178.9, -1.57, -25.95], 20, 1)
|
||||
elif "dev" in self.robot_wio:
|
||||
h = 0
|
||||
|
||||
if 165 < x < 180:
|
||||
h = 10
|
||||
elif x > 180:
|
||||
h = 20
|
||||
elif x < 135:
|
||||
h = -20
|
||||
self.pub_coords([x, y, 31.9+h, -178.9, -1, -25.95], 20, 1)
|
||||
|
||||
time.sleep(1.5)
|
||||
|
||||
self.pub_coords([x, y, 80, -178.9, -1.57, -25.95], 20, 1)
|
||||
|
||||
time.sleep(1)
|
||||
# open pump
|
||||
if self.raspi:
|
||||
self.gpio_status(True)
|
||||
|
|
@ -213,34 +204,9 @@ class Object_detect(Movement):
|
|||
else:
|
||||
self.cache_x = self.cache_y = 0
|
||||
# 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
|
||||
if "dev" in self.robot_wio:
|
||||
|
||||
if (y < -30 and x > 140) or (x > 150 and y < -10):
|
||||
x -= 10
|
||||
y += 10
|
||||
elif y > -10:
|
||||
y += 10
|
||||
elif x > 170:
|
||||
x -= 10
|
||||
y += 10
|
||||
elif "dev" in self.robot_raspi:
|
||||
if x > 160:
|
||||
y += 10
|
||||
elif y < -20:
|
||||
x -= 10
|
||||
y += 10
|
||||
elif "dev" in self.robot_jes:
|
||||
y += 13
|
||||
x += 4
|
||||
|
||||
elif "dev" in self.robot_m5:
|
||||
x -= 10
|
||||
if y < 0:
|
||||
y += 10
|
||||
if y < -30:
|
||||
y += 7
|
||||
print x, y
|
||||
self.move(x, y, color)
|
||||
|
||||
|
||||
self.move(x,y, color)
|
||||
|
||||
# init mycobot
|
||||
def run(self):
|
||||
|
|
|
|||
393
mycobot_ai/scripts/detect_obj_color_wio.py
Executable file
393
mycobot_ai/scripts/detect_obj_color_wio.py
Executable file
|
|
@ -0,0 +1,393 @@
|
|||
#encoding:utf-8
|
||||
|
||||
from tokenize import Pointfloat
|
||||
import cv2
|
||||
import numpy as np
|
||||
import time
|
||||
import json
|
||||
import os
|
||||
import rospy
|
||||
from visualization_msgs.msg import Marker
|
||||
|
||||
from moving_utils import Movement
|
||||
|
||||
IS_CV_4 = cv2.__version__[0] == '4'
|
||||
|
||||
class Object_detect(Movement):
|
||||
|
||||
def __init__(self, camera_x=150, camera_y=-10):
|
||||
# inherit the parent class
|
||||
super(Object_detect, self).__init__()
|
||||
# get path of file
|
||||
dir_path = os.path.dirname(__file__)
|
||||
# choose place to set cube
|
||||
self.color = 0
|
||||
# parameters to calculate camera clipping parameters
|
||||
self.x1 = self.x2 = self.y1 = self.y2 =0
|
||||
# set cache of real coord
|
||||
self.cache_x = self.cache_y = 0
|
||||
# set color HSV
|
||||
self.HSV = {
|
||||
"yellow": [np.array([11, 115, 70]), np.array([40, 255, 245])],
|
||||
"red": [np.array([0, 43, 46]), np.array([8, 255, 255])],
|
||||
}
|
||||
# use to calculate coord between cube and mycobot
|
||||
self.sum_x1= self.sum_x2= self.sum_y2= self.sum_y1= 0
|
||||
# The coordinates of the grab center point relative to the mycobot
|
||||
self.camera_x, self.camera_y = camera_x, camera_y
|
||||
# The coordinates of the cube relative to the mycobot
|
||||
self.c_x, self.c_y = 0,0
|
||||
# The ratio of pixels to actual values
|
||||
self.ratio = 0
|
||||
# Get ArUco marker dict that can be detected.
|
||||
self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
|
||||
# Get ArUco marker params.
|
||||
self.aruco_params = cv2.aruco.DetectorParameters_create()
|
||||
|
||||
# init a node and a publisher
|
||||
rospy.init_node("marker", anonymous=True)
|
||||
self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
|
||||
# init a Marker
|
||||
self.marker = Marker()
|
||||
self.marker.header.frame_id = "/joint1"
|
||||
self.marker.ns = "cube"
|
||||
self.marker.type = self.marker.CUBE
|
||||
self.marker.action = self.marker.ADD
|
||||
self.marker.scale.x = 0.04
|
||||
self.marker.scale.y = 0.04
|
||||
self.marker.scale.z = 0.04
|
||||
self.marker.color.a = 1.0
|
||||
self.marker.color.g = 1.0
|
||||
self.marker.color.r = 1.0
|
||||
|
||||
|
||||
# marker position initial
|
||||
self.marker.pose.position.x = 0
|
||||
self.marker.pose.position.y = 0
|
||||
self.marker.pose.position.z = 0.03
|
||||
self.marker.pose.orientation.x = 0
|
||||
self.marker.pose.orientation.y = 0
|
||||
self.marker.pose.orientation.z = 0
|
||||
self.marker.pose.orientation.w = 1.0
|
||||
|
||||
# publish marker
|
||||
def pub_marker(self, x, y , z=0.03):
|
||||
self.marker.header.stamp = rospy.Time.now()
|
||||
self.marker.pose.position.x = x
|
||||
self.marker.pose.position.y = y
|
||||
self.marker.pose.position.z = z
|
||||
self.marker.color.g = self.color
|
||||
self.pub.publish(self.marker)
|
||||
|
||||
|
||||
|
||||
# Grasping motion
|
||||
def move(self, x,y,color):
|
||||
angles = [
|
||||
[-7.11, -6.94, -55.01, -24.16, 0, -38.84], # init the point
|
||||
[-1.14, -10.63, -87.8, 9.05, -3.07, -37.7], # point to grab
|
||||
[17.4, -10.1, -87.27, 5.8, -2.02, -37.7], # point to grab
|
||||
]
|
||||
|
||||
coords = [
|
||||
[106.1, -141.6, 240.9, -173.34, -8.15, -83.11], # above the red bucket
|
||||
[208.2, -127.8, 246.9, -157.51, -17.5, -71.18], # above the green bucket
|
||||
[209.7, -18.6, 230.4, -168.48, -9.86, -39.38],
|
||||
[196.9, -64.7, 232.6, -166.66, -9.44, -52.47],
|
||||
[126.6, -118.1, 305.0, -157.57, -13.72, -75.3],
|
||||
|
||||
]
|
||||
|
||||
# send Angle to move mycobot
|
||||
self.pub_angles(angles[0], 20)
|
||||
time.sleep(1.5)
|
||||
self.pub_angles(angles[1], 20)
|
||||
time.sleep(1.5)
|
||||
self.pub_angles(angles[2], 20)
|
||||
time.sleep(1.5)
|
||||
# send coordinates to move mycobot
|
||||
self.pub_coords([x, y, 165, -178.9, -1.57, -25.95], 20, 1)
|
||||
time.sleep(1.5)
|
||||
self.pub_coords([x, y, 40, -178.9, -1.57, -25.95], 20, 1)
|
||||
time.sleep(1.5)
|
||||
# open pump
|
||||
self.pub_pump(True,[20,21])
|
||||
time.sleep(0.5)
|
||||
self.pub_angles(angles[2], 20)
|
||||
time.sleep(3)
|
||||
self.pub_marker(coords[2][0]/1000.0, coords[2][1]/1000.0, coords[2][2]/1000.0)
|
||||
|
||||
self.pub_angles(angles[1], 20)
|
||||
time.sleep(1.5)
|
||||
self.pub_marker(coords[3][0]/1000.0, coords[3][1]/1000.0, coords[3][2]/1000.0)
|
||||
|
||||
self.pub_angles(angles[0], 20)
|
||||
time.sleep(1.5)
|
||||
self.pub_marker(coords[4][0]/1000.0, coords[4][1]/1000.0, coords[4][2]/1000.0)
|
||||
|
||||
self.pub_coords(coords[color], 20, 1)
|
||||
self.pub_marker(coords[color][0]/1000.0, coords[color][1]/1000.0, coords[color][2]/1000.0)
|
||||
time.sleep(2)
|
||||
# close pump
|
||||
self.pub_pump(False,[20,21])
|
||||
if color==1:
|
||||
self.pub_marker(coords[color][0]/1000.0+0.04, coords[color][1]/1000.0-0.02)
|
||||
elif color==0:
|
||||
self.pub_marker(coords[color][0]/1000.0+0.03, coords[color][1]/1000.0)
|
||||
self.pub_angles(angles[0], 20)
|
||||
time.sleep(3)
|
||||
|
||||
|
||||
# decide whether grab cube
|
||||
def decide_move(self, x, y, color):
|
||||
|
||||
|
||||
print(x, y,self.cache_x, self.cache_y)
|
||||
# detect the cube status move or run
|
||||
if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm
|
||||
self.cache_x, self.cache_y = x, y
|
||||
return
|
||||
else:
|
||||
self.cache_x = self.cache_y = 0
|
||||
self.move(x-10,y+10,color)
|
||||
|
||||
# init mycobot
|
||||
def run(self):
|
||||
|
||||
for _ in range(10):
|
||||
self.pub_angles([-7.11, -6.94, -55.01, -24.16, 0, -38.84], 20)
|
||||
print(_)
|
||||
time.sleep(0.5)
|
||||
self.pub_pump(False,[20,21])
|
||||
|
||||
# draw aruco
|
||||
def draw_marker(self,img,x,y):
|
||||
# draw rectangle on img
|
||||
cv2.rectangle(
|
||||
img,
|
||||
(x - 20, y - 20),
|
||||
(x + 20, y + 20),
|
||||
(0, 255, 0),
|
||||
thickness=2,
|
||||
lineType=cv2.FONT_HERSHEY_COMPLEX,
|
||||
)
|
||||
# add text on rectangle
|
||||
cv2.putText(img,"({},{})".format(x,y),(x,y),cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (243, 0, 0), 2,)
|
||||
|
||||
# get points of two aruco
|
||||
def get_calculate_params(self,img):
|
||||
# Convert the image to a gray image
|
||||
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
|
||||
# Detect ArUco marker.
|
||||
corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
|
||||
gray, self.aruco_dict, parameters=self.aruco_params
|
||||
)
|
||||
|
||||
"""
|
||||
Two Arucos must be present in the picture and in the same order.
|
||||
There are two Arucos in the Corners, and each aruco contains the pixels of its four corners.
|
||||
Determine the center of the aruco by the four corners of the aruco.
|
||||
"""
|
||||
if len(corners) > 0:
|
||||
if ids is not None:
|
||||
if len(corners) <= 1 or ids[0]==1:
|
||||
return None
|
||||
x1=x2=y1=y2 = 0
|
||||
point_11,point_21,point_31,point_41 = corners[0][0]
|
||||
x1, y1 = int((point_11[0] + point_21[0] + point_31[0] + point_41[0]) / 4.0), int((point_11[1] + point_21[1] + point_31[1] + point_41[1]) / 4.0)
|
||||
point_1,point_2,point_3,point_4 = corners[1][0]
|
||||
x2, y2 = int((point_1[0] + point_2[0] + point_3[0] + point_4[0]) / 4.0), int((point_1[1] + point_2[1] + point_3[1] + point_4[1]) / 4.0)
|
||||
return x1,x2,y1,y2
|
||||
return None
|
||||
|
||||
# set camera clipping parameters
|
||||
def set_cut_params(self, x1, y1, x2, y2):
|
||||
self.x1 = int(x1)
|
||||
self.y1 = int(y1)
|
||||
self.x2 = int(x2)
|
||||
self.y2 = int(y2)
|
||||
print(self.x1,self.y1,self.x2,self.y2)
|
||||
|
||||
# set parameters to calculate the coords between cube and mycobot
|
||||
def set_params(self, c_x, c_y, ratio):
|
||||
self.c_x = c_x
|
||||
self.c_y = c_y
|
||||
self.ratio = 220.0/ratio
|
||||
|
||||
# calculate the coords between cube and mycobot
|
||||
def get_position(self, x, y):
|
||||
return ((y - self.c_y)*self.ratio + self.camera_x), ((x - self.c_x)*self.ratio + self.camera_y)
|
||||
|
||||
"""
|
||||
Calibrate the camera according to the calibration parameters.
|
||||
Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times.
|
||||
If two ARuco values have been calculated, clip the video.
|
||||
"""
|
||||
def transform_frame(self, frame):
|
||||
# enlarge the image by 1.5 times
|
||||
fx = 1.5
|
||||
fy = 1.5
|
||||
frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy, interpolation=cv2.INTER_CUBIC)
|
||||
if self.x1 != self.x2:
|
||||
# the cutting ratio here is adjusted according to the actual situation
|
||||
frame = frame[int(self.y2*0.2):int(self.y1*1.15), int(self.x1*0.7):int(self.x2*1.15)]
|
||||
return frame
|
||||
|
||||
# detect cube color
|
||||
def color_detect(self, img):
|
||||
# set the arrangement of color'HSV
|
||||
x = y = 0
|
||||
for mycolor, item in self.HSV.items():
|
||||
redLower = np.array(item[0])
|
||||
redUpper = np.array(item[1])
|
||||
# transfrom the img to model of gray
|
||||
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
|
||||
# wipe off all color expect color in range
|
||||
mask = cv2.inRange(hsv, item[0], item[1])
|
||||
# a etching operation on a picture to remove edge roughness
|
||||
erosion = cv2.erode(mask, np.ones((1, 1), np.uint8), iterations=2)
|
||||
# the image for expansion operation, its role is to deepen the color depth in the picture
|
||||
dilation =cv2.dilate(erosion, np.ones((1, 1), np.uint8), iterations=2)
|
||||
# adds pixels to the image
|
||||
target = cv2.bitwise_and(img, img, mask=dilation)
|
||||
# the filtered image is transformed into a binary image and placed in binary
|
||||
ret, binary = cv2.threshold(dilation, 127, 255, cv2.THRESH_BINARY)
|
||||
# get the contour coordinates of the image, where contours is the coordinate value, here only the contour is detected
|
||||
contours, hierarchy = cv2.findContours(
|
||||
dilation, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||||
|
||||
if len(contours) > 0:
|
||||
# do something about misidentification
|
||||
boxes = [
|
||||
box
|
||||
for box in [cv2.boundingRect(c) for c in contours]
|
||||
if min(img.shape[0], img.shape[1]) / 10
|
||||
< min(box[2], box[3])
|
||||
< min(img.shape[0], img.shape[1]) / 1
|
||||
]
|
||||
if boxes:
|
||||
for box in boxes:
|
||||
x, y, w, h = box
|
||||
# find the largest object that fits the requirements
|
||||
c = max(contours, key=cv2.contourArea)
|
||||
# get the lower left and upper right points of the positioning object
|
||||
x, y, w, h = cv2.boundingRect(c)
|
||||
# locate the target by drawing rectangle
|
||||
cv2.rectangle(img, (x, y), (x+w, y+h), (153, 153, 0), 2)
|
||||
# calculate the rectangle center
|
||||
x, y = (x*2+w)/2, (y*2+h)/2
|
||||
# calculate the real coordinates of mycobot relative to the target
|
||||
if mycolor == "yellow":
|
||||
self.color = 1
|
||||
elif mycolor == "red":
|
||||
self.color = 0
|
||||
|
||||
if abs(x) + abs(y) > 0:
|
||||
return x, y
|
||||
else:
|
||||
return None
|
||||
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
# open the camera
|
||||
cap_num = 0
|
||||
cap = cv2.VideoCapture(cap_num)
|
||||
if not cap.isOpened():
|
||||
cap.open()
|
||||
# init a class of Object_detect
|
||||
detect = Object_detect()
|
||||
# init mycobot
|
||||
detect.run()
|
||||
|
||||
_init_ = 20 #
|
||||
init_num = 0
|
||||
nparams = 0
|
||||
num = 0
|
||||
real_sx = real_sy = 0
|
||||
while cv2.waitKey(1) < 0:
|
||||
# read camera
|
||||
_,frame = cap.read()
|
||||
# deal img
|
||||
frame = detect.transform_frame(frame)
|
||||
|
||||
|
||||
if _init_ > 0:
|
||||
_init_-=1
|
||||
continue
|
||||
# calculate the parameters of camera clipping
|
||||
if init_num < 20:
|
||||
if detect.get_calculate_params(frame) is None:
|
||||
cv2.imshow("figure",frame)
|
||||
continue
|
||||
else:
|
||||
x1,x2,y1,y2 = detect.get_calculate_params(frame)
|
||||
detect.draw_marker(frame,x1,y1)
|
||||
detect.draw_marker(frame,x2,y2)
|
||||
detect.sum_x1+=x1
|
||||
detect.sum_x2+=x2
|
||||
detect.sum_y1+=y1
|
||||
detect.sum_y2+=y2
|
||||
init_num+=1
|
||||
continue
|
||||
elif init_num==20:
|
||||
detect.set_cut_params(
|
||||
(detect.sum_x1)/20.0,
|
||||
(detect.sum_y1)/20.0,
|
||||
(detect.sum_x2)/20.0,
|
||||
(detect.sum_y2)/20.0,
|
||||
)
|
||||
detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0
|
||||
init_num+=1
|
||||
continue
|
||||
|
||||
# calculate params of the coords between cube and mycobot
|
||||
if nparams < 10:
|
||||
if detect.get_calculate_params(frame) is None:
|
||||
cv2.imshow("figure",frame)
|
||||
continue
|
||||
else:
|
||||
x1,x2,y1,y2 = detect.get_calculate_params(frame)
|
||||
detect.draw_marker(frame,x1,y1)
|
||||
detect.draw_marker(frame,x2,y2)
|
||||
detect.sum_x1+=x1
|
||||
detect.sum_x2+=x2
|
||||
detect.sum_y1+=y1
|
||||
detect.sum_y2+=y2
|
||||
nparams+=1
|
||||
continue
|
||||
elif nparams==10:
|
||||
nparams+=1
|
||||
# calculate and set params of calculating real coord between cube and mycobot
|
||||
detect.set_params(
|
||||
(detect.sum_x1+detect.sum_x2)/20.0,
|
||||
(detect.sum_y1+detect.sum_y2)/20.0,
|
||||
abs(detect.sum_x1-detect.sum_x2)/10.0+abs(detect.sum_y1-detect.sum_y2)/10.0
|
||||
)
|
||||
print "ok"
|
||||
continue
|
||||
|
||||
# get detect result
|
||||
detect_result = detect.color_detect(frame)
|
||||
if detect_result is None:
|
||||
cv2.imshow("figure",frame)
|
||||
continue
|
||||
else:
|
||||
x, y = detect_result
|
||||
# calculate real coord between cube and mycobot
|
||||
real_x, real_y = detect.get_position(x, y)
|
||||
if num == 20:
|
||||
detect.pub_marker(real_sx/20.0/1000.0, real_sy/20.0/1000.0)
|
||||
detect.decide_move(real_sx/20.0, real_sy/20.0, detect.color)
|
||||
num = real_sx = real_sy = 0
|
||||
|
||||
else:
|
||||
num += 1
|
||||
real_sy += real_y
|
||||
real_sx += real_x
|
||||
|
||||
cv2.imshow("figure",frame)
|
||||
|
||||
|
||||
|
||||
29
mycobot_ai/scripts/detect_obj_img.py
Normal file → Executable file
29
mycobot_ai/scripts/detect_obj_img.py
Normal file → Executable file
|
|
@ -25,6 +25,8 @@ class Object_detect(Movement):
|
|||
super(Object_detect, self).__init__()
|
||||
# get path of file
|
||||
dir_path = os.path.dirname(__file__)
|
||||
|
||||
|
||||
# 移动角度
|
||||
self.move_angles = [
|
||||
[-7.11, -6.94, -55.01, -24.16, 0, 15], # init the point
|
||||
|
|
@ -64,7 +66,7 @@ class Object_detect(Movement):
|
|||
self.raspi = True
|
||||
if self.raspi:
|
||||
self.gpio_status(False)
|
||||
|
||||
self.Pin = [2, 5]
|
||||
# choose place to set cube
|
||||
self.color = 0
|
||||
# parameters to calculate camera clipping parameters
|
||||
|
|
@ -77,6 +79,10 @@ class Object_detect(Movement):
|
|||
# self.label_path = os.path.join(dir_path, "labels.json")
|
||||
# load class labels
|
||||
# self.labels = json.load(open(self.label_path))
|
||||
|
||||
|
||||
|
||||
|
||||
# use to calculate coord between cube and mycobot
|
||||
self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
|
||||
# The coordinates of the grab center point relative to the mycobot
|
||||
|
|
@ -121,6 +127,8 @@ class Object_detect(Movement):
|
|||
self.marker.pose.orientation.w = 1.0
|
||||
|
||||
self.cache_x = self.cache_y = 0
|
||||
|
||||
|
||||
# publish marker
|
||||
|
||||
def pub_marker(self, x, y, z=0.03):
|
||||
|
|
@ -152,8 +160,8 @@ class Object_detect(Movement):
|
|||
self.pub_coords([x, y, 165, -178.9, -1.57, -66], 20, 1)
|
||||
time.sleep(1.5)
|
||||
# 根据不同底板机械臂，调整吸泵高度
|
||||
if "dev" in self.robot_m5 or "dev" in self.robot_raspi:
|
||||
# m5 and raspi
|
||||
if "dev" in self.robot_m5:
|
||||
# m5 and jetson nano
|
||||
self.pub_coords([x, y, 90, -178.9, -1.57, -66], 25, 1)
|
||||
elif "dev" in self.robot_wio:
|
||||
h = 0
|
||||
|
|
@ -166,8 +174,8 @@ class Object_detect(Movement):
|
|||
self.pub_coords([x, y, 31.9+h, -178.9, -1, -66], 20, 1)
|
||||
elif "dev" in self.robot_jes:
|
||||
h = 0
|
||||
if x < 130:
|
||||
h = 15
|
||||
if x<130:
|
||||
h=15
|
||||
self.pub_coords([x, y, 90-h, -178.9, -1.57, -66], 25, 1)
|
||||
time.sleep(1.5)
|
||||
# open pump
|
||||
|
|
@ -237,11 +245,11 @@ class Object_detect(Movement):
|
|||
y += 5
|
||||
# print x,y
|
||||
elif "dev" in self.robot_jes:
|
||||
if y < 0:
|
||||
x += 5
|
||||
y += 3
|
||||
y += 10
|
||||
print x, y
|
||||
if y<0:
|
||||
x+=5
|
||||
y+=3
|
||||
y+=10
|
||||
print x,y
|
||||
self.move(x, y, color)
|
||||
|
||||
# init mycobot
|
||||
|
|
@ -476,6 +484,7 @@ def run():
|
|||
for i, j, k in os.walk(path):
|
||||
for l in k:
|
||||
goal.append(cv2.imread('local_photo/img/{}'.format(l)))
|
||||
|
||||
cap_num = 0
|
||||
cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
|
||||
if not cap.isOpened():
|
||||
|
|
|
|||
16
mycobot_ai/scripts/detect_obj_img_folder.py
Normal file → Executable file
16
mycobot_ai/scripts/detect_obj_img_folder.py
Normal file → Executable file
|
|
@ -315,7 +315,7 @@ class Object_detect(Movement):
|
|||
# detect object
|
||||
def obj_detect(self, img, goal):
|
||||
i = 0
|
||||
MIN_MATCH_COUNT = 10
|
||||
MIN_MATCH_COUNT = 5
|
||||
sift = cv2.xfeatures2d.SIFT_create()
|
||||
|
||||
# find the keypoints and descriptors with SIFT
|
||||
|
|
@ -430,20 +430,21 @@ def run():
|
|||
# init mycobot
|
||||
detect.run()
|
||||
|
||||
_init_ = 20 #
|
||||
# _init_ = 20 #
|
||||
init_num = 0
|
||||
nparams = 0
|
||||
num = 0
|
||||
real_sx = real_sy = 0
|
||||
while cv2.waitKey(1) < 0:
|
||||
start_time = time.time()
|
||||
# read camera
|
||||
_, frame = cap.read()
|
||||
# deal img
|
||||
frame = detect.transform_frame(frame)
|
||||
|
||||
if _init_ > 0:
|
||||
_init_ -= 1
|
||||
continue
|
||||
# if _init_ > 0:
|
||||
# _init_ -= 1
|
||||
# continue
|
||||
# calculate the parameters of camera clipping
|
||||
if init_num < 20:
|
||||
if detect.get_calculate_params(frame) is None:
|
||||
|
|
@ -519,8 +520,11 @@ def run():
|
|||
|
||||
cv2.imshow("figure", frame)
|
||||
|
||||
end_time = time.time()
|
||||
print("loop_time = ", end_time - start_time)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
run()
|
||||
# Object_detect().take_photo()
|
||||
# Object_detect().cut_photo()
|
||||
# Object_detect().cut_photo()
|
||||
|
|
|
|||
648
mycobot_ai/scripts/detect_obj_img_folder_opt.py
Normal file
648
mycobot_ai/scripts/detect_obj_img_folder_opt.py
Normal file
|
|
@ -0,0 +1,648 @@
|
|||
# encoding:utf-8
|
||||
#!/usr/bin/env python2
|
||||
|
||||
from multiprocessing import Process, Pipe
|
||||
|
||||
from cgi import parse
|
||||
from difflib import restore
|
||||
# import queue
|
||||
from sys import path
|
||||
from tokenize import Pointfloat
|
||||
from turtle import color
|
||||
# from typing_extensions import Self
|
||||
import cv2
|
||||
import numpy as np
|
||||
import time
|
||||
import json
|
||||
import os
|
||||
import rospy
|
||||
from visualization_msgs.msg import Marker
|
||||
from PIL import Image
|
||||
from threading import Thread
|
||||
import tkFileDialog as filedialog
|
||||
import Tkinter as tk
|
||||
from moving_utils import Movement
|
||||
|
||||
IS_CV_4 = cv2.__version__[0] == '4'
|
||||
__version__ = "1.0" # Adaptive seeed
|
||||
|
||||
|
||||
class Object_detect(Movement):
|
||||
|
||||
def __init__(self, camera_x=150, camera_y=-10):
|
||||
# inherit the parent class
|
||||
super(Object_detect, self).__init__()
|
||||
# get path of file
|
||||
dir_path = os.path.dirname(__file__)
|
||||
|
||||
# 移动角度
|
||||
self.move_angles = [
|
||||
[-26.11, -6.94, -55.01, -24.16, 0, 15], # init the point
|
||||
[-1.14, 0.63, -87.8, 9.05, -3.07, 15], # point to grab
|
||||
[17.4, -10.1, -87.27, 5.8, -2.02, 15], # point to grab
|
||||
]
|
||||
|
||||
# 移动坐标
|
||||
self.move_coords = [
|
||||
[120.1, -141.6, 240.9, -173.34, -8.15, -110.11], # above the red bucket
|
||||
# above the yello bucket
|
||||
#[208.2, -127.8, 260.9, -157.51, -17.5, -71.18],
|
||||
[205.6, -130.5, 263.0, -150.99, -0.07, -107.35], # above the green bucket
|
||||
[-20.0, 176.7, 242.6, -166.66, -9.44, -52.47], # above the gray bucket
|
||||
[104.9, 176.7, 242.6, -166.66, -9.44, -52.47], # above the blue bucket
|
||||
[126.6, -118.1, 305.0, -157.57, -13.72, -75.3],
|
||||
]
|
||||
|
||||
# 判断连接设备:ttyUSB*为M5，ttyACM*为seeed
|
||||
|
||||
self.raspi = False
|
||||
import RPi.GPIO as GPIO
|
||||
self.GPIO = GPIO
|
||||
GPIO.setwarnings(False)
|
||||
GPIO.setmode(GPIO.BCM)
|
||||
GPIO.setup(20, GPIO.OUT)
|
||||
GPIO.setup(21, GPIO.OUT)
|
||||
|
||||
self.gpio_status(False)
|
||||
|
||||
self.Pin = [2, 5]
|
||||
# choose place to set cube
|
||||
self.color = 0
|
||||
# parameters to calculate camera clipping parameters
|
||||
self.x1 = self.x2 = self.y1 = self.y2 = 0
|
||||
# set cache of real coord
|
||||
self.cache_x = self.cache_y = 0
|
||||
# load model of img recognition
|
||||
# self.model_path = os.path.join(dir_path, "frozen_inference_graph.pb")
|
||||
# self.pbtxt_path = os.path.join(dir_path, "graph.pbtxt")
|
||||
# self.label_path = os.path.join(dir_path, "labels.json")
|
||||
# # load class labels
|
||||
# self.labels = json.load(open(self.label_path))
|
||||
|
||||
# use to calculate coord between cube and mycobot
|
||||
self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
|
||||
# The coordinates of the grab center point relative to the mycobot
|
||||
self.camera_x, self.camera_y = camera_x, camera_y
|
||||
# The coordinates of the cube relative to the mycobot
|
||||
self.c_x, self.c_y = 0, 0
|
||||
# The ratio of pixels to actual values
|
||||
self.ratio = 0
|
||||
# Get ArUco marker dict that can be detected.
|
||||
self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
|
||||
# Get ArUco marker params.
|
||||
self.aruco_params = cv2.aruco.DetectorParameters_create()
|
||||
|
||||
# if IS_CV_4:
|
||||
# self.net = cv2.dnn.readNetFromTensorflow(self.model_path, self.pbtxt_path)
|
||||
# else:
|
||||
# print('Load tensorflow model need the version of opencv is 4.')
|
||||
# exit(0)
|
||||
# init a node and a publisher
|
||||
rospy.init_node("marker", anonymous=True)
|
||||
self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
|
||||
# init a Marker
|
||||
self.marker = Marker()
|
||||
self.marker.header.frame_id = "/joint1"
|
||||
self.marker.ns = "cube"
|
||||
self.marker.type = self.marker.CUBE
|
||||
self.marker.action = self.marker.ADD
|
||||
self.marker.scale.x = 0.04
|
||||
self.marker.scale.y = 0.04
|
||||
self.marker.scale.z = 0.04
|
||||
self.marker.color.a = 1.0
|
||||
self.marker.color.g = 1.0
|
||||
self.marker.color.r = 1.0
|
||||
|
||||
# marker position initial
|
||||
self.marker.pose.position.x = 0
|
||||
self.marker.pose.position.y = 0
|
||||
self.marker.pose.position.z = 0.03
|
||||
self.marker.pose.orientation.x = 0
|
||||
self.marker.pose.orientation.y = 0
|
||||
self.marker.pose.orientation.z = 0
|
||||
self.marker.pose.orientation.w = 1.0
|
||||
|
||||
self.cache_x = self.cache_y = 0
|
||||
|
||||
# publish marker
|
||||
|
||||
def pub_marker(self, x, y, z=0.03):
|
||||
self.marker.header.stamp = rospy.Time.now()
|
||||
self.marker.pose.position.x = x
|
||||
self.marker.pose.position.y = y
|
||||
self.marker.pose.position.z = z
|
||||
self.marker.color.g = self.color
|
||||
self.pub.publish(self.marker)
|
||||
|
||||
def gpio_status(self, flag):
|
||||
if flag:
|
||||
self.GPIO.output(20, 0)
|
||||
self.GPIO.output(21, 0)
|
||||
else:
|
||||
self.GPIO.output(20, 1)
|
||||
self.GPIO.output(21, 1)
|
||||
|
||||
# Grasping motion
|
||||
def move(self, x, y, color):
|
||||
# send Angle to move mycobot
|
||||
self.pub_angles(self.move_angles[0], 20)
|
||||
time.sleep(1.5)
|
||||
self.pub_angles(self.move_angles[1], 20)
|
||||
time.sleep(1.5)
|
||||
self.pub_angles(self.move_angles[2], 20)
|
||||
time.sleep(1.5)
|
||||
# send coordinates to move mycobot
|
||||
self.pub_coords([x, y, 165, -178.9, -1.57, -66], 20, 1)
|
||||
time.sleep(1.5)
|
||||
# 根据不同底板机械臂，调整吸泵高度
|
||||
self.pub_coords([x, y, 90, -178.9, -1.57, -66], 25, 1)
|
||||
time.sleep(1.5)
|
||||
# open pump
|
||||
self.gpio_status(True)
|
||||
|
||||
time.sleep(0.5)
|
||||
self.pub_angles(self.move_angles[2], 20)
|
||||
time.sleep(3)
|
||||
self.pub_marker(self.move_coords[2][0] / 1000.0,
|
||||
self.move_coords[2][1] / 1000.0,
|
||||
self.move_coords[2][2] / 1000.0)
|
||||
|
||||
self.pub_angles(self.move_angles[1], 20)
|
||||
time.sleep(1.5)
|
||||
self.pub_marker(self.move_coords[3][0] / 1000.0,
|
||||
self.move_coords[3][1] / 1000.0,
|
||||
self.move_coords[3][2] / 1000.0)
|
||||
|
||||
self.pub_angles(self.move_angles[0], 20)
|
||||
time.sleep(1.5)
|
||||
self.pub_marker(self.move_coords[4][0] / 1000.0,
|
||||
self.move_coords[4][1] / 1000.0,
|
||||
self.move_coords[4][2] / 1000.0)
|
||||
|
||||
print(self.move_coords[color])
|
||||
self.pub_coords(self.move_coords[color], 20, 1)
|
||||
self.pub_marker(self.move_coords[color][0] / 1000.0,
|
||||
self.move_coords[color][1] / 1000.0,
|
||||
self.move_coords[color][2] / 1000.0)
|
||||
time.sleep(4)
|
||||
# close pump
|
||||
self.gpio_status(False)
|
||||
|
||||
time.sleep(1)
|
||||
self.pub_marker(self.move_coords[color][0] / 1000.0 + 0.04,
|
||||
self.move_coords[color][1] / 1000.0 - 0.02)
|
||||
self.pub_angles(self.move_angles[0], 20)
|
||||
time.sleep(3)
|
||||
|
||||
# decide whether grab cube
|
||||
def decide_move(self, x, y, color):
|
||||
print(x, y, self.cache_x, self.cache_y)
|
||||
# detect the cube status move or run
|
||||
if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm
|
||||
self.cache_x, self.cache_y = x, y
|
||||
return
|
||||
else:
|
||||
self.cache_x = self.cache_y = 0
|
||||
# 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
|
||||
print(x, y)
|
||||
self.move(x, y, color)
|
||||
|
||||
# init mycobot
|
||||
def run(self):
|
||||
for _ in range(5):
|
||||
self.pub_angles([-26.11, -6.94, -55.01, -24.16, 0, -15], 20)
|
||||
print(_)
|
||||
time.sleep(0.5)
|
||||
|
||||
# draw aruco
|
||||
|
||||
def draw_marker(self, img, x, y):
|
||||
# draw rectangle on img
|
||||
cv2.rectangle(
|
||||
img,
|
||||
(x - 20, y - 20),
|
||||
(x + 20, y + 20),
|
||||
(0, 255, 0),
|
||||
thickness=2,
|
||||
lineType=cv2.FONT_HERSHEY_COMPLEX,
|
||||
)
|
||||
# add text on rectangle
|
||||
cv2.putText(
|
||||
img,
|
||||
"({},{})".format(x, y),
|
||||
(x, y),
|
||||
cv2.FONT_HERSHEY_COMPLEX_SMALL,
|
||||
1,
|
||||
(243, 0, 0),
|
||||
2,
|
||||
)
|
||||
|
||||
# get points of two aruco
|
||||
def get_calculate_params(self, img):
|
||||
# Convert the image to a gray image
|
||||
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
|
||||
# Detect ArUco marker.
|
||||
corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
|
||||
gray, self.aruco_dict, parameters=self.aruco_params)
|
||||
"""
|
||||
Two Arucos must be present in the picture and in the same order.
|
||||
There are two Arucos in the Corners, and each aruco contains the pixels of its four corners.
|
||||
Determine the center of the aruco by the four corners of the aruco.
|
||||
"""
|
||||
if len(corners) > 0:
|
||||
if ids is not None:
|
||||
if len(corners) <= 1 or ids[0] == 1:
|
||||
return None
|
||||
x1 = x2 = y1 = y2 = 0
|
||||
point_11, point_21, point_31, point_41 = corners[0][0]
|
||||
x1, y1 = int(
|
||||
(point_11[0] + point_21[0] + point_31[0] + point_41[0]) /
|
||||
4.0), int(
|
||||
(point_11[1] + point_21[1] + point_31[1] + point_41[1])
|
||||
/ 4.0)
|
||||
point_1, point_2, point_3, point_4 = corners[1][0]
|
||||
x2, y2 = int(
|
||||
(point_1[0] + point_2[0] + point_3[0] + point_4[0]) /
|
||||
4.0), int(
|
||||
(point_1[1] + point_2[1] + point_3[1] + point_4[1]) /
|
||||
4.0)
|
||||
return x1, x2, y1, y2
|
||||
return None
|
||||
|
||||
# set camera clipping parameters
|
||||
def set_cut_params(self, x1, y1, x2, y2):
|
||||
self.x1 = int(x1)
|
||||
self.y1 = int(y1)
|
||||
self.x2 = int(x2)
|
||||
self.y2 = int(y2)
|
||||
print(self.x1, self.y1, self.x2, self.y2)
|
||||
|
||||
# set parameters to calculate the coords between cube and mycobot
|
||||
def set_params(self, c_x, c_y, ratio):
|
||||
self.c_x = c_x
|
||||
self.c_y = c_y
|
||||
self.ratio = 220.0 / ratio
|
||||
|
||||
# calculate the coords between cube and mycobot
|
||||
def get_position(self, x, y):
|
||||
return ((y - self.c_y) * self.ratio +
|
||||
self.camera_x), ((x - self.c_x) * self.ratio + self.camera_y)
|
||||
|
||||
"""
|
||||
Calibrate the camera according to the calibration parameters.
|
||||
Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times.
|
||||
If two ARuco values have been calculated, clip the video.
|
||||
"""
|
||||
|
||||
def transform_frame(self, frame):
|
||||
# enlarge the image by 1.5 times
|
||||
fx = 1.5
|
||||
fy = 1.5
|
||||
frame = cv2.resize(frame, (0, 0),
|
||||
fx=fx,
|
||||
fy=fy,
|
||||
interpolation=cv2.INTER_CUBIC)
|
||||
if self.x1 != self.x2:
|
||||
# the cutting ratio here is adjusted according to the actual situation
|
||||
frame = frame[int(self.y2 * 0.2):int(self.y1 * 1.15),
|
||||
int(self.x1 * 0.7):int(self.x2 * 1.15)]
|
||||
return frame
|
||||
|
||||
# according the class_id to get object name
|
||||
def id_class_name(self, class_id):
|
||||
for key, value in self.labels.items():
|
||||
if class_id == int(key):
|
||||
return value
|
||||
|
||||
# detect object
|
||||
def obj_detect(self, img, goal, kp_img, desc_img, kp_list, desc_list, connection):
|
||||
i = 0
|
||||
MIN_MATCH_COUNT = 5
|
||||
# sift = cv2.xfeatures2d.SIFT_create()
|
||||
|
||||
# find the keypoints and descriptors with SIFT
|
||||
# kp = []
|
||||
# des = []
|
||||
kp = kp_list
|
||||
des = desc_list
|
||||
|
||||
# for i in goal:
|
||||
# kp0, des0 = sift.detectAndCompute(i, None)
|
||||
# kp.append(kp0)
|
||||
# des.append(des0)
|
||||
|
||||
# kp1, des1 = sift.detectAndCompute(goal, None)
|
||||
# kp2, des2 = sift.detectAndCompute(img, None)
|
||||
kp2, des2 = kp_img, desc_img
|
||||
|
||||
# FLANN parameters
|
||||
FLANN_INDEX_KDTREE = 0
|
||||
index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
|
||||
search_params = dict(checks=50) # or pass empty dictionary
|
||||
flann = cv2.FlannBasedMatcher(index_params, search_params)
|
||||
|
||||
x, y = 0, 0
|
||||
try:
|
||||
for i in range(len(des)):
|
||||
matches = flann.knnMatch(des[i], des2, k=2)
|
||||
# store all the good matches as per Lowe's ratio test. 根据Lowe比率测试存储所有良好匹配项。
|
||||
good = []
|
||||
for m, n in matches:
|
||||
if m.distance < 0.7 * n.distance:
|
||||
good.append(m)
|
||||
|
||||
# When there are enough robust matching point pairs 当有足够的健壮匹配点对（至少个MIN_MATCH_COUNT）时
|
||||
if len(good) > MIN_MATCH_COUNT:
|
||||
|
||||
# extract corresponding point pairs from matching 从匹配中提取出对应点对
|
||||
# query index of small objects, training index of scenarios 小对象的查询索引，场景的训练索引
|
||||
src_pts = np.float32([kp[i][m.queryIdx].pt
|
||||
for m in good]).reshape(-1, 1, 2)
|
||||
dst_pts = np.float32([kp2[m.trainIdx].pt
|
||||
for m in good]).reshape(-1, 1, 2)
|
||||
|
||||
# Using matching points to find homography matrix in cv2.ransac 利用匹配点找到CV2.RANSAC中的单应矩阵
|
||||
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,
|
||||
5.0)
|
||||
matchesMask = mask.ravel().tolist()
|
||||
# Calculate the distortion of image, that is the corresponding position in frame 计算图1的畸变，也就是在图2中的对应的位置
|
||||
h, w, d = goal[i].shape
|
||||
pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1],
|
||||
[w - 1, 0]]).reshape(-1, 1, 2)
|
||||
dst = cv2.perspectiveTransform(pts, M)
|
||||
coord = (dst[0][0] + dst[1][0] + dst[2][0] +
|
||||
dst[3][0]) / 4.0
|
||||
connection.send((DRAW_COORDS, coord))
|
||||
# cv2.putText(img, "{}".format(coord), (50, 60),
|
||||
# fontFace=None, fontScale=1,
|
||||
# color=(0, 255, 0), lineType=1)
|
||||
print(format(dst[0][0][0]))
|
||||
x = (dst[0][0][0] + dst[1][0][0] + dst[2][0][0] +
|
||||
dst[3][0][0]) / 4.0
|
||||
y = (dst[0][0][1] + dst[1][0][1] + dst[2][0][1] +
|
||||
dst[3][0][1]) / 4.0
|
||||
|
||||
# bound box 绘制边框
|
||||
# img = cv2.polylines(img, [np.int32(dst)], True, 244, 3, cv2.LINE_AA)
|
||||
connection.send((DRAW_RECT, dst))
|
||||
# cv2.polylines(mixture, [np.int32(dst)], True, (0, 255, 0), 2, cv2.LINE_AA)
|
||||
except Exception as e:
|
||||
pass
|
||||
|
||||
# else:
|
||||
# if(len(good) < MIN_MATCH_COUNT):
|
||||
|
||||
# i += 1
|
||||
# if(i % 10 == 0):
|
||||
# print("Not enough matches are found - %d/%d" %
|
||||
# (len(good), MIN_MATCH_COUNT))
|
||||
|
||||
# matchesMask = None
|
||||
if x + y > 0:
|
||||
return x, y
|
||||
else:
|
||||
return None
|
||||
|
||||
# The path to save the image folder
|
||||
def parse_folder(folder):
|
||||
restore = []
|
||||
path = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/' + folder
|
||||
for i, j, k in os.walk(path):
|
||||
for l in k:
|
||||
restore.append(cv2.imread(folder + '/{}'.format(l)))
|
||||
return restore
|
||||
|
||||
|
||||
def compute_keypoints_and_descriptors(sift, images_lists):
|
||||
kp_list = []
|
||||
desc_list = []
|
||||
for images in images_lists:
|
||||
kp_tmp = []
|
||||
desc_tmp = []
|
||||
for img in images:
|
||||
kp, desc = sift.detectAndCompute(img, None)
|
||||
kp_tmp.append(kp)
|
||||
desc_tmp.append(desc)
|
||||
kp_list.append(kp_tmp)
|
||||
desc_list.append(desc_tmp)
|
||||
|
||||
return kp_list, desc_list
|
||||
|
||||
|
||||
GET_FRAME = 1
|
||||
STOP_PROCESSING = 2
|
||||
DRAW_COORDS = 3
|
||||
DRAW_RECT = 4
|
||||
CLEAR_DRAW = 5
|
||||
CROP_FRAME = 6
|
||||
|
||||
|
||||
def get_frame(connection):
|
||||
connection.send(GET_FRAME)
|
||||
frame = connection.recv()
|
||||
return frame
|
||||
|
||||
|
||||
def process_transform_frame(frame, x1, y1, x2, y2):
|
||||
# enlarge the image by 1.5 times
|
||||
fx = 1.5
|
||||
fy = 1.5
|
||||
frame = cv2.resize(frame, (0, 0),
|
||||
fx=fx,
|
||||
fy=fy,
|
||||
interpolation=cv2.INTER_CUBIC)
|
||||
# if x1 != x2:
|
||||
# the cutting ratio here is adjusted according to the actual situation
|
||||
# frame = frame[int(y2 * 0.2):int(y1 * 1.15),
|
||||
# int(x1 * 0.7):int(x2 * 1.15)]
|
||||
return frame
|
||||
|
||||
|
||||
def process_display_frame(connection):
|
||||
cap_num = 0
|
||||
coord = None
|
||||
dst = None
|
||||
x1 = 0
|
||||
y1 = 0
|
||||
x2 = 0
|
||||
y2 = 0
|
||||
cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
|
||||
if not cap.isOpened():
|
||||
cap.open()
|
||||
while cv2.waitKey(1) < 0:
|
||||
_, frame = cap.read()
|
||||
frame = process_transform_frame(frame, x1, y1, x2, y2)
|
||||
if connection.poll():
|
||||
request = connection.recv()
|
||||
if request == GET_FRAME:
|
||||
connection.send(frame)
|
||||
elif request == CLEAR_DRAW:
|
||||
coord = None
|
||||
dst = None
|
||||
elif type(request) is tuple:
|
||||
if request[0] == DRAW_COORDS:
|
||||
coord = request[1]
|
||||
elif request[0] == DRAW_RECT:
|
||||
dst = request[1]
|
||||
elif request[0] == CROP_FRAME:
|
||||
x1 = request[1]
|
||||
y1 = request[2]
|
||||
x2 = request[3]
|
||||
y2 = request[4]
|
||||
|
||||
if not coord is None:
|
||||
cv2.putText(frame, "{}".format(coord), (50, 60), fontFace=None,
|
||||
fontScale=1, color=(0, 255, 0), lineType=1)
|
||||
if not dst is None:
|
||||
frame = cv2.polylines(frame, [np.int32(dst)], True, 244, 3, cv2.LINE_AA)
|
||||
cv2.imshow("figure", frame)
|
||||
time.sleep(0.04)
|
||||
connection.send(STOP_PROCESSING)
|
||||
|
||||
|
||||
def run():
|
||||
parent_conn, child_conn = Pipe()
|
||||
|
||||
child = Process(target=process_display_frame, args=(child_conn,))
|
||||
child.start()
|
||||
|
||||
# Object_detect().take_photo()
|
||||
# Object_detect().cut_photo()
|
||||
# goal = Object_detect().distinguist()
|
||||
|
||||
res_queue = [[], [], [], []]
|
||||
res_queue[0] = parse_folder('res/red')
|
||||
res_queue[1] = parse_folder('res/green')
|
||||
res_queue[2] = parse_folder('res/gray')
|
||||
res_queue[3] = parse_folder('res/blue')
|
||||
|
||||
# res_queue = []
|
||||
# res_queue.extend(parse_folder('res/red'))
|
||||
# res_queue.extend(parse_folder('res/green'))
|
||||
# res_queue.extend(parse_folder('res/gray'))
|
||||
# res_queue.extend(parse_folder('res/blue'))
|
||||
|
||||
sift = cv2.xfeatures2d.SIFT_create()
|
||||
kp_list, desc_list = compute_keypoints_and_descriptors(sift, res_queue)
|
||||
|
||||
# init a class of Object_detect
|
||||
detect = Object_detect()
|
||||
# init mycobot
|
||||
detect.run()
|
||||
|
||||
# _init_ = 20 #
|
||||
init_num = 0
|
||||
nparams = 0
|
||||
# num = 0
|
||||
# real_sx = real_sy = 0
|
||||
while True:
|
||||
start_time = time.time()
|
||||
if parent_conn.poll():
|
||||
data = parent_conn.recv()
|
||||
if data == STOP_PROCESSING:
|
||||
break
|
||||
# read camera
|
||||
frame = get_frame(parent_conn)
|
||||
# deal img
|
||||
#frame = detect.transform_frame(frame)
|
||||
|
||||
# if _init_ > 0:
|
||||
# _init_ -= 1
|
||||
# continue
|
||||
# calculate the parameters of camera clipping
|
||||
if init_num < 20:
|
||||
if detect.get_calculate_params(frame) is None:
|
||||
# cv2.imshow("figure", frame)
|
||||
continue
|
||||
else:
|
||||
x1, x2, y1, y2 = detect.get_calculate_params(frame)
|
||||
detect.draw_marker(frame, x1, y1)
|
||||
detect.draw_marker(frame, x2, y2)
|
||||
detect.sum_x1 += x1
|
||||
detect.sum_x2 += x2
|
||||
detect.sum_y1 += y1
|
||||
detect.sum_y2 += y2
|
||||
init_num += 1
|
||||
continue
|
||||
elif init_num == 20:
|
||||
detect.set_cut_params(
|
||||
(detect.sum_x1) / 20.0,
|
||||
(detect.sum_y1) / 20.0,
|
||||
(detect.sum_x2) / 20.0,
|
||||
(detect.sum_y2) / 20.0,
|
||||
)
|
||||
parent_conn.send((CROP_FRAME,
|
||||
(detect.sum_x1) / 20.0,
|
||||
(detect.sum_y1) / 20.0,
|
||||
(detect.sum_x2) / 20.0,
|
||||
(detect.sum_y2) / 20.0))
|
||||
detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0
|
||||
init_num += 1
|
||||
continue
|
||||
|
||||
# calculate params of the coords between cube and mycobot
|
||||
if nparams < 10:
|
||||
if detect.get_calculate_params(frame) is None:
|
||||
# cv2.imshow("figure", frame)
|
||||
continue
|
||||
else:
|
||||
x1, x2, y1, y2 = detect.get_calculate_params(frame)
|
||||
detect.draw_marker(frame, x1, y1)
|
||||
detect.draw_marker(frame, x2, y2)
|
||||
detect.sum_x1 += x1
|
||||
detect.sum_x2 += x2
|
||||
detect.sum_y1 += y1
|
||||
detect.sum_y2 += y2
|
||||
nparams += 1
|
||||
print "ok"
|
||||
continue
|
||||
elif nparams == 10:
|
||||
nparams += 1
|
||||
# calculate and set params of calculating real coord between cube and mycobot
|
||||
detect.set_params((detect.sum_x1 + detect.sum_x2) / 20.0,
|
||||
(detect.sum_y1 + detect.sum_y2) / 20.0,
|
||||
abs(detect.sum_x1 - detect.sum_x2) / 10.0 +
|
||||
abs(detect.sum_y1 - detect.sum_y2) / 10.0)
|
||||
print("ok")
|
||||
continue
|
||||
|
||||
# get detect result
|
||||
|
||||
kp_img, desc_img = sift.detectAndCompute(frame, None)
|
||||
|
||||
frame = get_frame(parent_conn)
|
||||
for i, v in enumerate(res_queue):
|
||||
# HACK: to update frame every time
|
||||
detect_result = detect.obj_detect(frame, v, kp_img, desc_img, kp_list[i], desc_list[i], parent_conn)
|
||||
if detect_result:
|
||||
x, y = detect_result
|
||||
# calculate real coord between cube and mycobot
|
||||
real_x, real_y = detect.get_position(x, y)
|
||||
detect.color = i
|
||||
detect.pub_marker(real_x / 1000.0, real_y / 1000.0)
|
||||
detect.decide_move(real_x, real_y, detect.color)
|
||||
# if num == 5:
|
||||
# detect.color = i
|
||||
# detect.pub_marker(real_sx / 5.0 / 1000.0,
|
||||
# real_sy / 5.0 / 1000.0)
|
||||
# detect.decide_move(real_sx / 5.0, real_sy / 5.0,
|
||||
# detect.color)
|
||||
# num = real_sx = real_sy = 0
|
||||
# else:
|
||||
# num += 1
|
||||
# real_sy += real_y
|
||||
# real_sx += real_x
|
||||
parent_conn.send(CLEAR_DRAW)
|
||||
|
||||
# cv2.imshow("figure", frame)
|
||||
time.sleep(0.05)
|
||||
end_time = time.time()
|
||||
print("loop_time = ", end_time - start_time)
|
||||
|
||||
child.join()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
run()
|
||||
# Object_detect().take_photo()
|
||||
# Object_detect().cut_photo()
|
||||
406
mycobot_ai/scripts/detect_obj_img_wio.py
Executable file
406
mycobot_ai/scripts/detect_obj_img_wio.py
Executable file
|
|
@ -0,0 +1,406 @@
|
|||
#encoding:utf-8
|
||||
|
||||
from tokenize import Pointfloat
|
||||
import cv2
|
||||
import numpy as np
|
||||
import time
|
||||
import json
|
||||
import os
|
||||
import rospy
|
||||
from visualization_msgs.msg import Marker
|
||||
|
||||
from moving_utils import Movement
|
||||
|
||||
IS_CV_4 = cv2.__version__[0] == '4'
|
||||
|
||||
class Object_detect(Movement):
|
||||
|
||||
def __init__(self, camera_x=150, camera_y=-10):
|
||||
# inherit the parent class
|
||||
super(Object_detect, self).__init__()
|
||||
# get path of file
|
||||
dir_path = os.path.dirname(__file__)
|
||||
# choose place to set cube
|
||||
self.color = 0
|
||||
# parameters to calculate camera clipping parameters
|
||||
self.x1 = self.x2 = self.y1 = self.y2 =0
|
||||
# set cache of real coord
|
||||
self.cache_x = self.cache_y = 0
|
||||
# load model of img recognition
|
||||
self.model_path = os.path.join(dir_path, "frozen_inference_graph.pb")
|
||||
self.pbtxt_path = os.path.join(dir_path, "graph.pbtxt")
|
||||
self.label_path = os.path.join(dir_path, "labels.json")
|
||||
# load class labels
|
||||
self.labels = json.load(open(self.label_path))
|
||||
# use to calculate coord between cube and mycobot
|
||||
self.sum_x1= self.sum_x2= self.sum_y2= self.sum_y1= 0
|
||||
# The coordinates of the grab center point relative to the mycobot
|
||||
self.camera_x, self.camera_y = camera_x, camera_y
|
||||
# The coordinates of the cube relative to the mycobot
|
||||
self.c_x, self.c_y = 0,0
|
||||
# The ratio of pixels to actual values
|
||||
self.ratio = 0
|
||||
# Get ArUco marker dict that can be detected.
|
||||
self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
|
||||
# Get ArUco marker params.
|
||||
self.aruco_params = cv2.aruco.DetectorParameters_create()
|
||||
|
||||
if IS_CV_4:
|
||||
self.net = cv2.dnn.readNetFromTensorflow(self.model_path, self.pbtxt_path)
|
||||
else:
|
||||
print('Load tensorflow model need the version of opencv is 4.')
|
||||
exit(0)
|
||||
# init a node and a publisher
|
||||
rospy.init_node("marker", anonymous=True)
|
||||
self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
|
||||
# init a Marker
|
||||
self.marker = Marker()
|
||||
self.marker.header.frame_id = "/joint1"
|
||||
self.marker.ns = "cube"
|
||||
self.marker.type = self.marker.CUBE
|
||||
self.marker.action = self.marker.ADD
|
||||
self.marker.scale.x = 0.04
|
||||
self.marker.scale.y = 0.04
|
||||
self.marker.scale.z = 0.04
|
||||
self.marker.color.a = 1.0
|
||||
self.marker.color.g = 1.0
|
||||
self.marker.color.r = 1.0
|
||||
|
||||
|
||||
# marker position initial
|
||||
self.marker.pose.position.x = 0
|
||||
self.marker.pose.position.y = 0
|
||||
self.marker.pose.position.z = 0.03
|
||||
self.marker.pose.orientation.x = 0
|
||||
self.marker.pose.orientation.y = 0
|
||||
self.marker.pose.orientation.z = 0
|
||||
self.marker.pose.orientation.w = 1.0
|
||||
|
||||
self.cache_x = self.cache_y = 0
|
||||
# publish marker
|
||||
def pub_marker(self, x, y , z=0.03):
|
||||
self.marker.header.stamp = rospy.Time.now()
|
||||
self.marker.pose.position.x = x
|
||||
self.marker.pose.position.y = y
|
||||
self.marker.pose.position.z = z
|
||||
self.marker.color.g = self.color
|
||||
self.pub.publish(self.marker)
|
||||
|
||||
# Grasping motion
|
||||
def move(self, x,y,color):
|
||||
angles = [
|
||||
[-7.11, -6.94, -55.01, -24.16, 0, -38.84], # init the point
|
||||
[-1.14, -10.63, -87.8, 9.05, -3.07, -37.7], # point to grab
|
||||
[17.4, -10.1, -87.27, 5.8, -2.02, -37.7], # point to grab
|
||||
]
|
||||
|
||||
coords = [
|
||||
[106.1, -141.6, 240.9, -173.34, -8.15, -83.11], # above the red bucket
|
||||
[208.2, -127.8, 246.9, -157.51, -17.5, -71.18], # above the green bucket
|
||||
[209.7, -18.6, 230.4, -168.48, -9.86, -39.38],
|
||||
[196.9, -64.7, 232.6, -166.66, -9.44, -52.47],
|
||||
[126.6, -118.1, 305.0, -157.57, -13.72, -75.3],
|
||||
|
||||
]
|
||||
|
||||
# send Angle to move mycobot
|
||||
self.pub_angles(angles[0], 20)
|
||||
time.sleep(1.5)
|
||||
self.pub_angles(angles[1], 20)
|
||||
time.sleep(1.5)
|
||||
self.pub_angles(angles[2], 20)
|
||||
time.sleep(1.5)
|
||||
# send coordinates to move mycobot
|
||||
self.pub_coords([x, y, 165, -178.9, -1.57, -25.95], 20, 1)
|
||||
time.sleep(1.5)
|
||||
self.pub_coords([x, y, 40, -178.9, -1.57, -25.95], 20, 1)
|
||||
time.sleep(1.5)
|
||||
# open pump
|
||||
self.pub_pump(True)
|
||||
time.sleep(0.5)
|
||||
self.pub_angles(angles[2], 20)
|
||||
time.sleep(3)
|
||||
self.pub_marker(coords[2][0]/1000.0, coords[2][1]/1000.0, coords[2][2]/1000.0)
|
||||
|
||||
self.pub_angles(angles[1], 20)
|
||||
time.sleep(1.5)
|
||||
self.pub_marker(coords[3][0]/1000.0, coords[3][1]/1000.0, coords[3][2]/1000.0)
|
||||
|
||||
self.pub_angles(angles[0], 20)
|
||||
time.sleep(1.5)
|
||||
self.pub_marker(coords[4][0]/1000.0, coords[4][1]/1000.0, coords[4][2]/1000.0)
|
||||
|
||||
self.pub_coords(coords[color], 20, 1)
|
||||
self.pub_marker(coords[color][0]/1000.0, coords[color][1]/1000.0, coords[color][2]/1000.0)
|
||||
time.sleep(2)
|
||||
# close pump
|
||||
self.pub_pump(False)
|
||||
if color==1:
|
||||
self.pub_marker(coords[color][0]/1000.0+0.04, coords[color][1]/1000.0-0.02)
|
||||
elif color==0:
|
||||
self.pub_marker(coords[color][0]/1000.0+0.03, coords[color][1]/1000.0)
|
||||
self.pub_angles(angles[0], 20)
|
||||
time.sleep(3)
|
||||
|
||||
|
||||
# decide whether grab cube
|
||||
def decide_move(self, x, y, color):
|
||||
|
||||
|
||||
print(x, y,self.cache_x, self.cache_y)
|
||||
# detect the cube status move or run
|
||||
if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm
|
||||
self.cache_x, self.cache_y = x, y
|
||||
return
|
||||
else:
|
||||
self.cache_x = self.cache_y = 0
|
||||
self.move(x,y,color)
|
||||
|
||||
# init mycobot
|
||||
def run(self):
|
||||
|
||||
for _ in range(10):
|
||||
self.pub_angles([-7.11, -6.94, -55.01, -24.16, 0, -38.84], 20)
|
||||
print(_)
|
||||
time.sleep(0.5)
|
||||
self.pub_pump(False)
|
||||
|
||||
# draw aruco
|
||||
def draw_marker(self,img,x,y):
|
||||
# draw rectangle on img
|
||||
cv2.rectangle(
|
||||
img,
|
||||
(x - 20, y - 20),
|
||||
(x + 20, y + 20),
|
||||
(0, 255, 0),
|
||||
thickness=2,
|
||||
lineType=cv2.FONT_HERSHEY_COMPLEX,
|
||||
)
|
||||
# add text on rectangle
|
||||
cv2.putText(img,"({},{})".format(x,y),(x,y),cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (243, 0, 0), 2,)
|
||||
|
||||
# get points of two aruco
|
||||
def get_calculate_params(self,img):
|
||||
# Convert the image to a gray image
|
||||
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
|
||||
# Detect ArUco marker.
|
||||
corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
|
||||
gray, self.aruco_dict, parameters=self.aruco_params
|
||||
)
|
||||
|
||||
"""
|
||||
Two Arucos must be present in the picture and in the same order.
|
||||
There are two Arucos in the Corners, and each aruco contains the pixels of its four corners.
|
||||
Determine the center of the aruco by the four corners of the aruco.
|
||||
"""
|
||||
if len(corners) > 0:
|
||||
if ids is not None:
|
||||
if len(corners) <= 1 or ids[0]==1:
|
||||
return None
|
||||
x1=x2=y1=y2 = 0
|
||||
point_11,point_21,point_31,point_41 = corners[0][0]
|
||||
x1, y1 = int((point_11[0] + point_21[0] + point_31[0] + point_41[0]) / 4.0), int((point_11[1] + point_21[1] + point_31[1] + point_41[1]) / 4.0)
|
||||
point_1,point_2,point_3,point_4 = corners[1][0]
|
||||
x2, y2 = int((point_1[0] + point_2[0] + point_3[0] + point_4[0]) / 4.0), int((point_1[1] + point_2[1] + point_3[1] + point_4[1]) / 4.0)
|
||||
return x1,x2,y1,y2
|
||||
return None
|
||||
|
||||
# set camera clipping parameters
|
||||
def set_cut_params(self, x1, y1, x2, y2):
|
||||
self.x1 = int(x1)
|
||||
self.y1 = int(y1)
|
||||
self.x2 = int(x2)
|
||||
self.y2 = int(y2)
|
||||
print(self.x1,self.y1,self.x2,self.y2)
|
||||
|
||||
# set parameters to calculate the coords between cube and mycobot
|
||||
def set_params(self, c_x, c_y, ratio):
|
||||
self.c_x = c_x
|
||||
self.c_y = c_y
|
||||
self.ratio = 220.0/ratio
|
||||
|
||||
# calculate the coords between cube and mycobot
|
||||
def get_position(self, x, y):
|
||||
return ((y - self.c_y)*self.ratio + self.camera_x), ((x - self.c_x)*self.ratio + self.camera_y)
|
||||
|
||||
"""
|
||||
Calibrate the camera according to the calibration parameters.
|
||||
Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times.
|
||||
If two ARuco values have been calculated, clip the video.
|
||||
"""
|
||||
def transform_frame(self, frame):
|
||||
# enlarge the image by 1.5 times
|
||||
fx = 1.5
|
||||
fy = 1.5
|
||||
frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy, interpolation=cv2.INTER_CUBIC)
|
||||
if self.x1 != self.x2:
|
||||
# the cutting ratio here is adjusted according to the actual situation
|
||||
frame = frame[int(self.y2*0.2):int(self.y1*1.15), int(self.x1*0.7):int(self.x2*1.15)]
|
||||
return frame
|
||||
|
||||
# according the class_id to get object name
|
||||
def id_class_name(self, class_id):
|
||||
for key, value in self.labels.items():
|
||||
if class_id == int(key):
|
||||
return value
|
||||
|
||||
|
||||
# detect object
|
||||
def obj_detect(self, frame):
|
||||
rows, cols = frame.shape[:-1]
|
||||
|
||||
# Resize image and swap BGR to RGB.
|
||||
blob = cv2.dnn.blobFromImage(
|
||||
frame,
|
||||
size=(300, 300),
|
||||
mean=(0, 0, 0),
|
||||
swapRB=True,
|
||||
crop=False,
|
||||
)
|
||||
|
||||
# Detecting.
|
||||
self.net.setInput(blob)
|
||||
out = self.net.forward()
|
||||
x, y = 0, 0
|
||||
|
||||
# Processing result.
|
||||
for detection in out[0, 0, :, :]:
|
||||
score = float(detection[2])
|
||||
if score > 0.3:
|
||||
class_id = detection[1]
|
||||
left = detection[3] * cols
|
||||
top = detection[4] * rows
|
||||
right = detection[5] * cols
|
||||
bottom = detection[6] * rows
|
||||
if abs(right + bottom - left - top) > 380:
|
||||
continue
|
||||
x, y = (left + right) / 2.0, (top + bottom) / 2.0
|
||||
cv2.rectangle(
|
||||
frame,
|
||||
(int(left), int(top)),
|
||||
(int(right), int(bottom)),
|
||||
(0, 230, 0),
|
||||
thickness=2,
|
||||
)
|
||||
cv2.putText(
|
||||
frame,
|
||||
"{}: {}%".format(self.id_class_name(class_id),round(score * 100, 2)),
|
||||
(int(left), int(top) - 10),
|
||||
cv2.FONT_HERSHEY_COMPLEX_SMALL,
|
||||
1,
|
||||
(243, 0, 0),
|
||||
2,
|
||||
)
|
||||
|
||||
if x+y > 0:
|
||||
return x, y
|
||||
else:
|
||||
return None
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
# open the camera
|
||||
cap_num = 0
|
||||
cap = cv2.VideoCapture(cap_num)
|
||||
if not cap.isOpened():
|
||||
cap.open()
|
||||
# init a class of Object_detect
|
||||
detect = Object_detect()
|
||||
# init mycobot
|
||||
detect.run()
|
||||
|
||||
_init_ = 20 #
|
||||
init_num = 0
|
||||
nparams = 0
|
||||
num = 0
|
||||
real_sx = real_sy = 0
|
||||
while cv2.waitKey(1) < 0:
|
||||
# read camera
|
||||
_,frame = cap.read()
|
||||
# deal img
|
||||
frame = detect.transform_frame(frame)
|
||||
|
||||
|
||||
if _init_ > 0:
|
||||
_init_-=1
|
||||
continue
|
||||
# calculate the parameters of camera clipping
|
||||
if init_num < 20:
|
||||
if detect.get_calculate_params(frame) is None:
|
||||
cv2.imshow("figure",frame)
|
||||
continue
|
||||
else:
|
||||
x1,x2,y1,y2 = detect.get_calculate_params(frame)
|
||||
detect.draw_marker(frame,x1,y1)
|
||||
detect.draw_marker(frame,x2,y2)
|
||||
detect.sum_x1+=x1
|
||||
detect.sum_x2+=x2
|
||||
detect.sum_y1+=y1
|
||||
detect.sum_y2+=y2
|
||||
init_num+=1
|
||||
continue
|
||||
elif init_num==20:
|
||||
detect.set_cut_params(
|
||||
(detect.sum_x1)/20.0,
|
||||
(detect.sum_y1)/20.0,
|
||||
(detect.sum_x2)/20.0,
|
||||
(detect.sum_y2)/20.0,
|
||||
)
|
||||
detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0
|
||||
init_num+=1
|
||||
continue
|
||||
|
||||
# calculate params of the coords between cube and mycobot
|
||||
if nparams < 10:
|
||||
if detect.get_calculate_params(frame) is None:
|
||||
cv2.imshow("figure",frame)
|
||||
continue
|
||||
else:
|
||||
x1,x2,y1,y2 = detect.get_calculate_params(frame)
|
||||
detect.draw_marker(frame,x1,y1)
|
||||
detect.draw_marker(frame,x2,y2)
|
||||
detect.sum_x1+=x1
|
||||
detect.sum_x2+=x2
|
||||
detect.sum_y1+=y1
|
||||
detect.sum_y2+=y2
|
||||
nparams+=1
|
||||
continue
|
||||
elif nparams==10:
|
||||
nparams+=1
|
||||
# calculate and set params of calculating real coord between cube and mycobot
|
||||
detect.set_params(
|
||||
(detect.sum_x1+detect.sum_x2)/20.0,
|
||||
(detect.sum_y1+detect.sum_y2)/20.0,
|
||||
abs(detect.sum_x1-detect.sum_x2)/10.0+abs(detect.sum_y1-detect.sum_y2)/10.0
|
||||
)
|
||||
print "ok"
|
||||
continue
|
||||
|
||||
# get detect result
|
||||
detect_result = detect.obj_detect(frame)
|
||||
if detect_result is None:
|
||||
cv2.imshow("figure",frame)
|
||||
continue
|
||||
else:
|
||||
x, y = detect_result
|
||||
# calculate real coord between cube and mycobot
|
||||
real_x, real_y = detect.get_position(x, y)
|
||||
if num == 5:
|
||||
detect.pub_marker(real_sx/5.0/1000.0, real_sy/5.0/1000.0)
|
||||
detect.decide_move(real_sx/5.0, real_sy/5.0, detect.color)
|
||||
num = real_sx = real_sy = 0
|
||||
|
||||
else:
|
||||
num += 1
|
||||
real_sy += real_y
|
||||
real_sx += real_x
|
||||
|
||||
cv2.imshow("figure",frame)
|
||||
|
||||
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load diff
|
|
@ -1,82 +1,82 @@
|
|||
{
|
||||
"1": "person",
|
||||
"2": "bicycle",
|
||||
"3": "car",
|
||||
"4": "motorcycle",
|
||||
"5": "airplane",
|
||||
"6": "bus",
|
||||
"7": "train",
|
||||
"8": "truck",
|
||||
"9": "boat",
|
||||
"10": "traffic light",
|
||||
"11": "fire hydrant",
|
||||
"13": "stop sign",
|
||||
"14": "parking meter",
|
||||
"15": "bench",
|
||||
"16": "bird",
|
||||
"17": "cat",
|
||||
"18": "dog",
|
||||
"19": "horse",
|
||||
"20": "sheep",
|
||||
"21": "cow",
|
||||
"22": "elephant",
|
||||
"23": "bear",
|
||||
"24": "zebra",
|
||||
"25": "giraffe",
|
||||
"27": "backpack",
|
||||
"28": "umbrella",
|
||||
"31": "handbag",
|
||||
"32": "tie",
|
||||
"33": "suitcase",
|
||||
"34": "frisbee",
|
||||
"35": "skis",
|
||||
"36": "snowboard",
|
||||
"37": "sports ball",
|
||||
"38": "kite",
|
||||
"39": "baseball bat",
|
||||
"40": "baseball glove",
|
||||
"41": "skateboard",
|
||||
"42": "surfboard",
|
||||
"43": "tennis racket",
|
||||
"44": "bottle",
|
||||
"46": "wine glass",
|
||||
"47": "cup",
|
||||
"48": "fork",
|
||||
"49": "knife",
|
||||
"50": "spoon",
|
||||
"51": "bowl",
|
||||
"52": "banana",
|
||||
"53": "apple",
|
||||
"54": "sandwich",
|
||||
"55": "orange",
|
||||
"56": "broccoli",
|
||||
"57": "carrot",
|
||||
"58": "hot dog",
|
||||
"59": "pizza",
|
||||
"60": "donut",
|
||||
"61": "cake",
|
||||
"62": "chair",
|
||||
"63": "couch",
|
||||
"64": "potted plant",
|
||||
"65": "bed",
|
||||
"67": "dining table",
|
||||
"70": "toilet",
|
||||
"72": "tv",
|
||||
"73": "laptop",
|
||||
"74": "mouse",
|
||||
"75": "remote",
|
||||
"76": "keyboard",
|
||||
"77": "cell phone",
|
||||
"78": "microwave",
|
||||
"79": "oven",
|
||||
"80": "toaster",
|
||||
"81": "sink",
|
||||
"82": "refrigerator",
|
||||
"84": "book",
|
||||
"85": "clock",
|
||||
"86": "vase",
|
||||
"87": "scissors",
|
||||
"88": "teddy bear",
|
||||
"89": "hair drier",
|
||||
"90": "toothbrush"
|
||||
{
|
||||
"1": "person",
|
||||
"2": "bicycle",
|
||||
"3": "car",
|
||||
"4": "motorcycle",
|
||||
"5": "airplane",
|
||||
"6": "bus",
|
||||
"7": "train",
|
||||
"8": "truck",
|
||||
"9": "boat",
|
||||
"10": "traffic light",
|
||||
"11": "fire hydrant",
|
||||
"13": "stop sign",
|
||||
"14": "parking meter",
|
||||
"15": "bench",
|
||||
"16": "bird",
|
||||
"17": "cat",
|
||||
"18": "dog",
|
||||
"19": "horse",
|
||||
"20": "sheep",
|
||||
"21": "cow",
|
||||
"22": "elephant",
|
||||
"23": "bear",
|
||||
"24": "zebra",
|
||||
"25": "giraffe",
|
||||
"27": "backpack",
|
||||
"28": "umbrella",
|
||||
"31": "handbag",
|
||||
"32": "tie",
|
||||
"33": "suitcase",
|
||||
"34": "frisbee",
|
||||
"35": "skis",
|
||||
"36": "snowboard",
|
||||
"37": "sports ball",
|
||||
"38": "kite",
|
||||
"39": "baseball bat",
|
||||
"40": "baseball glove",
|
||||
"41": "skateboard",
|
||||
"42": "surfboard",
|
||||
"43": "tennis racket",
|
||||
"44": "bottle",
|
||||
"46": "wine glass",
|
||||
"47": "cup",
|
||||
"48": "fork",
|
||||
"49": "knife",
|
||||
"50": "spoon",
|
||||
"51": "bowl",
|
||||
"52": "banana",
|
||||
"53": "apple",
|
||||
"54": "sandwich",
|
||||
"55": "orange",
|
||||
"56": "broccoli",
|
||||
"57": "carrot",
|
||||
"58": "hot dog",
|
||||
"59": "pizza",
|
||||
"60": "donut",
|
||||
"61": "cake",
|
||||
"62": "chair",
|
||||
"63": "couch",
|
||||
"64": "potted plant",
|
||||
"65": "bed",
|
||||
"67": "dining table",
|
||||
"70": "toilet",
|
||||
"72": "tv",
|
||||
"73": "laptop",
|
||||
"74": "mouse",
|
||||
"75": "remote",
|
||||
"76": "keyboard",
|
||||
"77": "cell phone",
|
||||
"78": "microwave",
|
||||
"79": "oven",
|
||||
"80": "toaster",
|
||||
"81": "sink",
|
||||
"82": "refrigerator",
|
||||
"84": "book",
|
||||
"85": "clock",
|
||||
"86": "vase",
|
||||
"87": "scissors",
|
||||
"88": "teddy bear",
|
||||
"89": "hair drier",
|
||||
"90": "toothbrush"
|
||||
}
|
||||
BIN
mycobot_ai/scripts/local_photo/takephoto.jpeg
Normal file
BIN
mycobot_ai/scripts/local_photo/takephoto.jpeg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 49 KiB |
0
mycobot_ai/scripts/moving_utils.py
Normal file → Executable file
0
mycobot_ai/scripts/moving_utils.py
Normal file → Executable file
0
mycobot_ai/scripts/openVideo.py
Normal file → Executable file
0
mycobot_ai/scripts/openVideo.py
Normal file → Executable file
37
mycobot_ai/scripts/pump.py
Normal file → Executable file
37
mycobot_ai/scripts/pump.py
Normal file → Executable file
|
|
@ -5,34 +5,19 @@ import rospy
|
|||
import time
|
||||
from moving_utils import Movement
|
||||
|
||||
|
||||
class Pump(Movement):
|
||||
|
||||
def __init__(self):
|
||||
super(Pump, self).__init__()
|
||||
rospy.init_node("pump", anonymous=True)
|
||||
|
||||
def run(self):
|
||||
self.pub_pump(False)
|
||||
time.sleep(1)
|
||||
self.pub_pump(True)
|
||||
time.sleep(5)
|
||||
self.pub_pump(False)
|
||||
|
||||
def gpiod(self):
|
||||
import RPi.GPIO as GPIO
|
||||
GPIO.setwarnings(False)
|
||||
GPIO.setmode(GPIO.BCM)
|
||||
GPIO.setup(20, GPIO.OUT)
|
||||
GPIO.setup(21, GPIO.OUT)
|
||||
GPIO.output(20, 0)
|
||||
GPIO.output(21, 0)
|
||||
time.sleep(3)
|
||||
print "close"
|
||||
GPIO.output(20, 1)
|
||||
GPIO.output(21, 1)
|
||||
def __init__(self):
|
||||
super(Pump, self).__init__()
|
||||
rospy.init_node("pump", anonymous=True)
|
||||
|
||||
def run(self):
|
||||
self.pub_pump(False)
|
||||
time.sleep(1)
|
||||
self.pub_pump(True)
|
||||
time.sleep(5)
|
||||
self.pub_pump(False)
|
||||
|
||||
if __name__ == "__main__":
|
||||
pump = Pump()
|
||||
pump.gpiod()
|
||||
pump = Pump()
|
||||
pump.run()
|
||||
BIN
mycobot_ai/scripts/scripts.tar.gz
Normal file
BIN
mycobot_ai/scripts/scripts.tar.gz
Normal file
Binary file not shown.
0
mycobot_ai/scripts/send_maker.py
Normal file → Executable file
0
mycobot_ai/scripts/send_maker.py
Normal file → Executable file
104
mycobot_ai/scripts/test.py
Executable file
104
mycobot_ai/scripts/test.py
Executable file
|
|
@ -0,0 +1,104 @@
|
|||
# -*- coding: utf-8 -*-
|
||||
|
||||
# from fileinput import filename
|
||||
# from genericpath import isfile
|
||||
# import os
|
||||
# from sys import path
|
||||
# import cv2
|
||||
# from PIL import Image
|
||||
|
||||
|
||||
# # #count=0
|
||||
# for file in dirs:
|
||||
# pic_dir=os.path.join(path,file) # res中子文件夹的路径
|
||||
# print(pic_dir)
|
||||
# for i in os.listdir(pic_dir):
|
||||
# imgdir=os.path.join(pic_dir,i)
|
||||
# print(imgdir)
|
||||
# for i in os.listdir(pic_dir):
|
||||
# image_dir=os.path.join(pic_dir,i) #res中每个子文件夹中图片的路径
|
||||
# img1 = cv2.imread(image_dir) # 读取res中每个子文件夹中的图片
|
||||
# #count+=1
|
||||
# print(image_dir)#输出图片的路径
|
||||
#print(img1)#输出图片
|
||||
#print(count)#图片个数
|
||||
# dir_path = os.path.dirname(__file__)
|
||||
# print(dir_path)
|
||||
# # path1 = os.path.split(os.path.realpath(__file__))[0]
|
||||
# # print(path1)
|
||||
# path3=os.path.join(os.path.split(dir_path)[0]+'/res/')
|
||||
|
||||
# print(path3)
|
||||
# #for file in os.listdir(path3):
|
||||
# pic_dir=os.path.join(path3+'red')
|
||||
# # print(pic_dir)
|
||||
# for i in os.listdir(pic_dir):
|
||||
# img=os.path.join(pic_dir,i)
|
||||
# print(img)
|
||||
# print(os.getcwd()+'/mycobot_ai/res/red')
|
||||
|
||||
# list1 = [[1, 2, 3], [4, 5, 6], [7, 8, 9], [10, 11, 12]]
|
||||
# print(list1[0][0])
|
||||
# for i, v in enumerate(list1):
|
||||
# print(i)
|
||||
|
||||
# res = []
|
||||
# for i in list1:
|
||||
# res.append(i)
|
||||
# print(res)
|
||||
|
||||
# # for i in list1:
|
||||
# # print(i)
|
||||
|
||||
# color=i
|
||||
# print(color)
|
||||
# for color in range(0,4):
|
||||
# print(color)
|
||||
|
||||
from pymycobot.mycobot import MyCobot
|
||||
from pymycobot.genre import Angle
|
||||
from pymycobot import PI_PORT, PI_BAUD # 当使用树莓派版本的mycobot时，可以引用这两个变量进行MyCobot初始化
|
||||
import time
|
||||
|
||||
# mc = MyCobot("/dev/ttyACM0", 115200)
|
||||
|
||||
mc = MyCobot("/dev/ttyAMA0", 1000000)
|
||||
# mc.send_angles([0,0,0,0,0,0], 20)
|
||||
|
||||
time.sleep(2)
|
||||
# move_coords = [
|
||||
# [120.1, -141.6, 240.9, -173.34, -8.15, -110.11], # above the red bucket
|
||||
# # above the green bucket
|
||||
# #[208.2, -127.8, 260.9, -157.51, -17.5, -71.18],
|
||||
# [205.6, -130.5, 263.0, -150.99, -0.07, -107.35],
|
||||
# [209.7, -18.6, 230.4, -168.48, -9.86, -39.38],
|
||||
# [196.9, -64.7, 232.6, -166.66, -9.44, -52.47],
|
||||
# [126.6, -118.1, 305.0, -157.57, -13.72, -75.3],
|
||||
# ]
|
||||
|
||||
# mc.send_coords([126.6, -118.1, 305.0, -157.57, -13.72, -75.3],20,1)
|
||||
# time.sleep(2)
|
||||
# mc.send_coords([104.9, 176.7, 242.6, -166.66, -9.44, -52.47],20,1) # above the blue bucket
|
||||
# time.sleep(2)
|
||||
# mc.send_coords([-20.0, 176.7, 242.6, -166.66, -9.44, -52.47],20,1) # abobe the gray bucket
|
||||
# time.sleep(2)
|
||||
# mc.send_coords([120.1,151.6,250.0,-173.34,-8.15,-110.11],20,1)
|
||||
# time.sleep(2)
|
||||
# mc.send_coords([104.9, 176.7, 242.6, -166.66, -9.44, -52.47],20,1)
|
||||
|
||||
mc.send_angles([-26.11, -6.94, -55.01, -24.16, 0, 15],20)
|
||||
# mc.release_all_servos()
|
||||
time.sleep(3)
|
||||
print(mc.get_angles())
|
||||
# mc.send_angles([-1.14, 3.63, -87.8, 9.05, -3.07, 15],20)
|
||||
time.sleep(2)
|
||||
# print(mc.get_angles())
|
||||
# mc.send_angles([17.4, -10.1, -87.27, 5.8, -2.02, 15],20)
|
||||
# time.sleep(2)
|
||||
# print(mc.get_angles())
|
||||
|
||||
# mc.release_servo(6)
|
||||
mc.release_all_servos()
|
||||
# mc.set_servo_calibration(6)
|
||||
# if os.listdir(path,filename='blue'):
|
||||
# pass
|
||||
10
mycobot_ai/scripts/tools.py
Normal file → Executable file
10
mycobot_ai/scripts/tools.py
Normal file → Executable file
|
|
@ -1,11 +1,13 @@
|
|||
from pymycobot.mycobot import MyCobot
|
||||
|
||||
import os
|
||||
# name of device
|
||||
port = "/dev/ttyUSB0"
|
||||
mc = MyCobot(port)
|
||||
# port = "/dev/ttyUSB0"
|
||||
# mc = MyCobot(port)
|
||||
|
||||
# release mycobot
|
||||
# mc.release_all_servos()
|
||||
|
||||
# calibrate the sixth servo
|
||||
mc.set_servo_calibration(6)
|
||||
# mc.set_servo_calibration(6)
|
||||
|
||||
print(os.getcwd())
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue