mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
add aikit-320 ros
This commit is contained in:
weijian
parent
c25b49b776
commit
ad412357e7
9 changed files with 1494 additions and 349 deletions
|
|
@ -6,10 +6,11 @@ Panels:
|
|||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /RobotModel1
|
||||
- /TF1
|
||||
- /TF1/Tree1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 617
|
||||
Tree Height: 603
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
|
|
@ -18,17 +19,18 @@ Panels:
|
|||
- /2D Nav Goal1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.588679016
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: ""
|
||||
Preferences:
|
||||
PromptSaveOnExit: true
|
||||
Toolbars:
|
||||
toolButtonStyle: 2
|
||||
Visualization Manager:
|
||||
|
|
@ -40,7 +42,7 @@ Visualization Manager:
|
|||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.0299999993
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
|
|
@ -67,6 +69,11 @@ Visualization Manager:
|
|||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
env:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link1:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
|
|
@ -105,11 +112,15 @@ Visualization Manager:
|
|||
Visual Enabled: true
|
||||
- Class: rviz/TF
|
||||
Enabled: true
|
||||
Filter (blacklist): ""
|
||||
Filter (whitelist): ""
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: true
|
||||
base:
|
||||
Value: true
|
||||
env:
|
||||
Value: true
|
||||
link1:
|
||||
Value: true
|
||||
link2:
|
||||
|
|
@ -122,13 +133,16 @@ Visualization Manager:
|
|||
Value: true
|
||||
link6:
|
||||
Value: true
|
||||
Marker Scale: 0.400000006
|
||||
Marker Alpha: 1
|
||||
Marker Scale: 0.20000000298023224
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: true
|
||||
Tree:
|
||||
base:
|
||||
env:
|
||||
{}
|
||||
link1:
|
||||
link2:
|
||||
link3:
|
||||
|
|
@ -153,7 +167,10 @@ Visualization Manager:
|
|||
- Class: rviz/FocusCamera
|
||||
- Class: rviz/Measure
|
||||
- Class: rviz/SetInitialPose
|
||||
Theta std deviation: 0.2617993950843811
|
||||
Topic: /initialpose
|
||||
X std deviation: 0.5
|
||||
Y std deviation: 0.5
|
||||
- Class: rviz/SetGoal
|
||||
Topic: /move_base_simple/goal
|
||||
- Class: rviz/PublishPoint
|
||||
|
|
@ -163,33 +180,33 @@ Visualization Manager:
|
|||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 1.44857454
|
||||
Distance: 1.4485745429992676
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.0599999987
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Field of View: 0.7853981852531433
|
||||
Focal Point:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.0500000007
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.00999999978
|
||||
Pitch: 0.394796789
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.834796667098999
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 3.10358953
|
||||
Yaw: 0.6185896992683411
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 898
|
||||
Height: 900
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000016a000002f8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002f8000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002f8000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba000002f800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000016a000002e6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002e6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002e6000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004b3000002e600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
|
|
@ -198,6 +215,6 @@ Window Geometry:
|
|||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1855
|
||||
X: 65
|
||||
Y: 24
|
||||
Width: 1848
|
||||
X: 72
|
||||
Y: 27
|
||||
|
|
|
|||
|
|
@ -52,16 +52,7 @@ def take_photo():
|
|||
|
||||
def cut_photo():
|
||||
|
||||
path1 = '/home/er/catkin_ws/src/mycobot_ros/mycobot_ai/aikit_280_pi' # pi
|
||||
path2 = r'D:/BaiduSyncdisk/PythonProject/OpenCV' # m5
|
||||
|
||||
if os.path.exists(path1):
|
||||
path = path1
|
||||
elif os.path.exists(path2):
|
||||
path = path2
|
||||
else:
|
||||
print("invalid file path! Please check whether the file path exists or modify it!")
|
||||
|
||||
path = '/home/er/catkin_ws/src/mycobot_ros/mycobot_ai/aikit_320_pi' # pi
|
||||
|
||||
path_red = path + '/res/A'
|
||||
for i, j, k in os.walk(path_red):
|
||||
|
|
@ -80,17 +71,10 @@ def cut_photo():
|
|||
file_len_blue = len(k)
|
||||
print("请截取要识别的部分")
|
||||
print("Please intercept the part to be identified")
|
||||
# root = tk.Tk()
|
||||
# root.withdraw()
|
||||
# temp1=filedialog.askopenfilename(parent=root) #rgb
|
||||
# temp2=Image.open(temp1,mode='r')
|
||||
# temp2= cv.cvtColor(np.asarray(temp2),cv.COLOR_RGB2BGR)
|
||||
# cut = np.array(temp2)
|
||||
|
||||
cut = cv2.imread(r"res/takephoto.jpeg")
|
||||
|
||||
cv2.imshow('original', cut)
|
||||
# C:\Users\Elephant\Desktop\pymycobot+opencv\local_photo/takephoto.jpeg
|
||||
|
||||
# 选择ROI
|
||||
roi = cv2.selectROI(windowName="original",
|
||||
|
|
|
|||
|
|
@ -16,24 +16,25 @@ __version__ = "1.0"
|
|||
|
||||
class Object_detect(Movement):
|
||||
|
||||
def __init__(self, camera_x = 155, camera_y = 15):
|
||||
def __init__(self, camera_x = 256, camera_y = 0):
|
||||
# inherit the parent class
|
||||
super(Object_detect, self).__init__()
|
||||
# declare mycobot280
|
||||
# declare mycobot320
|
||||
self.mc = None
|
||||
|
||||
# 移动角度
|
||||
self.move_angles = [
|
||||
[0.61, 45.87, -92.37, -41.3, 2.02, 9.58], # init the point
|
||||
[18.8, -7.91, -54.49, -23.02, -0.79, -14.76], # point to grab
|
||||
[17.22, -5.27, -52.47, -25.75, 89.73, -0.26],
|
||||
]
|
||||
|
||||
# 移动坐标
|
||||
self.move_coords = [
|
||||
[132.2, -136.9, 200.8, -178.24, -3.72, -107.17], # D Sorting area
|
||||
[238.8, -124.1, 204.3, -169.69, -5.52, -96.52], # C Sorting area
|
||||
[115.8, 177.3, 210.6, 178.06, -0.92, -6.11], # A Sorting area
|
||||
[-6.9, 173.2, 201.5, 179.93, 0.63, 33.83], # B Sorting area
|
||||
[28.9, -226, 246, -171.13, -3.94, -92.37], # D Sorting area
|
||||
[253.3, -216.1, 257, -163.12, -6.12, -95.27], # C Sorting area
|
||||
[241.8, 219.5, 270.6, -168.47, 10.42, -76.84], # A Sorting area
|
||||
[37.8, 233, 251.4, -170.6, -6.75, 88.53], # B Sorting area
|
||||
]
|
||||
|
||||
# which robot: USB* is m5; ACM* is wio; AMA* is raspi
|
||||
|
|
@ -42,25 +43,7 @@ class Object_detect(Movement):
|
|||
self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
|
||||
self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
|
||||
self.raspi = False
|
||||
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
|
||||
self.Pin = [2, 5]
|
||||
elif "dev" in self.robot_wio:
|
||||
self.Pin = [20, 21]
|
||||
for i in self.move_coords:
|
||||
i[2] -= 20
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
import RPi.GPIO as GPIO
|
||||
GPIO.setwarnings(False)
|
||||
self.GPIO = GPIO
|
||||
GPIO.setmode(GPIO.BCM)
|
||||
GPIO.setup(20, GPIO.OUT)
|
||||
GPIO.setup(21, GPIO.OUT)
|
||||
GPIO.output(20, 1)
|
||||
GPIO.output(21, 1)
|
||||
self.raspi = True
|
||||
if self.raspi:
|
||||
self.gpio_status(False)
|
||||
|
||||
|
||||
# choose place to set cube 选择放置立方体的地方
|
||||
self.color = 0
|
||||
# parameters to calculate camera clipping parameters 计算相机裁剪参数的参数
|
||||
|
|
@ -73,17 +56,17 @@ class Object_detect(Movement):
|
|||
# "yellow": [np.array([22, 93, 0]), np.array([45, 255, 245])],
|
||||
"red": [np.array([0, 43, 46]), np.array([8, 255, 255])],
|
||||
"green": [np.array([35, 43, 35]), np.array([90, 255, 255])],
|
||||
"blue": [np.array([100, 43, 46]), np.array([124, 255, 255])],
|
||||
"blue": [np.array([78, 43, 46]), np.array([110, 255, 255])],
|
||||
"cyan": [np.array([78, 43, 46]), np.array([99, 255, 255])],
|
||||
}
|
||||
|
||||
# use to calculate coord between cube and mycobot280
|
||||
|
||||
# use to calculate coord between cube and mycobot320
|
||||
# 用于计算立方体和 mycobot 之间的坐标
|
||||
self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
|
||||
# The coordinates of the grab center point relative to the mycobot280
|
||||
# The coordinates of the grab center point relative to the mycobot320
|
||||
# 抓取中心点相对于 mycobot 的坐标
|
||||
self.camera_x, self.camera_y = camera_x, camera_y
|
||||
# The coordinates of the cube relative to the mycobot280
|
||||
# The coordinates of the cube relative to the mycobot320
|
||||
# 立方体相对于 mycobot 的坐标
|
||||
self.c_x, self.c_y = 0, 0
|
||||
# The ratio of pixels to actual values
|
||||
|
|
@ -101,7 +84,7 @@ class Object_detect(Movement):
|
|||
self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
|
||||
# init a Marker
|
||||
self.marker = Marker()
|
||||
self.marker.header.frame_id = "joint1"
|
||||
self.marker.header.frame_id = "base"
|
||||
self.marker.ns = "cube"
|
||||
self.marker.type = self.marker.CUBE
|
||||
self.marker.action = self.marker.ADD
|
||||
|
|
@ -133,48 +116,46 @@ class Object_detect(Movement):
|
|||
# pump_control pi
|
||||
def gpio_status(self, flag):
|
||||
if flag:
|
||||
self.GPIO.output(20, 0)
|
||||
self.GPIO.output(21, 0)
|
||||
"""start the suction pump"""
|
||||
self.mc.set_basic_output(1, 0)
|
||||
self.mc.set_basic_output(2, 1)
|
||||
else:
|
||||
self.GPIO.output(20, 1)
|
||||
self.GPIO.output(21, 1)
|
||||
"""stop suction pump"""
|
||||
self.mc.set_basic_output(1, 1)
|
||||
self.mc.set_basic_output(2, 0)
|
||||
time.sleep(1)
|
||||
self.mc.set_basic_output(2, 1)
|
||||
|
||||
# 开启吸泵 m5
|
||||
def pump_on(self):
|
||||
# 让2号位工作
|
||||
self.mc.set_basic_output(2, 0)
|
||||
# 让5号位工作
|
||||
self.mc.set_basic_output(5, 0)
|
||||
|
||||
# 停止吸泵 m5
|
||||
def pump_off(self):
|
||||
# 让2号位停止工作
|
||||
"""Start the suction pump"""
|
||||
self.mc.set_basic_output(1, 0)
|
||||
self.mc.set_basic_output(2, 1)
|
||||
|
||||
def pump_off(self):
|
||||
"""stop suction pump m5"""
|
||||
self.mc.set_basic_output(1, 1)
|
||||
self.mc.set_basic_output(2, 0)
|
||||
time.sleep(1)
|
||||
self.mc.set_basic_output(2, 1)
|
||||
# 让5号位停止工作
|
||||
self.mc.set_basic_output(5, 1)
|
||||
|
||||
# Grasping motion
|
||||
def move(self, x, y, color):
|
||||
# send Angle to move mycobot280
|
||||
# send Angle to move mycobot320
|
||||
print(color)
|
||||
self.mc.send_angles(self.move_angles[1], 25)
|
||||
print('x,y:', round(x, 2), round(y, 2))
|
||||
self.mc.send_angles(self.move_angles[2], 50)
|
||||
time.sleep(3)
|
||||
|
||||
# send coordinates to move mycobot
|
||||
self.mc.send_coords([x, y, 170.6, 179.87, -3.78, -62.75], 25, 1) # usb :rx,ry,rz -173.3, -5.48, -57.9
|
||||
time.sleep(3)
|
||||
|
||||
# self.mc.send_coords([x, y, 150, 179.87, -3.78, -62.75], 25, 0)
|
||||
# time.sleep(3)
|
||||
self.mc.send_coords([x, y, 250, -173.84, -0.14, -74.37], 100, 1)
|
||||
time.sleep(2.5)
|
||||
|
||||
self.mc.send_coords([x, y, 103, 179.87, -3.78, -62.75], 25, 0)
|
||||
self.mc.send_coords([x, y, 150, -173.84, -0.14, -74.37], 100, 1)
|
||||
time.sleep(3)
|
||||
|
||||
# open pump
|
||||
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
|
||||
self.pump_on()
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
self.gpio_status(True)
|
||||
self.pump_on()
|
||||
time.sleep(1.5)
|
||||
|
||||
tmp = []
|
||||
|
|
@ -197,12 +178,8 @@ class Object_detect(Movement):
|
|||
time.sleep(3)
|
||||
|
||||
# close pump
|
||||
|
||||
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
|
||||
self.pump_off()
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
self.gpio_status(False)
|
||||
time.sleep(5)
|
||||
self.pump_off()
|
||||
time.sleep(6.5)
|
||||
|
||||
if color == 1:
|
||||
self.pub_marker(
|
||||
|
|
@ -228,16 +205,11 @@ class Object_detect(Movement):
|
|||
# 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
|
||||
self.move(x, y, color)
|
||||
|
||||
# init mycobot280
|
||||
# init mycobot320
|
||||
def run(self):
|
||||
if "dev" in self.robot_wio :
|
||||
self.mc = MyCobot(self.robot_wio, 115200)
|
||||
elif "dev" in self.robot_m5:
|
||||
self.mc = MyCobot(self.robot_m5, 115200)
|
||||
elif "dev" in self.robot_raspi:
|
||||
self.mc = MyCobot(self.robot_raspi, 1000000)
|
||||
if not self.raspi:
|
||||
self.pub_pump(False, self.Pin)
|
||||
if "dev" in self.robot_raspi:
|
||||
self.mc = MyCobot(self.robot_raspi, 115200)
|
||||
self.pump_off()
|
||||
self.mc.send_angles([0.61, 45.87, -92.37, -41.3, 2.02, 9.58], 20)
|
||||
time.sleep(2.5)
|
||||
|
||||
|
|
@ -292,14 +264,14 @@ class Object_detect(Movement):
|
|||
self.x2 = int(x2)
|
||||
self.y2 = int(y2)
|
||||
|
||||
# set parameters to calculate the coords between cube and mycobot280
|
||||
# set parameters to calculate the coords between cube and mycobot320
|
||||
# 设置参数以计算立方体和 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 mycobot280
|
||||
# calculate the coords between cube and mycobot320
|
||||
# 计算立方体和 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)
|
||||
|
|
@ -379,7 +351,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 mycobot280 relative to the target
|
||||
# calculate the real coordinates of mycobot320 relative to the target
|
||||
# 计算 mycobot 相对于目标的真实坐标
|
||||
|
||||
if mycolor == "yellow":
|
||||
|
|
@ -418,7 +390,7 @@ if __name__ == "__main__":
|
|||
cap.open()
|
||||
# init a class of Object_detect
|
||||
detect = Object_detect()
|
||||
# init mycobot280
|
||||
# init mycobot320
|
||||
detect.run()
|
||||
|
||||
_init_ = 20
|
||||
|
|
@ -461,7 +433,7 @@ if __name__ == "__main__":
|
|||
init_num += 1
|
||||
continue
|
||||
|
||||
# calculate params of the coords between cube and mycobot280 计算立方体和 mycobot 之间坐标的参数
|
||||
# calculate params of the coords between cube and mycobot320 计算立方体和 mycobot 之间坐标的参数
|
||||
if nparams < 10:
|
||||
if detect.get_calculate_params(frame) is None:
|
||||
cv2.imshow("figure", frame)
|
||||
|
|
@ -478,7 +450,7 @@ if __name__ == "__main__":
|
|||
continue
|
||||
elif nparams == 10:
|
||||
nparams += 1
|
||||
# calculate and set params of calculating real coord between cube and mycobot280
|
||||
# calculate and set params of calculating real coord between cube and mycobot320
|
||||
# 计算和设置计算立方体和mycobot之间真实坐标的参数
|
||||
detect.set_params(
|
||||
(detect.sum_x1+detect.sum_x2)/20.0,
|
||||
|
|
@ -496,7 +468,7 @@ if __name__ == "__main__":
|
|||
continue
|
||||
else:
|
||||
x, y = detect_result
|
||||
# calculate real coord between cube and mycobot280 计算立方体和 mycobot 之间的真实坐标
|
||||
# calculate real coord between cube and mycobot320 计算立方体和 mycobot 之间的真实坐标
|
||||
real_x, real_y = detect.get_position(x, y)
|
||||
# print('real_x',round(real_x, 3),round(real_y, 3))
|
||||
if num == 20:
|
||||
|
|
|
|||
|
|
@ -7,7 +7,6 @@ import os
|
|||
import rospy
|
||||
from visualization_msgs.msg import Marker
|
||||
from moving_utils import Movement
|
||||
import RPi.GPIO as GPIO
|
||||
|
||||
# y轴偏移量
|
||||
pump_y = -55
|
||||
|
|
@ -24,29 +23,6 @@ class Detect_marker(Movement):
|
|||
self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
|
||||
self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
|
||||
self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
|
||||
self.raspi = False
|
||||
|
||||
if "dev" in self.robot_m5:
|
||||
self.Pin = [2, 5]
|
||||
elif "dev" in self.robot_wio:
|
||||
# self.Pin = [20, 21]
|
||||
self.Pin = [2, 5]
|
||||
|
||||
# for i in self.move_coords:
|
||||
# i[2] -= 20
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
import RPi.GPIO as GPIO
|
||||
GPIO.setwarnings(False)
|
||||
self.GPIO = GPIO
|
||||
GPIO.setmode(GPIO.BCM)
|
||||
GPIO.setup(20, GPIO.OUT)
|
||||
GPIO.setup(21, GPIO.OUT)
|
||||
GPIO.output(20, 1)
|
||||
GPIO.output(21, 1)
|
||||
self.raspi = True
|
||||
if self.raspi:
|
||||
self.pub_pump(False)
|
||||
|
||||
|
||||
# Creating a Camera Object
|
||||
cap_num = 0
|
||||
|
|
@ -76,7 +52,7 @@ class Detect_marker(Movement):
|
|||
self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
|
||||
|
||||
self.marker = Marker()
|
||||
self.marker.header.frame_id = "joint1"
|
||||
self.marker.header.frame_id = "base"
|
||||
self.marker.ns = "cube"
|
||||
self.marker.type = self.marker.CUBE
|
||||
self.marker.action = self.marker.ADD
|
||||
|
|
@ -102,11 +78,15 @@ class Detect_marker(Movement):
|
|||
# 控制吸泵
|
||||
def pub_pump(self, flag):
|
||||
if flag:
|
||||
GPIO.output(20, 0)
|
||||
GPIO.output(21, 0)
|
||||
"""start the suction pump"""
|
||||
self.mc.set_basic_output(1, 0)
|
||||
self.mc.set_basic_output(2, 1)
|
||||
else:
|
||||
GPIO.output(20, 1)
|
||||
GPIO.output(21, 1)
|
||||
"""stop suction pump"""
|
||||
self.mc.set_basic_output(1, 1)
|
||||
self.mc.set_basic_output(2, 0)
|
||||
time.sleep(1)
|
||||
self.mc.set_basic_output(2, 1)
|
||||
|
||||
# Grasping motion
|
||||
def move(self, x, y, color):
|
||||
|
|
@ -114,17 +94,19 @@ class Detect_marker(Movement):
|
|||
print(color)
|
||||
|
||||
angles = [
|
||||
[0.61, 45.87, -92.37, -41.3, 2.02, 9.58], # init to point
|
||||
[18.8, -7.91, -54.49, -23.02, -0.79, -14.76],
|
||||
[0.61, 45.87, -92.37, -41.3, 89.56, 9.58], # init to point
|
||||
[18.8, -7.91, -54.49, -23.02, 89.56, -14.76],
|
||||
[17.22, -5.27, -52.47, -25.75, 89.73, -0.26],
|
||||
]
|
||||
|
||||
coords = [
|
||||
[145.6, -65.5, 285.1, 178.99, 7.67, 179.9], # 初始化点 init point
|
||||
[115.8, 177.3, 210.6, 178.06, -0.92, -6.11], # A分拣区 A sorting area
|
||||
[-6.9, 173.2, 201.5, 179.93, 0.63, 33.83], # B分拣区 B sorting area
|
||||
[238.8, -124.1, 204.3, -169.69, -5.52, -96.52], # C分拣区 C sorting area
|
||||
[132.2, -136.9, 200.8, -178.24, -3.72, -107.17], # D分拣区 D sorting area
|
||||
[145.0, -65.5, 280.1, 178.99, 7.67, -179.9], # 初始化点 init point
|
||||
[253.8, 236.8, 224.6, -170, 6.87, -77.91], # A分拣区 A sorting area
|
||||
[35.9, 235.4, 211.8, -169.33, -9.27, 88.3], # B分拣区 B sorting area
|
||||
[266.5, -219.7, 209.3, -170, -3.64, -94.62], # C分拣区 C sorting area
|
||||
[32, -228.3, 201.6, -168.07, -7.17, -92.56], # D分拣区 D sorting area
|
||||
]
|
||||
print('real_x, real_y:', round(coords[0][0] + x, 2), round(coords[0][1] + y, 2))
|
||||
|
||||
# publish marker
|
||||
self.marker.header.stamp = rospy.Time.now()
|
||||
|
|
@ -133,18 +115,13 @@ class Detect_marker(Movement):
|
|||
self.pub.publish(self.marker)
|
||||
|
||||
# send coordinates to move mycobot
|
||||
self.mc.send_angles(angles[1], 25)
|
||||
self.mc.send_angles(angles[2], 50)
|
||||
time.sleep(3)
|
||||
|
||||
#self.mc.send_coords([coords[0][0]+x, coords[0][1]+y, 240, 178.99, 5.38, -62.9], 25, 0)
|
||||
#time.sleep(2)
|
||||
self.mc.send_coords([coords[0][0]+x, coords[0][1]+y, 200, 178.99, -3.78, -62.9], 25, 0)
|
||||
self.mc.send_coords([coords[0][0] + x, coords[0][1] + y, 240, 178.99, -3.78, -62.9], 100, 1)
|
||||
time.sleep(2)
|
||||
# self.mc.send_coords([coords[0][0]+x, coords[0][1]+y, 105, 178.99, -3.78, -62.9], 25, 0)
|
||||
# time.sleep(2)
|
||||
self.mc.send_coords([coords[0][0]+x, coords[0][1]+y, 65.5, 178.99, -3.78, -62.9], 25, 0)
|
||||
|
||||
time.sleep(3.5)
|
||||
self.mc.send_coords([coords[0][0] + x, coords[0][1] + y, 100.5, 178.99, -3.78, -62.9], 100, 1)
|
||||
time.sleep(2.5)
|
||||
|
||||
# open pump
|
||||
if "dev" in self.robot_raspi:
|
||||
|
|
@ -163,13 +140,13 @@ class Detect_marker(Movement):
|
|||
self.mc.send_angles([tmp[0], -0.71, -54.49, -23.02, -0.79, tmp[5]],25) # [18.8, -7.91, -54.49, -23.02, -0.79, -14.76]
|
||||
time.sleep(3)
|
||||
|
||||
self.mc.send_coords(coords[color], 25, 1) # coords[1] 为A分拣区，coords[2] 为B分拣区, coords[3] 为C分拣区，coords[4] 为D分拣区
|
||||
time.sleep(4)
|
||||
self.mc.send_coords(coords[color], 100, 1) # coords[1] 为A分拣区，coords[2] 为B分拣区, coords[3] 为C分拣区，coords[4] 为D分拣区
|
||||
time.sleep(6.5)
|
||||
|
||||
# close pump
|
||||
if "dev" in self.robot_raspi:
|
||||
self.pub_pump(False)
|
||||
time.sleep(5)
|
||||
time.sleep(6.5)
|
||||
|
||||
# publish marker
|
||||
time.sleep(1)
|
||||
|
|
@ -179,7 +156,7 @@ class Detect_marker(Movement):
|
|||
self.pub.publish(self.marker)
|
||||
|
||||
self.mc.send_angles(angles[0], 25)
|
||||
time.sleep(2)
|
||||
time.sleep(4.5)
|
||||
|
||||
# decide whether grab cube
|
||||
def decide_move(self, x, y, color):
|
||||
|
|
@ -192,15 +169,14 @@ class Detect_marker(Movement):
|
|||
else:
|
||||
self.cache_x = self.cache_y = 0
|
||||
# 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
|
||||
self.move(x-15, y+145, color)
|
||||
self.move(x + 105, y + 130, color)
|
||||
|
||||
# init mycobot
|
||||
def init_mycobot(self):
|
||||
if "dev" in self.robot_raspi:
|
||||
self.mc = MyCobot(self.robot_raspi, 1000000)
|
||||
self.mc = MyCobot(self.robot_raspi, 115200)
|
||||
self.pub_pump(False)
|
||||
self.mc.send_angles([0.61, 45.87, -92.37, -41.3, 2.02, 9.58], 20)
|
||||
# self.mc.send_coords([135.0, -65.5, 280.1, 178.99, 5.38, -179.9], 20, 1)
|
||||
time.sleep(2.5)
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -15,7 +15,7 @@ __version__ = "1.0" # Adaptive seeed
|
|||
|
||||
class Object_detect(Movement):
|
||||
|
||||
def __init__(self, camera_x = 160, camera_y = 10):
|
||||
def __init__(self, camera_x = 263, camera_y = 0):
|
||||
# inherit the parent class
|
||||
super(Object_detect, self).__init__()
|
||||
|
||||
|
|
@ -23,16 +23,17 @@ class Object_detect(Movement):
|
|||
self.mc = None
|
||||
# 移动角度
|
||||
self.move_angles = [
|
||||
[0.61, 45.87, -92.37, -41.3, 2.02, 9.58], # init the point
|
||||
[18.8, -7.91, -54.49, -23.02, -0.79, -14.76], # point to grab
|
||||
[0.61, 45.87, -92.37, -41.3, 89.56, 9.58], # init the point
|
||||
[18.8, -7.91, -54.49, -23.02, 89.56, -14.76], # point to grab
|
||||
[17.22, -5.27, -52.47, -25.75, 89.73, -0.26],
|
||||
]
|
||||
|
||||
# 移动坐标
|
||||
self.move_coords = [
|
||||
[132.2, -136.9, 200.8, -178.24, -3.72, -107.17], # D Sorting area
|
||||
[238.8, -124.1, 204.3, -169.69, -5.52, -96.52], # C Sorting area
|
||||
[115.8, 177.3, 210.6, 178.06, -0.92, -6.11], # A Sorting area
|
||||
[-6.9, 173.2, 201.5, 179.93, 0.63, 33.83], # B Sorting area
|
||||
[32, -228.3, 201.6, -168.07, -7.17, -92.56], # D Sorting area
|
||||
[266.5, -219.7, 209.3, -170, -3.64, -94.62], # C Sorting area
|
||||
[253.8, 236.8, 224.6, -170, 6.87, -77.91], # A Sorting area
|
||||
[35.9, 235.4, 211.8, -169.33, -9.27, 88.3], # B Sorting area
|
||||
]
|
||||
|
||||
# which robot: USB* is m5; ACM* is wio; AMA* is raspi
|
||||
|
|
@ -40,28 +41,7 @@ class Object_detect(Movement):
|
|||
self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
|
||||
self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
|
||||
self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
|
||||
self.raspi = False
|
||||
if "dev" in self.robot_m5:
|
||||
self.Pin = [2, 5]
|
||||
elif "dev" in self.robot_wio:
|
||||
# self.Pin = [20, 21]
|
||||
self.Pin = [2, 5]
|
||||
|
||||
for i in self.move_coords:
|
||||
i[2] -= 20
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
import RPi.GPIO as GPIO
|
||||
GPIO.setwarnings(False)
|
||||
self.GPIO = GPIO
|
||||
GPIO.setmode(GPIO.BCM)
|
||||
GPIO.setup(20, GPIO.OUT)
|
||||
GPIO.setup(21, GPIO.OUT)
|
||||
GPIO.output(20, 1)
|
||||
GPIO.output(21, 1)
|
||||
self.raspi = True
|
||||
if self.raspi:
|
||||
self.gpio_status(False)
|
||||
|
||||
|
||||
# choose place to set cube
|
||||
self.color = 0
|
||||
# parameters to calculate camera clipping parameters
|
||||
|
|
@ -87,7 +67,7 @@ class Object_detect(Movement):
|
|||
self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
|
||||
# init a Marker
|
||||
self.marker = Marker()
|
||||
self.marker.header.frame_id = "joint1"
|
||||
self.marker.header.frame_id = "base"
|
||||
self.marker.ns = "cube"
|
||||
self.marker.type = self.marker.CUBE
|
||||
self.marker.action = self.marker.ADD
|
||||
|
|
@ -121,49 +101,44 @@ class Object_detect(Movement):
|
|||
# pump_control pi
|
||||
def gpio_status(self, flag):
|
||||
if flag:
|
||||
self.GPIO.output(20, 0)
|
||||
self.GPIO.output(21, 0)
|
||||
"""start the suction pump"""
|
||||
self.mc.set_basic_output(1, 0)
|
||||
self.mc.set_basic_output(2, 1)
|
||||
else:
|
||||
self.GPIO.output(20, 1)
|
||||
self.GPIO.output(21, 1)
|
||||
"""stop suction pump"""
|
||||
self.mc.set_basic_output(1, 1)
|
||||
self.mc.set_basic_output(2, 0)
|
||||
time.sleep(1)
|
||||
self.mc.set_basic_output(2, 1)
|
||||
|
||||
# 开启吸泵 m5
|
||||
def pump_on(self):
|
||||
# 让2号位工作
|
||||
self.mc.set_basic_output(2, 0)
|
||||
# 让5号位工作
|
||||
self.mc.set_basic_output(5, 0)
|
||||
|
||||
# 停止吸泵 m5
|
||||
def pump_off(self):
|
||||
# 让2号位停止工作
|
||||
"""Start the suction pump"""
|
||||
self.mc.set_basic_output(1, 0)
|
||||
self.mc.set_basic_output(2, 1)
|
||||
|
||||
def pump_off(self):
|
||||
"""stop suction pump m5"""
|
||||
self.mc.set_basic_output(1, 1)
|
||||
self.mc.set_basic_output(2, 0)
|
||||
time.sleep(1)
|
||||
self.mc.set_basic_output(2, 1)
|
||||
# 让5号位停止工作
|
||||
self.mc.set_basic_output(5, 1)
|
||||
|
||||
# Grasping motion
|
||||
def move(self, x, y, color):
|
||||
# send Angle to move mycobot 280
|
||||
self.mc.send_angles(self.move_angles[1], 25)
|
||||
print(color)
|
||||
print('x,y:', round(x, 2), round(y, 2))
|
||||
# send Angle to move mycobot320
|
||||
self.mc.send_angles(self.move_angles[2], 50)
|
||||
time.sleep(3)
|
||||
|
||||
# send coordinates to move mycobot
|
||||
self.mc.send_coords([x, y, 170.6, 179.87, -3.78, -62.75], 25, 1) # usb :rx,ry,rz -173.3, -5.48, -57.9
|
||||
self.mc.send_coords([x, y, 230, -173.84, -0.14, -74.37], 100, 1)
|
||||
time.sleep(2.5)
|
||||
self.mc.send_coords([x, y, 100, -173.84, -0.14, -74.37], 100, 1) #
|
||||
time.sleep(3)
|
||||
|
||||
# self.mc.send_coords([x, y, 150, 179.87, -3.78, -62.75], 25, 0)
|
||||
# time.sleep(3)
|
||||
|
||||
self.mc.send_coords([x, y, 65, 179.87, -3.78, -62.75], 25, 0)
|
||||
# self.mc.send_coords([x, y, 103, 179.87, -3.78, -62.75], 25, 0)
|
||||
|
||||
time.sleep(4)
|
||||
|
||||
# open pump
|
||||
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
|
||||
self.pump_on()
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
self.gpio_status(True)
|
||||
self.gpio_status(True)
|
||||
time.sleep(1.5)
|
||||
|
||||
tmp = []
|
||||
|
|
@ -180,19 +155,16 @@ class Object_detect(Movement):
|
|||
|
||||
|
||||
|
||||
self.mc.send_coords(self.move_coords[color], 25, 1)
|
||||
self.mc.send_coords(self.move_coords[color], 100, 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)
|
||||
time.sleep(6.5)
|
||||
|
||||
# close pump
|
||||
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
|
||||
self.pump_off()
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
self.gpio_status(False)
|
||||
time.sleep(5)
|
||||
self.gpio_status(False)
|
||||
time.sleep(6.5)
|
||||
|
||||
self.mc.send_angles(self.move_angles[0], 25)
|
||||
self.mc.send_angles(self.move_angles[0], 50)
|
||||
time.sleep(4.5)
|
||||
|
||||
# decide whether grab cube
|
||||
|
|
@ -211,15 +183,10 @@ class Object_detect(Movement):
|
|||
|
||||
# init mycobot
|
||||
def run(self):
|
||||
|
||||
if "dev" in self.robot_wio :
|
||||
self.mc = MyCobot(self.robot_wio, 115200)
|
||||
elif "dev" in self.robot_m5:
|
||||
self.mc = MyCobot(self.robot_m5, 115200)
|
||||
elif "dev" in self.robot_raspi:
|
||||
self.mc = MyCobot(self.robot_raspi, 1000000)
|
||||
if not self.raspi:
|
||||
self.pub_pump(False, self.Pin)
|
||||
|
||||
if "dev" in self.robot_raspi:
|
||||
self.mc = MyCobot(self.robot_raspi, 115200)
|
||||
self.gpio_status(False)
|
||||
self.mc.send_angles([0.61, 45.87, -92.37, -41.3, 2.02, 9.58], 20)
|
||||
time.sleep(2.5)
|
||||
|
||||
|
|
@ -336,13 +303,6 @@ class Object_detect(Movement):
|
|||
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
|
||||
|
|
@ -407,15 +367,8 @@ class Object_detect(Movement):
|
|||
# The path to save the image folder
|
||||
def parse_folder(folder):
|
||||
restore = []
|
||||
path = ''
|
||||
path1 = '/home/er/catkin_ws/src/mycobot_ros/mycobot_ai/aikit_280_pi/' + folder # pi
|
||||
path2 = r'D:/BaiduSyncdisk/PythonProject/OpenCV/' + folder # windows
|
||||
|
||||
if os.path.exists(path1):
|
||||
path = path1
|
||||
elif os.path.exists(path2):
|
||||
path = path2
|
||||
|
||||
path = '/home/er/catkin_ws/src/mycobot_ros/mycobot_ai/aikit_320_pi/' + folder # pi
|
||||
|
||||
for i, j, k in os.walk(path):
|
||||
for l in k:
|
||||
restore.append(cv2.imread(folder + '/{}'.format(l)))
|
||||
|
|
|
|||
|
|
@ -16,24 +16,25 @@ __version__ = "1.0"
|
|||
|
||||
class Object_detect(Movement):
|
||||
|
||||
def __init__(self, camera_x = 162, camera_y = 15):
|
||||
def __init__(self, camera_x = 260, camera_y = 0):
|
||||
# inherit the parent class
|
||||
super(Object_detect, self).__init__()
|
||||
# declare mycobot280
|
||||
# declare mycobot320
|
||||
self.mc = None
|
||||
|
||||
# 移动角度
|
||||
self.move_angles = [
|
||||
[0.61, 45.87, -92.37, -41.3, 2.02, 9.58], # init the point
|
||||
[18.8, -7.91, -54.49, -23.02, -0.79, -14.76], # point to grab
|
||||
[0.61, 45.87, -92.37, -41.3, 89.56, 9.58], # init the point
|
||||
[18.8, -7.91, -54.49, -23.02, 89.56, -14.76], # point to grab
|
||||
[17.22, -5.27, -52.47, -25.75, 89.73, -0.26],
|
||||
]
|
||||
|
||||
# 移动坐标
|
||||
self.move_coords = [
|
||||
[132.2, -136.9, 200.8, -178.24, -3.72, -107.17], # D Sorting area
|
||||
[238.8, -124.1, 204.3, -169.69, -5.52, -96.52], # C Sorting area
|
||||
[115.8, 177.3, 210.6, 178.06, -0.92, -6.11], # A Sorting area
|
||||
[-6.9, 173.2, 201.5, 179.93, 0.63, 33.83], # B Sorting area
|
||||
[32, -228.3, 201.6, -168.07, -7.17, -92.56], # D Sorting area
|
||||
[266.5, -219.7, 209.3, -170, -3.64, -94.62], # C Sorting area
|
||||
[253.8, 236.8, 224.6, -170, 6.87, -77.91], # A Sorting area
|
||||
[35.9, 235.4, 211.8, -169.33, -9.27, 88.3], # B Sorting area
|
||||
]
|
||||
|
||||
# which robot: USB* is m5; ACM* is wio; AMA* is raspi
|
||||
|
|
@ -41,29 +42,7 @@ class Object_detect(Movement):
|
|||
self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
|
||||
self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
|
||||
self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
|
||||
self.raspi = False
|
||||
if "dev" in self.robot_m5:
|
||||
self.Pin = [2, 5]
|
||||
elif "dev" in self.robot_wio:
|
||||
# self.Pin = [20, 21]
|
||||
self.Pin = [2, 5]
|
||||
|
||||
for i in self.move_coords:
|
||||
i[2] -= 20
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
import RPi.GPIO as GPIO
|
||||
GPIO.setwarnings(False)
|
||||
self.GPIO = GPIO
|
||||
GPIO.setmode(GPIO.BCM)
|
||||
GPIO.setup(20, GPIO.OUT)
|
||||
GPIO.setup(21, GPIO.OUT)
|
||||
GPIO.output(20, 1)
|
||||
GPIO.output(21, 1)
|
||||
self.raspi = True
|
||||
if self.raspi:
|
||||
self.gpio_status(False)
|
||||
|
||||
|
||||
|
||||
# choose place to set cube
|
||||
self.color = 0
|
||||
# parameters to calculate camera clipping parameters
|
||||
|
|
@ -92,7 +71,7 @@ class Object_detect(Movement):
|
|||
self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
|
||||
# init a Marker
|
||||
self.marker = Marker()
|
||||
self.marker.header.frame_id = "joint1"
|
||||
self.marker.header.frame_id = "base"
|
||||
self.marker.ns = "cube"
|
||||
self.marker.type = self.marker.CUBE
|
||||
self.marker.action = self.marker.ADD
|
||||
|
|
@ -124,54 +103,51 @@ class Object_detect(Movement):
|
|||
# pump_control pi
|
||||
def gpio_status(self, flag):
|
||||
if flag:
|
||||
self.GPIO.output(20, 0)
|
||||
self.GPIO.output(21, 0)
|
||||
"""start the suction pump"""
|
||||
self.mc.set_basic_output(1, 0)
|
||||
self.mc.set_basic_output(2, 1)
|
||||
else:
|
||||
self.GPIO.output(20, 1)
|
||||
self.GPIO.output(21, 1)
|
||||
|
||||
# 开启吸泵 m5
|
||||
def pump_on(self):
|
||||
# 让2号位工作
|
||||
self.mc.set_basic_output(2, 0)
|
||||
# 让5号位工作
|
||||
self.mc.set_basic_output(5, 0)
|
||||
"""stop suction pump"""
|
||||
self.mc.set_basic_output(1, 1)
|
||||
self.mc.set_basic_output(2, 0)
|
||||
time.sleep(1)
|
||||
self.mc.set_basic_output(2, 1)
|
||||
|
||||
# 停止吸泵 m5
|
||||
def pump_off(self):
|
||||
# 让2号位停止工作
|
||||
def pump_on(self):
|
||||
"""Start the suction pump"""
|
||||
self.mc.set_basic_output(1, 0)
|
||||
self.mc.set_basic_output(2, 1)
|
||||
|
||||
def pump_off(self):
|
||||
"""stop suction pump m5"""
|
||||
self.mc.set_basic_output(1, 1)
|
||||
self.mc.set_basic_output(2, 0)
|
||||
time.sleep(1)
|
||||
self.mc.set_basic_output(2, 1)
|
||||
# 让5号位停止工作
|
||||
self.mc.set_basic_output(5, 1)
|
||||
|
||||
# Grasping motion
|
||||
def move(self, x, y, color):
|
||||
# send Angle to move mycobot280
|
||||
# send Angle to move mycobot320
|
||||
print(color)
|
||||
self.mc.send_angles(self.move_angles[1], 25)
|
||||
print('x, y:', round(x, 2), round(y, 2))
|
||||
# send Angle to move mycobot320
|
||||
self.mc.send_angles(self.move_angles[2], 50)
|
||||
time.sleep(3)
|
||||
|
||||
# send coordinates to move mycobot
|
||||
self.mc.send_coords([x, y, 170.6, 179.87, -3.78, -62.75], 25, 1) # usb :rx,ry,rz -173.3, -5.48, -57.9
|
||||
self.mc.send_coords([x, y, 230, -173.84, -0.14, -74.37], 100, 1)
|
||||
time.sleep(2.5)
|
||||
self.mc.send_coords([x, y, 100, -173.84, -0.14, -74.37], 100, 1) #
|
||||
time.sleep(3)
|
||||
|
||||
# self.mc.send_coords([x, y, 150, 179.87, -3.78, -62.75], 25, 0)
|
||||
# time.sleep(3)
|
||||
|
||||
self.mc.send_coords([x, y, 65.5, 179.87, -3.78, -62.75], 25, 0)
|
||||
time.sleep(4)
|
||||
|
||||
# open pump
|
||||
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
|
||||
self.pump_on()
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
self.gpio_status(True)
|
||||
self.gpio_status(True)
|
||||
time.sleep(1.5)
|
||||
|
||||
tmp = []
|
||||
while True:
|
||||
if not tmp:
|
||||
tmp = self.mc.get_angles()
|
||||
if not tmp:
|
||||
tmp = self.mc.get_angles()
|
||||
else:
|
||||
break
|
||||
time.sleep(0.5)
|
||||
|
|
@ -181,19 +157,15 @@ class Object_detect(Movement):
|
|||
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.mc.send_coords(self.move_coords[color], 25, 1)
|
||||
self.mc.send_coords(self.move_coords[color], 100, 1)
|
||||
|
||||
self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color][1]/1000.0, self.move_coords[color][2]/1000.0)
|
||||
|
||||
time.sleep(3)
|
||||
time.sleep(6.5)
|
||||
|
||||
# close pump
|
||||
|
||||
if "dev" in self.robot_m5 or "dev" in self.robot_wio:
|
||||
self.pump_off()
|
||||
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
|
||||
self.gpio_status(False)
|
||||
time.sleep(5)
|
||||
self.gpio_status(False)
|
||||
time.sleep(6.5)
|
||||
|
||||
if color == 1:
|
||||
self.pub_marker(
|
||||
|
|
@ -219,17 +191,12 @@ class Object_detect(Movement):
|
|||
# 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
|
||||
self.move(x, y, color)
|
||||
|
||||
# init mycobot280
|
||||
# init mycobot320
|
||||
def run(self):
|
||||
|
||||
if "dev" in self.robot_wio :
|
||||
self.mc = MyCobot(self.robot_wio, 115200)
|
||||
elif "dev" in self.robot_m5:
|
||||
self.mc = MyCobot(self.robot_m5, 115200)
|
||||
elif "dev" in self.robot_raspi:
|
||||
self.mc = MyCobot(self.robot_raspi, 1000000)
|
||||
if not self.raspi:
|
||||
self.pub_pump(False, self.Pin)
|
||||
if "dev" in self.robot_raspi:
|
||||
self.mc = MyCobot(self.robot_raspi, 115200)
|
||||
self.gpio_status(False)
|
||||
self.mc.send_angles([0.61, 45.87, -92.37, -41.3, 2.02, 9.58], 20)
|
||||
time.sleep(2.5)
|
||||
|
||||
|
|
@ -284,13 +251,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 mycobot280
|
||||
# set parameters to calculate the coords between cube and mycobot320
|
||||
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 mycobot280
|
||||
# calculate the coords between cube and mycobot320
|
||||
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)
|
||||
|
||||
|
|
@ -388,12 +355,12 @@ class Object_detect(Movement):
|
|||
objectType = "Rectangle(长方形)"
|
||||
cv2.drawContours(img, [cnt], 0, (0, 0, 255), 3)
|
||||
self.color=2
|
||||
elif objCor>=5:
|
||||
elif objCor>=8:
|
||||
objectType = "Circle(圆形)"
|
||||
self.color=0
|
||||
cv2.drawContours(img, [cnt], 0, (0, 0, 255), 3)
|
||||
else:
|
||||
pass
|
||||
print('not recognized!')
|
||||
print(objectType)
|
||||
|
||||
if abs(x) + abs(y) > 0:
|
||||
|
|
@ -406,15 +373,15 @@ if __name__ == "__main__":
|
|||
|
||||
# open the camera
|
||||
cap_num = 0
|
||||
# cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
|
||||
cap = cv2.VideoCapture(cap_num)
|
||||
cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
|
||||
# cap = cv2.VideoCapture(cap_num)
|
||||
cap.set(3, 640)
|
||||
cap.set(4, 480)
|
||||
if not cap.isOpened():
|
||||
cap.open()
|
||||
# init a class of Object_detect
|
||||
detect = Object_detect()
|
||||
# init mycobot280
|
||||
# init mycobot320
|
||||
detect.run()
|
||||
|
||||
_init_ = 20
|
||||
|
|
@ -457,7 +424,7 @@ if __name__ == "__main__":
|
|||
init_num += 1
|
||||
continue
|
||||
|
||||
# calculate params of the coords between cube and mycobot280
|
||||
# calculate params of the coords between cube and mycobot320
|
||||
if nparams < 10:
|
||||
if detect.get_calculate_params(frame) is None:
|
||||
cv2.imshow("figure", frame)
|
||||
|
|
@ -474,7 +441,7 @@ if __name__ == "__main__":
|
|||
continue
|
||||
elif nparams == 10:
|
||||
nparams += 1
|
||||
# calculate and set params of calculating real coord between cube and mycobot280
|
||||
# calculate and set params of calculating real coord between cube and mycobot320
|
||||
detect.set_params(
|
||||
(detect.sum_x1+detect.sum_x2)/20.0,
|
||||
(detect.sum_y1+detect.sum_y2)/20.0,
|
||||
|
|
@ -494,7 +461,7 @@ if __name__ == "__main__":
|
|||
continue
|
||||
else:
|
||||
x, y = detect_result
|
||||
# calculate real coord between cube and mycobot280
|
||||
# calculate real coord between cube and mycobot320
|
||||
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)
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
#encoding: UTF-8
|
||||
#!/usr/bin/env python2
|
||||
#!/usr/bin/env python3
|
||||
import rospy
|
||||
import time,os
|
||||
|
||||
|
|
|
|||
1257
mycobot_description/urdf/new_320_pi/ai_kit320.dae
Normal file
1257
mycobot_description/urdf/new_320_pi/ai_kit320.dae
Normal file
File diff suppressed because one or more lines are too long
|
|
@ -4,6 +4,17 @@
|
|||
<xacro:property name="width" value=".2" />
|
||||
|
||||
|
||||
<link name="env">
|
||||
<visual>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 1.5708 3.14159-->
|
||||
<mesh filename="package://mycobot_description/urdf/new_320_pi/ai_kit320.dae"/>
|
||||
</geometry>
|
||||
<!-- <origin xyz = "0 0 -0.02 " rpy = "1.5708 0 -1.5708"/> -->
|
||||
<origin xyz = "0 0 0.0 " rpy = "0 0 0"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="base">
|
||||
<visual>
|
||||
<geometry>
|
||||
|
|
@ -181,5 +192,13 @@
|
|||
<origin xyz= "0 0.06635 0" rpy = "-1.5708 0 0"/>
|
||||
</joint>
|
||||
|
||||
<joint name="vision_joint" type="fixed">
|
||||
<axis xyz="0 0 0"/>
|
||||
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
|
||||
<parent link="base"/>
|
||||
<child link="env"/>
|
||||
<origin xyz= "0 0 0" rpy = "0 0 0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
</robot>
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue