From a12eacca6b7a834dc6fa429242673e9adf419c6e Mon Sep 17 00:00:00 2001 From: wangWking <842749351@qq.com> Date: Thu, 11 Jan 2024 16:26:10 +0800 Subject: [PATCH] update and fix mercury_a1 ros --- Mercury/mercury_a1/scripts/follow_display.py | 75 ++++++++++--------- Mercury/mercury_a1/scripts/listen_real.py | 41 +++++----- Mercury/mercury_a1/scripts/teleop_keyboard.py | 22 ++++-- .../scripts/mercury_services.py | 26 ++++--- 4 files changed, 92 insertions(+), 72 deletions(-) diff --git a/Mercury/mercury_a1/scripts/follow_display.py b/Mercury/mercury_a1/scripts/follow_display.py index 353330b1..1092c738 100755 --- 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 index 056e67ea..2f222037 100755 --- 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 index 36c71671..6d79c44d 100755 --- 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 index 2d7a358f..89ad0078 100755 --- 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):