Python path_planning.GapFinder类代码示例

class Navigation(object):

    def __init__(self):

        self.connection = LabNavigation()
        self.path_planner = GapFinder(.7)
        self.actuation = ROS2DimActuate()
        self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)

        self.tracker.setID(1)
        self.space = .9
        self.target_x = 0
        self.target_y = 2

        sleep(5)
        self.distance = []
        self.prev_closest_reading = 0.0
        self.prev_time = time()
        self.crash_avert_velocity=0.0

        self.subscriber = rospy.Subscriber('/scan', LaserScan, self.move, queue_size=1)
        rospy.spin()

    def move(self, data):
        distances = list(data.ranges)[0::every_other]
        self.path_planner.filterReadings(distances, angles)
        closest_reading = self.path_planner.getMinimumReading()
        time_now = time()
        self.crash_avert_velocity = (self.crash_avert_velocity+(closest_reading - self.prev_closest_reading) * kd / (time() - self.prev_time))/2
        self.crash_avert_velocity = min(0.0,self.crash_avert_velocity)
        controlled_velocity = closest_reading * kp + self.crash_avert_velocity
        controlled_velocity = max(0.0,min(controlled_velocity,1))
        print 'Controlled Velocity:', controlled_velocity,
        print 'closest_reading:',closest_reading,
        print 'Crash avert velocity:',self.crash_avert_velocity
        self.actuation.setTangentialVelocityLimit(controlled_velocity)

        agent_id, x, y, z, yaw, pitch, roll = self.connection.getStates(1)
        # print 'agent location (x,y):', x, y, yaw

        diff_x = self.target_x - x
        diff_y = self.target_y - y
        self.distance = sqrt(diff_x**2 + diff_y**2)

        if self.distance < .03:
            self.target_y = self.target_y * -1

        angle = arctan2(diff_y, diff_x) - yaw
        subgoal_distance, subgoal_angle = self.path_planner.planPath(self.distance, -angle)
        self.tracker.moveTowardsDynamicPoint(subgoal_distance, -subgoal_angle)
        self.prev_closest_reading = closest_reading
        self.prev_time = time_now

class Navigation(object):

    def __init__(self):
        sleep(5)
        self.distance = []
        self.connection = NaslabNetwork()
        self.path_planner = GapFinder(.5)
        self.actuation = ROS2DimActuate()
        self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)
        self.tracker.setID(0)
        self.target_body_number = 2
        self.space = .75
        self.direction = 1
        self.subscriber = rospy.Subscriber('/scan', LaserScan, self.move, queue_size=1)
        rospy.spin()

    # def start(self):

    def move(self, data):
        distances = list(data.ranges)[0::every_other]
        self.path_planner.filterReadings(distances, angles)

        x, y, theta = self.connection.getStates(0)
        target_x, target_y, target_theta = self.connection.getStates(self.target_body_number)
        target_x = target_x + self.space * cos(target_theta)
        target_y = target_y + self.space * sin(target_theta)
        diff_x = target_x - x
        diff_y = target_y - y
        self.distance = sqrt(diff_x**2 + diff_y**2)

        if self.distance < .03:

            if self.target_body_number == 1:
                self.tracker.faceDirection(target_theta)
                print 'ARRIVED!!!!!!!!!!!'
                self.subscriber.unregister()
                return

            if self.target_body_number == 2:
                self.tracker.faceDirection(pi+target_theta)
                self.target_body_number = 1
                self.space = -1.1
                print 'ARRIVED!!!!!!!!!!!'
                sleep(4)

        angle = arctan2(diff_y, diff_x) - theta
        subgoal_distance, subgoal_angle = self.path_planner.planPath(self.distance, -angle)
        self.tracker.moveTowardsDynamicPoint(subgoal_distance, -subgoal_angle)

    def __init__(self):
        # Set parameters
        self.flag = 1
        self.gap = .7   #space needed to pass through
        self.agent_id = 1   #Number for Robot $$change
        self.stage = 0      #initial target
        self.substage = 0   #initial task 0 = drive to objective

        self.topicConfig() #configure simulation namspaces or actual huskies

        self.connection = gpsLocalization(self.namespace, self.gpsTopic, self.imuTopic, self.xOffset, self.yOffset) #Connection which will give current position
        self.path_planner = GapFinder(self.gap) #Finds gaps that the robot can enter
        self.actuation = ROS2DimActuate(self.controlTopic)   #Controls the motion of the robot
        #Create a tracker which knows how to move the robot and get it's position
        self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)
        #Tell the tracker which robot to command
        self.tracker.setID(self.agent_id)

        #Countdown to start
        sleep_time = 3
        while sleep_time > 0:
            print "Mission starts in:", sleep_time
            sleep_time -= 1
            sleep(1)
        self.distance = []
        self.prev_closest_reading = 0.0
        self.prev_time = time()
        self.crash_avert_velocity = 0.0

        print 'Starting the Navigation'
        self.subscriber = rospy.Subscriber(self.lidarTopic, LaserScan, self.move, queue_size=1) #Call move for each laser scan


        rospy.spin()

    def __init__(self):

        self.gap = .6
        # self.space = .9
        self.target_x = .5
        self.target_y = 1
        self.agent_id = 0

        self.connection = LabNavigation()
        self.path_planner = GapFinder(self.gap)
        self.actuation = ROS2DimActuate()
        self.actuation.setAngularVelocityLimit(1)
        self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)

        self.tracker.setID(self.agent_id)

        self.distance = []
        self.prev_closest_reading = 0.0
        self.prev_time = time()
        self.crash_avert_velocity = 0.0

        print 'Starting the Navigation'
        sleep(7)
        self.subscriber = rospy.Subscriber('/scan', LaserScan, self.move, queue_size=1)

        rospy.spin()

    def __init__(self):

        #set parameters
        self.gap = .7 #space needed to pass through
        self.target_x = 5 #destination coordinates
        self.target_y = 4
        self.agent_id = 0

        self.connection = gpsLocalization()   #Connection which will give current position
        self.path_planner = GapFinder(self.gap)  #Finds gaps that the robot can enter
        self.actuation = ROS2DimActuate()   #Controls the motion of the robot
        self.actuation.setAngularVelocityLimit(.5)  #Sets the maximum velocity
        #Create a tracker which knows how to move the robot and get it's position
        self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)
        #Tell the tracker which robot to command
        self.tracker.setID(self.agent_id)

        self.distance = []
        self.prev_closest_reading = 0.0
        self.prev_time = time()
        self.crash_avert_velocity = 0.0

        print 'Starting the Navigation'
        sleep(2)
        self.subscriber = rospy.Subscriber('/husky1/scan', LaserScan, self.move, queue_size=1) #Call move for each laser scan

        rospy.spin()

    def __init__(self):

        self.gap = .7
        self.agent_id = 1
        self.stage = 0
        self.substage = 0

        self.connection = LabNavigation()
        self.path_planner = GapFinder(self.gap)
        self.actuation = ROS2DimActuate()
        self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)

        self.tracker.setID(self.agent_id)

        sleep_time = 7
        while sleep_time > 0:
            print "Mission starts in:", sleep_time
            sleep_time -= 1
            sleep(1)
        self.distance = []
        self.prev_closest_reading = 0.0
        self.prev_time = time()
        self.crash_avert_velocity = 0.0

        print 'Starting the Navigation'
        self.subscriber = rospy.Subscriber('/scan', LaserScan, self.move, queue_size=1)

        rospy.spin()

 def __init__(self):
     self.path_planner = GapFinder(.6)
     self.logger = []
     self.connection = gpsLocalization('husky1test','husky1/odometry/gps','husky1/imu/data',0,0)
     self.subscriber = rospy.Subscriber('husky1/scan', LaserScan, self.readDistance)
     self.rate = rospy.Rate(10)  # set rate to 10 Hz
     while not rospy.is_shutdown() and len(self.logger) < 1:
         self.rate.sleep()

 def __init__(self):
     self.path_planner = GapFinder(.6)
     rospy.init_node('lidarLogger')  # start the control node
     self.logger = []
     self.connection = LabNavigation()
     self.subscriber = rospy.Subscriber('/scan', LaserScan, self.readDistance)
     self.rate = rospy.Rate(10)  # set rate to 10 Hz
     # read = 0
     while not rospy.is_shutdown() and len(self.logger) < 1:
         self.rate.sleep()

 def __init__(self):
     sleep(5)
     self.distance = []
     self.connection = NaslabNetwork()
     self.path_planner = GapFinder(.5)
     self.actuation = ROS2DimActuate()
     self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)
     self.tracker.setID(0)
     self.target_body_number = 2
     self.space = .75
     self.direction = 1
     self.subscriber = rospy.Subscriber('/scan', LaserScan, self.move, queue_size=1)
     rospy.spin()

    def __init__(self):

        self.connection = LabNavigation()
        self.path_planner = GapFinder(.7)
        self.actuation = ROS2DimActuate()
        self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)

        self.tracker.setID(1)
        self.space = .9
        self.target_x = 0
        self.target_y = 2

        sleep(5)
        self.distance = []
        self.prev_closest_reading = 0.0
        self.prev_time = time()
        self.crash_avert_velocity=0.0

        self.subscriber = rospy.Subscriber('/scan', LaserScan, self.move, queue_size=1)
        rospy.spin()

class Navigation(object):

    def __init__(self):
        # Set parameters
        self.flag = 1
        self.gap = .7   #space needed to pass through
        self.agent_id = 1   #Number for Robot $$change
        self.stage = 0      #initial target
        self.substage = 0   #initial task 0 = drive to objective

        self.topicConfig() #configure simulation namspaces or actual huskies

        self.connection = gpsLocalization(self.namespace, self.gpsTopic, self.imuTopic, self.xOffset, self.yOffset) #Connection which will give current position
        self.path_planner = GapFinder(self.gap) #Finds gaps that the robot can enter
        self.actuation = ROS2DimActuate(self.controlTopic)   #Controls the motion of the robot
        #Create a tracker which knows how to move the robot and get it's position
        self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)
        #Tell the tracker which robot to command
        self.tracker.setID(self.agent_id)

        #Countdown to start
        sleep_time = 3
        while sleep_time > 0:
            print "Mission starts in:", sleep_time
            sleep_time -= 1
            sleep(1)
        self.distance = []
        self.prev_closest_reading = 0.0
        self.prev_time = time()
        self.crash_avert_velocity = 0.0

        print 'Starting the Navigation'
        self.subscriber = rospy.Subscriber(self.lidarTopic, LaserScan, self.move, queue_size=1) #Call move for each laser scan


        rospy.spin()

    def topicConfig(self):
        if len(argv)>1:
            self.namespace = argv[3]
            self.lidarTopic = '/' + argv[3] + '/scan'
            self.gpsTopic = '/' + argv[3] + '/odometry/gps'
            self.imuTopic = '/' + argv[3] + '/imu/data'
            self.controlTopic = '/' + argv[3] + '/cmd_vel'
            self.xOffset = float(argv[1])
            self.yOffset = float(argv[2])

        else:
            self.namespace = ''
            self.lidarTopic ='/scan'
            self.gpsTopic = '/navsat/enu'
            self.imuTopic = '/imu/data'
            self.controlTopic = '/cmd_vel'
            self.xOffset = 0
            self.yOffset = 0


    def move(self, data):
        agent_id, x, y, z, yaw, pitch, roll = self.connection.getStates(self.agent_id)
        print '-----------------------------'

        # extract distance data and analyze them
        distances = list(data.ranges)[0::every_other]

        #Drive to the setpoint for the target
        if self.substage == 0:
            self.path_planner.filterReadings(distances, angles)
            closest_reading, closest_reading_angle = self.path_planner.getMinimumReading()

            # dynamic obstacle collision avoidance
            closest_reading = min(closest_reading, 2 * self.gap)
            time_now = time()
            self.crash_avert_velocity = (self.crash_avert_velocity + (closest_reading - self.prev_closest_reading) * kd / (time() - self.prev_time)) / 2
            self.crash_avert_velocity = min(0.0, self.crash_avert_velocity)

            # set velocity based on dynamic obstacle movement
            controlled_velocity = (closest_reading) * kp + self.crash_avert_velocity
            controlled_velocity = max(0.0, min(controlled_velocity, 1.0))
            self.actuation.setTangentialVelocityLimit(min(1, controlled_velocity))

            # find destination and analyze it
            #get information about the current target
            #target_object = self.connection.getStates(targets[self.stage][0])
            if self.stage == 0:
                target_object = [0,-5,0,0,0]

            # Find where to approach target from
            target = withDistance(target_object[1], target_object[2], target_object[4], targets[self.stage][1][0])
            print target
            
            target_x = target[0]
            target_y = target[1]
            diff_x = target_x - x
            diff_y = target_y - y
            self.distance = sqrt(diff_x**2 + diff_y**2)

            # plan path to the target
            angle = arctan2(diff_y, diff_x) - yaw  # find direction towards target in robots coordinate frame
            subgoal_distance, subgoal_angle = self.path_planner.planPath(self.distance, -angle)
            subgoal_angle2 = -subgoal_angle
#.........这里部分代码省略.........

class LidarLogger(object):

    def __init__(self):
        self.path_planner = GapFinder(.6)
        rospy.init_node('lidarLogger')  # start the control node
        self.logger = []
        self.connection = LabNavigation()
        self.subscriber = rospy.Subscriber('/scan', LaserScan, self.readDistance)
        self.rate = rospy.Rate(10)  # set rate to 10 Hz
        # read = 0
        while not rospy.is_shutdown() and len(self.logger) < 1:
            self.rate.sleep()
            # rospy.spin()

        # try:
        #     pass
        # self.__run__() #start motion
        # except Exception, e:
            # print e
        #     print "Error during runtime. Exiting."
        #     logging.exception(e)
        # finally:
        #     print "Run complete."
        # print self.logger  # REMOVE LATER
        #     np.save('scan' + str(datetime.now()), self.logger)

    # def __run__(self):
    # sleep(5) #delay before startup
    #     while not rospy.is_shutdown():
    #         self.rate.sleep()

    def readDistance(self, data):
        self.subscriber.unregister()
        distances = list(data.ranges)[0::every_other]
        # print distances
        self.path_planner.filterReadings(distances, angles)
        # print self.path_planner
        x, y = self.path_planner.polarToCartesian()
        crap1, robot_x, robot_y, robot_z, robot_yaw, crap, crap2 = self.connection.getStates(0)
        target_x = -.5
        target_y = 2.7
        diff_x = target_x - robot_x
        diff_y = target_y - robot_y
        distance = sqrt(diff_x**2 + diff_y**2)
        # print 'here'
        # if distance < .1:
            # print '.'
        #     return
        # print 'here'
        angle = arctan2(diff_y, diff_x) - robot_yaw
        subgoal_distance, subgoal_angle = self.path_planner.planPath(distance, -angle)
        print distance, -angle, subgoal_distance, subgoal_angle
        # scan = data.ranges
        # print scan
        # self.logger = scan
        # print "Lidar Scan Recieved. Logging data..."
        f0 = plt.figure(1,figsize=(9,9))
        ax0 = f0.add_subplot(111)
        nums = len(self.path_planner.possible_travel)
        # for i in range(nums):
        # ax0.plot(x, y, 'r.')
        # print self.path_planner.subgoals
        reading_x = [0] * nums
        reading_y = [0] * nums
        subgoal_x = []
        subgoal_y = []
        for i in range(len(self.path_planner.possible_travel)):
            # print robot_x , self.path_planner.possible_travel[i] , -self.path_planner.readings_polar[i][1] , calibrating_theta,robot_yaw
            # print self.path_planner.possible_travel
            reading_x[i] = robot_x + self.path_planner.readings_polar[i][0] * cos(robot_yaw - self.path_planner.readings_polar[i][1])
            reading_y[i] = robot_y + self.path_planner.readings_polar[i][0] * sin(robot_yaw - self.path_planner.readings_polar[i][1])
            x[i] = robot_x + self.path_planner.possible_travel[i] * cos(-self.path_planner.readings_polar[i][1] + robot_yaw)
            y[i] = robot_y + self.path_planner.possible_travel[i] * sin(-self.path_planner.readings_polar[i][1] + robot_yaw)
            if i in self.path_planner.subgoals:
                # print x[i]
                subgoal_x = subgoal_x + [x[i]]
                subgoal_y = subgoal_y + [y[i]]

        # print subgoal_x
        # if environment_state is 'not_safe':
        # elif environment_state is 'safe':
        #     ax0.plot(target_distance * cos(target_angle),
        #              target_distance * sin(target_angle), 'go', markersize=20)
        # elif environment_state is 'close_to_obstacle':
        #     ax0.plot(target_distance * cos(target_angle),
        #              target_distance * sin(target_angle), 'ro', markersize=20)
        ax0.plot(subgoal_x, subgoal_y, 'ko', markersize=20, label='Subgoal Candidate')
        ax0.plot(robot_x + subgoal_distance * cos(robot_yaw - subgoal_angle),
                 robot_y + subgoal_distance * sin(robot_yaw - subgoal_angle), 'go', markersize=20, label='Best Subgoal')
        ax0.plot(robot_x, robot_y, 'ms', markersize=10, label='Robot')
        ax0.plot(target_x, target_y, 'cs', markersize=10, label='Destination')
        ax0.plot(x, y, 'b.', markersize=10, label='Possible Travel')
        ax0.plot(reading_x, reading_y, 'r.', markersize=10, label='Lidar Reading')
        ax0.set_xlabel('X (m)')
        ax0.set_ylabel('Y (m)')
        ax0.legend()
        ax0.axis('equal')
        plt.tight_layout()
        plt.draw()
        plt.pause(.1)
#.........这里部分代码省略.........

class Navigation(object):

    def __init__(self):

        self.gap = .7
        self.agent_id = 1
        self.stage = 0
        self.substage = 0

        self.connection = LabNavigation()
        self.path_planner = GapFinder(self.gap)
        self.actuation = ROS2DimActuate()
        self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)

        self.tracker.setID(self.agent_id)

        sleep_time = 7
        while sleep_time > 0:
            print "Mission starts in:", sleep_time
            sleep_time -= 1
            sleep(1)
        self.distance = []
        self.prev_closest_reading = 0.0
        self.prev_time = time()
        self.crash_avert_velocity = 0.0

        print 'Starting the Navigation'
        self.subscriber = rospy.Subscriber('/scan', LaserScan, self.move, queue_size=1)

        rospy.spin()

    def move(self, data):
        agent_id, x, y, z, yaw, pitch, roll = self.connection.getStates(self.agent_id)
        print '-----------------------------'
        global i
        global finished_edge

        # extract distance data and analyze them
        distances = list(data.ranges)[0::every_other]

        if self.substage == 0:
            self.path_planner.filterReadings(distances, angles)
            closest_reading, closest_reading_angle = self.path_planner.getMinimumReading()

            # dynamic obstacle collision avoidance
            closest_reading = min(closest_reading, 2 * self.gap)
            time_now = time()
            self.crash_avert_velocity = (self.crash_avert_velocity + (closest_reading - self.prev_closest_reading) * kd / (time() - self.prev_time)) / 2
            self.crash_avert_velocity = min(0.0, self.crash_avert_velocity)

            # set velocity based on dynamic obstacle movement
            controlled_velocity = (closest_reading) * kp + self.crash_avert_velocity
            controlled_velocity = max(0.0, min(controlled_velocity, 1.0))
            self.actuation.setTangentialVelocityLimit(min(1, controlled_velocity))

            # find destination and analyze it
            target_object = self.connection.getStates(targets[self.stage][0])
            target = withDistance(target_object[1], target_object[2], target_object[4], targets[self.stage][1][0])
            # print target
            target_x = target[0]
            target_y = target[1]
            diff_x = target_x - x
            diff_y = target_y - y
            self.distance = sqrt(diff_x**2 + diff_y**2)

            print 'here1'
            # plan path to the target
            angle = arctan2(diff_y, diff_x) - yaw  # find direction towards target in robots coordinate frame
            subgoal_distance, subgoal_angle = self.path_planner.planPath(self.distance, -angle)
            subgoal_angle2 = -subgoal_angle

            # go to the point designated by path planner
            self.tracker.moveTowardsDynamicPoint(subgoal_distance, subgoal_angle2)

            # See if reached the destination
            if self.distance < .1:
                print '\033[92m' + '\033[1m' + 'ARRIVED TO GATE' + '\033[0m'

                # face direction
                if targets[self.stage][1][0] < 0:
                    desired_facing = self.connection.getStates(targets[self.stage][0])[4]
                else:
                    desired_facing = pi + self.connection.getStates(targets[self.stage][0])[4]
                self.tracker.faceDirection(desired_facing)
                self.substage = 1
                sleep(1)

            # save some of the variable needed for next iteration
            self.prev_closest_reading = closest_reading
            self.prev_time = time_now

        elif self.substage == 1:
            
            front_travel = self.path_planner.getFrontTravel(distances, angles)
            front_error = front_travel - targets[self.stage][1][1]
            print front_travel, front_error
            if abs(front_error) < .03:
                self.substage = 2
                print 'BREAKINGGGGGGGGGGGGGGGGGGGG'
                sleep(5)
#.........这里部分代码省略.........

class LidarLogger(object):

    def __init__(self):
        self.path_planner = GapFinder(.6)
        self.logger = []
        self.connection = gpsLocalization('husky1test','husky1/odometry/gps','husky1/imu/data',0,0)
        self.subscriber = rospy.Subscriber('husky1/scan', LaserScan, self.readDistance)
        self.rate = rospy.Rate(10)  # set rate to 10 Hz
        while not rospy.is_shutdown() and len(self.logger) < 1:
            self.rate.sleep()

    def readDistance(self, data):
        self.subscriber.unregister()
        distances = list(data.ranges)[0::every_other]

        self.path_planner.filterReadings(distances, angles)

        x, y = self.path_planner.polarToCartesian()
        crap1, robot_x, robot_y, robot_z, robot_yaw, crap, crap2 = self.connection.getStates(0)
        target_x = 7
        target_y = 4
        diff_x = target_x - robot_x
        diff_y = target_y - robot_y
        print robot_x
        distance = sqrt(diff_x**2 + diff_y**2)

        angle = arctan2(diff_y, diff_x) - robot_yaw
        subgoal_distance, subgoal_angle = self.path_planner.planPath(distance, -angle)
        print distance, -angle, subgoal_distance, subgoal_angle

        f0 = plt.figure(1,figsize=(9,9))
        ax0 = f0.add_subplot(111)
        nums = len(self.path_planner.possible_travel)

        reading_x = [0] * nums
        reading_y = [0] * nums
        subgoal_x = []
        subgoal_y = []
        for i in range(len(self.path_planner.possible_travel)):

            reading_x[i] = robot_x + self.path_planner.readings_polar[i][0] * cos(robot_yaw - self.path_planner.readings_polar[i][1])
            reading_y[i] = robot_y + self.path_planner.readings_polar[i][0] * sin(robot_yaw - self.path_planner.readings_polar[i][1])
            x[i] = robot_x + self.path_planner.possible_travel[i] * cos(-self.path_planner.readings_polar[i][1] + robot_yaw)
            y[i] = robot_y + self.path_planner.possible_travel[i] * sin(-self.path_planner.readings_polar[i][1] + robot_yaw)
            if i in self.path_planner.subgoals:

                subgoal_x = subgoal_x + [x[i]]
                subgoal_y = subgoal_y + [y[i]]


        ax0.plot(subgoal_x, subgoal_y, 'ko', markersize=20, label='Subgoal Candidate')
        ax0.plot(robot_x + subgoal_distance * cos(robot_yaw - subgoal_angle),
                 robot_y + subgoal_distance * sin(robot_yaw - subgoal_angle), 'go', markersize=20, label='Best Subgoal')
        ax0.plot(robot_x, robot_y, 'ms', markersize=10, label='Robot')
        ax0.plot(target_x, target_y, 'cs', markersize=10, label='Destination')
        ax0.plot(x, y, 'b.', markersize=10, label='Possible Travel')
        ax0.plot(reading_x, reading_y, 'r.', markersize=10, label='Lidar Reading')
        ax0.set_xlabel('X (m)')
        ax0.set_ylabel('Y (m)')
        ax0.legend()
        ax0.axis('equal')
        plt.tight_layout()
        plt.draw()
        plt.pause(.1)
        raw_input("<Hit Enter To Close>")
        plt.close(f0)
        return

class Navigation(object):

    def __init__(self):

        #set parameters
        self.gap = .7 #space needed to pass through
        self.target_x = 5 #destination coordinates
        self.target_y = 4
        self.agent_id = 0

        self.connection = gpsLocalization()   #Connection which will give current position
        self.path_planner = GapFinder(self.gap)  #Finds gaps that the robot can enter
        self.actuation = ROS2DimActuate()   #Controls the motion of the robot
        self.actuation.setAngularVelocityLimit(.5)  #Sets the maximum velocity
        #Create a tracker which knows how to move the robot and get it's position
        self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)
        #Tell the tracker which robot to command
        self.tracker.setID(self.agent_id)

        self.distance = []
        self.prev_closest_reading = 0.0
        self.prev_time = time()
        self.crash_avert_velocity = 0.0

        print 'Starting the Navigation'
        sleep(2)
        self.subscriber = rospy.Subscriber('/husky1/scan', LaserScan, self.move, queue_size=1) #Call move for each laser scan

        rospy.spin()

    def move(self, data):
        agent_id, x, y, z, yaw, pitch, roll = self.connection.getStates(self.agent_id) #Get localization info
        #print 'x: ', x,'y: ', y,'theta: ', yaw

        print '-----------------------------'
        global i
        global stage
        global finished_logging
        distances = list(data.ranges)[0::every_other] #store the range readings from the lidar
        self.path_planner.filterReadings(distances, angles) #filter the results

        closest_reading, closest_reading_angle = self.path_planner.getMinimumReading()
        closest_reading = min(closest_reading, 2 * self.gap)
        time_now = time()
        self.crash_avert_velocity = (self.crash_avert_velocity + (closest_reading - self.prev_closest_reading) * kd / (time() - self.prev_time)) / 2
        self.crash_avert_velocity = min(0.0, self.crash_avert_velocity)

        controlled_velocity = (closest_reading) * kp + self.crash_avert_velocity
        controlled_velocity = max(0.0, min(controlled_velocity, 1.0))

        self.actuation.setTangentialVelocityLimit(min(.2, controlled_velocity))

        i += 1
        if i % temp_var is 0 and i < temp_var_2:
            log[i / temp_var] = [x, y, yaw, self.path_planner.readings_polar]

        diff_x = self.target_x - x
        diff_y = self.target_y - y
        self.distance = sqrt(diff_x**2 + diff_y**2)

        if self.distance < .1:
            stage += 1
            print 'ARRIVED!!!!!!!!!!'
            if finished_logging is False and i >= temp_var_2:
                self.tracker.saveLog()
                save('loginfo', log)
                finished_logging = True
            self.target_y = self.target_y * -1
            self.target_x = self.target_x * -1
            exit()

        angle = arctan2(diff_y, diff_x) - yaw
        #print 'dist: ', self.distance, 'angle: ', -angle
        subgoal_distance, subgoal_angle = self.path_planner.planPath(self.distance, -angle)
        subgoal_angle2 = -subgoal_angle

        self.tracker.moveTowardsDynamicPoint(subgoal_distance, subgoal_angle2)

        self.prev_closest_reading = closest_reading
        self.prev_time = time_now