update mira gui

Mira/mira/scripts/listen_real.py

@@ -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,16 +23,21 @@ 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():
@@ -42,18 +45,17 @@ def talker():
         #  从服务获取真实的角度
         
         res = func()
-        if 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),
             
+        if (res == None) or (res.joint_1 == res.joint_2 == res.joint_3 == 0.0):
+            continue
+                
+        radians_list = [
+            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

Mira/mira/scripts/simple_gui.py

@@ -22,8 +22,8 @@ class Window:
         self.connect_ser()
 
         # Get robotic arm data. 获取机械臂数据
-        self.record_coords = [0, 0, 0, self.speed]
+        self.record_coords = [0.0, 0.0, 0.0, self.speed]
-        self.res_angles = [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(
+        tk.Button(self.frmLB, text="Gripper Open", command=self.gripper_open, width=10).grid(
-        #     row=1, column=0, sticky="w", padx=3, pady=50
+            row=1, column=0, sticky="w", padx=3, pady=50
-        # )
+        )
-        # tk.Button(self.frmLB, text="Gripper Close", command=self.gripper_close, width=10).grid(
+        tk.Button(self.frmLB, text="Gripper Close", command=self.gripper_close, width=10).grid(
-        #     row=1, column=1, sticky="w", padx=3, pady=2
+            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(
+            self.switch_gripper = rospy.ServiceProxy(
-            #     "switch_gripper_status", GripperStatus
+                "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)
@@ -130,8 +125,6 @@ class Window:
         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)
         self.x.grid(row=0, column=1, pady=3, padx=0)
@@ -140,14 +133,10 @@ class Window:
         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,
@@ -262,6 +249,20 @@ class Window:
                 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 
         # 获取 coord 输入的数据，发送给机械臂
@@ -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:
+        if self.res and self.angles != None:
-                print('---------------->',self.res)
+            self.record_coords = [
-                self.record_coords = [
+                round(self.res.x, 2),
-                    round(self.res.x, 2),
+                round(self.res.y, 2),
-                    round(self.res.y, 2),
+                round(self.res.z, 2),
-                    round(self.res.z, 2),
+                self.speed,
-                    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),
     
-                ]
+            self.res_angles = [
-                # print('coord:',self.record_coords)
+                round(self.angles.joint_1, 2),
-                # print('angles:',self.res_angles)
+                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()
 
 

Mira/mira_communication/launch/communication_topic.launch

@@ -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)" />

Mira/mira_communication/msg/MiraAngles.msg

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

Mira/mira_communication/msg/MiraCoords.msg

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

Mira/mira_communication/msg/MiraPumpStatus.msg

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

Mira/mira_communication/msg/MiraSetAngles.msg

@@ -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

Mira/mira_communication/msg/MiraSetCoords.msg

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

Mira/mira_communication/scripts/mira_services.py

@@ -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(3)
-    time.sleep(5)
 
 
 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:
+    count = 0
-        angles = ma.get_angles_info()
+    while count < 10:
-        # angles = [0.0, 0.0, 0.0, 0.0]
+        if ma:
-        if angles != None:
+            angles = ma.get_angles_info()
-            # print('angles:',angles)
+            if angles != None:
-            return GetAnglesResponse(*angles)
+                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:
+    count = 0
-        # coords = ma.get_coords_info()
+    while count < 10:
-        coords = [0.0, 0.0, 0.0]
+        if ma:
-        if coords != None:
+            coords = ma.get_coords_info()
-            # print('coords:',coords)
+            # coords = [176.0, 0.0, 120.0]
-            return GetCoordsResponse(*coords)
+            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):
+def toggle_pump(req):
-#     if ma:
+    if ma:
-#         if req.Status:
+        if req.Status:
-#             ma.set_basic_output(req.Pin1, 0)
+            ma.set_gpio_state(1)
-#             ma.set_basic_output(req.Pin2, 0)
+        else:
-#         else:
+            ma.set_gpio_state(0)
-#             ma.set_basic_output(req.Pin1, 1)
-#             ma.set_basic_output(req.Pin2, 1)
 
-#     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__":

Mira/mira_communication/scripts/mira_topics.py

@@ -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_gpio_state(0)
-                self.mc.set_basic_output(data.Pin2, 0)
             else:
-                self.mc.set_basic_output(data.Pin1, 1)
+                self.mc.set_gpio_state(1)
-                self.mc.set_basic_output(data.Pin2, 1)
 
         sub = rospy.Subscriber(
             "Mypal/pump_status", MiraPumpStatus, callback=callback

Mira/mira_communication/scripts/mira_topics_seeed.py

@@ -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):

Mira/mira_communication/srv/GetAngles.srv

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

Mira/mira_communication/srv/GetCoords.srv

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

Mira/mira_communication/srv/PumpStatus.srv

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

Mira/mira_communication/srv/SetAngles.srv

@@ -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
 

Mira/mira_communication/srv/SetCoords.srv

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