text: import rospy
  import tf
  from numpy import array
  import actionlib
  from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
  from nav_msgs.srv import GetPlan
  from geometry_msgs.msg import PoseStamped
  from numpy import floor
  from numpy.linalg import norm
  from numpy import inf
  #________________________________________________________________________________
  class robot:
  	goal = MoveBaseGoal()
  	start = PoseStamped()
  	end = PoseStamped()
  	
  	def __init__(self,name):
  		self.assigned_point=[]
  		self.name=name
  		self.global_frame=rospy.get_param('~global_frame','/map')
  		self.listener=tf.TransformListener()
  		rospy.sleep(2)
  		self.listener.waitForTransform(self.global_frame, name+'/base_link', rospy.Time(0),rospy.Duration(10.0))
  		cond=0;
  		while cond==0:
  			try:
  				self.listener.waitForTransform(self.global_frame, name+'/base_link', rospy.Time(0),rospy.Duration(10.0))
  				(trans,rot) = self.listener.lookupTransform(self.global_frame, self.name+'/base_link', rospy.Time(0))
  				cond=1
  			except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
  				cond==0
  		self.position=array([trans[0],trans[1]])
  		self.assigned_point=self.position
  		self.client=actionlib.SimpleActionClient(self.name+'/move_base', MoveBaseAction)
  		self.client.wait_for_server()
  		robot.goal.target_pose.header.frame_id=self.global_frame
  		robot.goal.target_pose.header.stamp=rospy.Time.now()
  		rospy.wait_for_service(self.name+'/move_base/NavfnROS/make_plan')
  		self.make_plan = rospy.ServiceProxy(self.name+'/move_base/NavfnROS/make_plan', GetPlan)
  		robot.start.header.frame_id=self.global_frame
  		robot.end.header.frame_id=self.global_frame
  
  
  	def getPosition(self):
  		cond=0;	
  		while cond==0:	
  			try:
  				(trans,rot) = self.listener.lookupTransform(self.global_frame, self.name+'/base_link', rospy.Time(0))
  				cond=1
  			except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
  				cond==0
  		self.position=array([trans[0],trans[1]])
  		return self.position
  		
  	def sendGoal(self,point):
  		robot.goal.target_pose.pose.position.x=point[0]
  		robot.goal.target_pose.pose.position.y=point[1]
  		robot.goal.target_pose.pose.orientation.w = 1.0
  		self.client.send_goal(robot.goal)
  		self.assigned_point=array(point)
  	
  	def cancelGoal(self):
  		self.client.cancel_goal()
  		self.assigned_point=self.getPosition()
  	
  	def getState(self):
  		return self.client.get_state()
  		
  	def makePlan(self,start,end):
  		robot.start.pose.position.x=start[0]
  		robot.start.pose.position.y=start[1]
  		robot.end.pose.position.x=end[0]
  		robot.end.pose.position.y=end[1]
  		#start=self.listener.transformPose(self.name+'/map', robot.start)
  		start=self.listener.transformPose('/map', robot.start)
  		end=self.listener.transformPose('/map', robot.end)
  		plan=self.make_plan(start = start, goal = end, tolerance = 0.0)
  		return plan.plan.poses	
  #________________________________________________________________________________
  
  def index_of_point(mapData,Xp):
  	resolution=mapData.info.resolution
  	Xstartx=mapData.info.origin.position.x
  	Xstarty=mapData.info.origin.position.y
  	width=mapData.info.width
  	Data=mapData.data
  	index=int(	(  floor((Xp[1]-Xstarty)/resolution)*width)+( floor((Xp[0]-Xstartx)/resolution) ))
  	return index
  	
  def point_of_index(mapData,i):
  	y=mapData.info.origin.position.y+(i/mapData.info.width)*mapData.info.resolution
  	x=mapData.info.origin.position.x+(i-(i/mapData.info.width)*(mapData.info.width))*mapData.info.resolution
  	return array([x,y])
  #________________________________________________________________________________		
  
  def informationGain(mapData,point,r):
  	infoGain=0;
  	index=index_of_point(mapData,point)
  	r_region=int(r/mapData.info.resolution)
  	init_index=index-r_region*(mapData.info.width+1)	
  	for n in range(0,2*r_region+1):
  		start=n*mapData.info.width+init_index
  		end=start+2*r_region
  		limit=((start/mapData.info.width)+2)*mapData.info.width
  		for i in range(start,end+1):
  			if (i>=0 and i<limit and i<len(mapData.data)):
  				if(mapData.data[i]==-1 and norm(array(point)-point_of_index(mapData,i))<=r):
  					infoGain+=1
  	return infoGain*(mapData.info.resolution**2)
  #________________________________________________________________________________
  
  def discount(mapData,assigned_pt,centroids,infoGain,r):
  	index=index_of_point(mapData,assigned_pt)
  	r_region=int(r/mapData.info.resolution)
  	init_index=index-r_region*(mapData.info.width+1)
  	for n in range(0,2*r_region+1):
  		start=n*mapData.info.width+init_index
  		end=start+2*r_region
  		limit=((start/mapData.info.width)+2)*mapData.info.width	
  		for i in range(start,end+1):	
  			if (i>=0 and i<limit and i<len(mapData.data)):
  				for j in range(0,len(centroids)):
  					current_pt=centroids[j]
  					if(mapData.data[i]==-1 and norm(point_of_index(mapData,i)-current_pt)<=r and norm(point_of_index(mapData,i)-assigned_pt)<=r):
  						infoGain[j]-=1 #this should be modified, subtract the area of a cell, not 1
  	return infoGain
  #________________________________________________________________________________
  
  def pathCost(path):
  	if (len(path)>0):
  		i=len(path)/2
  		p1=array([path[i-1].pose.position.x,path[i-1].pose.position.y])
  		p2=array([path[i].pose.position.x,path[i].pose.position.y])
  		return (norm(p1-p2)*(len(path)-1))
  	else:
  		return inf
  #________________________________________________________________________________
  		
  def unvalid(mapData,pt):
  	index=index_of_point(mapData,pt)
  	r_region=5
  	init_index=index-r_region*(mapData.info.width+1)
  	for n in range(0,2*r_region+1):
  		start=n*mapData.info.width+init_index
  		end=start+2*r_region
  		limit=((start/mapData.info.width)+2)*mapData.info.width	
  		for i in range(start,end+1):	
  			if (i>=0 and i<limit and i<len(mapData.data)):
  				if(mapData.data[i]==1):
  					return True
  	return False
  #________________________________________________________________________________
  def Nearest(V,x):
   n=inf
   i=0
   for i in range(0,V.shape[0]):
      n1=norm(V[i,:]-x)
      if (n1<n):
  	n=n1
          result=i    
   return result
  
  #________________________________________________________________________________ 
  def Nearest2(V,x):
   n=inf
   result=0
   for i in range(0,len(V)):
  	n1=norm(V[i]-x)
      
  	if (n1<n):
  		n=n1
   return i
  
  #
  
  # ______________________________________________________________________________
  def gridValue(mapData,Xp):
   resolution=mapData.info.resolution
   Xstartx=mapData.info.origin.position.x
   Xstarty=mapData.info.origin.position.y
  
   width=mapData.info.width
   Data=mapData.data
   # returns grid value at "Xp" location
   #map data:  100 occupied      -1 unknown       0 free
   # index=(  floor((Xp[1]-Xstarty)/resolution)*width)+( floor((Xp[0]-Xstartx)/resolution) )
   #
   #
  	#
  	#
   # if int(index) < len(Data):
   # 	return Data[int(index)]
   # else:
   # 	return 100
  
  
   #added
   xi=[-1,1,0,0,1,-1,1,-1,0]
   yi=[0,0,-1,1,-1,1,1,-1,0]
  
   for i in range(len(xi)):
  
  	 dx=xi[i]
  	 dy=yi[i]
  	 pt=array([Xp[0]+dx,Xp[1]+dy])
  	 index=(  floor((pt[1]-Xstarty)/resolution)*width)+( floor((pt[0]-Xstartx)/resolution) )
  	 if (int(index) < len(Data)):
  
  		 if (Data[int(index)]>60):
  			 return 100
  	 	 elif (i==(len(xi)-1)):
  			 return (Data[int(index)])
  	 else:
  		print("wtf")
  		return 100
  
  
   
  
  		
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  	
  	
  	
  	
  	
  	
  	
  	
  	
  	
  