From b63b25e1b6593e0a062bafae970338bfe517e645 Mon Sep 17 00:00:00 2001
From: wangWking <842749351@qq.com>
Date: Fri, 8 Dec 2023 18:20:17 +0800
Subject: [PATCH] update 320 moveit sync_plan.py

---
 .../scripts/sync_plan.py                       | 18 +++++++++++-------
 1 file changed, 11 insertions(+), 7 deletions(-)

diff --git a/mycobot_320/new_mycobot_320_pi_moveit/scripts/sync_plan.py b/mycobot_320/new_mycobot_320_pi_moveit/scripts/sync_plan.py
index 9b2f0047..76e859f9 100755
--- a/mycobot_320/new_mycobot_320_pi_moveit/scripts/sync_plan.py
+++ b/mycobot_320/new_mycobot_320_pi_moveit/scripts/sync_plan.py
@@ -1,6 +1,7 @@
 #!/usr/bin/env python3
 # -*- coding: utf-8 -*-
 import time
+import math
 import subprocess
 import rospy
 from sensor_msgs.msg import JointState
@@ -12,14 +13,14 @@
 
 
 def callback(data):
-    rospy.loginfo(rospy.get_caller_id() + "%s", data)
+    # rospy.loginfo(rospy.get_caller_id() + "%s", data)
     data_list = []
     for index, value in enumerate(data.position):
-        # if index != 2:
-        #     value *= -1
-        data_list.append(value)
-
-    mc.send_radians(data_list, 80)
+        radians_to_angles = round(math.degrees(value), 2)
+        data_list.append(radians_to_angles)
+    rospy.loginfo(rospy.get_caller_id() + "%s", data)
+    # mc.send_radians(data_list, 80)
+    mc.send_angles(data_list, 60)
 
 
 def listener():
@@ -34,7 +35,10 @@ def listener():
     rospy.Subscriber("joint_states", JointState, callback)
 
     # spin() simply keeps python from exiting until this node is stopped
-    rospy.spin()
+    # rospy.spin()
+    rate = rospy.Rate(1)
+    while not rospy.is_shutdown():
+        rate.sleep()
 
 
 if __name__ == "__main__":