Topics2CSV

Topics2CSV.py

#!/usr/bin/env python
import rospy
import sys
import socket
import rosgraph
import roslib.message

def msgevalgen(pattern):
    """
    Generates a function that returns the relevant field (aka 'subtopic') of a Message object
    :param pattern: subtopic, e.g. /x. Must have a leading '/' if specified, ``str``
    :returns: function that converts a message into the desired value, ``fn(Message) -> value``
    """
    if not pattern or pattern == '/':
        return None
    assert pattern[0] == '/'
    msg_attribute = pattern[1:]

    # use slice arguments if present
    array_index_or_slice_object = None
    index = msg_attribute.find('[')
    if index != -1:
        if not msg_attribute.endswith(']'):
            sys.stderr.write("Topic name '%s' contains '[' but does not end with ']'\n" % msg_attribute)
            return None
        index_string = msg_attribute[index + 1:-1]
        try:
            array_index_or_slice_object = _get_array_index_or_slice_object(index_string)
        except AssertionError as e:
            sys.stderr.write("Topic name '%s' contains invalid slice argument '%s': %s\n" % (msg_attribute, index_string, str(e)))
            return None
        msg_attribute = msg_attribute[:index]




def _master_get_topic_types(master):
    try:
        val = master.getTopicTypes()
    except Fault:
        #TODO: remove, this is for 1.1
        sys.stderr.write("WARNING: rostopic is being used against an older version of ROS/roscore\n")
        val = master.getPublishedTopics('/')
    return val

def _get_topic_type(topic):
    """
    subroutine for getting the topic type
    :returns: topic type, real topic name and fn to evaluate the message instance
    if the topic points to a field within a topic, e.g. /rosout/msg, ``(str, str, fn)``
    """
    try:
        val = _master_get_topic_types(rosgraph.Master('/rostopic'))
    except socket.error:
        raise ROSTopicIOException("Unable to communicate with master!")

    # exact match first, followed by prefix match
    matches = [(t, t_type) for t, t_type in val if t == topic]
    if not matches:
        matches = [(t, t_type) for t, t_type in val if topic.startswith(t+'/')]
        # choose longest match
        matches.sort(key=itemgetter(0), reverse=True)

        # try to ignore messages which don't have the field specified as part of the topic name
        while matches:
            t, t_type = matches[0]
            msg_class = roslib.message.get_message_class(t_type)
            if not msg_class:
                # if any class is not fetchable skip ignoring any message types
                break
            msg = msg_class()
            nested_attributes = topic[len(t) + 1:].rstrip('/')
            nested_attributes = nested_attributes.split('[')[0]
            if nested_attributes == '':
                break
            try:
                _get_nested_attribute(msg, nested_attributes)
            except AttributeError:
                # ignore this type since it does not have the requested field
                matches.pop(0)
                continue
            matches = [(t, t_type)]
            break

    if matches:
        t, t_type = matches[0]
        if t_type == rosgraph.names.ANYTYPE:
            return None, None, None
        return t_type, t, msgevalgen(topic[len(t):])
    else:
        return None, None, None


def get_topic_type(topic, blocking=False):
    """
    Get the topic type.

    :param topic: topic name, ``str``
    :param blocking: (default False) block until topic becomes available, ``bool``
    
    :returns: topic type, real topic name and fn to evaluate the message instance
      if the topic points to a field within a topic, e.g. /rosout/msg. fn is None otherwise. ``(str, str, fn)``
    :raises: :exc:`ROSTopicException` If master cannot be contacted
    """
    topic_type, real_topic, msg_eval = _get_topic_type(topic)
    if topic_type:
        return topic_type, real_topic, msg_eval
    elif blocking:
        sys.stderr.write("WARNING: topic [%s] does not appear to be published yet\n"%topic)
        while not rospy.is_shutdown():
            topic_type, real_topic, msg_eval = _get_topic_type(topic)
            if topic_type:
                return topic_type, real_topic, msg_eval
            else:
                _sleep(0.1)
    return None, None, None

def _sleep(duration):
    rospy.rostime.wallsleep(duration)

def get_topic_class(topic, blocking=False):
    """
    Get the topic message class
    :returns: message class for topic, real topic
      name, and function for evaluating message objects into the subtopic
      (or ``None``). ``(Message, str, str)``
    :raises: :exc:`ROSTopicException` If topic type cannot be determined or loaded
    """
    topic_type, real_topic, msg_eval = get_topic_type(topic, blocking=blocking)
    if topic_type is None:
        return None, None, None
    msg_class = roslib.message.get_message_class(topic_type)
    if not msg_class:
        raise ROSTopicException("Cannot load message class for [%s]. Are your messages built?"%topic_type)
    return msg_class, real_topic, msg_eval

# Global Variables
topicList_=[]
SubscriberList=[]

def callback(data,callback_args):
  topicList_[callback_args]=data

def CreateSubscribers(topicNames):

  topicNum=len(topicNames)

  for itopic in range(topicNum):
    #Get Topic Class
    msg_class, real_topic, msg_eval = get_topic_class(topicNames[itopic], blocking=True)
    d=msg_class()
    topicList_.append(d)

    SubscriberList.append(rospy.Subscriber(real_topic, msg_class, callback, itopic))


def GetProperty(stream,topic):
  proparties=topic.__slots__
  for proparty in proparties:
    value=getattr(topic,proparty)
    print proparty
    print value
    print type(value)
    if type(value) in (int, float):
      stream+=(str(value)+',')
      print stream
    else:
      GetProperty(stream,value)

  return stream

def GetDataStream(topicList):
  stream=""
  for topic in topicList:
    GetProperty(stream,topic)
    print topic
    print stream
    a


def Main():
  fileName=sys.argv[1]
  f=open(fileName,'w')
  topicNames=sys.argv[2:]

  CreateSubscribers(topicNames)

  while not rospy.is_shutdown():
    GetDataStream(topicList_)
    #print topicList
    #print getattr(topicList[0],'header/secs')
    #print topicList[0]._slot_.types[1]
    _sleep(0.1)

  f.close()

if __name__=="__main__":
  rospy.init_node("Topics2CSV",anonymous=True)
  Main()