kenaug74/mycobot_ros
1
0
Fork 0
mirror of https://github.com/elephantrobotics/mycobot_ros.git synced 2026-07-05 19:47:04 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

add aikit code

Browse source
This commit is contained in:
weiquan 2021-10-26 10:45:58 +08:00
parent 74eb9b3dcf
commit c723b2bc0f
22 changed files with 4719 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
.gitignore vendored
View file

@ -4,6 +4,7 @@
.vscode/
.history
*.pyc
# Editor directories and files
.idea

204
mycobot_ai/CMakeLists.txt Normal file
View file

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

27
mycobot_ai/launch/vision.launch Normal file
View file

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

BIN
mycobot_ai/local_photo/img/goal01.jpeg Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 5.5 KiB

BIN
mycobot_ai/local_photo/img/goal02.jpeg Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 6.2 KiB

BIN
mycobot_ai/local_photo/img/goal03.jpeg Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 5.3 KiB

BIN
mycobot_ai/local_photo/img/goal04.jpeg Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 6.9 KiB

62
mycobot_ai/package.xml Normal file
View file

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

89
mycobot_ai/scripts/add_img.py Normal file
View file

@ -0,0 +1,89 @@
# coding:utf-8
import os, cv2, sys
def take_photo():
# 提醒用户操作字典
print("*********************************************")
print("* 热键(请在摄像头的窗口使用)： *")
print("* z: 拍摄图片 *")
print("* q: 退出 *")
print("*********************************************")
# 创建/使用local_photo文件夹
class_name = "local_photo"
if (os.path.exists("local_photo")):
pass
else:
os.mkdir(class_name)
# 设置特定值
index = 'takephoto'
cap = cv2.VideoCapture(0)
while True:
# 读入每一帧
ret, frame = cap.read()
cv2.imshow("capture", frame)
# 存储
input = cv2.waitKey(1) & 0xFF
# 拍照
if input == ord('z'):
cv2.imwrite(
"%s/%s.jpeg" % (class_name, index),
cv2.resize(frame, (600, 480), interpolation=cv2.INTER_AREA))
break
# 退出
if input == ord('q'):
# 关闭窗口
cap.release()
cv2.destroyAllWindows()
sys.exit()
def cut_photo():
path = os.getcwd() + '/local_photo/img'
for i, j, k in os.walk(path):
file_len = len(k)
print("请截取要识别的部分")
# root = tk.Tk()
# root.withdraw()
# temp1=filedialog.askopenfilename(parent=root) #rgb
# temp2=Image.open(temp1,mode='r')
# temp2= cv.cvtColor(np.asarray(temp2),cv.COLOR_RGB2BGR)
# cut = np.array(temp2)
cut = cv2.imread(r"local_photo/takephoto.jpeg")
cv2.imshow('original', cut)
# C:\Users\Elephant\Desktop\pymycobot+opencv\local_photo/takephoto.jpeg
# 选择ROI
roi = cv2.selectROI(windowName="original",
img=cut,
showCrosshair=False,
fromCenter=False)
x, y, w, h = roi
print(roi)
# 显示ROI并保存图片
if roi != (0, 0, 0, 0):
crop = cut[y:y + h, x:x + w]
cv2.imshow('crop', crop)
cv2.imwrite('local_photo/img/goal{}.jpeg'.format(str(file_len + 1)),
crop)
print('Saved!')
# 退出
cv2.waitKey(0)
cv2.destroyAllWindows()
if __name__ == "__main__":
take_photo()
cut_photo()

178
mycobot_ai/scripts/ai_windows.py Normal file
View file

@ -0,0 +1,178 @@
#!/usr/bin/env python3
# encoding:utf-8
from tkinter import ttk
from tkinter import *
import os, 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.ports = os.popen("ls /dev/ttyUSB*").readline()
if "dev" not in self.ports:
self.ports = os.popen("ls /dev/ttyACM*").readline()[:-1]
self.command = '<arg name="port" default="{}" />'.format(
self.ports)
# 根据通信口修改ros启动文件
os.system(
"sed -i '2c {}' /home/h/catkin_mycobot/src/mycobot_ros/mycobot_ai/launch/vision.launch"
.format(self.command))
except Exception as e:
pass
# 设置标题
self.win.title("aikit启动工具")
self.win.geometry(
"500x300+100+100") # 290 160为窗口大小，+10 +10 定义窗口弹出时的默认展示位置
# 打开ros按钮
self.btn = Button(self.win, text="open ROS", command=self.open_ros)
self.btn.grid(row=0)
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.set_xy = Label(self.win, text="set_xy:", width=10)
# self.set_xy.grid(row=1)
# self.x = Label(self.win, text="x:")
# self.x.grid(row=2)
# self.v1 = StringVar()
# self.e1 = Entry(self.win,textvariable=self.v1, width=10)
# self.e1.insert(0,0)
# self.e1.grid(row=2,column=1)
# self.y = Label(self.win, text="y:")
# self.y.grid(row=3)
# self.v2 = StringVar()
# self.e2 = Entry(self.win,textvariable=self.v2, width=10)
# self.e2.insert(0,0)
# self.e2.grid(row=3,column=1)
self.tips = "1、首先打开ros\n2、选择所要运行的程序点击运行即可"
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.code_list = ttk.Combobox(self.win, width=15)
# self.code_list["value"] = ("颜色识别", "物体识别", "二维码识别")
# self.code_list.current(0)
# self.code_list.grid(row=1, column=1)
def start_run(self):
try:
print(u"开始运行")
one = self.myCombox.get()
if one == u"颜色识别":
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 /home/h/catkin_mycobot/src/mycobot_ros/mycobot_ai/scripts/detect_obj_img.py"
)
def open_py1(self):
os.system(
"python /home/h/catkin_mycobot/src/mycobot_ros/mycobot_ai/scripts/detect_obj_color.py"
)
def open_py2(self):
os.system(
"python /home/h/catkin_mycobot/src/mycobot_ros/mycobot_ai/scripts/detect_encode.py"
)
def add_img(self):
os.system(
"python /home/h/catkin_mycobot/src/mycobot_ros/mycobot_ai/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.ros = True
def ross(self):
os.system(
"roslaunch ~/catkin_mycobot/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()

197
mycobot_ai/scripts/detect_encode.py Normal file
View file

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

432
mycobot_ai/scripts/detect_obj_color.py Normal file
View file

@ -0,0 +1,432 @@
#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'
__version__ = "1.0"
# Adaptive seeed
class Object_detect(Movement):
def __init__(self, camera_x=150, camera_y=-10):
# inherit the parent class
super(Object_detect, self).__init__()
# get path of file
dir_path = os.path.dirname(__file__)
# 移动角度
self.move_angles = [
[-7.11, -6.94, -55.01, -24.16, 0, -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
]
# 移动坐标
self.move_coords = [
[120.1, -141.6, 240.9, -173.34, -8.15, -83.11], # above the red bucket
[228.2, -127.8, 260.9, -157.51, -17.5, -71.18], # above the yello 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],
]
# which robot
self.robot = os.popen("ls /dev/ttyUSB*")
if "dev" in self.robot:
self.Pin = [2,5]
else:
self.Pin = [20,21]
for i in self.move_coords:
i[2] -= 20
# choose place to set cube
self.color = 0
# parameters to calculate camera clipping parameters
self.x1 = self.x2 = self.y1 = self.y2 =0
# set cache of real coord
self.cache_x = self.cache_y = 0
# set color HSV
self.HSV = {
"yellow": [np.array([11, 115, 70]), np.array([40, 255, 245])],
"red": [np.array([0, 43, 46]), np.array([8, 255, 255])],
"green": [np.array([35, 43, 46]), np.array([77, 255, 255])],
"blue": [np.array([100, 43, 46]), np.array([124, 255, 255])],
"cyan": [np.array([78, 43, 46]), np.array([99, 255, 255])],
}
# use to calculate coord between cube and 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):
# send Angle to move mycobot
print color
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, -25.95], 20, 1)
time.sleep(1.5)
if "dev" in self.robot:
self.pub_coords([x, y, 90, -178.9, -1.57, -25.95], 20, 1)
else:
h = 0
if 165<x<180:
h = 10
elif x>180:
h = 20
elif x<135:
h = -20
print 'down_1:',[x, y, 31.9+h, -178.9, -1, -25.95]
self.pub_coords([x, y, 31.9+h, -178.9, -1, -25.95], 20, 1)
time.sleep(1.5)
# open pump
self.pub_pump(True,self.Pin)
time.sleep(0.5)
self.pub_angles(self.move_angles[2], 20)
time.sleep(3)
self.pub_marker(self.move_coords[2][0]/1000.0, self.move_coords[2][1]/1000.0, self.move_coords[2][2]/1000.0)
self.pub_angles(self.move_angles[1], 20)
time.sleep(1.5)
self.pub_marker(self.move_coords[3][0]/1000.0, self.move_coords[3][1]/1000.0, self.move_coords[3][2]/1000.0)
self.pub_angles(self.move_angles[0], 20)
time.sleep(2)
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 'down:',self.move_coords[color]
self.pub_coords(self.move_coords[color], 20, 1)
self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color][1]/1000.0, self.move_coords[color][2]/1000.0)
time.sleep(2)
# close pump
self.pub_pump(False,self.Pin)
if color==1:
self.pub_marker(self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02)
elif color==0:
self.pub_marker(self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0)
self.pub_angles(self.move_angles[0], 20)
time.sleep(3)
# decide whether grab cube
def decide_move(self, x, y, color):
print(x, y,self.cache_x, self.cache_y)
# detect the cube status move or run
if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm
self.cache_x, self.cache_y = x, y
return
else:
self.cache_x = self.cache_y = 0
if "dev" not in self.robot:
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
print x,y
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,self.Pin)
# 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
else:
self.color = 1
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)

602
mycobot_ai/scripts/detect_obj_img.py Normal file
View file

@ -0,0 +1,602 @@
#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 PIL import Image
from threading import Thread
import tkFileDialog as filedialog
import Tkinter as tk
from moving_utils import Movement
IS_CV_4 = cv2.__version__[0] == '4'
__version__ = "1.0" # Adaptive seeed
class Object_detect(Movement):
def __init__(self, camera_x=150, camera_y=-10):
# inherit the parent class
super(Object_detect, self).__init__()
# get path of file
dir_path = os.path.dirname(__file__)
# 移动角度
self.move_angles = [
[-7.11, -6.94, -55.01, -24.16, 0, -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
]
# 移动坐标
self.move_coords = [
[120.1, -141.6, 240.9, -173.34, -8.15, -83.11], # above the red bucket
[228.2, -127.8, 260.9, -157.51, -17.5, -71.18], # above the yello 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],
]
# 判断连接设备:ttyUSB*为M5，ttyACM*为seeed
self.robot = os.popen("ls /dev/ttyUSB*")
if "dev" in self.robot:
self.Pin = [2,5]
else:
self.Pin = [20,21]
for i in self.move_coords:
i[2] -= 20
# 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):
# 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, -25.95], 20, 1)
time.sleep(1.5)
if "dev" in self.robot:
self.pub_coords([x, y, 90, -178.9, -1.57, -25.95], 20, 1)
else:
h = 0
if 165<x<180:
h = 10
elif x>180:
h = 20
elif x<135:
h = -20
#print 'down_1:',[x, y, 31.9+h, -178.9, -1, -25.95]
self.pub_coords([x, y, 31.9+h, -178.9, -1, -25.95], 20, 1)
time.sleep(1.5)
# open pump
self.pub_pump(True,self.Pin)
time.sleep(0.5)
self.pub_angles(self.move_angles[2], 20)
time.sleep(3)
self.pub_marker(self.move_coords[2][0]/1000.0, self.move_coords[2][1]/1000.0, self.move_coords[2][2]/1000.0)
self.pub_angles(self.move_angles[1], 20)
time.sleep(1.5)
self.pub_marker(self.move_coords[3][0]/1000.0, self.move_coords[3][1]/1000.0, self.move_coords[3][2]/1000.0)
self.pub_angles(self.move_angles[0], 20)
time.sleep(1.5)
self.pub_marker(self.move_coords[4][0]/1000.0, self.move_coords[4][1]/1000.0, self.move_coords[4][2]/1000.0)
self.pub_coords(self.move_coords[color], 20, 1)
self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color][1]/1000.0, self.move_coords[color][2]/1000.0)
time.sleep(2)
# close pump
self.pub_pump(False,self.
Pin)
if color==1:
self.pub_marker(self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02)
elif color==0:
self.pub_marker(self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0)
self.pub_angles(self.move_angles[0], 20)
time.sleep(3)
# decide whether grab cube
def decide_move(self, x, y, color):
print(x, y,self.cache_x, self.cache_y)
# detect the cube status move or run
if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm
self.cache_x, self.cache_y = x, y
return
else:
self.cache_x = self.cache_y = 0
if "dev" not in self.robot:
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
#print x,y
self.move(x,y,color)
# init mycobot
def run(self):
for _ in range(5):
self.pub_angles([-7.11, -6.94, -55.01, -24.16, 0, -38.84], 20)
print(_)
time.sleep(0.5)
self.pub_pump(False,self.Pin)
# draw aruco
def draw_marker(self,img,x,y):
# draw rectangle on img
cv2.rectangle(
img,
(x - 20, y - 20),
(x + 20, y + 20),
(0, 255, 0),
thickness=2,
lineType=cv2.FONT_HERSHEY_COMPLEX,
)
# add text on rectangle
cv2.putText(img,"({},{})".format(x,y),(x,y),cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (243, 0, 0), 2,)
# get points of two aruco
def get_calculate_params(self,img):
# Convert the image to a gray image
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Detect ArUco marker.
corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
gray, self.aruco_dict, parameters=self.aruco_params
)
"""
Two Arucos must be present in the picture and in the same order.
There are two Arucos in the Corners, and each aruco contains the pixels of its four corners.
Determine the center of the aruco by the four corners of the aruco.
"""
if len(corners) > 0:
if ids is not None:
if len(corners) <= 1 or ids[0]==1:
return None
x1=x2=y1=y2 = 0
point_11,point_21,point_31,point_41 = corners[0][0]
x1, y1 = int((point_11[0] + point_21[0] + point_31[0] + point_41[0]) / 4.0), int((point_11[1] + point_21[1] + point_31[1] + point_41[1]) / 4.0)
point_1,point_2,point_3,point_4 = corners[1][0]
x2, y2 = int((point_1[0] + point_2[0] + point_3[0] + point_4[0]) / 4.0), int((point_1[1] + point_2[1] + point_3[1] + point_4[1]) / 4.0)
return x1,x2,y1,y2
return None
# set camera clipping parameters
def set_cut_params(self, x1, y1, x2, y2):
self.x1 = int(x1)
self.y1 = int(y1)
self.x2 = int(x2)
self.y2 = int(y2)
print(self.x1,self.y1,self.x2,self.y2)
# set parameters to calculate the coords between cube and mycobot
def set_params(self, c_x, c_y, ratio):
self.c_x = c_x
self.c_y = c_y
self.ratio = 220.0/ratio
# calculate the coords between cube and mycobot
def get_position(self, x, y):
return ((y - self.c_y)*self.ratio + self.camera_x), ((x - self.c_x)*self.ratio + self.camera_y)
"""
Calibrate the camera according to the calibration parameters.
Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times.
If two ARuco values have been calculated, clip the video.
"""
def transform_frame(self, frame):
# enlarge the image by 1.5 times
fx = 1.5
fy = 1.5
frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy, interpolation=cv2.INTER_CUBIC)
if self.x1 != self.x2:
# the cutting ratio here is adjusted according to the actual situation
frame = frame[int(self.y2*0.2):int(self.y1*1.15), int(self.x1*0.7):int(self.x2*1.15)]
return frame
# according the class_id to get object name
def id_class_name(self, class_id):
for key, value in self.labels.items():
if class_id == int(key):
return value
# detect object
def obj_detect(self, img, goal):
# rows, cols = frame.shape[:-1]
# Resize image and swap BGR to RGB.
# blob = cv2.dnn.blobFromImage(
# frame,
# size=(300, 300),
# mean=(0, 0, 0),
# swapRB=True,
# crop=False,
# )
# Detecting.
# self.net.setInput(blob)
# out = self.net.forward()
# x, y = 0, 0
# Processing result.
# for detection in out[0, 0, :, :]:
# score = float(detection[2])
# if score > 0.3:
# class_id = detection[1]
# left = detection[3] * cols
# top = detection[4] * rows
# right = detection[5] * cols
# bottom = detection[6] * rows
# if abs(right + bottom - left - top) > 380:
# continue
# x, y = (left + right) / 2.0, (top + bottom) / 2.0
# cv2.rectangle(
# frame,
# (int(left), int(top)),
# (int(right), int(bottom)),
# (0, 230, 0),
# thickness=2,
# )
# cv2.putText(
# frame,
# "{}: {}%".format(self.id_class_name(class_id),round(score * 100, 2)),
# (int(left), int(top) - 10),
# cv2.FONT_HERSHEY_COMPLEX_SMALL,
# 1,
# (243, 0, 0),
# 2,
# )
i = 0
MIN_MATCH_COUNT = 10
sift = cv2.xfeatures2d.SIFT_create()
# find the keypoints and descriptors with SIFT
kp = []
des = []
for i in goal:
kp0,des0 = sift.detectAndCompute(i, None)
kp.append(kp0)
des.append(des0)
# kp1, des1 = sift.detectAndCompute(goal, None)
kp2, des2 = sift.detectAndCompute(img, None)
# FLANN parameters
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
search_params = dict(checks=50) # or pass empty dictionary
flann = cv2.FlannBasedMatcher(index_params, search_params)
x, y = 0, 0
try:
for i in range(len(des)):
matches = flann.knnMatch(des[i], des2, k=2)
# store all the good matches as per Lowe's ratio test. 根据Lowe比率测试存储所有良好匹配项。
good = []
for m, n in matches:
if m.distance < 0.7*n.distance:
good.append(m)
# When there are enough robust matching point pairs 当有足够的健壮匹配点对（至少个MIN_MATCH_COUNT）时
if len(good) > MIN_MATCH_COUNT:
# extract corresponding point pairs from matching 从匹配中提取出对应点对
# query index of small objects, training index of scenarios 小对象的查询索引，场景的训练索引
src_pts = np.float32(
[kp[i][m.queryIdx].pt for m in good]).reshape(-1, 1, 2)
dst_pts = np.float32(
[kp2[m.trainIdx].pt for m in good]).reshape(-1, 1, 2)
# Using matching points to find homography matrix in cv2.ransac 利用匹配点找到CV2.RANSAC中的单应矩阵
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
matchesMask = mask.ravel().tolist()
# Calculate the distortion of image, that is the corresponding position in frame 计算图1的畸变，也就是在图2中的对应的位置
h, w, d = goal[i].shape
pts = np.float32(
[[0, 0], [0, h-1], [w-1, h-1], [w-1, 0]]).reshape(-1, 1, 2)
dst = cv2.perspectiveTransform(pts, M)
ccoord = (dst[0][0]+dst[1][0]+dst[2][0]+dst[3][0])/4.0
cv2.putText(img, "{}".format(ccoord), (50, 60), fontFace=None,
fontScale=1, color=(0, 255, 0), lineType=1)
print(format(dst[0][0][0]))
x = (dst[0][0][0]+dst[1][0][0]+dst[2][0][0]+dst[3][0][0])/4.0
y = (dst[0][0][1]+dst[1][0][1]+dst[2][0][1]+dst[3][0][1])/4.0
# bound box 绘制边框
img = cv2.polylines(
img, [np.int32(dst)], True, 244, 3, cv2.LINE_AA)
# cv2.polylines(mixture, [np.int32(dst)], True, (0, 255, 0), 2, cv2.LINE_AA)
except Exception as e:
pass
else:
if(len(good) < MIN_MATCH_COUNT):
i += 1
if(i % 10 == 0):
print("Not enough matches are found - %d/%d" %
(len(good), MIN_MATCH_COUNT))
matchesMask = None
if x+y > 0:
return x, y
else:
return None
def take_photo(self):
# 提醒用户操作字典
print("*********************************************")
print("* 热键(请在摄像头的窗口使用)： *")
print("* z: 拍摄图片 *")
print("* q: 退出 *")
print("*********************************************")
# 创建/使用local_photo文件夹
class_name = "local_photo"
if(os.path.exists("local_photo")):
pass
else:
os.mkdir(class_name)
# 设置特定值
index = 'takephoto'
cap = cv2.VideoCapture(0)
while True:
# 读入每一帧
ret, frame = cap.read()
cv2.imshow("capture", frame)
# 存储
input = cv2.waitKey(1) & 0xFF
# 拍照
if input == ord('z'):
cv2.imwrite("%s/%s.jpeg" % (class_name, index),
cv2.resize(frame, (600, 480), interpolation=cv2.INTER_AREA))
break
# 退出
if input == ord('q'):
break
# 关闭窗口
cap.release()
cv2.destroyAllWindows()
def cut_photo(self):
path = os.getcwd()+'/local_photo/img'
print path
for i,j,k in os.walk(path):
file_len = len(k)
print("请截取要识别的部分")
# root = tk.Tk()
# root.withdraw()
# temp1=filedialog.askopenfilename(parent=root) #rgb
# temp2=Image.open(temp1,mode='r')
# temp2= cv.cvtColor(np.asarray(temp2),cv.COLOR_RGB2BGR)
# cut = np.array(temp2)
cut = cv2.imread(r"local_photo/takephoto.jpeg")
cv2.imshow('original', cut)
# C:\Users\Elephant\Desktop\pymycobot+opencv\local_photo/takephoto.jpeg
# 选择ROI
roi = cv2.selectROI(windowName="original", img=cut,
showCrosshair=False, fromCenter=False)
x, y, w, h = roi
print(roi)
# 显示ROI并保存图片
if roi != (0, 0, 0, 0):
crop = cut[y:y+h, x:x+w]
cv2.imshow('crop', crop)
cv2.imwrite('local_photo/img/goal{}.jpeg'.format(str(file_len+1)), crop)
print('Saved!')
# 退出
cv2.waitKey(0)
cv2.destroyAllWindows()
def distinguist(self):
print("请选择要识别的物体图片")
root = tk.Tk() # 显式创建根窗体
root.withdraw() # 将根窗体隐藏
file = filedialog.askopenfilename(parent=root)
load = Image.open(file, mode='r')
load = cv.cvtColor(np.asarray(load), cv.COLOR_RGB2BGR)
goal = np.array(load)
return goal
def run(stop):
#Object_detect().take_photo()
#Object_detect().cut_photo()
# goal = Object_detect().distinguist()
goal = []
path = os.getcwd()+'/local_photo/img'
print path
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)
if not cap.isOpened():
cap.open()
# init a class of Object_detect
detect = Object_detect()
# init mycobot
detect.run()
_init_ = 20 #
init_num = 0
nparams = 0
num = 0
real_sx = real_sy = 0
while cv2.waitKey(1) < 0:
# read camera
_,frame = cap.read()
# deal img
frame = detect.transform_frame(frame)
if _init_ > 0:
_init_-=1
continue
# calculate the parameters of camera clipping
if init_num < 20:
if detect.get_calculate_params(frame) is None:
cv2.imshow("figure",frame)
continue
else:
x1,x2,y1,y2 = detect.get_calculate_params(frame)
detect.draw_marker(frame,x1,y1)
detect.draw_marker(frame,x2,y2)
detect.sum_x1+=x1
detect.sum_x2+=x2
detect.sum_y1+=y1
detect.sum_y2+=y2
init_num+=1
continue
elif init_num==20:
detect.set_cut_params(
(detect.sum_x1)/20.0,
(detect.sum_y1)/20.0,
(detect.sum_x2)/20.0,
(detect.sum_y2)/20.0,
)
detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0
init_num+=1
continue
# calculate params of the coords between cube and mycobot
if nparams < 10:
if detect.get_calculate_params(frame) is None:
cv2.imshow("figure",frame)
continue
else:
x1,x2,y1,y2 = detect.get_calculate_params(frame)
detect.draw_marker(frame,x1,y1)
detect.draw_marker(frame,x2,y2)
detect.sum_x1+=x1
detect.sum_x2+=x2
detect.sum_y1+=y1
detect.sum_y2+=y2
nparams+=1
continue
elif nparams==10:
nparams+=1
# calculate and set params of calculating real coord between cube and mycobot
detect.set_params(
(detect.sum_x1+detect.sum_x2)/20.0,
(detect.sum_y1+detect.sum_y2)/20.0,
abs(detect.sum_x1-detect.sum_x2)/10.0+abs(detect.sum_y1-detect.sum_y2)/10.0
)
print "ok"
continue
# get detect result
detect_result = detect.obj_detect(frame,goal)
if detect_result is None:
cv2.imshow("figure",frame)
continue
else:
x, y = detect_result
# calculate real coord between cube and mycobot
real_x, real_y = detect.get_position(x, y)
if num == 5:
detect.pub_marker(real_sx/5.0/1000.0, real_sy/5.0/1000.0)
detect.decide_move(real_sx/5.0, real_sy/5.0, detect.color)
num = real_sx = real_sy = 0
else:
num += 1
real_sy += real_y
real_sx += real_x
cv2.imshow("figure",frame)
if __name__ == "__main__":
run(0)
#Object_detect().take_photo()
#Object_detect().cut_photo()

BIN
mycobot_ai/scripts/frozen_inference_graph.pb Normal file
View file

Binary file not shown.

2682
mycobot_ai/scripts/graph.pbtxt Normal file
View file

File diff suppressed because it is too large Load diff

82
mycobot_ai/scripts/labels.json Normal file
View file

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

49
mycobot_ai/scripts/moving_utils.py Normal file
View file

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

BIN
mycobot_ai/scripts/mtx_dist.npz Normal file
View file

Binary file not shown.

8
mycobot_ai/scripts/openVideo.py Normal file
View file

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

23
mycobot_ai/scripts/pump.py Normal file
View file

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

72
mycobot_ai/scripts/send_maker.py Normal file
View file

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

11
mycobot_ai/scripts/tools.py Normal file
View file

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