Beispiel #1
0
  def run(self):
    self.sock.settimeout(1.0)

    # Aggregate message for republishing the sensor config as a single blob.
    all_msgs = applanix_generated_msgs.msg.AllMsgs()
    all_msgs_pub = rospy.Publisher("config", all_msgs.__class__, latch=True) 

    # Listener object which tracks what topics have been subscribed to.
    listener = SubscribeListenerManager()
  
    # Set up handlers for translating different Applanix messages as they arrive.
    handlers = {}
    for group_num in groups.keys():
      include = True
      for prefix in self.opts['exclude_prefixes']:
        if groups[group_num][0].startswith(prefix): include = False
      if include:
        handlers[(applanix_msgs.msg.CommonHeader.START_GROUP, group_num)] = \
            GroupHandler(*groups[group_num], listener=listener.listener_for(group_num))
    for msg_num in msgs.keys():
      handlers[(applanix_msgs.msg.CommonHeader.START_MESSAGE, msg_num)] = \
          MessageHandler(*msgs[msg_num], all_msgs=all_msgs)

    pkt_counters = {}
    bad_pkts = set()

    while not self.finish.is_set():
      try:
        pkt_id, pkt_str = self.recv()
        if pkt_id != None:
          handlers[pkt_id].handle(StringIO(pkt_str))

      except ValueError as e:
        # Some problem in the recv() routine.
        rospy.logwarn(str(e))
        continue

      except KeyError:
        # No handler for this pkt_id. Only warn on the first sighting.
        if pkt_id not in pkt_counters:
          rospy.logwarn("Unhandled packet: %s.%d" % pkt_id)

      except translator.TranslatorError:
        if pkt_id not in bad_pkts:
          rospy.logwarn("Error parsing %s.%d" % pkt_id)
          bad_pkts.add(pkt)

      if pkt_id not in pkt_counters:
        pkt_counters[pkt_id] = 0
      pkt_counters[pkt_id] += 1


      # Since the config messages come all at once in a burst, we can time out in between those
      # bursts and send this aggregate message out with all of them at once.
      if all_msgs.last_changed > all_msgs.last_sent and \
          rospy.get_rostime() > all_msgs.last_changed + self.ALLMSGS_SEND_TIMEOUT:
        all_msgs_pub.publish(all_msgs)
        all_msgs.last_sent = rospy.get_rostime() 
Beispiel #2
0
#

import roslib; roslib.load_manifest('applanix_generated_msgs')
from roslib.packages import get_pkg_dir
from mapping import groups, msgs
from os import path
from sys import stdout

pkg_dir = get_pkg_dir('applanix_generated_msgs')
output = []

all_msgs_filename = path.join(pkg_dir, "msg", "AllMsgs.msg")
with open(all_msgs_filename, "w") as all_msgs_f:
  all_msgs_f.write("time last_changed\n")
  all_msgs_f.write("time last_sent\n")
  for msg_num in msgs.keys():
    msg_tuple = msgs[msg_num]
    if msg_tuple:
      name, msg_cls = msg_tuple
      if msg_cls.in_all_msgs: 
        all_msgs_f.write("applanix_msgs/%s %s\n" % (msg_cls.__name__, name))

      if name != "ack":
        msg_srv_filename = path.join(pkg_dir, "srv", "%s.srv" % msg_cls.__name__)
        output.append("%s.srv" % msg_cls.__name__)
        with open(msg_srv_filename, "w") as msg_srv_f:
          msg_srv_f.write("applanix_msgs/%s request\n" % msg_cls.__name__)
          msg_srv_f.write("---\n")
          msg_srv_f.write("applanix_msgs/Ack ack")

stdout.write(";".join(output))