matlogger2.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 '''
00003 Created on Feb 04, 2013
00004 
00005 @author: dnad
00006 '''
00007 import rospy;
00008 from threading import Lock
00009 from collections import OrderedDict
00010 import functools
00011 
00012 #\todo Add this to useful tools module in python
00013 #\todo Add imu message, GPS message logging, imu raw logging
00014 def dataToList(data, logOrder): 
00015     return [reduce(getattr, elem.split("."), data)
00016             for elem in logOrder];
00017             
00018 def dataToStrList(data, logOrder): 
00019     return [str(reduce(getattr, elem.split("."), data))
00020             for elem in logOrder];
00021             
00022 class MessageLogger:
00023     def __init__(self, names, Type, logOrder, isList = False):
00024         #self.names = rospy.get_param(names);
00025         self.names = names;
00026         if isList:
00027              self.states = OrderedDict.fromkeys(self.names, [0.0 for elem in logOrder]);
00028         else:
00029             self.states = OrderedDict.fromkeys(self.names, Type());
00030             
00031         self.Type = Type;
00032         self.stateMux = Lock();
00033         self.logOrder=logOrder;
00034         self.isList = isList;
00035            
00036     def subscribe(self):
00037         for key in self.states.keys():
00038             rospy.Subscriber(key, self.Type,
00039                              functools.partial(self.onState,key));
00040     
00041     def onState(self,name,data):
00042         self.stateMux.acquire();
00043         if self.isList:
00044             self.states[name] = data.data;
00045         else:
00046             self.states[name] = data;
00047             
00048         self.stateMux.release();
00049         
00050     def getHeader(self):
00051         retval=[];
00052         for name in self.names:
00053             retval.append("%element: "+name+" \n");
00054             retval.extend(["% " + elem + "\n" for elem in self.logOrder]);
00055 
00056         return retval;
00057                 
00058     def getLogLine(self):
00059         self.stateMux.acquire();
00060         if self.isList:
00061             retval = ["," + ",".join([str(elem) for elem in self.states[name]]) for name in self.names];
00062         else:
00063             retval = ["," + ",".join(dataToStrList(self.states[name], self.logOrder))  for name in self.names];
00064             
00065         self.stateMux.release();
00066         return retval;
00067      
00068 class MatLogger:
00069     def __init__(self):
00070         logData = rospy.get_param("log_data");
00071         
00072         self.loggers=[]
00073         
00074         for dataType in logData:
00075             print ("Logging messages {}" 
00076                     "of type {}.").format(
00077                                           str(dataType[1]),
00078                                           str(dataType[0]));
00079             logOrder = rospy.get_param(str(dataType[0]));
00080             print ("Importing message type:"
00081                    "from {}.msg import {}").format(
00082                                              logOrder[0],
00083                                              logOrder[1]);
00084             module=__import__(name=logOrder[0]+".msg",
00085                                  fromlist=[logOrder[1]]);      
00086             print ("Adding message logger of type"
00087                  "{} with log order {}.").format(
00088                                                  getattr(module, logOrder[1]),
00089                                                  logOrder[2])
00090             isList = (len(logOrder) == 4) and logOrder[3] == 'list';
00091             self.loggers.append(MessageLogger(dataType[1], 
00092                                 getattr(module, logOrder[1]), 
00093                                 logOrder[2], isList))
00094                              
00095         from datetime import datetime;
00096         name = rospy.get_param("~filename","log");
00097         dir = rospy.get_param("~dir",".");             
00098         self.logFile = open(dir + "/" + name + "_" + datetime.today().isoformat() + ".csv",'w');
00099                             
00100         for logger in self.loggers: 
00101             logger.subscribe();
00102                
00103         self.writeHeader();
00104 
00105     def writeHeader(self):
00106         self.logFile.write("%The following element order can be found:\n");
00107         self.logFile.write("%element: stamp \n% t \n");
00108         for logger in self.loggers:
00109             self.logFile.writelines(logger.getHeader());                          
00110                            
00111     def start(self):
00112         rate = rospy.Rate(10);
00113         
00114         while not rospy.is_shutdown():
00115             self.logFile.write(str(rospy.Time.now().to_sec()));     
00116             for logger in self.loggers:
00117                 self.logFile.writelines(logger.getLogLine());
00118             
00119             self.logFile.write("\n");            
00120             rate.sleep();
00121             
00122     def stop(self):
00123         self.logFile.close();
00124         
00125                            
00126 if __name__ == "__main__":
00127     rospy.init_node("logger");
00128     log = MatLogger(); 
00129     log.start();
00130     while not rospy.is_shutdown():
00131         rospy.spin();
00132     log.stop();
00133         
00134         
00135         


matlogger
Author(s): Gyula Nagy
autogenerated on Sun Mar 1 2015 12:12:39