diff --git a/CobotX/cobotx_a450/scripts/follow_display.py b/CobotX/cobotx_a450/scripts/follow_display.py index 57136d81..9bf4f144 100755 --- a/CobotX/cobotx_a450/scripts/follow_display.py +++ b/CobotX/cobotx_a450/scripts/follow_display.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 import time - +import math import rospy from sensor_msgs.msg import JointState from std_msgs.msg import Header @@ -28,7 +28,7 @@ def talker(): """ ) exit(1) - mc.release_all_servos(0) + mc.release_all_servos() time.sleep(0.1) print("Rlease all servos over.\n") @@ -60,10 +60,11 @@ def talker(): while not rospy.is_shutdown(): joint_state_send.header.stamp = rospy.Time.now() - angles = mc.get_radians() + angles = mc.get_angles() data_list = [] for index, value in enumerate(angles): - data_list.append(value) + radians = math.radians(value) + data_list.append(radians) # rospy.loginfo('{}'.format(data_list)) joint_state_send.position = data_list diff --git a/CobotX/cobotx_a450/scripts/simple_gui.py b/CobotX/cobotx_a450/scripts/simple_gui.py index 703f3c34..ac79b514 100755 --- a/CobotX/cobotx_a450/scripts/simple_gui.py +++ b/CobotX/cobotx_a450/scripts/simple_gui.py @@ -493,7 +493,7 @@ def run(self): def main(): window = tk.Tk() - window.title("myarm ros GUI") + window.title("CobotX ros GUI") Window(window).run() diff --git a/CobotX/cobotx_a450/scripts/teleop_keyboard.py b/CobotX/cobotx_a450/scripts/teleop_keyboard.py index 033f4a8f..c61fd36f 100755 --- a/CobotX/cobotx_a450/scripts/teleop_keyboard.py +++ b/CobotX/cobotx_a450/scripts/teleop_keyboard.py @@ -56,7 +56,7 @@ def __exit__(self, type, value, traceback): def teleop_keyboard(): rospy.init_node("teleop_keyboard") - model = 0 + model = 1 speed = rospy.get_param("~speed", 50) change_percent = rospy.get_param("~change_percent", 5) @@ -81,9 +81,9 @@ def teleop_keyboard(): exit(1) init_pose = [0, 0, 0, 0, 0, 0, 0, speed] - home_pose = [0, 0, 0, -90, 0, 0, 0, speed] + home_pose = [0, 0, 0, -90, 0, 90, 0, speed] - # rsp = set_angles(*init_pose) + rsp = set_angles(*home_pose) while True: res = get_coords() @@ -92,7 +92,7 @@ def teleop_keyboard(): time.sleep(0.1) record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model] - print(record_coords) + print('init_coords:', record_coords) try: print(msg) @@ -149,6 +149,11 @@ def teleop_keyboard(): rsp = set_angles(*init_pose) elif key in "2": rsp = set_angles(*home_pose) + time.sleep(3) + res = get_coords() + time.sleep(0.1) + record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model] + print('home_coords:', record_coords) elif key in "3": rep = get_angles() home_pose[0] = rep.joint_1