update and fix mercury_a1 ros

Mercury/mercury_a1/scripts/follow_display.py

@@ -60,6 +60,7 @@ def talker():
     while not rospy.is_shutdown():
         joint_state_send.header.stamp = rospy.Time.now()
 
+        try:
             angles = mc.get_angles()
             data_list = []
             for index, value in enumerate(angles):
@@ -96,6 +97,8 @@ def talker():
             pub_marker.publish(marker_)
 
             rate.sleep()
+        except Exception as e:
+            pass
 
 
 if __name__ == "__main__":

Mercury/mercury_a1/scripts/listen_real.py

@@ -39,6 +39,7 @@ def talker():
 
     rospy.loginfo("start loop ...")
     while not rospy.is_shutdown():
+        try:
             # get real angles from server.从服务器获得真实的角度。
             res = func()
             if res.joint_1 == res.joint_2 == res.joint_3 == 0.0:
@@ -59,6 +60,8 @@ def talker():
             joint_state_send.position = radians_list
             pub.publish(joint_state_send)
             rate.sleep()
+        except Exception as e:
+            pass
 
 
 if __name__ == "__main__":

Mercury/mercury_a1/scripts/teleop_keyboard.py

@@ -83,6 +83,16 @@ def teleop_keyboard():
 
     rsp = set_angles(*home_pose)
 
+    # while True:
+    #     res = get_coords()
+    #     if res.x > 1:
+    #         break
+    #     time.sleep(0.1)
+
+    # record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
+    # print('init_coords:', record_coords)
+
+    try:
         while True:
             res = get_coords()
             if res.x > 1:
@@ -91,8 +101,6 @@ def teleop_keyboard():
 
         record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
         print('init_coords:', record_coords)
-
-    try:
         print(msg)
         print(vels(speed, change_percent))
         # Keyboard keys call different motion functions. 键盘按键调用不同的运动功能

Mercury/mercury_a1_communication/scripts/mercury_services.py

@@ -95,11 +95,14 @@ def set_angles(req):
 
 def get_angles(req):
     """get angles,获取角度"""
+    try:
         if mc:
             lock = acquire("/tmp/mercury_lock")
             angles = mc.get_angles()
             release(lock)
             return GetAnglesResponse(*angles)
+    except Exception as e:
+        pass
 
 
 def set_coords(req):
@@ -123,11 +126,14 @@ def set_coords(req):
 
 
 def get_coords(req):
+    try:
         if mc:
             lock = acquire("/tmp/mercury_lock")
             coords = mc.get_coords()
             release(lock)
             return GetCoordsResponse(*coords)
+    except Exception as e:
+        pass
 
 
 def switch_status(req):