Python Marker.points方法代码示例

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import points [as 别名]
 def add_marker(self, array, track, color, tid):
     m1 = Marker()
     m1.header.frame_id = "camera_rgb_optical_frame"
     m1.ns = "line"
     m1.id = tid
     m1.type = 4 #lines
     m1.action = 0
     m1.scale.x = .002
     m1.color.a = 1.
     m1.color.r = color[0]/255.
     m1.color.g = color[1]/255.
     m1.color.b = color[2]/255.
     m1.points = track
     array.append(m1)
     m2 = Marker()
     m2.header.frame_id = "camera_rgb_optical_frame"
     m2.ns = "point"
     m2.id = tid
     m2.type = 8 #points
     m2.action = 0
     m2.scale.x = .008
     m2.scale.y = .008
     m2.color.a = 1.
     m2.color.r = color[0]/255.
     m2.color.g = color[1]/255.
     m2.color.b = color[2]/255.
     m2.points = [ track[-1] ]
     array.append(m2)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import points [as 别名]
def node():
    pub = rospy.Publisher('shapes', Marker, queue_size=10)
    rospy.init_node('plotter', anonymous=False)
    rate = rospy.Rate(1)
   

    points=Marker()
    line=Marker()
#Set the frame ID and timestamp.  See the TF tutorials for information on these.
    points.header.frame_id=line.header.frame_id="/map"
    points.header.stamp=line.header.stamp=rospy.Time.now()

    points.ns=line.ns = "markers"
    points.id = 0
    line.id =2

    points.type = Marker.POINTS
    line.type=Marker.LINE_STRIP
#Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
    points.action = line.action = Marker.ADD;

    points.pose.orientation.w = line.pose.orientation.w = 1.0;

    line.scale.x = 0.02;
    points.scale.x=0.03; 
    line.scale.y= 0.02;
    points.scale.y=0.03; 

    line.color.r = 1.0;
    points.color.g = 1.0;
   
    points.color.a=line.color.a = 1.0;
    points.lifetime =line.lifetime = rospy.Duration();

    p=Point()
    p.x = 1;
    p.y = 1;
    p.z = 1;

    pp=[]
    
    pp.append(copy(p))
       
    while not rospy.is_shutdown():
        p.x+=0.1  
        pp.append(copy(p)) 
        points.points=pp
        #print points.points,'\n','----------------','\n'
        line.points=pp

        pub.publish(points)
        pub.publish(line)
        rate.sleep()

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import points [as 别名]
def display_init_config(points, old=False, clear=False):
    """
    Displays config on screen.
    Assumes markers already in the right place for frame camera1_rgb_optical_frame.
    """
    global rviz_marker, rviz_pub
    
    if rviz_pub is None:
        rviz_pub = rospy.Publisher('init_config', Marker)
    if rviz_marker is None:
        rviz_marker = Marker()
        rviz_marker.type = Marker.POINTS
        rviz_marker.action = Marker.ADD
    
    if clear:
        rviz_marker.points = []
        rviz_pub.publish(rviz_marker)
        return
        
    if old and points is not None:
        rviz_pub.publish(rviz_marker)
        return
    
    if points is None:
        return
    
#         rviz_marker.type = Marker.SPHERE
    
    color = ColorRGBA(0.5,0,1,1)
    rviz_marker.scale.x = 0.01
    rviz_marker.scale.y = 0.01
    rviz_marker.scale.z = 0.01
    rviz_marker.pose.position.x = 0.0
    rviz_marker.pose.position.y = 0.0
    rviz_marker.pose.position.z = 0.0
    rviz_marker.pose.orientation.w = 1.0
    rviz_marker.pose.orientation.x = 0.0
    rviz_marker.pose.orientation.y = 0.0
    rviz_marker.pose.orientation.z = 0.0
    rviz_marker.color = color
    rviz_marker.points = []
    for point in points: 
        rviz_marker.points.append(Point(*point))
        rviz_marker.colors.append(color)
    
    rviz_marker.header.frame_id = 'camera1_rgb_optical_frame'
    rviz_marker.header.stamp = rospy.Time.now()

    rviz_pub.publish(rviz_marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import points [as 别名]
 def get_visualization_markers(self, type='hough_lines'):
     """
     Returns a visualization_msgs.Marker Message
     with the lines from the Hough Transform in the frame of the scanner.
     """
     if type == 'hough_lines':
         marker = Marker()
         marker.header.frame_id = self.scan.frame_id
         marker.header.stamp = self.scan.timestamp
         marker.ns = "hough_lines"
         marker.id = 0
         marker.scale.x = 0.01
         marker.scale.y = 0.01
         marker.scale.z = 0.01
         marker.type = marker.LINE_LIST
         marker.action = marker.ADD
         marker.lifetime = rospy.Duration()
         marker.points = []
         for (pt1, pt2) in self.get_lines_meters():
             marker.points.append(Point(pt1[1], pt1[0], 0))
             marker.points.append(Point(pt2[1], pt2[0], 0))
         marker.color.r = 0.0
         marker.color.g = 1.0
         marker.color.b = 0.0
         marker.color.a = 1.0
     return marker

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import points [as 别名]
    def marker(self, position=None, orientation=None, points=None, colors=None, scale=1.0):
        mark = Marker()
        mark.header.stamp = rospy.Time.now()
        mark.header.frame_id = self.frame_id
        mark.ns = self.ns
        mark.type = self.type
        mark.id = self.counter
        mark.action = self.action
        mark.scale.x = self.scale[0]
        mark.scale.y = self.scale[1]
        mark.scale.z = self.scale[2]
        mark.color.r = self.color[0]
        mark.color.g = self.color[1]
        mark.color.b = self.color[2]
        mark.color.a = self.color[3]
        mark.lifetime = rospy.Duration(self.lifetime)

        if points is not None:
            mark.points = []
            for point in points:
                mark.points.append(get_point(point, scale))
        if colors is not None:
            mark.colors = colors

        if position is not None or orientation is not None:
            mark.pose.position = get_point(position, scale)
            mark.pose.orientation = get_quat(orientation)

           

        self.counter+=1
        return mark

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import points [as 别名]
    def create_line_marker(p1, p2, frame_id):
        h = Header()
        h.frame_id = frame_id #tie marker visualization to laser it comes from
        h.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work
        
        #create marker:person_marker, modify a red cylinder, last indefinitely
        mark = Marker()
        mark.header = h
        mark.ns = "unit_vector"
        mark.id = 0
        mark.type = Marker.LINE_STRIP
        mark.action = 0
        mark.scale.x = 0.2 
        mark.color = color = ColorRGBA(0.2, 0.5, 0.7, 1.0)
        mark.text = "unit_vector"

        points = []
        pt1 = Point(p1[0], p1[1], p1[2])
        pt2 = Point(p2[0], p2[1], p2[2])
        # print "Pt 1 ", pt1
        # print "Pt 2 ", pt2
        points.append(pt1)
        points.append(pt2)
        mark.points = points

        return mark

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import points [as 别名]
def animate_path(circle_length, rate_animate):

    animate_pub = rospy.Publisher("marker_visualization", Marker, queue_size=10)
    rate = rospy.Rate(rate_animate)

    marker = Marker()
    marker.header.frame_id = "base"  # marker is referenced to base frame
    marker.header.stamp = rospy.Time.now()

    marker.type = marker.LINE_STRIP
    marker.action = marker.ADD
    marker.scale.x = 0.01  # define the size of marker

    marker.color.a = 1.0
    marker.color.g = 1.0

    while not rospy.is_shutdown():
        try:
            (trans, rot) = listener.lookupTransform("base", "end", rospy.Time())
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

        p = Point()  # Define a point array that takes in x, y and z values

        p.x = trans[0]
        p.y = trans[1]
        p.z = trans[2]

        marker.points = deque(
            marker.points, circle_length
        )  # Use deque to prevent list to increasing indefinitely; with this, after a certain circle_length, the list starts building from the beginning while replacing earlier elements
        marker.points.append(p)

        animate_pub.publish(marker)
        rate.sleep()

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import points [as 别名]
def visualize_traj(points):

    traj = Marker()
    traj.header.frame_id = FRAME
    traj.header.stamp = rospy.get_rostime()
    traj.ns = "love_letter"
    traj.action = Marker.ADD
    traj.pose.orientation.w = 1.0
    traj.type = Marker.LINE_STRIP
    traj.scale.x = 0.001 # line width
    traj.color.r = 1.0
    traj.color.b = 0.0
    traj.color.a = 1.0
    
    if(WRITE_MULTIPLE_SHAPES):
        traj.id = shapeCount;
    else:
        traj.id = 0; #overwrite any existing shapes
        traj.lifetime.secs = 1; #timeout for display
    
    traj.points = list(points)
    
    # use interactive marker from place_paper instead
    #pub_markers.publish(a4_sheet()) 
    pub_markers.publish(traj)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import points [as 别名]
    def line(self, frame_id, p, r, t=[0.0, 0.0], key=None):
        """
        line: t r + p

        This will be drawn for t[0] .. t[1]
        """

        r = np.array(r)
        p = np.array(p)

        m = Marker()
        m.header.frame_id = frame_id
        m.ns = key or 'lines'
        m.id = 0 if key else len(self.lines)
        m.type = Marker.LINE_STRIP
        m.action = Marker.MODIFY
        m.pose = Pose(Point(0,0,0), Quaternion(0,0,0,1))

        m.scale = Vector3(0.005, 0.005, 0.005)
        m.color = ColorRGBA(0,0.8,0.8,1)

        m.points = [Point(*(p+r*t)) for t in np.linspace(t[0], t[1], 10)]
        m.colors = [m.color] * 10

        key = key or element_key(m)

        with self.mod_lock:
            self.lines[key] = m

        return key

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import points [as 别名]
 def get_marker(self, line_seg, frame_id, scale=0.3):
     p0, p1 = line_seg
     v = np.asarray([p1.position.x - p0.position.x,
                     p1.position.y - p0.position.y,
                     p1.position.z - p0.position.z])
     vnorm = np.linalg.norm(v)
     if vnorm > 0.0:
         v /= np.linalg.norm(v)
     p2 = copy.deepcopy(p0)
     p2.position.x += scale * v[0]
     p2.position.y += scale * v[1]
     p2.position.z += scale * v[2]
     m = Marker()
     m.header.stamp = rospy.Time.now()
     m.header.frame_id = frame_id
     m.ns = "beam"
     m.type = Marker.ARROW
     m.action = Marker.MODIFY
     m.lifetime = rospy.Duration(1)
     m.points = [p0.position, p1.position]
     m.scale.x = 0.02
     m.scale.y = 0.04
     m.color.r = 1.0
     m.color.a = 1.0
     return m

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import points [as 别名]
    def draw_risk_grid(self, risk_grid):
        if not rospy.is_shutdown():
            p_list = list()
            c_list = list()
            x_gen = xrange(0, self.x_dim, 1)
            y_gen = xrange(0, self.y_dim, 1)

            for i in x_gen:
                for j in y_gen:
                    risk = risk_grid[i, j]
                    pnt = Point(i, j, 0)
                    r, g, b, a = self.sm.to_rgba(risk)
                    clr = ColorRGBA(r, g, b, a)
                    p_list.append(pnt)
                    c_list.append(clr)

            marker = Marker()
            marker.header.frame_id = "/my_frame"
            marker.lifetime = rospy.Duration(10000000)
            marker.type = marker.POINTS
            marker.scale.x = 1
            marker.scale.y = 1
            marker.action = marker.ADD
            marker.points = p_list
            marker.colors = c_list
            marker.id = -1
            self.pub.publish(marker)
        return self

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import points [as 别名]
    def create_marker(self, markerArray, view, pose, view_values):
        marker1 = Marker()
        marker1.id = self.marker_id
        self.marker_id += 1
        marker1.header.frame_id = "/map"
        marker1.type = marker1.TRIANGLE_LIST
        marker1.action = marker1.ADD
        marker1.scale.x = 1
        marker1.scale.y = 1
        marker1.scale.z = 2
        marker1.color.a = 0.25

        vals = view_values.values()
        max_val = max(vals)
        non_zero_vals = filter(lambda a: a != 0, vals)
        min_val = min(non_zero_vals)
        
        print min_val, max_val, view_values[view.ID]
        
        marker1.color.r = r_func( float((view_values[view.ID] - min_val)) / float((max_val - min_val + 1)))
        marker1.color.g = g_func( float((view_values[view.ID] - min_val)) / float((max_val - min_val + 1)))
        marker1.color.b = b_func( float((view_values[view.ID] - min_val)) /  float((max_val - min_val + 1)))

        marker1.pose.orientation = pose.orientation
        marker1.pose.position = pose.position
        marker1.points = [Point(0,0,0.01),Point(2.5,-1.39,0.01),Point(2.5,1.39,0.01)]
        
        markerArray.markers.append(marker1)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import points [as 别名]
def arrow_marker(position, direction, shaft_radius=0.02, head_radius=0.04, rgba=[1.0, 1.0, 1.0, 1.0], 
                     ns="", id=0, frame_id="", lifetime=None, stamp=None):
    marker = Marker()
    if stamp is None:
        marker.header.stamp = rospy.Time.now()
    else:
        marker.header.stamp = stamp
    marker.header.frame_id = frame_id
    marker.ns = ns
    marker.id = id
    marker.type = Marker.ARROW
    marker.action = Marker.ADD
    #marker.pose.position.x = position[0]
    #marker.pose.position.y = position[1]
    #marker.pose.position.z = position[2]
    marker.points = [Point(*tuple(position)), Point(*tuple(position+direction*1.0))]
    marker.scale.x = shaft_radius
    marker.scale.y = head_radius
    marker.scale.z = 1.0
    marker.color.r = rgba[0]
    marker.color.g = rgba[1]
    marker.color.b = rgba[2]
    marker.color.a = rgba[3]
    if lifetime is None:
        marker.lifetime = rospy.Duration()
    else:
        marker.lifetime = rospy.Duration(lifetime)
    return marker

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import points [as 别名]
 def addPolygonFilled(self, points):
     oid = self.oid+1
     name = "/polygonFilled"+str(self.oid)
     marker = Marker()
     marker.id = self.oid
     marker.ns = "/polygonFilled"
     marker.header.frame_id = name
     marker.type = marker.TRIANGLE_LIST
     marker.action = marker.ADD
     marker.scale.x = 1
     marker.scale.y = 1
     marker.scale.z = 1
     marker.color.r = 1.0
     marker.color.g = 1.0
     marker.color.b = 1.0
     marker.color.a = 1.0
     marker.pose.orientation.w = 1.0
     marker.pose.position.x = 0
     marker.pose.position.y = 0
     marker.pose.position.z = 0
     marker.points = []
     for i in range(0,len(points)-2,1):
             pt = Point(points[0][0], points[0][1], points[0][2])
             marker.points.append(pt)
             pt = Point(points[i+1][0], points[i+1][1], points[i+1][2])
             marker.points.append(pt)
             pt = Point(points[i+2][0], points[i+2][1], points[i+2][2])
             marker.points.append(pt)
     self.markerArray.markers.append(marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import points [as 别名]
	def otld_callback(self, data):
		print "Heard: " + str(data)
		# get the depth image
		s = rospy.Subscriber("/camera/depth_registered/points", PointCloud2, self.p2callback)
		while not self.got_pointcloud:
			# do nothing
			print "Waiting for pointcloud"
			rospy.sleep(0.1)

		s.unregister()
		self.got_pointcloud = False

		# get the depth of the tracked area
		# Header header
		# int32 x
		# int32 y
		# int32 width
		# int32 height
		# float32 confidence

		# probably first get the centroid, we should do a median here or something
		centerx = data.x + data.width / 2
		centery = data.y + data.height / 2
		#print "self.pointcloud.row_step: " + str(self.pointcloud.row_step) + " self.pointcloud.point_step: " + str(self.pointcloud.point_step)
		print "centerx: " + str(centerx) + " centery: " + str(centery) 
		#pointdepth = self.pointcloud.data[self.pointcloud.row_step * centery : self.pointcloud.row_step * centery + self.pointcloud.point_step]
		#point = read_points(self.pointcloud, field_names=None, skip_nans=False, uvs=[centerx, centery])
		point = read_points(self.pointcloud, field_names=None, skip_nans=False, uvs=[[centerx, centery]])
		print "point is: " + str(point)
		for item in point:
			print "item is: " + str(item)
			print "x: "+ str(item[0]) + " y: " + str(item[1]) + ", z: " + str(item[2])

		# create PoseStamped
		pose = PoseStamped()
		pose.header = std_msgs.msg.Header()
		pose.header.stamp = rospy.Time.now()
		pose.header.frame_id = "camera_link" 
		pose.pose = Pose()
		pose.pose.position.x = item[2] #  kinect Z value, [2], is X in TF of camera_link
		pose.pose.position.y = - item[0] # kinect X value, [0], is -Y in TF of camera_link
		pose.pose.position.z = - item[1] # kinect Y value, [1], is -Z in TF of camera_link
		pose.pose.orientation.w = 1
		# send PoseStamped
		self.pub.publish(pose)

		arrow = Marker()
		arrow.header.frame_id = "camera_link"
		arrow.header.stamp = rospy.Time.now()
		arrow.type = Marker.ARROW
		arrow.points = [Point(0,0,0), Point(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)]
		#arrow.points = [Point(0,0,0), Point(0,0,1)]
		arrow.scale.x = 0.1 # anchura del palo
		arrow.scale.y = 0.15 # anchura de la punta
		# arrow.scale.z = 0.1
		arrow.color.a = 1.0
		arrow.color.r = 1.0
		arrow.color.g = 1.0
		arrow.color.b = 1.0
		self.marker_publisher.publish(arrow)