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()
#
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))