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
mycobot_ros/scripts/simple_gui.py
张立军 c2d87585c5 feat: added simple gui. 
- fixed and updated `script/test.py`
2021-05-21 15:53:46 +08:00

349 lines
15 KiB
Python
Executable file
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import Tkinter as tk
from mycobot_ros.srv import (
GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus)
import rospy
import time
from rospy import ServiceException
class Window:
def __init__(self,handle):
self.win = handle
self.win.resizable(0,0) # 固定窗口大小
self.model = 0
self.speed = rospy.get_param("~speed", 50)
#设置默认速度
self.speed_d = tk.StringVar()
self.speed_d.set(str(self.speed))
# print(self.speed)
self.connect_ser()
# 获取机械臂数据
self.record_coords = [0, 0, 0, 0, 0, 0, self.speed,self.model]
self.res_angles = [0, 0, 0, 0, 0, 0, self.speed,self.model]
self.get_date()
# get screen width and height
self.ws = self.win.winfo_screenwidth() # width of the screen
self.hs = self.win.winfo_screenheight() # height of the screen
# calculate x and y coordinates for the Tk root window
x = (self.ws/2) - 190
y = (self.hs/2) - 250
self.win.geometry('430x370+{}+{}'.format(x,y))
# 布局
self.set_layout()
# 输入部分
self.need_input()
# 展示部分
self.show_init()
# joint 设置按钮
tk.Button(self.frmLT,text="设置",width=5, command=self.get_joint_input).grid(row=6, column=1, sticky="w", padx=3, pady=2)
# coordination 设置按钮
tk.Button(self.frmRT,text="设置",width=5, command=self.get_coord_input).grid(row=6, column=1, sticky="w", padx=3, pady=2)
# 夹爪开关按钮
tk.Button(self.frmLB,text="夹爪(开)",command=self.gripper_open,width=5).grid(row=1, column=0, sticky="w", padx=3, pady=50)
tk.Button(self.frmLB,text="夹爪(关)",command=self.gripper_close,width=5).grid(row=1, column=1, sticky="w", padx=3, pady=2)
def connect_ser(self):
rospy.init_node('simple_gui', anonymous=True)
rospy.wait_for_service('get_joint_angles')
rospy.wait_for_service('set_joint_angles')
rospy.wait_for_service('get_joint_coords')
rospy.wait_for_service('set_joint_coords')
rospy.wait_for_service('switch_gripper_status')
try:
self.get_coords = rospy.ServiceProxy('get_joint_coords', GetCoords)
self.set_coords = rospy.ServiceProxy('set_joint_coords', SetCoords)
self.get_angles = rospy.ServiceProxy('get_joint_angles', GetAngles)
self.set_angles = rospy.ServiceProxy('set_joint_angles', SetAngles)
self.switch_gripper = rospy.ServiceProxy(
'switch_gripper_status', GripperStatus)
except:
print('start error ...')
exit(1)
print('Connect service success.')
def set_layout(self):
self.frmLT = tk.Frame(width=200, height=200)
self.frmLC = tk.Frame(width=200, height=200)
self.frmLB = tk.Frame(width=200, height=200)
self.frmRT = tk.Frame(width=200, height=200)
self.frmLT.grid(row=0, column=0,padx=1,pady=3)
self.frmLC.grid(row=1, column=0,padx=1,pady=3)
self.frmLB.grid(row=1, column=1,padx=2,pady=3)
self.frmRT.grid(row=0, column=1,padx=2,pady=3)
def need_input(self):
# 输入提示
tk.Label(self.frmLT,text="Joint 1 ").grid(row=0)
tk.Label(self.frmLT,text="Joint 2 ").grid(row=1)#第二行
tk.Label(self.frmLT,text="Joint 3 ").grid(row=2)
tk.Label(self.frmLT,text="Joint 4 ").grid(row=3)
tk.Label(self.frmLT,text="Joint 5 ").grid(row=4)
tk.Label(self.frmLT,text="Joint 6 ").grid(row=5)
tk.Label(self.frmRT,text=" x ").grid(row=0)
tk.Label(self.frmRT,text=" y ").grid(row=1)#第二行
tk.Label(self.frmRT,text=" z ").grid(row=2)
tk.Label(self.frmRT,text=" rx ").grid(row=3)
tk.Label(self.frmRT,text=" ry ").grid(row=4)
tk.Label(self.frmRT,text=" rz ").grid(row=5)
# 设置输入框的默认值
self.j1_default = tk.StringVar()
self.j1_default.set(self.res_angles[0])
self.j2_default = tk.StringVar()
self.j2_default.set(self.res_angles[1])
self.j3_default = tk.StringVar()
self.j3_default.set(self.res_angles[2])
self.j4_default = tk.StringVar()
self.j4_default.set(self.res_angles[3])
self.j5_default = tk.StringVar()
self.j5_default.set(self.res_angles[4])
self.j6_default = tk.StringVar()
self.j6_default.set(self.res_angles[5])
self.x_default = tk.StringVar()
self.x_default.set(self.record_coords[0])
self.y_default = tk.StringVar()
self.y_default.set(self.record_coords[1])
self.z_default = tk.StringVar()
self.z_default.set(self.record_coords[2])
self.rx_default = tk.StringVar()
self.rx_default.set(self.record_coords[3])
self.ry_default = tk.StringVar()
self.ry_default.set(self.record_coords[4])
self.rz_default = tk.StringVar()
self.rz_default.set(self.record_coords[5])
# joint 输入框
self.J_1 = tk.Entry(self.frmLT, textvariable=self.j1_default)
self.J_1.grid(row=0,column=1,pady=3)
self.J_2 = tk.Entry(self.frmLT, textvariable=self.j2_default)
self.J_2.grid(row=1,column=1,pady=3)
self.J_3 = tk.Entry(self.frmLT, textvariable=self.j3_default)
self.J_3.grid(row=2,column=1,pady=3)
self.J_4 = tk.Entry(self.frmLT, textvariable=self.j4_default)
self.J_4.grid(row=3,column=1,pady=3)
self.J_5 = tk.Entry(self.frmLT, textvariable=self.j5_default)
self.J_5.grid(row=4,column=1,pady=3)
self.J_6 = tk.Entry(self.frmLT, textvariable=self.j6_default)
self.J_6.grid(row=5,column=1,pady=3)
# coord 输入框
self.x = tk.Entry(self.frmRT, textvariable=self.x_default)
self.x.grid(row=0,column=1,pady=3,padx=0)
self.y = tk.Entry(self.frmRT, textvariable=self.y_default)
self.y.grid(row=1,column=1,pady=3)
self.z = tk.Entry(self.frmRT, textvariable=self.z_default)
self.z.grid(row=2,column=1,pady=3)
self.rx = tk.Entry(self.frmRT, textvariable=self.rx_default)
self.rx.grid(row=3,column=1,pady=3)
self.ry = tk.Entry(self.frmRT, textvariable=self.ry_default)
self.ry.grid(row=4,column=1,pady=3)
self.rz = tk.Entry(self.frmRT, textvariable=self.rz_default)
self.rz.grid(row=5,column=1,pady=3)
# 所有输入框，用于拿输入的数据
self.all_j = [self.J_1,self.J_2,self.J_3,self.J_4,self.J_5,self.J_6]
self.all_c = [self.x,self.y,self.z,self.rx,self.ry,self.rz]
# 速度输入框
tk.Label(self.frmLB,text="speed",).grid(row=0,column=0)
self.get_speed = tk.Entry(self.frmLB, textvariable=self.speed_d,width=10)
self.get_speed.grid(row=0,column=1)
def show_init(self):
# 显示
tk.Label(self.frmLC,text="Joint 1 ").grid(row=0)
tk.Label(self.frmLC,text="Joint 2 ").grid(row=1)#第二行
tk.Label(self.frmLC,text="Joint 3 ").grid(row=2)
tk.Label(self.frmLC,text="Joint 4 ").grid(row=3)
tk.Label(self.frmLC,text="Joint 5 ").grid(row=4)
tk.Label(self.frmLC,text="Joint 6 ").grid(row=5)
# get数据
# ，展示出来
self.cont_1 = tk.StringVar(self.frmLC)
self.cont_1.set(str(self.res_angles[0])+"°")
self.cont_2 = tk.StringVar(self.frmLC)
self.cont_2.set(str(self.res_angles[1])+"°")
self.cont_3 = tk.StringVar(self.frmLC)
self.cont_3.set(str(self.res_angles[2])+"°")
self.cont_4 = tk.StringVar(self.frmLC)
self.cont_4.set(str(self.res_angles[3])+"°")
self.cont_5 = tk.StringVar(self.frmLC)
self.cont_5.set(str(self.res_angles[4])+"°")
self.cont_6 = tk.StringVar(self.frmLC)
self.cont_6.set(str(self.res_angles[5])+"°")
self.cont_all = [self.cont_1,self.cont_2,self.cont_3,self.cont_4,self.cont_5,self.cont_6,self.speed,self.model]
self.show_j1 = tk.Label(self.frmLC,
textvariable=self.cont_1,
font=('Arial',9),width=7,height=1,bg='white').grid(row=0,column=1,padx=0,pady=5)
self.show_j2 = tk.Label(self.frmLC,
textvariable=self.cont_2,
font=('Arial',9),width=7,height=1,bg='white').grid(row=1,column=1,padx=0,pady=5)
self.show_j3 = tk.Label(self.frmLC,
textvariable=self.cont_3,
font=('Arial',9),width=7,height=1,bg='white').grid(row=2,column=1,padx=0,pady=5)
self.show_j4 = tk.Label(self.frmLC,
textvariable=self.cont_4,
font=('Arial',9),width=7,height=1,bg='white').grid(row=3,column=1,padx=0,pady=5)
self.show_j5 = tk.Label(self.frmLC,
textvariable=self.cont_5,
font=('Arial',9),width=7,height=1,bg='white').grid(row=4,column=1,padx=0,pady=5)
self.show_j6 = tk.Label(self.frmLC,
textvariable=self.cont_6,
font=('Arial',9),width=7,height=1,bg='white').grid(row=5,column=1,padx=5,pady=5)
self.all_jo = [self.show_j1,self.show_j2,self.show_j3,self.show_j4,self.show_j5,self.show_j6]
# 显示
tk.Label(self.frmLC,text=" x ").grid(row=0,column=3)
tk.Label(self.frmLC,text=" y ").grid(row=1,column=3)#第二行
tk.Label(self.frmLC,text=" z ").grid(row=2,column=3)
tk.Label(self.frmLC,text=" rx ").grid(row=3,column=3)
tk.Label(self.frmLC,text=" ry ").grid(row=4,column=3)
tk.Label(self.frmLC,text=" rz ").grid(row=5,column=3)
self.coord_x = tk.StringVar()
self.coord_x.set(str(self.record_coords[0]))
self.coord_y = tk.StringVar()
self.coord_y.set(str(self.record_coords[1]))
self.coord_z = tk.StringVar()
self.coord_z.set(str(self.record_coords[2]))
self.coord_rx = tk.StringVar()
self.coord_rx.set(str(self.record_coords[3]))
self.coord_ry = tk.StringVar()
self.coord_ry.set(str(self.record_coords[4]))
self.coord_rz = tk.StringVar()
self.coord_rz.set(str(self.record_coords[5]))
self.coord_all = [self.coord_x,self.coord_y,self.coord_z,self.coord_rx,self.coord_ry,self.coord_rz,self.speed,self.model]
self.show_x = tk.Label(self.frmLC,
textvariable=self.coord_x,
font=('Arial',9),width=7,height=1,bg='white').grid(row=0,column=4,padx=5,pady=5)
self.show_y = tk.Label(self.frmLC,
textvariable=self.coord_y,
font=('Arial',9),width=7,height=1,bg='white').grid(row=1,column=4,padx=5,pady=5)
self.show_z = tk.Label(self.frmLC,
textvariable=self.coord_z,
font=('Arial',9),width=7,height=1,bg='white').grid(row=2,column=4,padx=5,pady=5)
self.show_rx = tk.Label(self.frmLC,
textvariable=self.coord_rx,
font=('Arial',9),width=7,height=1,bg='white').grid(row=3,column=4,padx=5,pady=5)
self.show_ry = tk.Label(self.frmLC,
textvariable=self.coord_ry,
font=('Arial',9),width=7,height=1,bg='white').grid(row=4,column=4,padx=5,pady=5)
self.show_rz = tk.Label(self.frmLC,
textvariable=self.coord_rz,
font=('Arial',9),width=7,height=1,bg='white').grid(row=5,column=4,padx=5,pady=5)
# mm 单位展示
self.unit = tk.StringVar()
self.unit.set('mm')
for i in range(6):
tk.Label(self.frmLC,textvariable=self.unit,font=('Arial',9)).grid(row=i,column=5)
def gripper_open(self):
try:
self.switch_gripper(True)
except ServiceException:
# 可能由于该方法没有返回值，服务抛出无法处理的错误
pass
def gripper_close(self):
try:
self.switch_gripper(False)
except ServiceException:
pass
def get_coord_input(self):
# 获取 coord 输入的数据，发送给机械臂
c_value = []
for i in self.all_c:
# print(type(i.get()))
c_value.append(float(i.get()))
self.speed = int(float(self.get_speed.get())) if self.get_speed.get() else self.speed
c_value.append(self.speed)
c_value.append(self.model)
# print(c_value)
try:
self.set_coords(*c_value)
except ServiceException:
pass
self.show_j_date(c_value[:-2],'coord')
def get_joint_input(self):
# 获取joint输入的数据，发送给机械臂
j_value = []
for i in self.all_j:
# print(type(i.get()))
j_value.append(float(i.get()))
self.speed = int(float(self.get_speed.get())) if self.get_speed.get() else self.speed
j_value.append(self.speed)
try:
self.set_angles(*j_value)
except ServiceException:
pass
self.show_j_date(j_value[:-1])
# return j_value,c_value,speed
def get_date(self):
# 拿机械臂的数据，用于展示
t = time.time()
while time.time() - t < 2:
self.res = self.get_coords()
if self.res.x > 1:
break
time.sleep(.1)
t = time.time()
while time.time() - t < 2:
self.angles = self.get_angles()
if self.angles.joint_1 > 1:
break
time.sleep(.1)
# print(self.angles.joint_1)
self.record_coords = [
round(self.res.x,2), round(self.res.y,2), round(self.res.z,2), round(self.res.rx,2), round(self.res.ry,2), round(self.res.rz,2), self.speed, self.model
]
self.res_angles = [round(self.angles.joint_1,2),round(self.angles.joint_2,2),round(self.angles.joint_3,2),round(self.angles.joint_4,2),round(self.angles.joint_5,2),round(self.angles.joint_6,2)]
# print('coord:',self.record_coords)
# print('angles:',self.res_angles)
# def send_input(self,dates):
def show_j_date(self,date,way=""):
# 展示数据
if way == "coord":
for i,j in zip(date,self.coord_all):
# print(i)
j.set(str(i))
else:
for i,j in zip(date,self.cont_all):
j.set(str(i)+"°")
def run(self):
self.win.mainloop()
def main():
window = tk.Tk()
window.title('mycobot ros GUI')
Window(window).run()
if __name__ == "__main__":
main()
Reference in a new issue View git blame Copy permalink