Python PlanarTracker.saveLog方法代码示例

# 需要导入模块: from tracker import PlanarTracker [as 别名]
# 或者: from tracker.PlanarTracker import saveLog [as 别名]
class Navigation(object):

    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 move(self, data):
        agent_id, x, y, z, yaw, pitch, roll = self.connection.getStates(self.agent_id)
        # sleep(1)
        print '-----------------------------'
        global i
        global stage
        global finished_logging
        distances = list(data.ranges)[0::every_other]
        self.path_planner.filterReadings(distances, angles)

        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)
        # print 'Crash avert velocity:% 4.2f'%self.crash_avert_velocity
        controlled_velocity = (closest_reading) * kp + self.crash_avert_velocity
        controlled_velocity = max(0.0, min(controlled_velocity, 1.0))
        # print 'Controlled Velocity:', controlled_velocity,
        # print 'closest_reading:',closest_reading,
        # print 'Crash avert velocity:',self.crash_avert_velocity
        self.actuation.setTangentialVelocityLimit(min(1, 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)
        # print 'distance',self.distance
        if self.distance < .1:
            stage += 1
            print 'ARRIVED!!!!!!!!!!'
            if finished_logging is False and i >= temp_var_2:
                self.tracker.saveLog()
                save('/home/administrator/barzin_catkin_ws/src/path_tracking/scripts/experimental_results/env', log)
                finished_logging = True
            if stage % 2 is 0:
                self.target_y = self.target_y * -1
            else:
                self.target_x = self.target_x * -1
            # exit()

        angle = arctan2(diff_y, diff_x) - yaw
        subgoal_distance, subgoal_angle = self.path_planner.planPath(self.distance, -angle)
        subgoal_angle2 = -subgoal_angle
        # print angle,subgoal_angle2
        # faz = 1
        # var = min(max(0,self.gap*(1+faz)-closest_reading),faz)
        # offset = var*pi/faz/4
        # subgoal_angle2 = subgoal_angle2+offset*sign(subgoal_angle2-(-closest_reading_angle))
        # print '% 4.2f, % 4.2f, % 4.2f' % (var, offset,offset*sign(subgoal_angle2-(-closest_reading_angle)))
        # print self.distance,-angle,subgoal_distance,subgoal_angle2
        self.tracker.moveTowardsDynamicPoint(subgoal_distance, subgoal_angle2)
        # print 'target angle:',yaw+subgoal_angle2
        self.prev_closest_reading = closest_reading
        self.prev_time = time_now

# 需要导入模块: from tracker import PlanarTracker [as 别名]
# 或者: from tracker.PlanarTracker import saveLog [as 别名]
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

# 需要导入模块: from tracker import PlanarTracker [as 别名]
# 或者: from tracker.PlanarTracker import saveLog [as 别名]

#.........这里部分代码省略.........
        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:
            # print self.path_planner.getFrontTravel(), distances[len(distances) / 2]+self.path_planner.lidar_offset - self.gap
            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)
                # break
            self.actuation.actuate(.5 * front_error, 0)

        elif self.substage == 2:
            front_travel = self.path_planner.getFrontTravel(distances, angles)
            front_error = front_travel - targets[self.stage][1][1] - .2
            print front_travel, front_error
            if abs(front_error) < .03:
                self.stage += 1
                self.substage = 0
                print 'BREAKINGGGGGGGGGGGGGGGGGGGG'
                # break
            self.actuation.actuate(.5 * front_error, 0)

        else:
            print 'stupid fuck'

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

        if self.stage == len(targets):
            self.tracker.saveLog()
            save('/home/administrator/barzin_catkin_ws/src/path_tracking/scripts/experimental_results/planner_of_agent_' + str(self.agent_id), log)
            self.subscriber.unregister()
            print '\033[92m' + '\033[1m' + 'AND DONE' + '\033[0m'
        elif self.stage > len(targets):  # just do nothing after that
            print "Stupid shit didn't unregister"
            sleep(100)