functions.py

Go to the documentation of this file.

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()
                self.listener.waitForTransform(self.global_frame, name+'/base_link', rospy.Time(0),rospy.Duration(10.0))
                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]])                
                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_node/NavfnROS/make_plan')
                self.make_plan = rospy.ServiceProxy(self.name+'/move_base_node/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)
                end=self.listener.transformPose(self.name+'/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