280 模型跟随增加异常处理机制

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__":

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__":

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__":