src/TrafficCoordinator.py
def one_swap_sort(self, bid_queue,verbose=False):
        ''' Looks for single swap improvements for switching the queue.  Iterate through a queue.
            Each agent (first request) can consider swapping with the next agent.  This will only occur if the utility
            for both agents improve (its a consentual swap)
            Used in IROS2019
        '''
        self.lane_queues = self.update_lane_queues(self.lane_queues)  # Make sure lane queue is up-to-date

        sorted_queue = self.sort_queue(bid_queue,self.bid_queue_sort_type)
        final_reservation = []        
        print("Received bids from %d agents"%len(sorted_queue),":",[r[-1].agent_id for r in sorted_queue])
        if len(sorted_queue) == 1:
            first_request = sorted_queue[0]
            requested_start_time1, fake_interval_matrix, fake_available_matrix, fake_lane_queues = self.attempt_one_reservation(first_request)
            if requested_start_time1 > 0:
                self.time_matrix_intervals, self.time_available_matrix, self.lane_queues = fake_interval_matrix, fake_available_matrix, fake_lane_queues
                final_reservation += [(first_request[-1], requested_start_time1)] # Reserve the first request and increment the request   
                for car, reservation_start_time in final_reservation:
                    car.intersection_window = (reservation_start_time, 9999999)   
                print("Reserving intersection for vehicles...",[car_reservation[0].agent_id for car_reservation in final_reservation])
            return final_reservation 
        else:
            first_request = sorted_queue[0]
            for queue_index in range(min(self.k_max_number_cars_assigned,len(sorted_queue)-1)):
                second_request = sorted_queue[queue_index+1]
                #
                u1, u2, requested_start_time1, requested_start_time2, fake_interval_matrix, fake_available_matrix, fake_lane_queues = self.attempt_two_reservations(first_request, second_request)
                if verbose: print("Original Attempted:  Agent %d then %d.  t_s1t= %.02f,  t_s1= %.02f u1= %0.02f u2=%0.02f"%(first_request[-2],second_request[-2],  requested_start_time1, requested_start_time2, u1, u2))
                if requested_start_time1 < 0 or requested_start_time2 < 0:
                    print("Did not return solution for %dth car in queue"%(queue_index+1))
                    break
                
                if first_request[-1].current_lane == second_request[-1].current_lane or (self.nocomm_no_swap and (first_request[-1].notcommunicating or second_request[-1].notcommunicating)) or (self.nocollab_no_swap and (first_request[-1].notcollaborating or second_request[-1].notcollaborating)) :
                    '''Second request in same lane as first request.  Swap is not possible'''
                    self.time_matrix_intervals, self.time_available_matrix, self.lane_queues = fake_interval_matrix, fake_available_matrix, fake_lane_queues
                    final_reservation += [(first_request[-1], requested_start_time1)] # Reserve the first request and increment the request           
                    first_request = second_request               
                    continue
                    
                u1_swap, u2_swap, requested_start_time1_swap, requested_start_time2_swap, fake_interval_matrix_swap, fake_available_matrix_swap, fake_lane_queues_swap = self.attempt_two_reservations(second_request, first_request)
                if requested_start_time1_swap < 0 or requested_start_time2_swap < 0:
                    print("Did not return solution for %dth car in queue in swap"%(queue_index+1))
                    break

                if verbose:
                    print("\n First Agent %d. t_start_original = %.02f t_start_swap = %.02f.   Original U= %.02f Swap U= %.02f"%(first_request[-2], requested_start_time1, requested_start_time2_swap, u1, u2_swap))
                    print("Second Agent %d. t_start_original = %.02f t_start_swap = %.02f.   Original U= %.02f Swap U= %.02f"%(second_request[-2], requested_start_time2, requested_start_time1_swap, u2, u1_swap))
                
                u_epsilon = 0.001

                
                if second_request[-1].notcommunicating:
                    p_swap_back = first_request[-1].prob_swap_back_notcomm # Non-communicating swap back 100%
                else:
                    p_swap_back = first_request[-1].prob_swap_back_comm # communicating

                if (u1_swap - u2 > u_epsilon) and (u2_swap - u1 > u_epsilon) and self.swaps_allowed and np.random.uniform() < p_swap_back:
                    print("SWAP! Agent %d H%r before Agent %d H%r"%(second_request[-1].agent_id, second_request[-1].notcommunicating, first_request[-1].agent_id, first_request[-1].notcommunicating))
                    self.time_matrix_intervals, self.time_available_matrix, self.lane_queues = fake_interval_matrix_swap, fake_available_matrix_swap, fake_lane_queues_swap
                    final_reservation += [(second_request[-1], requested_start_time1_swap)] # Reserve the second request due to swap   
                    n_notcommunicatings = second_request[-1].notcommunicating + first_request[-1].notcommunicating              
                    self.list_of_swapped_reservations += [(u1, u2, u1_swap, u2_swap, first_request[-1].agent_id, second_request[-1].agent_id, n_notcommunicatings)]                
                else:
                    self.time_matrix_intervals, self.time_available_matrix, self.lane_queues = fake_interval_matrix, fake_available_matrix, fake_lane_queues
                    final_reservation += [(first_request[-1], requested_start_time1)] # Reserve the first request and increment the request           
                    first_request = second_request                  
            print("Reserving intersection for vehicles...",[car_reservation[0].agent_id for car_reservation in final_reservation])
            for car, reservation_start_time in final_reservation:
                car.intersection_window = (reservation_start_time, 9999999)
            return final_reservation
 Looks for single swap improvements for switching the queue.  Iterate through a queue.
            Each agent (first request) can consider swapping with the next agent.  This will only occur if the utility
            for both agents improve (its a consentual swap)
            Used in IROS2019
        
Looks for single swap improvements for switching the queue.  Iterate through a queue.
Each agent (first request) can consider swapping with the next agent.  This will only occur if the utility
for both agents improve (its a consentual swap)
Used in IROS2019
Looks for single swap improvements for switching the queue . Iterate through a queue . Each agent ( first request ) can consider swapping with the next agent . This will only occur if the utility for both agents improve ( its a consentual swap ) Used in IROS2019
def one_swap_sort(self, bid_queue,verbose=False):
        self.lane_queues = self.update_lane_queues(self.lane_queues)  
        sorted_queue = self.sort_queue(bid_queue,self.bid_queue_sort_type)
        final_reservation = []        
        print("Received bids from %d agents"%len(sorted_queue),":",[r[-1].agent_id for r in sorted_queue])
        if len(sorted_queue) == 1:
            first_request = sorted_queue[0]
            requested_start_time1, fake_interval_matrix, fake_available_matrix, fake_lane_queues = self.attempt_one_reservation(first_request)
            if requested_start_time1 > 0:
                self.time_matrix_intervals, self.time_available_matrix, self.lane_queues = fake_interval_matrix, fake_available_matrix, fake_lane_queues
                final_reservation += [(first_request[-1], requested_start_time1)] 
                for car, reservation_start_time in final_reservation:
                    car.intersection_window = (reservation_start_time, 9999999)   
                print("Reserving intersection for vehicles...",[car_reservation[0].agent_id for car_reservation in final_reservation])
            return final_reservation 
        else:
            first_request = sorted_queue[0]
            for queue_index in range(min(self.k_max_number_cars_assigned,len(sorted_queue)-1)):
                second_request = sorted_queue[queue_index+1]
                u1, u2, requested_start_time1, requested_start_time2, fake_interval_matrix, fake_available_matrix, fake_lane_queues = self.attempt_two_reservations(first_request, second_request)
                if verbose: print("Original Attempted:  Agent %d then %d.  t_s1t= %.02f,  t_s1= %.02f u1= %0.02f u2=%0.02f"%(first_request[-2],second_request[-2],  requested_start_time1, requested_start_time2, u1, u2))
                if requested_start_time1 < 0 or requested_start_time2 < 0:
                    print("Did not return solution for %dth car in queue"%(queue_index+1))
                    break
                if first_request[-1].current_lane == second_request[-1].current_lane or (self.nocomm_no_swap and (first_request[-1].notcommunicating or second_request[-1].notcommunicating)) or (self.nocollab_no_swap and (first_request[-1].notcollaborating or second_request[-1].notcollaborating)) :
                    self.time_matrix_intervals, self.time_available_matrix, self.lane_queues = fake_interval_matrix, fake_available_matrix, fake_lane_queues
                    final_reservation += [(first_request[-1], requested_start_time1)]         
                    first_request = second_request               
                    continue
                u1_swap, u2_swap, requested_start_time1_swap, requested_start_time2_swap, fake_interval_matrix_swap, fake_available_matrix_swap, fake_lane_queues_swap = self.attempt_two_reservations(second_request, first_request)
                if requested_start_time1_swap < 0 or requested_start_time2_swap < 0:
                    print("Did not return solution for %dth car in queue in swap"%(queue_index+1))
                    break
                if verbose:
                    print("\n First Agent %d. t_start_original = %.02f t_start_swap = %.02f.   Original U= %.02f Swap U= %.02f"%(first_request[-2], requested_start_time1, requested_start_time2_swap, u1, u2_swap))
                    print("Second Agent %d. t_start_original = %.02f t_start_swap = %.02f.   Original U= %.02f Swap U= %.02f"%(second_request[-2], requested_start_time2, requested_start_time1_swap, u2, u1_swap))
                u_epsilon = 0.001
                if second_request[-1].notcommunicating:
                    p_swap_back = first_request[-1].prob_swap_back_notcomm  Non-communicating swap back 100%
                else:
                    p_swap_back = first_request[-1].prob_swap_back_comm  communicating
                if (u1_swap - u2 > u_epsilon) and (u2_swap - u1 > u_epsilon) and self.swaps_allowed and np.random.uniform() < p_swap_back:
                    print("SWAP! Agent %d H%r before Agent %d H%r"%(second_request[-1].agent_id, second_request[-1].notcommunicating, first_request[-1].agent_id, first_request[-1].notcommunicating))
                    self.time_matrix_intervals, self.time_available_matrix, self.lane_queues = fake_interval_matrix_swap, fake_available_matrix_swap, fake_lane_queues_swap
                    final_reservation += [(second_request[-1], requested_start_time1_swap)]  Reserve the second request due to swap   
                    n_notcommunicatings = second_request[-1].notcommunicating + first_request[-1].notcommunicating              
                    self.list_of_swapped_reservations += [(u1, u2, u1_swap, u2_swap, first_request[-1].agent_id, second_request[-1].agent_id, n_notcommunicatings)]                
                else:
                    self.time_matrix_intervals, self.time_available_matrix, self.lane_queues = fake_interval_matrix, fake_available_matrix, fake_lane_queues
                    final_reservation += [(first_request[-1], requested_start_time1)]         
                    first_request = second_request                  
            print("Reserving intersection for vehicles...",[car_reservation[0].agent_id for car_reservation in final_reservation])
            for car, reservation_start_time in final_reservation:
                car.intersection_window = (reservation_start_time, 9999999)
            return final_reservation
def one_swap_sort ( self , bid_queue , verbose = False ) : self . lane_queues = self . update_lane_queues ( self . lane_queues ) sorted_queue = self . sort_queue ( bid_queue , self . bid_queue_sort_type ) final_reservation = [ ] print ( "Received bids from %d agents" % len ( sorted_queue ) , ":" , [ r [ - 1 ] . agent_id for r in sorted_queue ] ) if len ( sorted_queue ) == 1 : first_request = sorted_queue [ 0 ] requested_start_time1 , fake_interval_matrix , fake_available_matrix , fake_lane_queues = self . attempt_one_reservation ( first_request ) if requested_start_time1 > 0 : self . time_matrix_intervals , self . time_available_matrix , self . lane_queues = fake_interval_matrix , fake_available_matrix , fake_lane_queues final_reservation += [ ( first_request [ - 1 ] , requested_start_time1 ) ] for car , reservation_start_time in final_reservation : car . intersection_window = ( reservation_start_time , 9999999 ) print ( "Reserving intersection for vehicles..." , [ car_reservation [ 0 ] . agent_id for car_reservation in final_reservation ] ) return final_reservation else : first_request = sorted_queue [ 0 ] for queue_index in range ( min ( self . k_max_number_cars_assigned , len ( sorted_queue ) - 1 ) ) : second_request = sorted_queue [ queue_index + 1 ] u1 , u2 , requested_start_time1 , requested_start_time2 , fake_interval_matrix , fake_available_matrix , fake_lane_queues = self . attempt_two_reservations ( first_request , second_request ) if verbose : print ( "Original Attempted:  Agent %d then %d.  t_s1t= %.02f,  t_s1= %.02f u1= %0.02f u2=%0.02f" % ( first_request [ - 2 ] , second_request [ - 2 ] , requested_start_time1 , requested_start_time2 , u1 , u2 ) ) if requested_start_time1 < 0 or requested_start_time2 < 0 : print ( "Did not return solution for %dth car in queue" % ( queue_index + 1 ) ) break if first_request [ - 1 ] . current_lane == second_request [ - 1 ] . current_lane or ( self . nocomm_no_swap and ( first_request [ - 1 ] . notcommunicating or second_request [ - 1 ] . notcommunicating ) ) or ( self . nocollab_no_swap and ( first_request [ - 1 ] . notcollaborating or second_request [ - 1 ] . notcollaborating ) ) : '''Second request in same lane as first request.  Swap is not possible''' self . time_matrix_intervals , self . time_available_matrix , self . lane_queues = fake_interval_matrix , fake_available_matrix , fake_lane_queues final_reservation += [ ( first_request [ - 1 ] , requested_start_time1 ) ] first_request = second_request continue u1_swap , u2_swap , requested_start_time1_swap , requested_start_time2_swap , fake_interval_matrix_swap , fake_available_matrix_swap , fake_lane_queues_swap = self . attempt_two_reservations ( second_request , first_request ) if requested_start_time1_swap < 0 or requested_start_time2_swap < 0 : print ( "Did not return solution for %dth car in queue in swap" % ( queue_index + 1 ) ) break if verbose : print ( "\n First Agent %d. t_start_original = %.02f t_start_swap = %.02f.   Original U= %.02f Swap U= %.02f" % ( first_request [ - 2 ] , requested_start_time1 , requested_start_time2_swap , u1 , u2_swap ) ) print ( "Second Agent %d. t_start_original = %.02f t_start_swap = %.02f.   Original U= %.02f Swap U= %.02f" % ( second_request [ - 2 ] , requested_start_time2 , requested_start_time1_swap , u2 , u1_swap ) ) u_epsilon = 0.001 if second_request [ - 1 ] . notcommunicating : p_swap_back = first_request [ - 1 ] . prob_swap_back_notcomm else : p_swap_back = first_request [ - 1 ] . prob_swap_back_comm if ( u1_swap - u2 > u_epsilon ) and ( u2_swap - u1 > u_epsilon ) and self . swaps_allowed and np . random . uniform ( ) < p_swap_back : print ( "SWAP! Agent %d H%r before Agent %d H%r" % ( second_request [ - 1 ] . agent_id , second_request [ - 1 ] . notcommunicating , first_request [ - 1 ] . agent_id , first_request [ - 1 ] . notcommunicating ) ) self . time_matrix_intervals , self . time_available_matrix , self . lane_queues = fake_interval_matrix_swap , fake_available_matrix_swap , fake_lane_queues_swap final_reservation += [ ( second_request [ - 1 ] , requested_start_time1_swap ) ] n_notcommunicatings = second_request [ - 1 ] . notcommunicating + first_request [ - 1 ] . notcommunicating self . list_of_swapped_reservations += [ ( u1 , u2 , u1_swap , u2_swap , first_request [ - 1 ] . agent_id , second_request [ - 1 ] . agent_id , n_notcommunicatings ) ] else : self . time_matrix_intervals , self . time_available_matrix , self . lane_queues = fake_interval_matrix , fake_available_matrix , fake_lane_queues final_reservation += [ ( first_request [ - 1 ] , requested_start_time1 ) ] first_request = second_request print ( "Reserving intersection for vehicles..." , [ car_reservation [ 0 ] . agent_id for car_reservation in final_reservation ] ) for car , reservation_start_time in final_reservation : car . intersection_window = ( reservation_start_time , 9999999 ) return final_reservation
Looks for single swap improvements for switching the queue.
Looks for single swap improvements for switching the queue .
''' Looks for single swap improvements for switching the queue.  Iterate through a queue.
            Each agent (first request) can consider swapping with the next agent.  This will only occur if the utility
            for both agents improve (its a consentual swap)
            Used in IROS2019
        ''' # Make sure lane queue is up-to-date # Reserve the first request and increment the request    # '''Second request in same lane as first request.  Swap is not possible''' # Reserve the first request and increment the request            # Non-communicating swap back 100% # communicating # Reserve the second request due to swap    # Reserve the first request and increment the request           
self
bid_queue
verbose