jsk_pr2_startup: mux_selector.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/python
00002 import roslib; roslib.load_manifest('jsk_pr2_startup')
00003 import rospy
00004 import thread
00005 import sys
00006 
00007 import rostopic
00008 
00009 from topic_tools.srv import MuxSelect
00010 
00011 def callback (m, expr, topic, index):
00012     global selects, lockobj
00013     lockobj.acquire()
00014     try:
00015         if(eval(expr)):
00016             selects[index] = (topic, rospy.Time.now())
00017         else:
00018             selects[index] = (None, rospy.Time.now())
00019     except Error, e:
00020         rospy.loginfo('error:'+str(e))
00021     lockobj.release()
00022 
00023 def gen_callback(expr, select, index):
00024     return (lambda m: callback(m, expr, select, index))
00025 
00026 def add_trigger(topic, expr, select, index):
00027     topic_type, _, _ = rostopic.get_topic_type(topic)
00028     topic_class, _, _ = rostopic.get_topic_class(topic)
00029 
00030     if(topic_type == None):
00031         rospy.loginfo('%s is not published yet', topic)
00032         return None
00033 
00034     if(topic_class == None):
00035         rospy.loginfo('%s is not builded yet', topic_type)
00036         return None
00037 
00038     try:
00039         cb_obj = gen_callback(expr, select, index)
00040         rospy.loginfo("start subscribe (topic=%s type=%s)", topic, topic_type);
00041         sub = rospy.Subscriber(topic,topic_class,cb_obj)
00042     except Exception, e:
00043         rospy.loginfo(str(e))
00044         return None
00045 
00046     return sub
00047 
00048 def update_trigger(conditions):
00049     # setting triggers
00050     global subs
00051     for index in range(len(conditions)):
00052         if subs[index] != None:
00053             continue
00054         cond = conditions[index]
00055         subs[index] = add_trigger(cond[0],cond[1],cond[2],index)
00056 
00057 
00058 if __name__ == "__main__":
00059     global selects, subs
00060     global lockobj
00061     lockobj = thread.allocate_lock()
00062     selects = []
00063     subs = []
00064 
00065     # parse arguments
00066     conditions = [x for x in sys.argv[1:] if not ':=' in x]
00067 
00068     if len(conditions) % 3 != 0:
00069         rospy.logerr('args must be (read_topic condition select_topic)...')
00070         exit(0)
00071 
00072     conditions = [(conditions[i],conditions[i+1],conditions[i+2])
00073                   for i in range(len(conditions)) if i % 3 == 0]
00074 
00075     # ros node initialization
00076     rospy.init_node('mux_selector')
00077     deadtime = rospy.get_param('~patient', 0.5)
00078     freq = rospy.get_param('~frequency', 20.0)
00079     default_select = rospy.get_param('~default_select', None)
00080 
00081     mux_name_ = rospy.resolve_name('mux')
00082     rospy.wait_for_service(mux_name_+'/select')
00083     mux_client = rospy.ServiceProxy(mux_name_+'/select',MuxSelect)
00084 
00085     size = len(conditions)
00086     selects = [(None, rospy.Time(0))] * size
00087     subs = [None] * size
00088 
00089     # loop
00090     try:
00091         before = default_select
00092         update_trigger(conditions)
00093         looprate = rospy.Rate(freq)
00094         while not rospy.is_shutdown():
00095             lockobj.acquire()
00096             cand = [x[0] for x in selects if x[0] != None and (rospy.Time.now()-x[1]).to_sec()<deadtime]
00097             lockobj.release()
00098 
00099             if cand != []:
00100                 next_topic = cand[0]
00101             else:
00102                 next_topic = default_select
00103             try:
00104                 if before != next_topic and before != None:
00105                     mux_client(next_topic)
00106             except rospy.ServiceException, e:
00107                 rospy.loginfo("Service did not process request: %s", str(e))
00108             before = next_topic
00109             looprate.sleep()
00110     except Exception, e:
00111         rospy.loginfo('error:'+str(e))