sam_topic_loader

#! /usr/bin/env python
import sys
import socket
import yaml
import rospy
import roslib, roslib.message
import rosnode
from sam_helpers.writer import SAMWriter
from monarch_situational_awareness.srv import GetSlotInfo, GetSlotInfoResponse

def cb(m, w):
    w.publish(m)

if __name__ == '__main__':
    rospy.init_node('sam_topic_loader')

    argv = rospy.myargv(argv=sys.argv)

    if len(argv) < 2:
        rospy.logfatal("Usage: sam_topic_loader <path to YAML configuration file>")
        sys.exit()


    found_sam = False
    d = 5.0
    sam_name = "sam_node"
    while found_sam == False:
        try:
            names = rosnode.get_node_names()
            l = len(sam_name)
            ns = ""
            for n in names:
                if len(n) > l and n[-l:] == sam_name:
                    ns = rospy.names.namespace(n)
                    found_sam = True
                    break
            if found_sam == False:
                rospy.logwarn("Could not find the SAM node in the ROS namespace (%s)",sam_name)
                rospy.logwarn("Retrying in %d secs.",d)
                rospy.sleep(d)
        except rosnode.ROSNodeIOException as e:
            rospy.logwarn(e)
            rospy.logwarn("Retrying in %d secs.",d)
            rospy.sleep(d)

    g = rospy.ServiceProxy(ns+'get_slot_info', GetSlotInfo)

    subs = []
    writers = []
    yaml_path = argv[1]
    f = open(yaml_path, 'r')
    d = yaml.load(f)
    if type(d) is not dict:
        raise IOError("Bad syntax in the YAML configuration file. It should contain a dictionary.")
        sys.exit()
    if not d.has_key("singles") and not d.has_key("groups"):
        raise IOError("Bad syntax in the YAML configuration file. It should contain either the 'singles' or 'groups' keys (same syntax as slot_config)")
        sys.exit()

    if d.has_key("singles") and d["singles"] is not None:
        topics_to_load = d["singles"]
        for slot,topic in topics_to_load.items():
            r = g(slot,"")
            if len(r.properties.name) > 0:
                writers.append(SAMWriter(slot))
                type_class = roslib.message.get_message_class(r.properties.type_str)
                subs.append(rospy.Subscriber(topic,
                                             type_class,
                                             cb,
                                             callback_args=writers[-1]))
            else:
                rospy.logerr("SAM Topic Loader: Slot '%s' does not exist!", slot)

    if d.has_key("groups") and d["groups"] is not None:
        topics_to_load = d["groups"]
        host = socket.gethostname()
        """MONARCH-SPECIFIC Code"""
        host = host[0:-1]
        """This removes the suffix (l/w) from the mbot hostnames"""
        for slot,topic in topics_to_load.items():
            r = g(slot,host)
            if len(r.properties.name) > 0:
                writers.append(SAMWriter(slot,host))
                type_class = roslib.message.get_message_class(r.properties.type_str)
                subs.append(rospy.Subscriber(topic,
                                             type_class,
                                             cb,
                                             callback_args=writers[-1]))
            else:
                rospy.logerr("SAM Topic Loader: Slot '%s' does not exist for host '%s'!", slot, host)

    rospy.spin()