rviz浮层显示文字内容功能节点

#!/usr/bin/env python
# coding=utf-8
#coding:utf-8

import rospy
import sys

import cv2
import os
import math


from std_msgs.msg import String
#from satellite_msg.msg import SatelliteState
from jsk_rviz_plugins.msg import OverlayText
import string
import json
import time

import threading
import re

class OverlayTextNode():

    def __init__(self):
        
        rospy.init_node('OverlayTextNode',anonymous = True)
        self.rate_hz = rospy.get_param("~rate", 10)
        self.threshold_s = rospy.get_param("~threshold", 5)
        

        self.ot_action=  rospy.get_param("~ot_action", 0)
        self.ot_width= rospy.get_param("~ot_width", 360)
        self.ot_height= rospy.get_param("~ot_height", 600)
        self.ot_left= rospy.get_param("~ot_left", 0)
        self.ot_top= rospy.get_param("~ot_top", 0)
        self.ot_bg_color= rospy.get_param("~ot_bg_color", '00000010')
        self.ot_line_width= rospy.get_param("~ot_line_width", 360)
        self.ot_text_size= rospy.get_param("~ot_text_size", 14)
        self.ot_font= rospy.get_param("~ot_font", "黑体")#'arial'
        self.ot_fg_color= rospy.get_param("~ot_fg_color", '2878c5c0')
        #05179fff 蓝
        #2878c5ff 兰
        #C08001ff 土黄
        self.ot_bg_color= self.check_hex(self.ot_bg_color,'00000010')
        self.ot_fg_color=  self.check_hex(self.ot_fg_color,'2878c5c0')
        r,g,b,a=self.parse_rgba( self.ot_bg_color)
        self.ot_bg_color_r=float(r)/255.0
        self.ot_bg_color_g=float(g)/255.0
        self.ot_bg_color_b=float(b)/255.0
        self.ot_bg_color_a=float(a)/255.0
        r,g,b,a=self.parse_rgba( self.ot_fg_color)
        self.ot_fg_color_r=float(r)/255.0
        self.ot_fg_color_g=float(g)/255.0
        self.ot_fg_color_b=float(b)/255.0
        self.ot_fg_color_a=float(a)/255.0
        #text':

        self.buffer=[]
        self.rate = rospy.Rate(self.rate_hz)
        rospy.Subscriber('/overlaytext', String, self.recv_string) 
        self.puber = rospy.Publisher('/overlaytext_rviz', OverlayText, queue_size=1)

    def check_hex(self,s,v):
        p = re.compile(r'\b([0-9a-fA-F]+)\b')
        m = re.search(p, s)
        if m:
           return m.group(1)
        else:
           return v


    def parse_rgba(self, s):
        r=self.str2int(s[0:2],0)
        g=self.str2int(s[2:4],0)
        b=self.str2int(s[4:6],0)
        a=self.str2int(s[6:8],255)
        return (r,g,b,a)


    def str2int(self, s,v):
        try:
            r=int(s,16)
        except:
            r=v
        return r
        
        
    def recv_string(self, msg):
        o=None
        if type(msg) is String:
            try:
                #print msg
                #print msg.data
                o = json.loads(msg.data)
                #print o
                if(o["flag"] and   o["sort"] and   o["text"]):
                    pass
                
            except:
                o = {"flag":"temp","sort":0,"text":msg.data}

                pass
        else:
            
            pass  
        #print o
        self.save_to_buff(o)
        #{"flag":"statellite_info","sort":0,"text":"xxxx"}

    def save_to_buff(self, o):
        if o :
            o["time"]=time.time()
            f=o["flag"]
            s=o["sort"]
            bo=self.get_from_buff(f,s)
            if bo:
                #update
                print "update", o["flag"]
                bo["text"]=o["text"]
                bo["time"]=o["time"]
            else:
                #insert
                print "insert", o["flag"]
                inid=self.get_sort_id(s)
                self.buffer.insert(inid,o)
            
        else:
            pass 

    def get_from_buff(self,flag,sort):

        for d in self.buffer:
            if d["flag"]==flag and d["sort"]==sort:
                return d
        return None

    def get_sort_id(self,sort):
        l=len( self.buffer)
        in_id=0
        is_ok=0
        for i in  range(l):
            d=self.buffer[i]
            s=d["sort"]
            if sort>=s:
                in_id=i
                is_ok=1
                break
        if is_ok==0:
            in_id=l
        return in_id
            
    def check_time_buff(self):
        l=len( self.buffer)

        for i in list(reversed( range(l))):
            d=self.buffer[i]
            print d["flag"], i ,time.time()- d["time"]
            if time.time()- d["time"]>self.threshold_s:
                print "del",i
                del self.buffer[i]
           
    def pack_buff(self):
        l=len( self.buffer)
        s=""
        for i in  range(l):
            d=self.buffer[i]
            s=s+d["flag"]+":\n"+d["text"]+"\n\n"
        return s
    def pub_buff(self):
        s=self.pack_buff()
        msg=OverlayText()
        #rostopic pub -r 1 statellite_info1 jsk_rviz_plugins/OverlayText -- "{'action':0, 'width':200, 'height':200, 'left':50, 'top':50, 'bg_color':{'r':0.22,'g':0.22,'b':0.22,'a':0.5}, 'line_width':200, 'text_size':24, 'font':'arial', 'fg_color':{'r':1,'g':1,'b':1,'a':0.9}, 'text':'thetextishere'}"   
        

        msg.action=self.ot_action
        msg.width=self.ot_width
        msg.height=self.ot_height
        msg.left=self.ot_left
        msg.top=self.ot_top
        msg.bg_color.r=self.ot_bg_color_r
        msg.bg_color.g=self.ot_bg_color_g
        msg.bg_color.b=self.ot_bg_color_b
        msg.bg_color.a=self.ot_bg_color_a
        msg.line_width=self.ot_line_width
        msg.text_size=self.ot_text_size
        msg.font=self.ot_font
        msg.fg_color.r=self.ot_fg_color_r
        msg.fg_color.g=self.ot_fg_color_g
        msg.fg_color.b=self.ot_fg_color_b
        msg.fg_color.a=self.ot_fg_color_a
        msg.text=s
        self.puber.publish(msg)


    def run(self):
        print("OverlayTextNode running...")
        c=0
        while not rospy.is_shutdown():
                c=c+1
                self.rate.sleep()
                self.pub_buff()
                if c%self.rate_hz==0:
                    self.check_time_buff()
                if rospy.is_shutdown():
                    break



if __name__ == '__main__':


    try:
        m=OverlayTextNode()
        m.run()
    except rospy.ROSInterruptException:
        pass