rosspeed.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Monitor ROS speed control topics for data analysis.
00004 #
00005 #   Copyright (C) 2009 Austin Robot Technology
00006 #
00007 #   License: Modified BSD Software License Agreement
00008 #
00009 # $Id: rosspeed.py 1541 2011-05-09 19:07:23Z jack.oquin $
00010 
00011 PKG_NAME = 'art_pilot'
00012 
00013 # standard Python packages
00014 import sys
00015 import getopt
00016 import os
00017 import threading
00018 
00019 # ROS node setup
00020 import roslib;
00021 roslib.load_manifest(PKG_NAME)
00022 import rospy
00023 
00024 # ROS messages
00025 from nav_msgs.msg import Odometry
00026 from art_msgs.msg import BrakeCommand
00027 from art_msgs.msg import BrakeState
00028 from art_msgs.msg import CarCommand
00029 from art_msgs.msg import CarDriveStamped
00030 from art_msgs.msg import ThrottleCommand
00031 from art_msgs.msg import ThrottleState
00032 
00033 import plot_speed                       # speed data tools
00034 
00035 class commandThread(threading.Thread):
00036     "Separate thread for shell commands."
00037 
00038     def __init__(self, command):
00039         "Shell command thread initialization."
00040         self.command = command
00041         threading.Thread.__init__(self)
00042 
00043     def run(self):
00044         "Run a shell command in this thread."
00045         rc = os.system(self.command)
00046         rospy.signal_shutdown('bag finished')
00047         return rc
00048 
00049 class speedTopics:
00050     "Monitor ROS speed topics for data analysis."
00051 
00052     def __init__(self, name=None):
00053         "Constructor method."
00054 
00055         # set default name prefix for plots
00056         if name:
00057             self.name = name
00058         else:
00059             self.name = 'speed'
00060         
00061         self.plt = plot_speed.speedData()
00062 
00063     def get_pilot_drive(self, cmd):
00064         "ROS callback for /pilot/drive topic."
00065         self.plt.set_pilot_cmd(cmd.header.stamp.to_sec(),
00066                                cmd.control.speed)
00067 
00068     def get_pilot_cmd(self, cmd):
00069         "ROS callback for /pilot/cmd topic."
00070         self.plt.set_pilot_cmd(cmd.header.stamp.to_sec(),
00071                                cmd.control.velocity)
00072 
00073     def get_odometry(self, odom):
00074         "ROS callback for /odom topic."
00075         if odom.header.stamp.to_sec() != 0.0:
00076             self.plt.set_odometry(odom.header.stamp.to_sec(),
00077                                   odom.twist.twist.linear.x)
00078 
00079     def get_brake_cmd(self, cmd):
00080         "ROS callback for /brake/cmd topic."
00081         self.plt.set_brake_cmd(cmd.header.stamp.to_sec(), cmd.position)
00082 
00083     def get_brake_state(self, state):
00084         "ROS callback for /brake/state topic."
00085         self.plt.set_brake_state(state.header.stamp.to_sec(), state.position)
00086 
00087     def get_throttle_cmd(self, cmd):
00088         "ROS callback for /throttle/cmd topic."
00089         self.plt.set_throttle_cmd(cmd.header.stamp.to_sec(), cmd.position)
00090 
00091     def get_throttle_state(self, state):
00092         "ROS callback for /throttle/state topic."
00093         self.plt.set_throttle_state(state.header.stamp.to_sec(), state.position)
00094 
00095     def get_bag(self, filename):
00096         "Acquire speed topic data from ROS bag."
00097         filename = os.path.expanduser(filename)
00098         self.name, fileext = os.path.splitext(os.path.basename(filename))
00099         commandThread('rosplay -a ' + filename).start()
00100         self.get_data(False)
00101 
00102     def get_data(self, verbose=True):
00103         "Acquire speed topic data until ROS shutdown."
00104         rospy.Subscriber('pilot/drive', CarDriveStamped, self.get_pilot_drive)
00105         rospy.Subscriber('pilot/cmd', CarCommand, self.get_pilot_cmd)
00106         rospy.Subscriber('odom', Odometry, self.get_odometry)
00107         rospy.Subscriber('brake/cmd', BrakeCommand, self.get_brake_cmd)
00108         rospy.Subscriber('brake/state', BrakeState, self.get_brake_state)
00109         rospy.Subscriber('throttle/cmd', ThrottleCommand, self.get_throttle_cmd)
00110         rospy.Subscriber('throttle/state', ThrottleState,
00111                          self.get_throttle_state)
00112 
00113         rospy.init_node('rosspeed')
00114         if verbose:
00115             print 'begin acquiring speed data -- interrupt when finished'
00116         try:
00117             rospy.spin()           # invoke callbacks as data arrive
00118         except rospy.ROSInterruptException: pass
00119         if verbose:
00120             print 'done acquiring speed data'
00121 
00122     def plot(self, save=False, plotall=False):
00123         """ Plot some interesting data from the speed test run.
00124 
00125         When save=True write the figures to PNG files, otherwise show
00126         them interactively.
00127         """
00128         self.plt.name = self.name       # set title for plots
00129         if plotall:
00130             self.plt.plot(save)
00131         else:
00132             self.plt.plot_summary(save)
00133         
00134 
00135 b = speedTopics()
00136 
00137 def usage(progname):
00138     "Print usage message."
00139     print "\n", progname, """[-h] [ <file.bag> ]
00140 
00141  -a, --all         generate all available plots (default: just position)
00142  -h, --help        print this message
00143  -i, --interactive display plots to terminal (default: save as files)
00144 
00145  <file.bag>\tname of ROS bag file to analyze
00146 
00147 If a bag file is specified, read speed topics from it, saving plots of
00148 the data.  Otherwise, read the ROS topics directly from the running
00149 system.
00150  """
00151 
00152 # main program -- for either script or interactive use
00153 def main(argv=None):
00154     "Main program, called as a script or interactively."
00155 
00156     if argv is None:
00157         argv = sys.argv                 # use command args
00158     argv = rospy.myargv(argv)           # filter out ROS args
00159 
00160     # extract base name of command, will be '' when imported
00161     progname = os.path.basename(argv[0])
00162     if progname is "":
00163         progname = "rosspeed.py"
00164 
00165     # process parameters
00166     try:
00167         opts, files = getopt.gnu_getopt(argv[1:], 'ahi',
00168                                         ('all', 'help', 'interactive'))
00169     except getopt.error, msg:
00170         print msg
00171         print "for help use --help"
00172         return 9
00173 
00174     plotall = False
00175     save = True
00176 
00177     for k,v in opts:
00178         if k in ("-a", "--all"):
00179             plotall = True
00180         if k in ("-h", "--help"):
00181             usage(progname)
00182             return 2
00183         if k in ("-i", "--interactive"):
00184             save = False
00185 
00186     if len(files) < 1:
00187         print "no bag file, reading speed topics directly"
00188         b.get_data()
00189         b.plot(save, plotall)
00190 
00191     elif len(files) == 1:
00192         print "Analyzing " + files[0]
00193         b.get_bag(files[0])
00194         b.plot(save, plotall)
00195 
00196     else:
00197         print "only one ROS bag file can be processed at a time"
00198         usage(progname)
00199         return 9
00200 
00201     return 0
00202 
00203 # when called as a script or via python-send-buffer
00204 if __name__ == "__main__":
00205     # run main function and exit
00206     sys.exit(main())
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


art_pilot
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Tue Sep 24 2013 10:44:02