data.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 #     _____
00004 #    /  _  \
00005 #   / _/ \  \
00006 #  / / \_/   \
00007 # /  \_/  _   \  ___  _    ___   ___   ____   ____   ___   _____  _   _
00008 # \  / \_/ \  / /  _\| |  | __| / _ \ | ┌┐ \ | ┌┐ \ / _ \ |_   _|| | | |
00009 #  \ \_/ \_/ /  | |  | |  | └─┐| |_| || └┘ / | └┘_/| |_| |  | |  | └─┘ |
00010 #   \  \_/  /   | |_ | |_ | ┌─┘|  _  || |\ \ | |   |  _  |  | |  | ┌─┐ |
00011 #    \_____/    \___/|___||___||_| |_||_| \_\|_|   |_| |_|  |_|  |_| |_|
00012 #            ROBOTICS™
00013 #
00014 #
00015 #  Copyright © 2012 Clearpath Robotics, Inc. 
00016 #  All Rights Reserved
00017 #  
00018 # Redistribution and use in source and binary forms, with or without
00019 # modification, are permitted provided that the following conditions are met:
00020 #     * Redistributions of source code must retain the above copyright
00021 #       notice, this list of conditions and the following disclaimer.
00022 #     * Redistributions in binary form must reproduce the above copyright
00023 #       notice, this list of conditions and the following disclaimer in the
00024 #       documentation and/or other materials provided with the distribution.
00025 #     * Neither the name of Clearpath Robotics, Inc. nor the
00026 #       names of its contributors may be used to endorse or promote products
00027 #       derived from this software without specific prior written permission.
00028 # 
00029 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00030 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00031 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00032 # DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY
00033 # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00034 # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00035 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00036 # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00037 # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00038 # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00039 #
00040 # Please send comments, questions, or patches to skynet@clearpathrobotics.com
00041 #
00042 
00043 # ROS
00044 import rospy
00045 
00046 # ROS messages
00047 import applanix_msgs.msg
00048 import applanix_generated_msgs.msg
00049 
00050 # Node source
00051 from port import Port
00052 from mapping import groups, msgs
00053 from handlers import GroupHandler, MessageHandler
00054 import translator
00055 
00056 # Python
00057 from cStringIO import StringIO
00058 from threading import Lock
00059 
00060 
00061 class DataPort(Port):
00062   ALLMSGS_SEND_TIMEOUT = rospy.Duration.from_sec(0.01)
00063 
00064   def run(self):
00065     self.sock.settimeout(1.0)
00066 
00067     # Aggregate message for republishing the sensor config as a single blob.
00068     all_msgs = applanix_generated_msgs.msg.AllMsgs()
00069     all_msgs_pub = rospy.Publisher("config", all_msgs.__class__, latch=True) 
00070 
00071     # Listener object which tracks what topics have been subscribed to.
00072     listener = SubscribeListenerManager()
00073   
00074     # Set up handlers for translating different Applanix messages as they arrive.
00075     handlers = {}
00076     for group_num in groups.keys():
00077       include = True
00078       for prefix in self.opts['exclude_prefixes']:
00079         if groups[group_num][0].startswith(prefix): include = False
00080       if include:
00081         handlers[(applanix_msgs.msg.CommonHeader.START_GROUP, group_num)] = \
00082             GroupHandler(*groups[group_num], listener=listener.listener_for(group_num))
00083     for msg_num in msgs.keys():
00084       handlers[(applanix_msgs.msg.CommonHeader.START_MESSAGE, msg_num)] = \
00085           MessageHandler(*msgs[msg_num], all_msgs=all_msgs)
00086 
00087     pkt_counters = {}
00088     bad_pkts = set()
00089 
00090     while not self.finish.is_set():
00091       try:
00092         pkt_id, pkt_str = self.recv()
00093         if pkt_id != None:
00094           handlers[pkt_id].handle(StringIO(pkt_str))
00095 
00096       except ValueError as e:
00097         # Some problem in the recv() routine.
00098         rospy.logwarn(str(e))
00099         continue
00100 
00101       except KeyError:
00102         # No handler for this pkt_id. Only warn on the first sighting.
00103         if pkt_id not in pkt_counters:
00104           rospy.logwarn("Unhandled packet: %s.%d" % pkt_id)
00105 
00106       except translator.TranslatorError:
00107         if pkt_id not in bad_pkts:
00108           rospy.logwarn("Error parsing %s.%d" % pkt_id)
00109           bad_pkts.add(pkt)
00110 
00111       if pkt_id not in pkt_counters:
00112         pkt_counters[pkt_id] = 0
00113       pkt_counters[pkt_id] += 1
00114 
00115 
00116       # Since the config messages come all at once in a burst, we can time out in between those
00117       # bursts and send this aggregate message out with all of them at once.
00118       if all_msgs.last_changed > all_msgs.last_sent and \
00119           rospy.get_rostime() > all_msgs.last_changed + self.ALLMSGS_SEND_TIMEOUT:
00120         all_msgs_pub.publish(all_msgs)
00121         all_msgs.last_sent = rospy.get_rostime() 
00122 
00123 
00124 class SubscribeListenerManager():
00125   def __init__(self):
00126     self.lock = Lock()
00127     self.groups = set()
00128     self.publisher = rospy.Publisher("subscribed_groups", applanix_msgs.msg.Groups, latch=True)
00129     self.publish()
00130 
00131   def publish(self):
00132     self.publisher.publish(groups=list(self.groups))
00133 
00134   def listener_for(self, group_num):
00135     return self.Listener(self, group_num)
00136 
00137   class Listener(rospy.SubscribeListener):
00138     def __init__(self, manager, group_num):
00139       self.manager = manager
00140       self.group_num = group_num
00141 
00142     def peer_subscribe(self, topic_name, topic_publish, peer_publish):
00143       with self.manager.lock:
00144         self.manager.groups.add(self.group_num)
00145         self.manager.publish()
00146 
00147     def peer_unsubscribe(self, topic_name, num_peers):
00148       if num_peers == 0:
00149         with self.lock:
00150           self.manager.groups.discard(self.group_num)
00151           self.manager.publish()


applanix_bridge
Author(s): Mike Purvis
autogenerated on Thu Jan 2 2014 11:05:14