From f00bbb2258b1a83db45d9f59ed40c960745546ad Mon Sep 17 00:00:00 2001
From: wangWking <842749351@qq.com>
Date: Wed, 4 Dec 2024 15:21:41 +0800
Subject: [PATCH] =?UTF-8?q?280=20=E6=A8=A1=E5=9E=8B=E8=B7=9F=E9=9A=8F?=
 =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=BC=82=E5=B8=B8=E5=A4=84=E7=90=86=E6=9C=BA?=
 =?UTF-8?q?=E5=88=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../mycobot_280/scripts/follow_display.py     | 75 +++++++++---------
 .../mycobot_280jn/scripts/follow_display.py   | 76 ++++++++++---------
 .../mycobot_280pi/scripts/follow_display.py   | 74 +++++++++---------
 3 files changed, 116 insertions(+), 109 deletions(-)

diff --git a/mycobot_280/mycobot_280/scripts/follow_display.py b/mycobot_280/mycobot_280/scripts/follow_display.py
index d6619747..75e2bc14 100755
--- a/mycobot_280/mycobot_280/scripts/follow_display.py
+++ b/mycobot_280/mycobot_280/scripts/follow_display.py
@@ -58,42 +58,45 @@ def talker():
     print("publishing ...")
     while not rospy.is_shutdown():
         joint_state_send.header.stamp = rospy.Time.now()
-
-        angles = mycobot.get_radians()
-        data_list = []
-        for index, value in enumerate(angles):
-            data_list.append(value)
-
-        # rospy.loginfo('{}'.format(data_list))
-        joint_state_send.position = data_list
-
-        pub.publish(joint_state_send)
-
-        coords = mycobot.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 = mycobot.get_radians()
+            data_list = []
+            for index, value in enumerate(angles):
+                data_list.append(value)
+
+            # rospy.loginfo('{}'.format(data_list))
+            joint_state_send.position = data_list
+
+            pub.publish(joint_state_send)
+
+            coords = mycobot.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:
+            print(e)
 
 
 if __name__ == "__main__":
diff --git a/mycobot_280/mycobot_280jn/scripts/follow_display.py b/mycobot_280/mycobot_280jn/scripts/follow_display.py
index 68e80951..fe7ffa60 100755
--- a/mycobot_280/mycobot_280jn/scripts/follow_display.py
+++ b/mycobot_280/mycobot_280jn/scripts/follow_display.py
@@ -65,43 +65,45 @@ def talker():
     print("publishing ...")
     while not rospy.is_shutdown():
         joint_state_send.header.stamp = rospy.Time.now()
-
-        angles = mycobot.get_radians()
-        print('angles:',angles)
-        data_list = []
-        for index, value in enumerate(angles):
-            data_list.append(value)
-
-        # rospy.loginfo('{}'.format(data_list))
-        joint_state_send.position = data_list
-        print('data_list:',data_list)
-        pub.publish(joint_state_send)
-
-        coords = mycobot.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 = mycobot.get_radians()
+            print('angles:',angles)
+            data_list = []
+            for index, value in enumerate(angles):
+                data_list.append(value)
+
+            # rospy.loginfo('{}'.format(data_list))
+            joint_state_send.position = data_list
+            print('data_list:',data_list)
+            pub.publish(joint_state_send)
+
+            coords = mycobot.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:
+            print(e)
 
 
 if __name__ == "__main__":
diff --git a/mycobot_280/mycobot_280pi/scripts/follow_display.py b/mycobot_280/mycobot_280pi/scripts/follow_display.py
index fa4bb50b..6fbd70a8 100755
--- a/mycobot_280/mycobot_280pi/scripts/follow_display.py
+++ b/mycobot_280/mycobot_280pi/scripts/follow_display.py
@@ -59,42 +59,44 @@ def talker():
     print("publishing ...")
     while not rospy.is_shutdown():
         joint_state_send.header.stamp = rospy.Time.now()
-
-        angles = mycobot.get_radians()
-        data_list = []
-        for index, value in enumerate(angles):
-            data_list.append(value)
-
-        # rospy.loginfo('{}'.format(data_list))
-        joint_state_send.position = data_list
-
-        pub.publish(joint_state_send)
-
-        coords = mycobot.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 = mycobot.get_radians()
+            data_list = []
+            for index, value in enumerate(angles):
+                data_list.append(value)
+
+            # rospy.loginfo('{}'.format(data_list))
+            joint_state_send.position = data_list
+
+            pub.publish(joint_state_send)
+
+            coords = mycobot.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:
+            print(e)
 
 
 if __name__ == "__main__":