def run_simulation(req):
''' launch the ros node that calls gazebo using the appropriate world and urdf model
on the commandline, this command would look like this:
roslaunch atlas_utils atlas_cga.launch gzworld:=/home/ben/atlas_cga.world gzrobot:=/home/ben/Desktop/gz_walk_dynopt/gz_walking/models/atlas_cga/model2.urdf
we grab the output of this node from stdout and analyze it
'''
# req may contain a preset non-clashing gazebo simulation ID for parallel execution
trqTotal = 0
global world_loc, model_loc
cmd = "roslaunch atlas_utils atlas_cga.launch gzworld:=%s gzrobot:=%s" % (world_loc, model_loc)
proc = Process(cmd.split(), stdout=True)
while True:
try:
line = proc.readline('stdout', block=True)
except KeyboardInterrupt, rospy.ROSInterruptException:
proc.kill()
break
if line and line.startswith('~~~'):
trq = float(line.split()[4])
trqTotal += trq*trq
if line and line.startswith('=== END '):
print line
print 'Total Torque Squared', trqTotal
break
