$search
00001 #!/usr/bin/env python 00002 00003 import roslib; roslib.load_manifest('visualization_marker_tutorials') 00004 from visualization_msgs.msg import Marker 00005 from visualization_msgs.msg import MarkerArray 00006 import rospy 00007 import math 00008 00009 topic = 'visualization_marker_array' 00010 publisher = rospy.Publisher(topic, MarkerArray) 00011 00012 rospy.init_node('register') 00013 00014 markerArray = MarkerArray() 00015 00016 count = 0 00017 MARKERS_MAX = 100 00018 00019 while not rospy.is_shutdown(): 00020 00021 marker = Marker() 00022 marker.header.frame_id = "/base_link" 00023 marker.type = marker.SPHERE 00024 marker.action = marker.ADD 00025 marker.scale.x = 0.2 00026 marker.scale.y = 0.2 00027 marker.scale.z = 0.2 00028 marker.color.a = 1.0 00029 marker.color.r = 1.0 00030 marker.color.g = 1.0 00031 marker.color.b = 0.0 00032 marker.pose.orientation.w = 1.0 00033 marker.pose.position.x = math.cos(count / 50.0) 00034 marker.pose.position.y = math.cos(count / 40.0) 00035 marker.pose.position.z = math.cos(count / 30.0) 00036 00037 # We add the new marker to the MarkerArray, removing the oldest marker from it when necessary 00038 if(count > MARKERS_MAX): 00039 markerArray.markers.pop(0) 00040 00041 markerArray.markers.append(marker) 00042 00043 # Renumber the marker IDs 00044 id = 0 00045 for m in markerArray.markers: 00046 m.id = id 00047 id += 1 00048 00049 # Publish the MarkerArray 00050 publisher.publish(markerArray) 00051 00052 count += 1 00053 00054 rospy.sleep(0.01)