diff --git a/mycobot_280/mycobot_280/scripts/follow_display.py b/mycobot_280/mycobot_280/scripts/follow_display.py index d661974..75e2bc1 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() + + try: + angles = mycobot.get_radians() + data_list = [] + for index, value in enumerate(angles): + data_list.append(value) - 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 - # rospy.loginfo('{}'.format(data_list)) - joint_state_send.position = data_list + pub.publish(joint_state_send) - pub.publish(joint_state_send) + coords = mycobot.get_coords() - 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 - 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 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_.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_) - marker_.color.a = 1.0 - marker_.color.g = 1.0 - pub_marker.publish(marker_) - - rate.sleep() + 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 68e8095..fe7ffa6 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() + try: + angles = mycobot.get_radians() + print('angles:',angles) + data_list = [] + for index, value in enumerate(angles): + data_list.append(value) - 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) - # 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() - 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 - 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 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_.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_) - marker_.color.a = 1.0 - marker_.color.g = 1.0 - pub_marker.publish(marker_) - - rate.sleep() + 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 fa4bb50..6fbd70a 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() + try: + angles = mycobot.get_radians() + data_list = [] + for index, value in enumerate(angles): + data_list.append(value) - 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 - # rospy.loginfo('{}'.format(data_list)) - joint_state_send.position = data_list + pub.publish(joint_state_send) - pub.publish(joint_state_send) + coords = mycobot.get_coords() - 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 - 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 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_.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_) - marker_.color.a = 1.0 - marker_.color.g = 1.0 - pub_marker.publish(marker_) - - rate.sleep() + rate.sleep() + except Exception as e: + print(e) if __name__ == "__main__":