00001
00002
00003
00004
00005
00006
00007 import subprocess as sp
00008 import os.path as op
00009 import os
00010 import optparse
00011 import time
00012
00013 def make_dirname(i):
00014 return 'laser_slam.{0}'.format(i)
00015
00016 def setup_dir(path):
00017 """
00018 Set up new location for storing output files
00019 """
00020 path = op.expanduser(op.expandvars(path))
00021 if not op.exists(path):
00022 os.makedirs(path)
00023 i = 0
00024 loc = op.join(path, make_dirname(i))
00025 while op.exists(loc):
00026 i += 1
00027 loc = op.join(path, make_dirname(i))
00028 loc = op.join(path, make_dirname(i))
00029 os.mkdir(loc)
00030 latest = op.join(path, 'latest')
00031 if op.exists(latest):
00032 os.remove(latest)
00033 os.symlink(loc, latest)
00034 return loc
00035
00036
00037 def main(path, max_time):
00038 loc = setup_dir(path)
00039 print('Using output directory {0}'.format(loc))
00040
00041
00042 rosbag_outfile = open(op.join(loc, 'rosbag.out'), 'w')
00043 args = ['rosbag', 'record', '-a', '-O{0}'.format(op.join(loc, 'slam.bag'))]
00044 rosbag = sp.Popen(args, shell=False, stdout=rosbag_outfile, stderr=sp.STDOUT)
00045 print('Invoked {0}'.format(' '.join(args)))
00046
00047
00048 roslaunch_outfile = open(op.join(loc, 'roslaunch.out'), 'w')
00049 args = ['roslaunch', 'laser_slam', 'move_base_slam_5cm.launch', 'graph_loc:={0}'.\
00050 format(op.join(loc, 'graph'))]
00051 slam_launch = sp.Popen(args, shell=False, stdout=roslaunch_outfile,
00052 stderr=sp.STDOUT, bufsize=1)
00053 print('Invoked {0}'.format(' '.join(args)))
00054
00055 i = 0
00056 while True:
00057 time.sleep(1)
00058 i += 1
00059 if i % 100 == 0:
00060 print('t = {0}'.format(i))
00061 if max_time > 0 and i > max_time:
00062 print('Time limit reached')
00063 slam_launch.terminate()
00064 rosbag.terminate()
00065 break
00066 if (rosbag.poll()):
00067 print('Rosbag exited.')
00068 slam_launch.terminate()
00069 break
00070 if (slam_launch.poll()):
00071 print('roslaunch exited.')
00072 rosbag.terminate()
00073 break
00074
00075
00076
00077
00078 if __name__ == "__main__":
00079 parser = optparse.OptionParser()
00080 parser.add_option('-d', '--output_dir', dest='path',
00081 action='store', type='string', default='~/.ros/laser_slam')
00082 parser.add_option('-t', '--time', dest='time', action='store', type='int',
00083 default=0)
00084 (options, args) = parser.parse_args()
00085 main(options.path, options.time)