Source Code for Module rosh.impl.bag

  1  # Software License Agreement (BSD License) 
  2  # 
  3  # Copyright (c) 2010, Willow Garage, Inc. 
  4  # All rights reserved. 
  5  # 
  6  # Redistribution and use in source and binary forms, with or without 
  7  # modification, are permitted provided that the following conditions 
  8  # are met: 
  9  # 
 10  #  * Redistributions of source code must retain the above copyright 
 11  #    notice, this list of conditions and the following disclaimer. 
 12  #  * Redistributions in binary form must reproduce the above 
 13  #    copyright notice, this list of conditions and the following 
 14  #    disclaimer in the documentation and/or other materials provided 
 15  #    with the distribution. 
 16  #  * Neither the name of Willow Garage, Inc. nor the names of its 
 17  #    contributors may be used to endorse or promote products derived 
 18  #    from this software without specific prior written permission. 
 19  # 
 20  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
 21  # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
 22  # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
 23  # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
 24  # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
 25  # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
 26  # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
 27  # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
 28  # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
 29  # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 
 30  # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 31  # POSSIBILITY OF SUCH DAMAGE. 
 32  # 
 33  # Revision $Id: bagy.py 11435 2010-10-07 18:03:49Z kwc $ 
 34   
 35  from __future__ import with_statement 
 36   
 37  import threading 
 38   
 39  import roslib.message 
 40  import rosbag 
 41           
 42  import rosh 
 43  from rosh.impl.exceptions import ROSHException 
 44  from rosh.impl.namespace import Context, NamespaceConfig, Namespace 
 45  from rosh.impl.topic import TopicNS 
 46   
 47 -class BagTopic(Namespace): 
 48   
 49 -    def __init__(self, name, config): 
 50          super(BagTopic, self).__init__(name, config) 
 51          self._type = None 
 52           
 53 -    def __iter__(self): 
 54          return self._config.bag.read_messages(topics=[self._name]) 
 55   
 56 -    def _list(self): 
 57          return self._config.connections.iterkeys() 
 58   
 59 -    def _get_type(self): 
 60          # rostype API 
 61          if self._type is None: 
 62              try: 
 63                  self._type = roslib.message.get_message_class(self._config.connections[self._name].datatype) 
 64              except KeyError: 
 65                  raise TypeError("[%s] does not have a rostype"%(self._name)) 
 66          return self._type 
 67   
 68  # NamespaceConfig infrastructure requires a lock 
 69  _lock = threading.RLock() 
 70   
 71  ZERO_STATE = 0 
 72  RECORD_STATE = 1 
 73  STOP_STATE = 2 
 74   
 75 -class Bag(rosbag.Bag): 
 76       
 77 -    def __init__(self, *args, **kwds): 
 78          """ 
 79          Constructor is the same as rosbag.Bag, with the addition of the 'ctx' keyword argument. 
 80   
 81          @param ctx: ROSH Context 
 82          @type  ctx: Context 
 83          """ 
 84          if 'ctx' in kwds: 
 85              self._ctx = kwds['ctx'] 
 86              del kwds['ctx'] 
 87          else: 
 88              self._ctx = rosh.get_default_plugin_context().ctx 
 89          super(Bag, self).__init__(*args, **kwds) 
 90          self._lock = threading.Lock() 
 91          self._record_topics = None 
 92          self._state = ZERO_STATE 
 93   
 94 -    def __iter__(self): 
 95          return self.read_messages() 
 96       
 97 -    def _get_topics(self): 
 98          d = {} 
 99          for c in self._get_connections(): 
100              d[c.topic] = c 
101   
102          config = NamespaceConfig(self._ctx, _lock) 
103          config.connections = d 
104          config.bag = self 
105          return BagTopic('', config) 
106    
107      topics = property(_get_topics) 
108   
109 -    def _sub_callback(self, msg, topic_name): 
110          with self._lock: 
111              # should this check for header? 
112              self.write(topic_name, msg, rosh.now()) 
113   
114 -    def record(self, topic): 
115          if isinstance(topic, TopicNS): 
116              pass 
117          elif type(topic) == str: 
118              topic = self._ctx.topics[topic] 
119   
120          if self._record_topics is None: 
121              self._record_topics = [] 
122   
123          # start recording if we haven't been explicitly stopped() 
124          self._record_topics.append(topic) 
125           
126          if self._state == ZERO_STATE: 
127              self._state = RECORD_STATE 
128   
129          if self._state == RECORD_STATE: 
130              # TODO: clean this up once I add proper subscribe() to topic 
131              topic._add_subscriber_callback(self._sub_callback, topic._name) 
132   
133 -    def close(self): 
134          if self._state == RECORD_STATE: 
135              self.stop() 
136              self._state = ZERO_STATE 
137          del self._record_topics 
138          super(Bag, self).close() 
139           
140 -    def start(self): 
141          if self._record_topics is None or self._state == ZERO_STATE: 
142              raise ROSHException("not recording") 
143          if self._state == STOP_STATE: 
144              for topic in self._record_topics: 
145                  topic._add_subscriber_callback(self._sub_callback, topic._name) 
146              self._state = RECORD_STATE 
147               
148 -    def stop(self): 
149          if self._record_topics is None or self._state == ZERO_STATE: 
150              raise ROSHException("not recording") 
151           
152          if self._state == RECORD_STATE: 
153              for topic in self._record_topics: 
154                  topic._remove_subscriber_callback(self._sub_callback, topic._name) 
155              self._state = STOP_STATE 
156