setinitposes.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 
00004 import sys, time, os
00005 from math import degrees, radians
00006 
00007 colors = ['blue', 'red', 'cyan', 'magenta', 'blue', 'red', 'cyan', 'magenta', 'blue', 'red', 'cyan', 'magenta']
00008 
00009 def setIP(mapname,vip):
00010     cmd = 'rosparam set initial_pos "'+vip+'"'
00011     print cmd
00012     os.system(cmd)
00013     ip = eval(vip)
00014     n = len(ip)/2
00015     print 'Set initial poses of ',n,' robots'
00016 
00017     fnr = 'maps/'+mapname+'/robots.inc'
00018     fr = open(fnr, 'w')
00019 
00020     for i in range(0,n):
00021         x = ip[i*2]
00022         y = ip[i*2+1]
00023         th = 90
00024         fn = 'params/amcl/robot_'+str(i)+'_initial_pose.xml'
00025         f = open(fn, 'w')
00026         f.write('<launch>\n')
00027         f.write('  <param name="amcl/initial_pose_x" value="'+str(x)+'"/>\n')
00028         f.write('  <param name="amcl/initial_pose_y" value="'+str(y)+'"/>\n')
00029         f.write('  <param name="amcl/initial_pose_a" value="'+str(radians(th))+'"/>\n')
00030         f.write('  <param name="srrg_localizer/initial_pose_x" value="'+str(x)+'"/>\n')
00031         f.write('  <param name="srrg_localizer/initial_pose_y" value="'+str(y)+'"/>\n')
00032         f.write('  <param name="srrg_localizer/initial_pose_a" value="'+str(radians(th))+'"/>\n')
00033         f.write('</launch>\n')
00034         f.close()
00035 
00036         fr.write('crobot( pose [ '+str(x)+'  '+str(y)+'  0  '+str(th)+' ]   name "robot'+str(i)+'"  color "'+colors[i]+'")\n')
00037         
00038 
00039     if (n==1):
00040         # inactive robot for having correct namespaces
00041         fr.write('crobot( pose [ -2.0   -2.0    0   0.0 ]   name "robot1"  color "red")\n')
00042 
00043     fr.close()
00044 
00045 
00046 
00047 if __name__ == '__main__':
00048     if (len(sys.argv)<3):
00049         sys.exit(0)
00050     mapname = sys.argv[1]
00051     vip = sys.argv[2]
00052     setIP(mapname,vip)


patrolling_sim
Author(s):
autogenerated on Thu Jun 6 2019 20:27:09