Python GapFinder.getFrontTravel方法代码示例

# 需要导入模块: from path_planning import GapFinder [as 别名]
# 或者: from path_planning.GapFinder import getFrontTravel [as 别名]

#.........这里部分代码省略.........
                else:
                    desired_facing = pi
                    #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

            #Plot the results

            # xpol, ypol = self.path_planner.polarToCartesian()

            # 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] = x + self.path_planner.readings_polar[i][0] * cos(yaw - self.path_planner.readings_polar[i][1])
            #     reading_y[i] = y + self.path_planner.readings_polar[i][0] * sin(yaw - self.path_planner.readings_polar[i][1])
            #     xpol[i] = x + self.path_planner.possible_travel[i] * cos(-self.path_planner.readings_polar[i][1] + yaw)
            #     ypol[i] = y + self.path_planner.possible_travel[i] * sin(-self.path_planner.readings_polar[i][1] + yaw)
            #     if i in self.path_planner.subgoals:

            #         subgoal_x = subgoal_x + [xpol[i]]
            #         subgoal_y = subgoal_y + [ypol[i]]
            # if self.flag:
            #     self.f0 = plt.figure(1,figsize=(9,9))
            #     self.ax0 = self.f0.add_subplot(111)

            #     self.sgc, = self.ax0.plot(subgoal_x, subgoal_y, 'ko', markersize=20, label='Subgoal Candidate')
            #     self.bsg, = self.ax0.plot(x + subgoal_distance * cos(yaw - subgoal_angle),
            #              y + subgoal_distance * sin(yaw - subgoal_angle), 'go', markersize=20, label='Best Subgoal')
            #     self.ro, = self.ax0.plot(x, y, 'ms', markersize=10, label='Robot')
            #     self.dest, = self.ax0.plot(target_x, target_y, 'cs', markersize=10, label='Destination')
            #     self.pot, = self.ax0.plot(xpol, ypol, 'b.', markersize=10, label='Possible Travel')
            #     self.lidr, = self.ax0.plot(reading_x, reading_y, 'r.', markersize=10, label='Lidar Reading')
            #     self.ax0.set_xlabel('X (m)')
            #     self.ax0.set_ylabel('Y (m)')
            #     self.ax0.legend()
            #     self.ax0.axis('equal')
            #     plt.tight_layout()
            #     plt.draw()
            #     plt.pause(.1)
            #     self.flag = 0

            # else:
            #     self.sgc.set_xdata(subgoal_x)
            #     self.sgc.set_ydata(subgoal_y)
            #     self.bsg.set_xdata(x + subgoal_distance * cos(yaw - subgoal_angle))
            #     self.bsg.set_ydata(y + subgoal_distance * sin(yaw - subgoal_angle))
            #     self.ro.set_xdata(x)
            #     self.ro.set_ydata(y)
            #     self.dest.set_xdata(target_x)
            #     self.dest.set_ydata(target_y)
            #     self.pot.set_xdata(xpol)
            #     self.pot.set_ydata(ypol)
            #     self.lidr.set_xdata(reading_x)
            #     self.lidr.set_ydata(reading_y)
            #     plt.draw()



        #approach the target
        elif self.substage == 1:
            return
            
            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 'Connection made. Reversing.'
                sleep(5)
                
            self.actuation.actuate(.5 * front_error, 0)

        #reverse a set distance
        elif self.substage == 2:
            front_travel = self.path_planner.getFrontTravel(distances, angles)
            front_error = front_travel - targets[self.stage][1][1] - 1
            print front_travel, front_error
            if abs(front_error) < .03:
                self.stage += 1
                self.substage = 0
                print 'Connection complete. Proceeding to next operation.'
                
            self.actuation.actuate(.5 * front_error, 0)

        if self.stage == len(targets):
        
            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 "didn't unregister"
            sleep(100)

# 需要导入模块: from path_planning import GapFinder [as 别名]
# 或者: from path_planning.GapFinder import getFrontTravel [as 别名]
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)
#.........这里部分代码省略.........