#!/usr/bin/env python


#--------Include modules---------------
from copy import copy
import rospy
from visualization_msgs.msg import Marker
from std_msgs.msg import String
from geometry_msgs.msg import Point
from nav_msgs.msg import OccupancyGrid



from os import system
from random import random
from numpy import array
from numpy import concatenate
from numpy import vstack
from numpy import delete
from numpy import linalg as LA
from functions import Nearest
from functions import Steer
from functions import Near
import parameters as param
from functions import ObstacleFree
from functions import Find
from functions import Cost
from functions import prepEdges
from numpy import all as All


def callback(data):
    rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)


def node():
    	
    	rospy.Subscriber("map", OccupancyGrid, callback)
    	rospy.init_node('testNode', anonymous=False)
    	rate = rospy.Rate(1)

#-------------------------------RRT------------------------------------------
	while not rospy.is_shutdown():


	   	
        	rate.sleep()



#_____________________________________________________________________________

if __name__ == '__main__':
    try:
        node()
    except rospy.ROSInterruptException:
        pass
 
 
 
 
