Go to the documentation of this file.00001
00002 import rospy
00003 import rostopic
00004 import inspect
00005 from flexbe_core import EventState, Logger
00006
00007 from flexbe_core.proxy import ProxySubscriberCached
00008
00009 '''
00010 Created on 11.06.2013
00011
00012 @author: Philipp Schillinger
00013 '''
00014
00015 class SubscriberState(EventState):
00016 '''
00017 Gets the latest message on the given topic and stores it to userdata.
00018
00019 -- topic string The topic on which should be listened.
00020 -- blocking bool Blocks until a message is received.
00021 -- clear bool Drops last message on this topic on enter
00022 in order to only handle message received since this state is active.
00023
00024 #> message object Latest message on the given topic of the respective type.
00025
00026 <= received Message has been received and stored in userdata or state is not blocking.
00027 <= unavailable The topic is not available when this state becomes actives.
00028
00029 '''
00030
00031
00032 def __init__(self, topic, blocking = True, clear = False):
00033 '''
00034 Constructor
00035 '''
00036 super(SubscriberState, self).__init__(outcomes=['received', 'unavailable'],
00037 output_keys=['message'])
00038
00039 self._topic = topic
00040 self._blocking = blocking
00041 self._clear = clear
00042 self._connected = False
00043
00044 (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
00045
00046 if msg_topic == self._topic:
00047 msg_type = self._get_msg_from_path(msg_path)
00048 self._sub = ProxySubscriberCached({self._topic: msg_type})
00049 self._connected = True
00050 else:
00051 Logger.logwarn('Topic %s for state %s not yet available.\nFound: %s\nWill try again when entering the state...' % (self._topic, self.name, str(msg_topic)))
00052
00053
00054 def execute(self, userdata):
00055 '''
00056 Execute this state
00057 '''
00058 if not self._connected:
00059 userdata.message = None
00060 return 'unavailable'
00061
00062 if self._sub.has_msg(self._topic) or not self._blocking:
00063 userdata.message = self._sub.get_last_msg(self._topic)
00064 self._sub.remove_last_msg(self._topic)
00065 return 'received'
00066
00067
00068 def on_enter(self, userdata):
00069 if not self._connected:
00070 (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
00071 if msg_topic == self._topic:
00072 msg_type = self._get_msg_from_path(msg_path)
00073 self._sub = ProxySubscriberCached({self._topic: msg_type})
00074 self._connected = True
00075 Logger.loginfo('Successfully subscribed to previously unavailable topic %s' % self._topic)
00076 else:
00077 Logger.logwarn('Topic %s still not available, giving up.' % self._topic)
00078
00079 if self._connected and self._clear and self._sub.has_msg(self._topic):
00080 self._sub.remove_last_msg(self._topic)
00081
00082
00083 def _get_msg_from_path(self, msg_path):
00084 msg_import = msg_path.split('/')
00085 msg_module = '%s.msg' % (msg_import[0])
00086 package = __import__(msg_module, fromlist=[msg_module])
00087 clsmembers = inspect.getmembers(package, lambda member: inspect.isclass(member) and member.__module__.endswith(msg_import[1]))
00088 return clsmembers[0][1]