diff --git a/mainHRP_SLAM.py b/mainHRP_SLAM.py
old mode 100644
new mode 100755
index 5108ff81f02a418090c6a82f2879ddf55e8c8546..fe7c1a681a9d09d5b0d2ce47cb4ba5c1b3011e13
--- a/mainHRP_SLAM.py
+++ b/mainHRP_SLAM.py
@@ -8,7 +8,7 @@ from nav_msgs.msg import Odometry
 from nav_msgs.msg import OccupancyGrid
 import subprocess
 import os
-
+import threading
 class Mower(object):
 	
 	def __init__(self):
@@ -17,7 +17,7 @@ class Mower(object):
 		self.pub_twist=rospy.Publisher('/cmd_vel',Twist,queue_size=1)
 		rospy.Subscriber('/scan',LaserScan,self.lidar_msg)
 		rospy.Subscriber('/odom',Odometry,self.placePos)
-		rospy.Subscriber('/map', OccupancyGrid,self.loadMap)
+		#rospy.Subscriber('/map', OccupancyGrid,self.loadMap)
 		self.twist = Twist()
 		self.f = np.vectorize(lambda x: 30 if x.__abs__()>30 else x)
 		self.reset()
@@ -28,8 +28,13 @@ class Mower(object):
 		self.side=0
 		self.devnull = open(os.devnull, 'w')
 		subprocess.Popen(["rosrun","urg_node","urg_node","_ip_address:=192.168.1.11"])
-		self.proc = subprocess.Popen(["rosrun","gmapping","slam_gmapping","_temporalUpdate:=2","_map_update_interval:=2"],stdout=self.devnull, stderr=self.devnull)
-		subprocess.Popen(["rosrun","tf","static_transform_publisher","0","0","0","0","0","0","base_link","laser","100"])
+		time.sleep(10)
+		print("Starting")
+		#self.proc = subprocess.Popen(["rosrun","gmapping","slam_gmapping","_temporalUpdate:=2","_map_update_interval:=2"],stdout=self.devnull, stderr=self.devnull)
+		t1 = threading.Thread(target=self.loadMap)
+		t1.start()
+		time.sleep(10)
+		#subprocess.Popen(["rosrun","tf","static_transform_publisher","0","0","0","0","0","0","base_link","laser","100"])
 
 	def isClear(self,x,y):
 		dx=x-self.x
@@ -225,32 +230,33 @@ class Mower(object):
 				
 			
 
-	def loadMap(self,data):
-		# Origin is at [2000][2000], resolution 0.05
-		x=self.x
-		y=self.y
-		angle=self.angle
-		ranges=np.array(self.ranges)
-		print('new scan')
-		for i in range(0,1081):
-			if not ranges[i]<0:
-				if ranges[i]>20:
-					ranges[i]=21
-				dx = np.cos(angle+np.pi/180*135*i/540-135*np.pi/180)
-				dy = np.sin(angle+np.pi/180*135*i/540-135*np.pi/180)
-				steps=(np.round(ranges[i]*20)).astype(np.int16)
-				#print(steps)
-				if steps>0:
-					xSpace=np.linspace(np.round(x*20),np.round((x+dx*ranges[i])*20),steps).round().astype(np.int16)
-					ySpace=np.linspace(np.round(y*20),np.round((y+dy*ranges[i])*20),steps).round().astype(np.int16)
-					#print(xSpace)
-					#print(ySpace)
-					for j in range(0,steps):
-						self.map[2000+xSpace[j],2000+ySpace[j]]=0
-					if not ranges[i]>20:
-						#print('notSkipped')
-						self.map[2000+xSpace[-1],2000+ySpace[-1]]=100
-		print('scan done')
+	def loadMap(self):
+		while(True):
+			# Origin is at [2000][2000], resolution 0.05
+			x=self.x
+			y=self.y
+			angle=self.angle
+			ranges=np.array(self.ranges)
+			print('new scan')
+			for i in range(0,1081):
+				if not ranges[i]<0:
+					if ranges[i]>20:
+						ranges[i]=21
+					dx = np.cos(angle+np.pi/180*135*i/540-135*np.pi/180)
+					dy = np.sin(angle+np.pi/180*135*i/540-135*np.pi/180)
+					steps=(np.round(ranges[i]*20)).astype(np.int16)
+					#print(steps)
+					if steps>0:
+						xSpace=np.linspace(np.round(x*20),np.round((x+dx*ranges[i])*20),steps).round().astype(np.int16)
+						ySpace=np.linspace(np.round(y*20),np.round((y+dy*ranges[i])*20),steps).round().astype(np.int16)
+						#print(xSpace)
+						#print(ySpace)
+						for j in range(0,steps):
+							self.map[2000+xSpace[j],2000+ySpace[j]]=0
+						if not ranges[i]>20:
+							#print('notSkipped')
+							self.map[2000+xSpace[-1],2000+ySpace[-1]]=100
+			print('scan done')
 
 
 	def placePos(self,data):