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

fix

Browse source
This commit is contained in:
wangWking 2022-09-16 17:31:16 +08:00
parent e9c73780bd
commit da979bbe04
4 changed files with 54 additions and 56 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
mycobot_ai/ai_mira/scripts/add_img.py
View file

@ -49,8 +49,8 @@ def take_photo():
def cut_photo():
path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mypalletizer_260/' # pi
path2 = '/home/u20/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mira/' # m5
path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mira/' # pi
path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mira/' # m5
if os.path.exists(path1):
path = path1

37
mycobot_ai/ai_mira/scripts/combine_detect_obj_color.py
View file

@ -1,4 +1,4 @@
#!/usr/bin/env python3
#!/usr/bin/env python2
# -*- coding:utf-8 -*-
from operator import imod
from tokenize import Pointfloat
@ -19,13 +19,13 @@ __version__ = "1.0"
class Object_detect(Movement):
def __init__(self, camera_x = 150, camera_y = 10):
def __init__(self, camera_x = 148, camera_y = 10):
# inherit the parent class
super(Object_detect, self).__init__()
# get path of file
dir_path = os.path.dirname(__file__)
# declare mypal260
# declare mira
self.mc = None
# 移动角度
@ -37,8 +37,8 @@ class Object_detect(Movement):
# 移动坐标
self.move_coords = [
[-6.91, 175.86, 120.0], # above the red bucket
[136.45, 149.22, 117.11], # above the green bucket
[121.35, 127.48, 120.0], # above the red bucket
[217.09, 113.01, 98.36], # above the green bucket
[107.54, -171.23, 117.11], # above the blue bucket
[-6.91, -175.86, 120.0], # above the gray bucket
]
@ -60,14 +60,14 @@ class Object_detect(Movement):
# "yellow": [np.array([26, 43, 46]), np.array([34, 255, 255])],
"red": [np.array([0, 43, 46]), np.array([8, 255, 255])],
"green": [np.array([35, 43, 46]), np.array([77, 255, 255])],
# "blue": [np.array([100, 43, 46]), np.array([124, 255, 255])],
"blue": [np.array([100, 43, 46]), np.array([124, 255, 255])],
"cyan": [np.array([78, 43, 46]), np.array([99, 255, 255])],
}
# use to calculate coord between cube and mypal260
# use to calculate coord between cube and mira
self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
# The coordinates of the grab center point relative to the mypal260
# The coordinates of the grab center point relative to the mira
self.camera_x, self.camera_y = camera_x, camera_y
# The coordinates of the cube relative to the mypal260
# The coordinates of the cube relative to the mira
self.c_x, self.c_y = 0, 0
# The ratio of pixels to actual values
self.ratio = 0
@ -134,7 +134,7 @@ class Object_detect(Movement):
self.mc.set_angles(self.move_angles[0], 20)
time.sleep(3)
# send coordinates to move mypal260
# send coordinates to move mira
self.mc.set_coords([x, -y, 58.84], 20)
time.sleep(1.5)
self.mc.set_coords([x, -y, 21.8], 20)
@ -181,7 +181,7 @@ class Object_detect(Movement):
# 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
self.move(x, y, color)
# init mypal260
# init mira
def run(self):
self.mc = Mira(self.robot_m5, 115200)
self.mc.go_zero()
@ -239,13 +239,13 @@ class Object_detect(Movement):
self.y2 = int(y2)
print(self.x1, self.y1, self.x2, self.y2)
# set parameters to calculate the coords between cube and mypal260
# set parameters to calculate the coords between cube and mira
def set_params(self, c_x, c_y, ratio):
self.c_x = c_x
self.c_y = c_y
self.ratio = 220.0/ratio
# calculate the coords between cube and mypal260
# calculate the coords between cube and mira
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)
@ -319,8 +319,7 @@ class Object_detect(Movement):
cv2.rectangle(img, (x, y), (x+w, y+h), (153, 153, 0), 2)
# calculate the rectangle center
x, y = (x*2+w)/2, (y*2+h)/2
# calculate the real coordinates of mypal260 relative to the target
print('mira_mycolor:', mycolor)
# calculate the real coordinates of mira relative to the target
if mycolor == "red":
self.color = 0
elif mycolor == "green":
@ -344,7 +343,7 @@ if __name__ == "__main__":
cap.open()
# init a class of Object_detect
detect = Object_detect()
# init mypal260
# init mira
detect.run()
_init_ = 20
@ -387,7 +386,7 @@ if __name__ == "__main__":
init_num += 1
continue
# calculate params of the coords between cube and mypal260
# calculate params of the coords between cube and mira
if nparams < 10:
if detect.get_calculate_params(frame) is None:
cv2.imshow("figure", frame)
@ -404,7 +403,7 @@ if __name__ == "__main__":
continue
elif nparams == 10:
nparams += 1
# calculate and set params of calculating real coord between cube and mypal260
# calculate and set params of calculating real coord between cube and mira
detect.set_params(
(detect.sum_x1+detect.sum_x2)/20.0,
(detect.sum_y1+detect.sum_y2)/20.0,
@ -421,7 +420,7 @@ if __name__ == "__main__":
continue
else:
x, y = detect_result
# calculate real coord between cube and mypal260
# calculate real coord between cube and mira
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)

13
mycobot_ai/ai_mira/scripts/combine_detect_obj_img_folder_opt.py
View file

@ -1,4 +1,4 @@
#!/usr/bin/env python3
#!/usr/bin/env python2
# encoding:utf-8
from multiprocessing import Process, Pipe
from cgi import parse
@ -18,7 +18,7 @@ from visualization_msgs.msg import Marker
from PIL import Image
from threading import Thread
import tkFileDialog as filedialog
import tkinter as tk
import Tkinter as tk
from moving_utils import Movement
from pymycobot.mira import Mira
@ -28,7 +28,7 @@ __version__ = "1.0" # Adaptive seeed
class Object_detect(Movement):
def __init__(self, camera_x = 150, camera_y = 10):
def __init__(self, camera_x = 148, camera_y = 10):
# inherit the parent class
super(Object_detect, self).__init__()
# get path of file
@ -40,13 +40,12 @@ class Object_detect(Movement):
self.move_angles = [
[0.0, 0.0, 0.0], # init the point
[19.48, 0.0, 0.0], # point to grab
# [17.4, -10.1, -87.27, 5.8, -2.02, 15], # point to grab
]
# 移动坐标
self.move_coords = [
[-6.91, 175.86, 120.0], # above the red bucket
[136.45, 149.22, 117.11], # above the green bucket
[121.35, 127.48, 120.0], # above the red bucket
[217.09, 113.01, 98.36], # above the green bucket
[107.54, -171.23, 117.11], # above the blue bucket
[-6.91, -175.86, 120.0], # above the gray bucket
]
@ -360,7 +359,7 @@ class Object_detect(Movement):
def parse_folder(folder):
restore = []
path1 = '/home/ubuntu/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mira/' + folder
path2 = '/home/u20/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mira/' + folder
path2 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mira/' + folder
if os.path.exists(path1):
path = path1

56
mycobot_ai/ai_mira/scripts/tools.py
View file

@ -31,13 +31,13 @@ move_gray = [-1.61, 0.0, 0.0]
gray_angles = [-92.25, 0.0, 0.0]
gray_coords = [-6.91, -175.86, 120.0]
move_green = [0.83, 0.22, 0]
green_angles = [47.56, 12.61, 0.0]
green_coords = [136.45, 149.22, 117.11]
move_green = [0.48, 0.61, 0]
green_angles = [27.5, 34.95, 0.0]
green_coords = [217.09, 113.01, 98.36]
move_red = [1.61, 0.0, 0.0]
red_angles = [92.25, 0.0, 0.0]
red_coords = [-6.91, 175.86, 120.0]
move_red = [0.81, 0.0, 0.0]
red_angles = [46.41, 0.0, 0.0]
red_coords = [121.35, 127.48, 120.0]
radians_chushi= [0.34, 0, 0]
@ -65,17 +65,17 @@ chishi_coords = [165.93, 58.69, 120.0]
# print('qilai_angles:', mc.get_angles_info())
# print('qilai_coords:', mc.get_coords_info())
mc.set_radians(move_blue, 50)
time.sleep(4)
print('blue_angles:', mc.get_angles_info())
print('blue_coords:', mc.get_coords_info())
# mc.set_gpio_state(1)
time.sleep(2)
# mc.set_radians(move_blue, 50)
# time.sleep(4)
# print('blue_angles:', mc.get_angles_info())
# print('blue_coords:', mc.get_coords_info())
# # mc.set_gpio_state(1)
# time.sleep(2)
mc.set_radians(radians_chushi, 50)
time.sleep(5)
print('chushi_angles:', mc.get_angles_info())
print('chishi_coords:', mc.get_coords_info())
# mc.set_radians(radians_chushi, 50)
# time.sleep(5)
# print('chushi_angles:', mc.get_angles_info())
# print('chishi_coords:', mc.get_coords_info())
# mc.set_radians(move_gray, 50)
# time.sleep(6)
@ -85,17 +85,17 @@ print('chishi_coords:', mc.get_coords_info())
# mc.set_radians(radians_chushi, 50)
# time.sleep(2)
# mc.set_radians(move_green, 50)
# time.sleep(4)
# print('green_angles:', mc.get_angles_info())
# print('green_coords:', mc.get_coords_info())
# mc.set_radians(radians_chushi, 50)
# time.sleep(2)
# mc.set_radians(move_red, 50)
# time.sleep(4)
# print('red_angles:', mc.get_angles_info())
# print('red_coords:', mc.get_coords_info())
mc.set_radians(move_green, 50)
time.sleep(4)
print('green_angles:', mc.get_angles_info())
print('green_coords:', mc.get_coords_info())
mc.set_radians(radians_chushi, 50)
time.sleep(2)
mc.set_radians(move_red, 50)
time.sleep(4)
print('red_angles:', mc.get_angles_info())
print('red_coords:', mc.get_coords_info())
mc.set_radians(radians_chushi, 50)