Go to the documentation of this file.00001
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="thin_localizer/initial_pose_x" value="'+str(x)+'"/>\n')
00031 f.write(' <param name="thin_localizer/initial_pose_y" value="'+str(y)+'"/>\n')
00032 f.write(' <param name="thin_localizer/initial_pose_theta" 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
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)