rosbrake.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Monitor ROS brake topics for data analysis.
00004 #
00005 #   Copyright (C) 2009 Austin Robot Technology
00006 #
00007 #   License: Modified BSD Software License Agreement
00008 #
00009 # $Id: rosbrake.py 866 2010-11-27 05:10:34Z jack.oquin $
00010 
00011 import roslib;
00012 roslib.load_manifest('art_servo')
00013 
00014 import rospy
00015 from art_msgs.msg import BrakeCommand
00016 from art_msgs.msg import BrakeState
00017 
00018 import sys
00019 import getopt
00020 import os
00021 import threading
00022 
00023 import plot_brake                       # brake data tools
00024 
00025 class commandThread(threading.Thread):
00026     "Separate thread for shell commands."
00027 
00028     def __init__(self, command):
00029         "Shell command thread initialization."
00030         self.command = command
00031         threading.Thread.__init__(self)
00032 
00033     def run(self):
00034         "Run a shell command in this thread."
00035         rc = os.system(self.command)
00036         rospy.signal_shutdown('bag finished')
00037         return rc
00038 
00039 class brakeTopics:
00040     "Monitor ROS brake topics for data analysis."
00041 
00042     def __init__(self, name=None):
00043         "Constructor method."
00044 
00045         # set default name prefix for plots
00046         if name:
00047             self.name = name
00048         else:
00049             self.name = 'brake'
00050         
00051         self.plt = plot_brake.brakeData()
00052 
00053     def get_cmd(self, cmd):
00054         "ROS callback for /brake/cmd topic."
00055         self.plt.set_cmd(cmd.header.stamp.to_sec(), cmd.position)
00056 
00057     def get_state(self, state):
00058         "ROS callback for /brake/state topic."
00059         self.plt.set_state(state.header.stamp.to_sec(), state.position,
00060                            state.potentiometer, state.encoder, state.pressure)
00061 
00062     def get_bag(self, filename):
00063         "Acquire brake topic data from ROS bag."
00064         filename = os.path.expanduser(filename)
00065         self.name, fileext = os.path.splitext(os.path.basename(filename))
00066         commandThread('rosplay -a ' + filename).start()
00067         self.get_data(False)
00068 
00069     def get_data(self, verbose=True):
00070         "Acquire brake topic data until ROS shutdown."
00071         rospy.Subscriber('brake/cmd', BrakeCommand, self.get_cmd)
00072         rospy.Subscriber('brake/state', BrakeState, self.get_state)
00073         rospy.init_node('rosbrake')
00074         if verbose:
00075             print('begin acquiring brake data -- interrupt when finished')
00076         try:
00077             rospy.spin()           # invoke callbacks as data arrive
00078         except rospy.ROSInterruptException: pass
00079         if verbose:
00080             print('done acquiring brake data')
00081 
00082     def plot(self, save=False, plotall=False):
00083         """ Plot some interesting data from the brake test run.
00084 
00085         When save=True write the figures to PNG files, otherwise show
00086         them interactively.
00087         """
00088         self.plt.name = self.name       # use name for plots
00089         if plotall:
00090             self.plt.plot(save)
00091         else:
00092             self.plt.plot_position(save)
00093         
00094 
00095 b = brakeTopics()
00096 
00097 def usage(progname):
00098     "Print usage message."
00099     print("\n" + str(progname) + """[-h] [ <file.bag> ]
00100 
00101  -a, --all         generate all available plots (default: just position)
00102  -h, --help        print this message
00103  -i, --interactive display plots to terminal (default: save as files)
00104 
00105  <file.bag>\tname of ROS bag file to analyze
00106 
00107 If a bag file is specified, read brake topics from it, saving plots of
00108 the data.  Otherwise, read the ROS topics directly from the running
00109 system.
00110  """)
00111 
00112 # main program -- for either script or interactive use
00113 def main(argv=None):
00114     "Main program, called as a script or interactively."
00115 
00116     if argv is None:
00117         argv = sys.argv                 # use command args
00118     argv = rospy.myargv(argv)           # filter out ROS args
00119 
00120     # extract base name of command, will be '' when imported
00121     progname = os.path.basename(argv[0])
00122     if progname is "":
00123         progname = "rosbrake.py"
00124 
00125     # process parameters
00126     try:
00127         opts, files = getopt.gnu_getopt(argv[1:], 'ahi',
00128                                         ('all', 'help', 'interactive'))
00129     except getopt.error, msg:
00130         print(msg)
00131         print("for help use --help")
00132         return 9
00133 
00134     plotall = False
00135     save = True
00136 
00137     for k,v in opts:
00138         if k in ("-a", "--all"):
00139             plotall = True
00140         if k in ("-h", "--help"):
00141             usage(progname)
00142             return 2
00143         if k in ("-i", "--interactive"):
00144             save = False
00145 
00146     if len(files) < 1:
00147         print("no bag file, reading brake topics directly")
00148         b.get_data()
00149         b.plot(save, plotall)
00150 
00151     elif len(files) == 1:
00152         print("Analyzing " + files[0])
00153         b.get_bag(files[0])
00154         b.plot(save, plotall)
00155 
00156     else:
00157         print("only one ROS bag file can be processed at a time")
00158         usage(progname)
00159         return 9
00160 
00161     return 0
00162 
00163 # when called as a script or via python-send-buffer
00164 if __name__ == "__main__":
00165     # run main function and exit
00166     sys.exit(main())


art_servo
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:09:12