update and fix mercury_a1 ros

elephantrobotics · Jan 11, 2024 · a12eacc · a12eacc
1 parent ea1af4e
commit a12eacc
Show file tree

Hide file tree

Showing 4 changed files with 92 additions and 72 deletions.
diff --git a/Mercury/mercury_a1/scripts/follow_display.py b/Mercury/mercury_a1/scripts/follow_display.py
@@ -60,42 +60,45 @@ def talker():
     while not rospy.is_shutdown():
         joint_state_send.header.stamp = rospy.Time.now()
 
-        angles = mc.get_angles()
-        data_list = []
-        for index, value in enumerate(angles):
-            radians = math.radians(value)
-            data_list.append(radians)
-
-        # rospy.loginfo('{}'.format(data_list))
-        joint_state_send.position = data_list
-
-        pub.publish(joint_state_send)
-
-        coords = mc.get_coords()
-
-        # marker
-        marker_.header.stamp = rospy.Time.now()
-        marker_.type = marker_.SPHERE
-        marker_.action = marker_.ADD
-        marker_.scale.x = 0.04
-        marker_.scale.y = 0.04
-        marker_.scale.z = 0.04
-
-        # marker position initial.标记位置初始
-        # print(coords)
-        if not coords:
-            coords = [0, 0, 0, 0, 0, 0]
-            rospy.loginfo("error [101]: can not get coord values")
-
-        marker_.pose.position.x = coords[1] / 1000 * -1
-        marker_.pose.position.y = coords[0] / 1000
-        marker_.pose.position.z = coords[2] / 1000
-
-        marker_.color.a = 1.0
-        marker_.color.g = 1.0
-        pub_marker.publish(marker_)
-
-        rate.sleep()
+        try:
+            angles = mc.get_angles()
+            data_list = []
+            for index, value in enumerate(angles):
+                radians = math.radians(value)
+                data_list.append(radians)
+
+            # rospy.loginfo('{}'.format(data_list))
+            joint_state_send.position = data_list
+
+            pub.publish(joint_state_send)
+
+            coords = mc.get_coords()
+
+            # marker
+            marker_.header.stamp = rospy.Time.now()
+            marker_.type = marker_.SPHERE
+            marker_.action = marker_.ADD
+            marker_.scale.x = 0.04
+            marker_.scale.y = 0.04
+            marker_.scale.z = 0.04
+
+            # marker position initial.标记位置初始
+            # print(coords)
+            if not coords:
+                coords = [0, 0, 0, 0, 0, 0]
+                rospy.loginfo("error [101]: can not get coord values")
+
+            marker_.pose.position.x = coords[1] / 1000 * -1
+            marker_.pose.position.y = coords[0] / 1000
+            marker_.pose.position.z = coords[2] / 1000
+
+            marker_.color.a = 1.0
+            marker_.color.g = 1.0
+            pub_marker.publish(marker_)
+
+            rate.sleep()
+        except Exception as e:
+            pass
 
 
 if __name__ == "__main__":

diff --git a/Mercury/mercury_a1/scripts/listen_real.py b/Mercury/mercury_a1/scripts/listen_real.py
@@ -39,26 +39,29 @@ def talker():
 
     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:
-            continue
-        radians_list = [
-            res.joint_1 * (math.pi / 180),
-            res.joint_2 * (math.pi / 180),
-            res.joint_3 * (math.pi / 180),
-            res.joint_4 * (math.pi / 180),
-            res.joint_5 * (math.pi / 180),
-            res.joint_6 * (math.pi / 180),
-            res.joint_7 * (math.pi / 180),
-        ]
-        rospy.loginfo("res: {}".format(radians_list))
+        try:
+            # get real angles from server.从服务器获得真实的角度。
+            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),
+                res.joint_4 * (math.pi / 180),
+                res.joint_5 * (math.pi / 180),
+                res.joint_6 * (math.pi / 180),
+                res.joint_7 * (math.pi / 180),
+            ]
+            rospy.loginfo("res: {}".format(radians_list))
 
-        # publish angles.发布角度
-        joint_state_send.header.stamp = rospy.Time.now()
-        joint_state_send.position = radians_list
-        pub.publish(joint_state_send)
-        rate.sleep()
+            # publish angles.发布角度
+            joint_state_send.header.stamp = rospy.Time.now()
+            joint_state_send.position = radians_list
+            pub.publish(joint_state_send)
+            rate.sleep()
+        except Exception as e:
+            pass
 
 
 if __name__ == "__main__":

diff --git a/Mercury/mercury_a1/scripts/teleop_keyboard.py b/Mercury/mercury_a1/scripts/teleop_keyboard.py
@@ -83,16 +83,24 @@ def teleop_keyboard():
 
     rsp = set_angles(*home_pose)
 
-    while True:
-        res = get_coords()
-        if res.x > 1:
-            break
-        time.sleep(0.1)
+    # 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)
+    # 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:
+                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)
         print(msg)
         print(vels(speed, change_percent))
         # Keyboard keys call different motion functions. 键盘按键调用不同的运动功能

diff --git a/Mercury/mercury_a1_communication/scripts/mercury_services.py b/Mercury/mercury_a1_communication/scripts/mercury_services.py
@@ -95,11 +95,14 @@ def set_angles(req):
 
 def get_angles(req):
     """get angles,获取角度"""
-    if mc:
-        lock = acquire("/tmp/mercury_lock")
-        angles = mc.get_angles()
-        release(lock)
-        return GetAnglesResponse(*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):
-    if mc:
-        lock = acquire("/tmp/mercury_lock")
-        coords = mc.get_coords()
-        release(lock)
-        return GetCoordsResponse(*coords)
+    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):