update mira gui

This commit is contained in:
weijian 2022-09-29 11:45:09 +08:00
parent ebe619ca14
commit e7bfd91695
16 changed files with 112 additions and 149 deletions

View file

@ -1,5 +1,5 @@
#!/usr/bin/env python3
# license removed for brevity
# -*- coding: utf-8 -*-
import time
import math
@ -8,8 +8,6 @@ from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from mira_communication.srv import GetAngles
from pymycobot.mira import Mira
def talker():
rospy.loginfo("start ...")
@ -25,35 +23,39 @@ def talker():
"joint1_to_base",
"joint2_to_joint1",
"joint3_to_joint2",
# "joint4_to_joint3",
# "joint5_to_joint4",
]
joint_state_send.velocity = [0.0]
joint_state_send.velocity = [0]
joint_state_send.effort = []
# waiting util server `get_joint_angles` enable.
rospy.loginfo("wait service")
rospy.wait_for_service("get_joint_angles")
func = rospy.ServiceProxy("get_joint_angles", GetAngles)
# func = rospy.ServiceProxy("get_joint_angles", GetAngles)
while True:
try:
func = rospy.ServiceProxy("get_joint_angles", GetAngles)
break
except rospy.ServiceException as e:
print('service_error:', e)
rospy.loginfo("start loop ...")
while not rospy.is_shutdown():
# get real angles from server.
# 从服务获取真实的角度
res = func()
if res.joint_1 == res.joint_2 == res.joint_3 == 0.0:
if (res == None) or (res.joint_1 == res.joint_2 == res.joint_3 == 0.0):
continue
radians_list = [
res.joint_1 * (math.pi / 180),
res.joint_2 * (math.pi / 180),
res.joint_3 * (math.pi / 180),
0,
# res.joint_4 * (math.pi / 180),
round(res.joint_1 * (math.pi / 180), 2),
round(res.joint_2 * (math.pi / 180), 2),
round(res.joint_3 * (math.pi / 180), 2),
]
rospy.loginfo("res: {}".format(radians_list))
print('res:',res)
# publish angles. 发布角度
joint_state_send.header.stamp = rospy.Time.now()
joint_state_send.position = radians_list

View file

@ -11,7 +11,7 @@ class Window:
def __init__(self, handle):
self.win = handle
self.win.resizable(0, 0) # fixed window size 固定窗口大小
# self.model = 0
self.speed = rospy.get_param("~speed", 50)
@ -22,8 +22,8 @@ class Window:
self.connect_ser()
# Get robotic arm data. 获取机械臂数据
self.record_coords = [0, 0, 0, self.speed]
self.res_angles = [0, 0, 0, self.speed]
self.record_coords = [0.0, 0.0, 0.0, self.speed]
self.res_angles = [0.0, 0.0, 0.0, self.speed]
self.get_date()
# get screen width and height. 获取屏幕宽度和高度
@ -52,12 +52,12 @@ class Window:
)
# Gripper Switch. 夹爪开关按钮
# tk.Button(self.frmLB, text="Gripper Open", command=self.gripper_open, width=10).grid(
# row=1, column=0, sticky="w", padx=3, pady=50
# )
# tk.Button(self.frmLB, text="Gripper Close", command=self.gripper_close, width=10).grid(
# row=1, column=1, sticky="w", padx=3, pady=2
# )
tk.Button(self.frmLB, text="Gripper Open", command=self.gripper_open, width=10).grid(
row=1, column=0, sticky="w", padx=3, pady=50
)
tk.Button(self.frmLB, text="Gripper Close", command=self.gripper_close, width=10).grid(
row=1, column=1, sticky="w", padx=3, pady=2
)
def connect_ser(self):
rospy.init_node("simple_gui", anonymous=True, disable_signals=True)
@ -72,9 +72,9 @@ class Window:
self.set_coords = rospy.ServiceProxy("set_joint_coords", SetCoords)
self.get_angles_info = rospy.ServiceProxy("get_joint_angles", GetAngles)
self.set_angles = rospy.ServiceProxy("set_joint_angles", SetAngles)
# self.switch_gripper = rospy.ServiceProxy(
# "switch_gripper_status", GripperStatus
# )
self.switch_gripper = rospy.ServiceProxy(
"switch_gripper_status", GripperStatus
)
except:
print("start error ...")
exit(1)
@ -97,12 +97,10 @@ class Window:
tk.Label(self.frmLT, text="Joint 2 ").grid(row=1)
tk.Label(self.frmLT, text="Joint 3 ").grid(row=2)
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)
# Set the default value of the input box.
# 设置输入框的默认值
self.j1_default = tk.StringVar()
@ -112,8 +110,6 @@ class Window:
self.j3_default = tk.StringVar()
self.j3_default.set(self.res_angles[2])
self.x_default = tk.StringVar()
self.x_default.set(self.record_coords[0])
self.y_default = tk.StringVar()
@ -121,7 +117,6 @@ class Window:
self.z_default = tk.StringVar()
self.z_default.set(self.record_coords[2])
# joint input box. 输入框
self.J_1 = tk.Entry(self.frmLT, textvariable=self.j1_default)
self.J_1.grid(row=0, column=1, pady=3)
@ -129,8 +124,6 @@ class Window:
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)
# coord input box. 输入框
self.x = tk.Entry(self.frmRT, textvariable=self.x_default)
@ -139,15 +132,11 @@ class Window:
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)
# All input boxes, used to get the input data. 所有输入框,用于拿输入的数据
self.all_j = [self.J_1, self.J_2, self.J_3]
self.all_c = [self.x, self.y, self.z]
# speed input box. 速度输入框
tk.Label(
self.frmLB,
@ -169,7 +158,6 @@ class Window:
self.cont_3 = tk.StringVar(self.frmLC)
self.cont_3.set(str(self.res_angles[2]) + "°")
self.cont_all = [
self.cont_1,
self.cont_2,
@ -221,7 +209,6 @@ class Window:
self.coord_z = tk.StringVar()
self.coord_z.set(str(self.record_coords[2]))
self.coord_all = [
self.coord_x,
self.coord_y,
@ -261,6 +248,20 @@ class Window:
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:
# Probably because the method has no return value, the service throws an unhandled error
# 可能由于该方法没有返回值,服务抛出无法处理的错误
pass
def gripper_close(self):
try:
self.switch_gripper(False)
except ServiceException:
pass
def get_coord_input(self):
# Get the data input by coord and send it to the robotic arm
@ -302,7 +303,7 @@ class Window:
def get_date(self):
# Get the data of robotic arm for display. 拿机械臂的数据,用于展示
t = time.time()
while time.time() - t < 2:
while time.time() - t < 3:
self.res = self.get_coords_info()
print(self.res)
@ -313,31 +314,26 @@ class Window:
time.sleep(0.1)
t = time.time()
while time.time() - t < 2:
while time.time() - t < 3:
self.angles = self.get_angles_info()
# if self.angles.joint_1 > 1:
if self.angles != []:
break
time.sleep(0.1)
# print(self.angles.joint_1)
if self.res and self.angles != None:
print('---------------->',self.res)
self.record_coords = [
round(self.res.x, 2),
round(self.res.y, 2),
round(self.res.z, 2),
self.speed
]
# self.record_coords[0] = self.res[:3]
# self.res_angles[0] = self.angles[:3]
self.res_angles = [
round(self.angles.joint_1, 2),
round(self.angles.joint_2, 2),
round(self.angles.joint_3, 2),
if self.res and self.angles != None:
self.record_coords = [
round(self.res.x, 2),
round(self.res.y, 2),
round(self.res.z, 2),
self.speed,
]
]
# print('coord:',self.record_coords)
# print('angles:',self.res_angles)
self.res_angles = [
round(self.angles.joint_1, 2),
round(self.angles.joint_2, 2),
round(self.angles.joint_3, 2),
]
# def send_input(self,dates):
def show_j_date(self, date, way=""):
@ -361,10 +357,9 @@ class Window:
else:
raise
def main():
window = tk.Tk()
window.title("mycobot ros GUI")
window.title("Mira ros GUI")
Window(window).run()

View file

@ -4,7 +4,7 @@
<!-- Open communication service -->
<!-- <node name="mycobot_services" pkg="mycobot_communication" type="mycobot_topics.py" output="screen"> -->
<node name="mira_services" pkg="mira_communication" type="mira_topics.py" output="screen">
<node name="Mira_topics" pkg="mira_communication" type="mira_topics.py" output="screen">
<param name="port" type="string" value="$(arg port)" />
<param name="baud" type="int" value="$(arg baud)" />

View file

@ -1,7 +1,3 @@
# float32 base
float32 joint_1
float32 joint_2
float32 joint_3
# float32 joint_4
# float32 joint_5
# float32 joint_6

View file

@ -1,6 +1,3 @@
float32 x
float32 y
float32 z
# float32 rx
# float32 ry
# float32 rz
float32 z

View file

@ -1,3 +1,2 @@
bool Status
# int8 Pin1
# int8 Pin2

View file

@ -1,9 +1,5 @@
# float32 base
float32 joint_1
float32 joint_2
float32 joint_3
# float32 joint_4
# float32 joint_5
# float32 joint_6
int8 speed

View file

@ -1,9 +1,6 @@
float32 x
float32 y
float32 z
# float32 rx
# float32 ry
# float32 rz
int8 speed
# int8 model

View file

@ -2,9 +2,7 @@
# -*- coding: utf-8 -*-
import time
import rospy
# from mycobot_communication.srv import *
from mira_communication.srv import *
from pymycobot.mira import Mira
ma = None
@ -23,8 +21,7 @@ def create_handle():
ma.power_on()
# calibrate the zero position of the robot arm
ma.go_zero()
time.sleep(5)
# time.sleep(3)
def create_services():
@ -33,19 +30,19 @@ def create_services():
rospy.Service("set_joint_coords", SetCoords, set_coords)
rospy.Service("get_joint_coords", GetCoords, get_coords)
rospy.Service("switch_gripper_status", GripperStatus, switch_status)
# rospy.Service("switch_pump_status", PumpStatus, toggle_pump)
rospy.Service("switch_pump_status", PumpStatus, toggle_pump)
rospy.loginfo("ready")
rospy.spin()
def set_angles(req):
"""set angles"""
angles = [
req.joint_1,
req.joint_2,
req.joint_3,
]
sp = req.speed
print('mira_services:',angles)
if ma:
ma.set_angles(angles, sp)
@ -53,12 +50,16 @@ def set_angles(req):
def get_angles(req):
if ma:
angles = ma.get_angles_info()
# angles = [0.0, 0.0, 0.0, 0.0]
if angles != None:
# print('angles:',angles)
return GetAnglesResponse(*angles)
count = 0
while count < 10:
if ma:
angles = ma.get_angles_info()
if angles != None:
return GetAnglesResponse(*angles)
count += 1
continue
else:
return GetAnglesResponse(0.0, 0.0, 0.0)
def set_coords(req):
@ -68,8 +69,6 @@ def set_coords(req):
req.z,
]
sp = req.speed
# mod = req.model
print('mira_services:',coords)
if ma:
ma.set_coords(coords, sp)
@ -77,12 +76,17 @@ def set_coords(req):
def get_coords(req):
if ma:
# coords = ma.get_coords_info()
coords = [0.0, 0.0, 0.0]
if coords != None:
# print('coords:',coords)
return GetCoordsResponse(*coords)
count = 0
while count < 10:
if ma:
coords = ma.get_coords_info()
# coords = [176.0, 0.0, 120.0]
if coords != None:
return GetCoordsResponse(*coords)
count += 1
continue
else:
return GetCoordsResponse(176.0, 0.0, 120.0)
def switch_status(req):
@ -96,16 +100,14 @@ def switch_status(req):
return GripperStatusResponse(True)
# def toggle_pump(req):
# if ma:
# if req.Status:
# ma.set_basic_output(req.Pin1, 0)
# ma.set_basic_output(req.Pin2, 0)
# else:
# ma.set_basic_output(req.Pin1, 1)
# ma.set_basic_output(req.Pin2, 1)
def toggle_pump(req):
if ma:
if req.Status:
ma.set_gpio_state(1)
else:
ma.set_gpio_state(0)
# return PumpStatusResponse(True)
return PumpStatusResponse(True)
robot_msg = """
@ -116,13 +118,16 @@ Joint Limit:
joint 2: 0 ~ 90
joint 3: 0 ~ 75
Coords Limit:
x: 0 ~ 270
y: 0 ~ 270
z: 0 ~ 125
Connect Status: %s
Servo Infomation: %s
Servo Temperature: %s
Atom Version: %s
"""
@ -130,22 +135,15 @@ def output_robot_message():
connect_status = False
servo_infomation = "unknown"
servo_temperature = "unknown"
atom_version = "unknown"
if ma:
# cn = ma.is_controller_connected()
# if cn == 1:
connect_status = True
# time.sleep(0.1)
# si = ma.is_all_servo_enable()
# if si == 1:
servo_infomation = "all connected"
print(
robot_msg % (connect_status, servo_infomation,
servo_temperature, atom_version)
servo_temperature)
)
time.sleep(2)
if __name__ == "__main__":

View file

@ -74,6 +74,7 @@ class MiraTopics(object):
baud = rospy.get_param("~baud", 115200)
rospy.loginfo("%s,%s" % (port, baud))
self.mc = Mira(port, baud)
self.mc.go_zero()
self.lock = threading.Lock()
def start(self):
@ -112,7 +113,7 @@ class MiraTopics(object):
ma = MiraAngles()
while not rospy.is_shutdown():
self.lock.acquire()
angles = self.mc.get_angles()
angles = self.mc.get_angles_info()
self.lock.release()
if angles:
ma.joint_1 = angles[0]
@ -133,7 +134,7 @@ class MiraTopics(object):
while not rospy.is_shutdown():
self.lock.acquire()
coords = self.mc.get_coords()
coords = self.mc.get_coords_info()
self.lock.release()
if coords:
ma.x = coords[0]
@ -196,11 +197,9 @@ class MiraTopics(object):
def sub_pump_status(self):
def callback(data):
if data.Status:
self.mc.set_basic_output(data.Pin1, 0)
self.mc.set_basic_output(data.Pin2, 0)
self.mc.set_gpio_state(0)
else:
self.mc.set_basic_output(data.Pin1, 1)
self.mc.set_basic_output(data.Pin2, 1)
self.mc.set_gpio_state(1)
sub = rospy.Subscriber(
"Mypal/pump_status", MiraPumpStatus, callback=callback

View file

@ -8,7 +8,7 @@ import threading
import rospy
from mira_communication import (
from mira_communication.msg import (
MiraAngles,
MiraCoords,
MiraSetAngles,
@ -16,7 +16,7 @@ from mira_communication import (
MiraGripperStatus,
MiraPumpStatus,
)
from pymycobot.mypalletizer import Mypalletizer
from pymycobot.mira import Mira
class Watcher:
@ -73,7 +73,8 @@ class MiraTopics(object):
port = rospy.get_param("~port", "/dev/ttyUSB0")
baud = rospy.get_param("~baud", 115200)
rospy.loginfo("%s,%s" % (port, baud))
self.mc = Mypalletizer(port,baud)
self.mc = Mira(port,baud)
self.mc.go_zero()
self.lock = threading.Lock()
def start(self):

View file

@ -1,11 +1,9 @@
---
# float32 base
float32 joint_1
float32 joint_2
float32 joint_3
# float32 joint_4
# float32 joint_5
# float32 joint_6

View file

@ -4,6 +4,3 @@
float32 x
float32 y
float32 z
# float32 rx
# float32 ry
# float32 rz

View file

@ -1,6 +1,4 @@
bool Status
int8 Pin1
int8 Pin2
---

View file

@ -1,12 +1,6 @@
# float32 base
float32 joint_1
float32 joint_2
float32 joint_3
# float32 joint_4
# float32 joint_5
# float32 joint_6
int8 speed

View file

@ -1,12 +1,8 @@
float32 x
float32 y
float32 z
# float32 rx
# float32 ry
# float32 rz
int8 speed
# int8 model
---