update mercury ros

elephantrobotics · Dec 13, 2023 · 9f87b2c · 9f87b2c
1 parent 84b01b4
commit 9f87b2c
Show file tree

Hide file tree

Showing 8 changed files with 121 additions and 125 deletions.
diff --git a/Mercury/mercury_a1/scripts/slider_control.py b/Mercury/mercury_a1/scripts/slider_control.py
@@ -43,7 +43,7 @@ def listener():
     print(port, baud)
     mc = Mercury(port, baud)
     time.sleep(0.05)
-    mc.set_free_mode(1)
+    mc.set_fresh_mode(1)
     time.sleep(0.05)
 
     # spin() simply keeps python from exiting until this node is stopped

diff --git a/Mercury/mercury_a1_communication/scripts/mercury_services.py b/Mercury/mercury_a1_communication/scripts/mercury_services.py
@@ -57,7 +57,9 @@ def create_handle():
     baud = rospy.get_param("~baud")
     rospy.loginfo("%s,%s" % (port, baud))
     mc = Mercury(port, baud)
-
+    time.sleep(0.05)
+    mc.set_fresh_mode(1)
+    time.sleep(0.05)
 
 def create_services():
     rospy.Service("set_joint_angles", SetAngles, set_angles)

diff --git a/Mercury/mercury_a1_moveit/scripts/sync_plan.py b/Mercury/mercury_a1_moveit/scripts/sync_plan.py
@@ -9,6 +9,7 @@
     port: serial prot string. Defaults is '/dev/ttyAMA1'
     baud: serial prot baudrate. Defaults is 115200.
 """
+import time
 import math
 import rospy
 from sensor_msgs.msg import JointState
@@ -39,7 +40,9 @@ def listener():
     baud = rospy.get_param("~baud", 115200)
     print(port, baud)
     mc = Mercury(port, baud)
-
+    time.sleep(0.05)
+    mc.set_fresh_mode(1)
+    time.sleep(0.05)
     # spin() simply keeps python from exiting until this node is stopped
     # spin()只是阻止python退出，直到该节点停止
     print("spin ...")

diff --git a/Mercury/mercury_b1/config/mercury_b1.rviz b/Mercury/mercury_b1/config/mercury_b1.rviz
@@ -289,9 +289,9 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.4097958207130432
+      Pitch: 0.3747957646846771
       Target Frame: <Fixed Frame>
-      Yaw: 6.260410308837891
+      Yaw: 0.002224991098046303
     Saved: ~
 Window Geometry:
   Displays:

diff --git a/Mercury/mercury_b1/scripts/follow_display.py b/Mercury/mercury_b1/scripts/follow_display.py
@@ -1,6 +1,7 @@
 #!/usr/bin/env python3
 import time
 import math
+import traceback
 import rospy
 from sensor_msgs.msg import JointState
 from std_msgs.msg import Header
@@ -16,12 +17,11 @@ def talker():
     port1 = rospy.get_param("~port1", "/dev/ttyTHS1")
     port2 = rospy.get_param("~port2", "/dev/ttyS0")
     baud = rospy.get_param("~baud", 115200)
-    print("port1: {}, baud: {}\n".format(port1, baud))
-    print("port2: {}, baud: {}\n".format(port2, baud))
+    print("left arm: {}, baud: {}\n".format(port1, baud))
+    print("right arm: {}, baud: {}\n".format(port2, baud))
     try:
         # left arm
         l = Mercury(port1, baud)
-        time.sleep(0.02)
         # right arm
         r = Mercury(port2, baud)
     except Exception as e:
@@ -35,9 +35,9 @@ def talker():
         )
         exit(1)
     l.release_all_servos()
-    time.sleep(0.02)
+    time.sleep(0.05)
     r.release_all_servos()
-    time.sleep(0.1)
+    time.sleep(0.05)
     print("Rlease all servos over.\n")
 
     pub = rospy.Publisher("joint_states", JointState, queue_size=10)
@@ -78,82 +78,74 @@ def talker():
     print("publishing ...")
     while not rospy.is_shutdown():
         joint_state_send.header.stamp = rospy.Time.now()
-
-        left_angles = l.get_angles()
-        right_angles = r.get_angles()
-        eye_angle = r.get_angle(11)
-        head_angle = r.get_angle(12)
-        body_angle = r.get_angle(13)
-
-        print('left:', left_angles)
-        print('right:', right_angles)
-        print('eye:', eye_angle)
-        print('head:', head_angle)
-        print('body:', body_angle)
-
-        all_angles = left_angles + right_angles + eye_angle + head_angle + body_angle
-        data_list = []
-        for index, value in enumerate(all_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)
-
-        left_coords = l.get_coords()
-
-        right_coords = r.get_coords()
-
-        eye_coords = r.get_angle(11)
-
-        head_coords = r.get_angle(12)
-
-        body_coords = r.get_angle(13)
-
-
-        # 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 = left_coords[1] / 1000 * -1
-        marker_.pose.position.y = left_coords[0] / 1000
-        marker_.pose.position.z = left_coords[2] / 1000
-
-        time.sleep(0.02)
-
-        marker_.pose.position.x = right_coords[1] / 1000 * -1
-        marker_.pose.position.y = right_coords[0] / 1000
-        marker_.pose.position.z = right_coords[2] / 1000
-
-        time.sleep(0.02)
-
-        marker_.pose.position.x = eye_coords[0] / 1000 * -1
-
-        time.sleep(0.02)
-
-        marker_.pose.position.x = head_coords[0] / 1000 * -1
-
-        time.sleep(0.02)
-
-        marker_.pose.position.x = body_coords[0] / 1000 * -1
-
-        marker_.color.a = 1.0
-        marker_.color.g = 1.0
-        pub_marker.publish(marker_)
-
-        rate.sleep()
+        try:
+            left_angles = l.get_angles()
+            right_angles = r.get_angles()
+            eye_angle = r.get_angle(11)
+            head_angle = r.get_angle(12)
+            body_angle = r.get_angle(13)
+
+            print('left: {}'.format(left_angles))
+            print('right: {}'.format(right_angles))
+            print('eye: {}'.format(eye_angle))
+            print('head: {}'.format(head_angle))
+            print('body: {}'.format(body_angle))
+
+            all_angles = left_angles + right_angles + [eye_angle] + [head_angle] + [body_angle]
+            data_list = []
+            for index, value in enumerate(all_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)
+
+            left_coords = l.get_coords()
+
+            right_coords = r.get_coords()
+
+            eye_coords = r.get_angle(11)
+
+            head_coords = r.get_angle(12)
+
+            body_coords = r.get_angle(13)
+
+            # 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 left_coords:
+                left_coords = [0, 0, 0, 0, 0, 0]
+                rospy.loginfo("error [101]: can not get coord values")
+
+            marker_.pose.position.x = left_coords[1] / 1000 * -1
+            marker_.pose.position.y = left_coords[0] / 1000
+            marker_.pose.position.z = left_coords[2] / 1000
+
+            marker_.pose.position.x = right_coords[1] / 1000 * -1
+            marker_.pose.position.y = right_coords[0] / 1000
+            marker_.pose.position.z = right_coords[2] / 1000
+
+            marker_.pose.position.x = eye_coords[0] / 1000 * -1
+            marker_.pose.position.x = head_coords[0] / 1000 * -1
+            marker_.pose.position.x = body_coords[0] / 1000 * -1
+
+            marker_.color.a = 1.0
+            marker_.color.g = 1.0
+            pub_marker.publish(marker_)
+
+            rate.sleep()
+        except Exception as e:
+            e = traceback.format_exc()
+            print(str(e))
 
 
 if __name__ == "__main__":

diff --git a/Mercury/mercury_b1/scripts/slider_control.py b/Mercury/mercury_b1/scripts/slider_control.py
@@ -5,8 +5,9 @@
 and then sends it directly to the real manipulator using `pymycobot` API.
 This file is [slider_control.launch] related script.
 Passable parameters:
-    port: serial prot string. Defaults is '/dev/ttyAMA1'
-    baud: serial prot baudrate. Defaults is 115200.
+      port1: Left arm serial port string. Default is "/dev/ttyTHS1"
+      port2: Right arm serial port string. Default is "/dev/ttyS0"
+      Baud rate: Left and right arm serial port baud rate. The default value is 115200.
 """
 import math
 import time
@@ -15,10 +16,10 @@
 
 from pymycobot.mercury import Mercury
 
-# left arm port
+# left arm
 l = None
 
-# right arm port
+# right arm
 r = None
 
 
@@ -29,14 +30,15 @@ def callback(data):
     for index, value in enumerate(data.position):
         radians_to_angles = round(math.degrees(value), 2)
         data_list.append(radians_to_angles)
-    print('data_list:', data_list)
+
+    print('data_list: {}'.format(data_list))
     left_arm = data_list[:7]
     right_arm = data_list[7:-3]
     middle_arm = data_list[-3:]
 
-    print('left_arm:', left_arm)
-    print('right_arm:', right_arm)
-    print('middle_arm:', middle_arm)
+    print('left_angles: {}'.format(left_arm))
+    print('right_angles: {}'.format(right_arm))
+    print('middle_arm: {}'.format(middle_arm))
 
     l.send_angles(left_arm, 25)
     time.sleep(0.02)
@@ -58,13 +60,13 @@ def listener():
     port1 = rospy.get_param("~port1", "/dev/ttyTHS1")
     port2 = rospy.get_param("~port2", "/dev/ttyS0")
     baud = rospy.get_param("~baud", 115200)
-    print(port1, baud)
-    print(port2, baud)
+    print('left arm: {}, {}'.format(port1, baud))
+    print('right arm: {}, {}'.format(port2, baud))
     l = Mercury(port1, baud)
     r = Mercury(port2, baud)
     time.sleep(0.05)
-    l.set_free_mode(1)
-    r.set_free_mode(1)
+    l.set_fresh_mode(1)
+    r.set_fresh_mode(1)
     time.sleep(0.05)
     # spin() simply keeps python from exiting until this node is stopped
     # spin()只是阻止python退出，直到该节点停止

diff --git a/Mercury/mercury_b1_moveit/scripts/sync_plan.py b/Mercury/mercury_b1_moveit/scripts/sync_plan.py
@@ -6,8 +6,9 @@
 and then sends it directly to the real manipulator using `pymycobot` API.
 This file is [slider_control.launch] related script.
 Passable parameters:
-    port: serial prot string. Defaults is '/dev/ttyAMA1'
-    baud: serial prot baudrate. Defaults is 115200.
+      port1: Left arm serial port string. Default is "/dev/ttyTHS1"
+      port2: Right arm serial port string. Default is "/dev/ttyS0"
+      Baud rate: Left and right arm serial port baud rate. The default value is 115200.
 """
 import math
 import time
@@ -16,10 +17,10 @@
 
 from pymycobot.mercury import Mercury
 
-# left arm port
+# left arm
 l = None
 
-# right arm port
+# right arm
 r = None
 
 
@@ -30,43 +31,39 @@ def callback(data):
     for index, value in enumerate(data.position):
         radians_to_angles = round(math.degrees(value), 2)
         data_list.append(radians_to_angles)
-    print('data_list:', data_list)
+    print('data_list: {}'.format(data_list))
     left_arm = data_list[:7]
     right_arm = data_list[7:-3]
     middle_arm = data_list[-3:]
 
-    print('left_arm:', left_arm)
-    print('right_arm:', right_arm)
-    print('middle_arm:', middle_arm)
+    print('left_angles: {}'.format(left_arm))
+    print('right_angles: {}'.format(right_arm))
+    print('middle_angles: {}'.format(middle_arm))
 
     l.send_angles(left_arm, 25)
-    time.sleep(0.02)
     r.send_angles(right_arm, 25)
-    time.sleep(0.02)
     r.send_angle(11, middle_arm[0], 25)
-    time.sleep(0.02)
     r.send_angle(12, middle_arm[1], 25)
-    time.sleep(0.02)
     r.send_angle(13, middle_arm[2], 25)
-    time.sleep(0.02)
 
 
 def listener():
     global l, r
     rospy.init_node("control_slider", anonymous=True)
 
     rospy.Subscriber("joint_states", JointState, callback)
-    port1 = rospy.get_param("~port1", "/dev/ttyS0")
-    port2 = rospy.get_param("~port2", "/dev/ttyTHS1")
+    port1 = rospy.get_param("~port1", "/dev/ttyTHS1")
+    port2 = rospy.get_param("~port2", "/dev/ttyS0")
     baud = rospy.get_param("~baud", 115200)
-    print(port1, baud)
-    print(port2, baud)
+    print('left arm: {}, {}'.format(port1, baud))
+    print('right arm: {}, {}'.format(port2, baud))
     l = Mercury(port1, baud)
     r = Mercury(port2, baud)
     time.sleep(0.05)
-    l.set_free_mode(1)
-    r.set_free_mode(1)
+    l.set_fresh_mode(1)
+    r.set_fresh_mode(1)
     time.sleep(0.05)
+
     # spin() simply keeps python from exiting until this node is stopped
     # spin()只是阻止python退出，直到该节点停止
     print("spin ...")