mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
feat: added simple gui.
- fixed and updated `script/test.py`
This commit is contained in:
张立军
parent
5b7df9b7c2
commit
c2d87585c5
5 changed files with 379 additions and 3 deletions
|
|
@ -6,4 +6,8 @@
|
|||
- Specification file nmae.
|
||||
- Added teleop keyboard control.
|
||||
- Intergrated MoveIt.
|
||||
- Update sync moveit plan script.
|
||||
- Update sync moveit plan script.
|
||||
|
||||
## 05.21
|
||||
|
||||
- Feature: simple gui.
|
||||
|
|
@ -49,6 +49,7 @@ catkin_install_python(PROGRAMS
|
|||
scripts/client.py
|
||||
scripts/detect_marker.py
|
||||
scripts/following_marker.py
|
||||
scripts/simple_gui.py
|
||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
|
|
|
|||
22
launch/mycobot_simple_gui.launch
Normal file
22
launch/mycobot_simple_gui.launch
Normal file
|
|
@ -0,0 +1,22 @@
|
|||
<launch>
|
||||
<arg name="port" default="/dev/ttyUSB0" />
|
||||
<arg name="baud" default="115200" />
|
||||
|
||||
<arg name="model" default="$(find mycobot_ros)/urdf/mycobot_urdf.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_ros)/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" />
|
||||
<!-- Show in Rviz -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
|
||||
<include file="$(find mycobot_ros)/launch/communication_service.launch">
|
||||
<arg name="port" value="$(arg port)" />
|
||||
<arg name="baud" value="$(arg baud)" />
|
||||
</include>
|
||||
<node name="real_listener" pkg="mycobot_ros" type="listen_real.py" />
|
||||
<node name="simple_gui" pkg="mycobot_ros" type="simple_gui.py" />
|
||||
</launch>
|
||||
349
scripts/simple_gui.py
Executable file
349
scripts/simple_gui.py
Executable file
|
|
@ -0,0 +1,349 @@
|
|||
#!/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()
|
||||
|
|
@ -52,7 +52,7 @@ if __name__ == '__main__':
|
|||
print('==> send coord id: X, coord value: -40, speed: 70\n')
|
||||
time.sleep(2)
|
||||
|
||||
print('::set_free_mode()')
|
||||
mycobot.set_free_mode()
|
||||
print('::release_all_servos()')
|
||||
mycobot.release_all_servos()
|
||||
print('==> into free moving mode.')
|
||||
print('=== check end <==\n')
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue