From 9f87b2c31b1027e2be4fcaac48265d16f255f05b Mon Sep 17 00:00:00 2001 From: wangWking <842749351@qq.com> Date: Wed, 13 Dec 2023 17:51:55 +0800 Subject: [PATCH] update mercury ros --- Mercury/mercury_a1/scripts/slider_control.py | 2 +- .../scripts/mercury_services.py | 4 +- .../mercury_a1_moveit/scripts/sync_plan.py | 5 +- Mercury/mercury_b1/config/mercury_b1.rviz | 4 +- Mercury/mercury_b1/scripts/follow_display.py | 138 +++++++++--------- Mercury/mercury_b1/scripts/slider_control.py | 26 ++-- .../mercury_b1_moveit/scripts/sync_plan.py | 35 ++--- .../urdf/mercury_b1/mercury_b1.urdf | 16 +- 8 files changed, 113 insertions(+), 117 deletions(-) diff --git a/Mercury/mercury_a1/scripts/slider_control.py b/Mercury/mercury_a1/scripts/slider_control.py index 7051197..25ebc00 100755 --- 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 index d9aac9e..5936f65 100755 --- 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 index 02387f4..bf7877d 100755 --- a/Mercury/mercury_a1_moveit/scripts/sync_plan.py +++ b/Mercury/mercury_a1_moveit/scripts/sync_plan.py @@ -9,6 +9,7 @@ Passable parameters: 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 index 7cf2ce5..4141bf3 100755 --- 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: - 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 index 59111e0..ca200e4 100755 --- 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() + 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) - 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 - # rospy.loginfo('{}'.format(data_list)) - joint_state_send.position = data_list + pub.publish(joint_state_send) - 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 - 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 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 - 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_.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_) + marker_.color.a = 1.0 + marker_.color.g = 1.0 + pub_marker.publish(marker_) - rate.sleep() + 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 index 66a860b..7d7238a 100755 --- a/Mercury/mercury_b1/scripts/slider_control.py +++ b/Mercury/mercury_b1/scripts/slider_control.py @@ -5,8 +5,9 @@ This file obtains the joint angle of the manipulator in ROS, 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 sensor_msgs.msg import JointState 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 index 6a5b66f..5ef090e 100755 --- a/Mercury/mercury_b1_moveit/scripts/sync_plan.py +++ b/Mercury/mercury_b1_moveit/scripts/sync_plan.py @@ -6,8 +6,9 @@ This file obtains the joint angle of the manipulator in ROS, 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 sensor_msgs.msg import JointState from pymycobot.mercury import Mercury -# left arm port +# left arm l = None -# right arm port +# right arm r = None @@ -30,25 +31,20 @@ 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(): @@ -56,17 +52,18 @@ def listener(): 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 ...") diff --git a/mycobot_description/urdf/mercury_b1/mercury_b1.urdf b/mycobot_description/urdf/mercury_b1/mercury_b1.urdf index cd7a8b2..6fb72ad 100644 --- a/mycobot_description/urdf/mercury_b1/mercury_b1.urdf +++ b/mycobot_description/urdf/mercury_b1/mercury_b1.urdf @@ -3,7 +3,7 @@ - + @@ -19,7 +19,7 @@ - + @@ -35,7 +35,7 @@ - + @@ -51,19 +51,19 @@ - + - + - + @@ -392,10 +392,10 @@ - + - +