Python Marker.colors方法代码示例

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import colors [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 colors [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 colors [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 colors [as 别名]
 def draw_curve(
     self,
     pose_array,
     id=None,
     rgba=(0, 1, 0, 1),
     width=.01,
     ns='default_ns',
     duration=0,
     type=Marker.LINE_STRIP,
     ):
     if id is None:
         id = self.get_unused_id()
     marker = Marker(type=type, action=Marker.ADD)
     marker.header = pose_array.header
     marker.points = [pose.position for pose in pose_array.poses]
     marker.lifetime = rospy.Duration(0)
     if isinstance(rgba, list):
         assert len(rgba) == len(pose_array.poses)
         marker.colors = [stdm.ColorRGBA(*c) for c in rgba]
     else:
         marker.color = stdm.ColorRGBA(*rgba)
     marker.scale = gm.Vector3(width, width, width)
     marker.id = id
     marker.ns = ns
     self.pub.publish(marker)
     self.ids.add(id)
     return MarkerHandle(marker, self.pub)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import colors [as 别名]
def test():
    rospy.init_node('intersect_plane_test')
    marker_pub = rospy.Publisher('table_marker', Marker)
    pose_pub = rospy.Publisher('pose', PoseStamped)
    int_pub = rospy.Publisher('intersected_points', PointCloud2)
    tfl = tf.TransformListener()
    
    # Test table
    table = Table()
    table.pose.header.frame_id = 'base_link'
    table.pose.pose.orientation.x, table.pose.pose.orientation.y, table.pose.pose.orientation.z, table.pose.pose.orientation.w = (0,0,0,1)
    table.x_min = -0.5
    table.x_max =  0.5
    table.y_min = -0.5
    table.y_max =  0.5

    # A marker for that table
    marker = Marker()
    marker.header.frame_id = table.pose.header.frame_id
    marker.id = 1
    marker.type = Marker.LINE_STRIP
    marker.action = 0
    marker.pose = table.pose.pose
    marker.scale.x, marker.scale.y, marker.scale.z = (0.005,0.005,0.005)
    marker.color.r, marker.color.g, marker.color.b, marker.color.a = (0.0,1.0,1.0,1.0) 
    marker.frame_locked = False
    marker.ns = 'table'
    marker.points = [
        Point(table.x_min,table.y_min, table.pose.pose.position.z),
        Point(table.x_min,table.y_max, table.pose.pose.position.z),
        Point(table.x_max,table.y_max, table.pose.pose.position.z),
        Point(table.x_max,table.y_min, table.pose.pose.position.z),
        Point(table.x_min,table.y_min, table.pose.pose.position.z),
    ]
    marker.colors = []
    marker.text = ''
    # marker.mesh_resource = ''
    marker.mesh_use_embedded_materials = False
    marker.header.stamp = rospy.Time.now()

    # A test pose
    pose = PoseStamped()
    pose.header = table.pose.header
    pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = (0,0,0.5)
    pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = quaternion_from_euler(0,-pi/5,pi/5)
    
    intersection = cast_ray(pose, table, tfl)
    cloud = xyz_array_to_pointcloud2(np.array([[intersection.point.x, intersection.point.y, intersection.point.z]]))
    cloud.header = pose.header
    
    while not rospy.is_shutdown():
        marker_pub.publish(marker)
        pose_pub.publish(pose)
        int_pub.publish(cloud)
        rospy.loginfo('published')
        rospy.sleep(0.1)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import colors [as 别名]
    def create_trajectory_marker(self, traj):
        # create an interactive marker for our server
        int_marker = InteractiveMarker()
        int_marker.header.frame_id = "/map"
        int_marker.name = traj.uuid
        int_marker.description = traj.uuid
        pose = Pose()
        pose.position.x = traj.pose[0]['position']['x']
        pose.position.y = traj.pose[0]['position']['y']
        int_marker.pose = pose

        line_marker = Marker()
        line_marker.type = Marker.LINE_STRIP
        line_marker.scale.x = 0.05

        # random.seed(traj.uuid)
        # val = random.random()
        # line_marker.color.r = r_func(val)
        # line_marker.color.g = g_func(val)
        # line_marker.color.b = b_func(val)
        # line_marker.color.a = 1.0

        line_marker.points = []

        MOD  = 1
        for i, point in enumerate(traj.pose):
            if i % MOD == 0:
                x = point['position']['x']
                y = point['position']['y']
                p = Point()
                p.x = x - int_marker.pose.position.x  
                p.y = y - int_marker.pose.position.y
                line_marker.points.append(p)

        line_marker.colors = []
        for i, vel in enumerate(traj.vel):
            if i % MOD == 0:
                color = ColorRGBA()
                val = vel / traj.max_vel
                color.r = r_func(val)
                color.g = g_func(val)
                color.b = b_func(val)
                color.a = 1.0
                line_marker.colors.append(color)
                

                
        # create a control which will move the box
        # this control does not contain any markers,
        # which will cause RViz to insert two arrows
        control = InteractiveMarkerControl()
        control.markers.append(line_marker) 
        int_marker.controls.append(control)
        
        return int_marker

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import colors [as 别名]
    def plane(self, frame_id, p, n, r=1.0, key=None, draw_triad=False):
        """
        plane: n . ( X - p ) = 0

        This will be drawn as a disc of radius r about p.
        """

        n = np.array(n)
        p = np.array(p)

        m = Marker()
        m.header.frame_id = frame_id
        m.ns = key or 'planes'
        m.id = 0 if key else len(self.planes)
        m.type = Marker.TRIANGLE_LIST
        m.action = Marker.MODIFY

        # Compute plane rotation
        nz = n / np.linalg.norm(n)
        nx = np.cross(nz, np.array([0,0,1])); nx = nx / np.linalg.norm(nx)
        ny = np.cross(nz, nx); ny = ny / np.linalg.norm(ny)
        R = kdl.Rotation(
            kdl.Vector(*nx),
            kdl.Vector(*ny),
            kdl.Vector(*nz))

        # define the pose of the plane marker 
        m.pose = Pose(Point(*(p)), Quaternion(*R.GetQuaternion()))
        m.scale = Vector3(1, 1, 1)

        # flatten the triangle list
        for triangle in self.mesh_t:
            m.points.extend([
                Point(r*self.mesh_p[t][0],r*self.mesh_p[t][1],0)
                for t in triangle])

        m.color = ColorRGBA(0.8,0,0.8,0.9)
        m.colors = [m.color] * len(m.points)

        if draw_triad:
            self.line(frame_id, p, nx, t=[0.0,r],key=key+'-x')
            self.line(frame_id, p, ny, t=[0.0,r],key=key+'-y')
            self.line(frame_id, p, nz, t=[0.0,r],key=key+'-z')

            self.point(frame_id, p, 0.005, key=key+'-center-point')


        key = key or element_key(m)

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

        return key

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import colors [as 别名]
    def create_plan_marker(self, plan, plan_values):
        # create an interactive marker for our server
        int_marker = InteractiveMarker()
        int_marker.header.frame_id = "/map"
        int_marker.name = plan.ID
        int_marker.description = plan.ID
        pose = Pose()
        #pose.position.x = traj.pose[0]['position']['x']
        #pose.position.y = traj.pose[0]['position']['y']
        int_marker.pose = pose
        
        line_marker = Marker()
        line_marker.type = Marker.LINE_STRIP
        line_marker.scale.x = 0.1

        # random.seed(float(plan.ID))
        # val = random.random()
        # line_marker.color.r = r_func(val)
        # line_marker.color.g = g_func(val)
        # line_marker.color.b = b_func(val)
        # line_marker.color.a = 1.0

        line_marker.points = []
        for view in plan.views:
            x = view.get_ptu_pose().position.x
            y = view.get_ptu_pose().position.y
            z = 0.0 # float(plan.ID) / 10
            p = Point()
            p.x = x - int_marker.pose.position.x  
            p.y = y - int_marker.pose.position.y
            p.z = z - int_marker.pose.position.z
            line_marker.points.append(p)

            line_marker.colors = []
            for i, view in enumerate(plan.views):
                color = ColorRGBA()
                val = float(i) / len(plan.views)
                color.r = r_func(val)
                color.g = g_func(val)
                color.b = b_func(val)
                color.a = 1.0
                line_marker.colors.append(color)
                

        # create a control which will move the box
        # this control does not contain any markers,
        # which will cause RViz to insert two arrows
        control = InteractiveMarkerControl()
        control.markers.append(line_marker) 
        int_marker.controls.append(control)
        
        return int_marker

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import colors [as 别名]
    def transformStampedArrayToLabeledLineStripMarker(cls, tsdata_lst, fmt="%Y-%m-%d %H:%M:%S", zoffset=0.05, label_downsample=1, discrete=False):
        "[[transformStamped, meta],...] -> LineStrip / String"
        res = []
        # make line strip
        points = []
        colors = []
        t_first = tsdata_lst[0][0]
        prev_t = t_first.transform.translation
        for ts, _ in tsdata_lst:
            t = ts.transform.translation
            dist = distanceOfVector3(prev_t, t) * 0.65
            rgb = colorsys.hsv_to_rgb(dist, 1.0, 1.0)
            points.append(Point(x=t.x, y=t.y, z=t.z))
            colors.append(ColorRGBA(rgb[0],rgb[1],rgb[2],1.0))
            prev_t = t

        h = Header()
        h.stamp = rospy.Time(0) #rospy.Time.now()
        h.frame_id = "eng2" #t_first.child_frame_id
        if discrete:
            m_type = Marker.POINTS
        else:
            m_type = Marker.LINE_STRIP
        m = Marker(type=m_type,
                   action=Marker.ADD,
                   header=h,
                   id=cls.marker_id)
        cls.marker_id += 1
        m.scale.x = 0.1
        m.scale.y = 0.1
        m.points = points
        m.colors = colors
        m.ns = "labeled_line"
        m.lifetime = rospy.Time(3000)
        res.append(m)

        for t, t_meta in tsdata_lst[::label_downsample]:
            text = Marker(type=Marker.TEXT_VIEW_FACING,
                          action=Marker.ADD,
                          header=h,
                          id=cls.marker_id)
            cls.marker_id += 1
            text.scale.z = 0.1
            text.pose = T.poseFromTransform(t.transform)
            text.pose.position.z += zoffset
            text.color = ColorRGBA(0.0,0.0,1.0,1.0)
            text.text = t_meta["inserted_at"].strftime(fmt)
            text.ns = "labeled_line_text"
            text.lifetime = rospy.Time(3000)
            res.append(text)
        return res

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import colors [as 别名]
    def calculate_error(self, opt_all_vec):
        opt_param_vec, full_pose_arr = self.split_all(opt_all_vec)

        full_param_vec = self.calculate_full_param_vec(opt_param_vec)

        # Update the primitives with the new set of parameters
        self._robot_params.inflate(full_param_vec)

        # Update all the blocks' configs
        for multisensor in self._multisensors:
            multisensor.update_config(self._robot_params)

        r_list = []
        for multisensor, cb_pose_vec in zip(self._multisensors, list(full_pose_arr)):
            # Process cb pose
            cb_points = SingleTransform(cb_pose_vec).transform * self._robot_params.checkerboards[multisensor.checkerboard].generate_points()
            if (self._use_cov):
                r_list.append(multisensor.compute_residual_scaled(cb_points))
            else:
                r_list.append(multisensor.compute_residual(cb_points))

            cb_points_msgs = [ geometry_msgs.msg.Point(cur_pt[0,0], cur_pt[0,1], cur_pt[0,2]) for cur_pt in cb_points.T]
            cb_colors_msgs = [ ColorRGBA(1,0,1,1) for cur_pt in cb_points.T]
            cb_colors_msgs[0] = ColorRGBA(0,1,0,1)
            cb_colors_msgs[1] = ColorRGBA(0,1,0,1)
            
            m = Marker()
            m.header.frame_id = self._robot_params.base_link 
            m.pose.orientation.w = 1;
            m.ns = "points_3d"
            m.id = id
            m.type = Marker.SPHERE_LIST
            m.action = Marker.MODIFY
            m.points = cb_points_msgs
            m.colors = cb_colors_msgs
            m.scale.x = 0.02
            m.scale.y = 0.02
            m.scale.z = 0.02
            self.marker_pub.publish(m)
            id += 1
                
        r_vec = concatenate(r_list)

        rms_error = numpy.sqrt( numpy.mean(r_vec**2) )
        
        print "\t\t\t\t\tRMS error: %.3f    \r" % rms_error,
        sys.stdout.flush()

        return array(r_vec)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import colors [as 别名]
 def lineMarker(self, p, d):
   marker = Marker()
   header = Header()
   header.stamp = rospy.Time.now()
   header.frame_id = self.base_frame
   marker.header = header
   marker.ns = "rays"
   marker.id = self.recent_next
   marker.type = Marker.LINE_LIST
   marker.action = Marker.ADD
   marker.scale = Vector3(*[0.02 for i in range(3)])
   marker.points = toPoints(np.column_stack([p, (p+2*d/np.linalg.norm(d))]))
   color = self.recent_color
   marker.color = color
   marker.colors = [color for i in range(len(marker.points))]
   return marker

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import colors [as 别名]
 def toMarker(self, points, observation_type):
   marker = Marker()
   header = Header()
   header.stamp = rospy.Time.now()
   header.frame_id = self.base_frame
   marker.header = header
   marker.ns = "object_tracker"
   marker.id = observation_type
   marker.type = Marker.SPHERE_LIST
   marker.action = Marker.ADD
   marker.scale = Vector3(*[self.sigma_observation for i in range(3)])
   marker.points = toPoints(points)
   color = None
   if observation_type == ObjectTracker.RECENT:
     color = self.recent_color
   elif observation_type == ObjectTracker.ALL:
     color = self.all_color
   else:
     raise ValueError
   marker.color = color
   marker.colors = [color for i in range(len(marker.points))]
   return marker

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import colors [as 别名]
    def point(self, frame_id, p, s, key=None):
        """ point: p
        size: s"""

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

        m.scale = Vector3(s,s,1.0)

        m.points = [Point(*p)]
        m.colors = [ColorRGBA(0.8,0.8,0,1)]

        key = key or element_key(m)

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

        return key

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import colors [as 别名]
def marker_maker():
    pub = rospy.Publisher("markers",Marker,queue_size=10)
    rospy.init_node("marker_maker")
    r = rospy.Rate(1)
    q = 0
    i = 1
    j = 0
    while not rospy.is_shutdown():
      msg = Marker()
      msg.ns = "markers"
      msg.type=Marker.SPHERE
      msg.id=q
      msg.action = 0
      msg.color.r = .75
      msg.color.g = 0
      msg.color.b = .5
      msg.color.a = 1
      msg.colors = [ColorRGBA(1,1,1,1),ColorRGBA(0,0,0,0)]
      msg.pose.position.x = j
      msg.pose.position.y = j
      msg.pose.position.z = j
      msg.pose.orientation.x = 0
      msg.pose.orientation.y = 0
      msg.pose.orientation.z = 0
      msg.pose.orientation.w = 1
      msg.scale.x = 1
      msg.scale.y = 1
      msg.scale.z = 1
      msg.lifetime = rospy.Duration()
      msg.header.frame_id = "my_frame"
      msg.header.stamp = rospy.Time.now()
      pub.publish(msg)
      q += 1
      i /= 1.1
      j += 1
      r.sleep()

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import colors [as 别名]
	def imageCallback(self,data):
		#Convert image to CV2 supported numpy array
		bridge=CvBridge()
		cv_image = bridge.imgmsg_to_cv(data, "mono8")
		searched = np.array(cv_image, dtype=np.uint8)
		
		#Create copy and clear searched space in copy (leaving map only)
		searchedCopy=copy.copy(searched)
		searchedCopy[searchedCopy==255]=0
		
		#Take Sobel Derivatives of searched and map only
		sobel_x=np.uint8(np.absolute(cv2.Sobel(searched,cv2.CV_16S,1,0,ksize=1)))
		sobel_y=np.uint8(np.absolute(cv2.Sobel(searched,cv2.CV_16S,0,1,ksize=1)))
		sobel_xy=cv2.addWeighted(sobel_x,0.5,sobel_y,0.5,0)
		sobel_x_base=np.uint8(np.absolute(cv2.Sobel(searchedCopy,cv2.CV_16S,1,0,ksize=1)))
		sobel_y_base=np.uint8(np.absolute(cv2.Sobel(searchedCopy,cv2.CV_16S,0,1,ksize=1)))
		sobel_xy_base=cv2.addWeighted(sobel_x_base,0.5,sobel_y_base,0.5,0)
		ret,sobel_xy_thres = cv2.threshold(sobel_xy,0,255,cv2.THRESH_BINARY)
		ret,sobel_xy_base_thres = cv2.threshold(sobel_xy_base,0,255,cv2.THRESH_BINARY)
		
		#Subtract Sobel Derivatives (Leaves Frontiers Only)
		sobelCombined=sobel_xy_base_thres-sobel_xy_thres
		ret,sobelCombined_thresh = cv2.threshold(sobelCombined,0,255,cv2.THRESH_BINARY)
		
		#Dialate Frontiers To Form Continuous Contours
		dialate=cv2.dilate(sobelCombined_thresh,np.ones((3,3),'uint8'))
		
		#Find Contours (make copy since dialate destorys original image)
		dialateCopy=copy.copy(dialate)
		contours,hier=cv2.findContours(dialateCopy,cv2.RETR_LIST,cv2.CHAIN_APPROX_NONE)

		#Convert the image back to mat format for publishing as ROS image
		frontiers = cv.fromarray(dialate)
		
		#Create List Data for Marker Message
		centroids = []
		colors = []
		#Filter Frontier Contour by number of pixels
		for i in contours:
			if len(i) > 50:
				moments=cv2.moments(i)
				cx = int(moments['m10']/moments['m00'])
				cy = int(moments['m01']/moments['m00'])
				centroidPoint = Point()
				centroidColor = ColorRGBA()
				centroidPoint.x = cx*self.mapRes+self.mapOrigin.position.x
				centroidPoint.y = cy*self.mapRes+self.mapOrigin.position.y
				centroidPoint.z = 0; 
				centroidColor.r = 0 
				centroidColor.g = 0 
				centroidColor.b = 1 
				centroidColor.a = 1
				centroids.append(centroidPoint)
				colors.append(centroidColor)
				
		#Pack Marker Message
		markerMsg = Marker()
		markerMsg.header.frame_id = "/map"
		markerMsg.header.stamp = rospy.Time.now()
		markerMsg.ns = ""
		markerMsg.id = 0
		markerMsg.type = 7			#Sphere List Type
		markerMsg.action = 0		#Add Mode
		markerMsg.scale.x = 0.5
		markerMsg.scale.y = 0.5
		markerMsg.scale.z = 0.5
		markerMsg.points = centroids
		markerMsg.colors = colors
	
		#Publish Marker and Image messages
		self.imagePub.publish(bridge.cv_to_imgmsg(frontiers, "mono8"))
		self.markerPub.publish(markerMsg)