$search

interactive_marker_client.py

Go to the documentation of this file.

#!/usr/bin/env python
import roslib; roslib.load_manifest('interactive_marker_client')
import rospy
from threading import Lock
from visualization_msgs.msg import *
from interactive_marker_client.msg import *
from interactive_marker_client.srv import *

class InteractiveMarkerClient:
  def __init__(self,topic):
    self.init = True
    self.stopFlag = True
    self.pub_postfix = '_web'
    self.pub_topic = topic + self.pub_postfix
    self.lock = Lock()
    
    self.topic = topic
    self.topic_full = topic + '_full'

    self.full_marker_list = []
    self.full_marker_list_index = 0

  def start(self): 
    self.sub_update      = rospy.Subscriber(self.topic,InteractiveMarkerUpdate,self.update)
    self.sub_update_full = rospy.Subscriber(self.topic_full,InteractiveMarkerInit, self.update_full)
    self.pub             = rospy.Publisher(self.pub_topic,InteractiveMarkerUpdateWeb)

  def stop(self):
    self.sub_update.unregister()
    self.sub_update_full.unregister()
#self.pub.unregister()

  def reinit(self):
    self.init = True

  def update(self,msg):
#print "Update       = " + str(msg.seq_num)
    if self.init:
      m = self.getMatchedMarker(msg.seq_num,msg.server_id)  
      if m == 0: # seq lower
        print "seq == lower "
        return
      elif m == 1: # seq higher
        print "seq == higher "
        rospy.sleep(0.1)
        return
      else:
        newMsg = self.copyMsg(msg)
        newMsg.type = 2
        newMsg.markers = m
        newMsg.poses = []
        newMsg.erases = []
        self.init = False
    else:
      newMsg = self.copyMsg(msg)

    self.pub.publish(newMsg)

  def update_full(self,msg):
    print "Seq = " + str(msg.seq_num)
    index = self.full_marker_list_index
    
    self.full_marker_list.insert(index,[msg.seq_num,msg.server_id,msg.markers])
    index = index + 1
    if index == 100:
      index = 0
    

  def copyMsg(self,msg):
    newMsg = InteractiveMarkerUpdateWeb()
    newMsg.server_id = msg.server_id
    newMsg.seq_num = msg.seq_num
    newMsg.type = msg.type
    newMsg.markers = msg.markers
    newMsg.poses = msg.poses
    newMsg.erases = msg.erases
    return newMsg

  def getMatchedMarker(self,seq,serverid):
    val = 0
    for marker in self.full_marker_list:
      if marker[1] == serverid and marker[0] < seq:
        val = 1
      elif marker[1] == serverid and marker[0] > seq:
        val = 0
      elif marker[1] == serverid:
        val = marker[2]
        break
    return val

class NodeClientMaster:
  def __init__(self):
    self.requestName = 'interactive_marker_request';
    self.initName = 'interactive_marker_init_request';
    self.topicDict = {}


  def receiveStartRequest(self,req):
    print req
    topic = self.setTopic(req.topic)
    if topic in self.topicDict:
      if not req.on:
        self.topicDict[topic].stop()
        del self.topicDict[topic]
        rospy.loginfo(topic + ' is removed')
        return RequestInteractiveMarkerResponse(req.topic,True)
      return RequestInteractiveMarkerResponse(req.topic,False)
    else:
      if req.on:
        node = InteractiveMarkerClient(topic)
        node.start()
        self.topicDict[topic] = node
        rospy.loginfo(topic + ' is started')
        return RequestInteractiveMarkerResponse(req.topic,True)
    return RequestInteractiveMarkerResponse(req.topic, False)

  def receiveInitRequest(self,req):
    topic = self.setTopic(req.topic)
    if topic in self.topicDict:
      self.topicDict[topic].reinit()
      rospy.loginfo(topic + ' : reinitialization requested')
    else:
      rospy.loginfo(topic + ' : does not exist')

    return RequestInitResponse()

  def setTopic(self,topic):
    start = 0
    end = len(topic)
    if topic[0] == '/':
      start = 1
    return topic[start:end-4]
      


  def spin(self):
    self.requestService = rospy.Service(self.requestName,RequestInteractiveMarker,self.receiveStartRequest)
    self.initService = rospy.Service(self.initName,RequestInit,self.receiveInitRequest)

    rospy.loginfo('Interactive Marker client Initialized')

    rospy.spin()
    rospy.loginfo('byebye')

if __name__ == "__main__":
  try:
    rospy.init_node('interactive_marker_client')
    node = NodeClientMaster()
    node.spin() 
  except rospy.ROSInterruptException: pass

---

interactive_marker_client
Author(s): Jihoon Lee
autogenerated on Sun Mar 3 12:25:45 2013