def __init__(self, arguments, geometry, import_manager): self._settings = arguments.get_settings("planning_assignment") self._geometry = geometry self._import_manager = import_manager self._home_locations = self._settings.get("vehicle_home_locations") self._home_directions = self._settings.get("vehicle_home_directions") self._turning_cost = self._settings.get("turning_cost") self._number_of_vehicles = len(self._home_locations) self._vehicle_pairs = list( itertools.permutations(range(1, self._number_of_vehicles + 1), r=2)) self._collision_avoider = Collision_Avoidance(arguments, geometry) self._export = True self._assignment = None self._positions = None self._current_positions = None self._current_directions = None self._number_of_waits = None
def __init__(self, arguments, geometry, import_manager): self._settings = arguments.get_settings("planning_assignment") self._geometry = geometry self._import_manager = import_manager self._home_locations = self._settings.get("vehicle_home_locations") self._home_directions = self._settings.get("vehicle_home_directions") self._turning_cost = self._settings.get("turning_cost") self._number_of_vehicles = len(self._home_locations) self._vehicle_pairs = list( itertools.permutations(range(1, self._number_of_vehicles + 1), r=2) ) self._collision_avoider = Collision_Avoidance(arguments, geometry) self._export = True self._assignment = None self._positions = None self._current_positions = None self._current_directions = None self._number_of_waits = None
class Greedy_Assignment(object): """ Synchronized traveling salesmen's tasks assignment problem algorithm. We use a greedy strategy to appoint a number of vehicles to visit a number of waypoint pairs. It does not matter which vehicle visits which waypoint, but each waypoint within a pair must be visited by different vehicles at the same moment in time (or they have to wait for the other vehicle to arrive). We want to minimize the time loss due to this synchronization as well as the time needed to visit the waypoint pairs, which can be done in any order. The time is calculated using the Manhattan distance in this class. Other implementations could add in the time needed for turning and synchronization or use a different distance function. """ def __init__(self, arguments, geometry, import_manager): self._settings = arguments.get_settings("planning_assignment") self._geometry = geometry self._import_manager = import_manager self._home_locations = self._settings.get("vehicle_home_locations") self._home_directions = self._settings.get("vehicle_home_directions") self._turning_cost = self._settings.get("turning_cost") self._number_of_vehicles = len(self._home_locations) self._vehicle_pairs = list( itertools.permutations(range(1, self._number_of_vehicles + 1), r=2) ) self._collision_avoider = Collision_Avoidance(arguments, geometry) self._export = True self._assignment = None self._positions = None self._current_positions = None self._current_directions = None self._number_of_waits = None def _add_waypoint(self, vehicle, position, waypoint_type, wait_id=0, wait_count=1, wait_waypoint=-1, home_direction=0): """ Create a waypoint for the vehicle with sensor ID `vehicle`, based on the given coordinate tuple `position` and additional type-specific data, and add it to the assignment. The assignment can be altered to add different waypoint types using the `waypoint_type` keyword argument, which expects a `Waypoint_Type` enum value. For wait types, `wait_id`, `wait_count` and `wait_waypoint` integers are allowed, and for the home waypoint type, `home_direction` is allowed. """ if self._export: # The wait ID and home direction share a "union" in a the exported # waypoints lists. if waypoint_type == Waypoint_Type.HOME: wait_id = home_direction waypoint = list(position) + [ 0, waypoint_type, wait_id, wait_count, wait_waypoint ] else: location = self._geometry.make_location(*position) waypoint = Waypoint.create(self._import_manager, waypoint_type, vehicle, self._geometry, location, wait_id=wait_id, wait_count=wait_count, wait_waypoint=wait_waypoint, home_direction=home_direction) self._assignment[vehicle].append(waypoint) def _calculate_vehicle_distances(self): V = np.full((self._number_of_vehicles, 2, len(self._positions)), np.nan) for vehicle in range(self._number_of_vehicles): for i in range(2): # The traveling distance: Manhattan grid distance D = self._positions[:, i, :] - self._current_positions[vehicle] # Indications of whether we travel upward/downward/neutral and # leftward/rightward/neutral. S = np.sign(D) # Determine how many turns the vehicle needs to make to get to # the positions, so whether we need to turn left/right and then # optionally turn in the same direction again, or turn around # completely. cur = self._current_directions[vehicle] right = abs(S[:, (cur.axis + 1) % 2]) straight = (2 - right) * (S[:, cur.axis] == -cur.sign) T = straight + right V[vehicle, i, :] = abs(D).sum(axis=1) + self._turning_cost * T return V def _get_new_direction(self, vehicle, new_position): up = new_position[0] - self._current_positions[vehicle-1][0] right = new_position[1] - self._current_positions[vehicle-1][1] cur = [self._current_directions[vehicle-1] == d for d in range(4)] if up == 0 or (up > 0 and cur[Line_Follower_Direction.UP]) or \ (up < 0 and cur[Line_Follower_Direction.DOWN]): if right > 0: return Line_Follower_Direction.RIGHT if right < 0: return Line_Follower_Direction.LEFT return self._current_directions[vehicle-1] if right == 0 or (right > 0 and cur[Line_Follower_Direction.RIGHT]) or \ (right < 0 and cur[Line_Follower_Direction.LEFT]): if up > 0: return Line_Follower_Direction.UP return Line_Follower_Direction.DOWN # up != 0, right != 0, and either up or right is in the wrong direction # compared to the current direction. thus the next direction is the # inverse of the current direction. return self._current_directions[vehicle-1].invert() def _get_closest_pair(self): V = self._calculate_vehicle_distances() distances = np.array([ [ V[vehicle-1, i, :] for i, vehicle in enumerate(vehicle_pair) ] for vehicle_pair in self._vehicle_pairs ]) # Given that both vehicles operate at the same time and synchronize at # the next waypoint, the time needed depends on the longest distance # that either vehicle needs to move. Thus take the maximum. totals = distances.max(axis=1) # Determine the indices of the combination of vehicle and sensor pair # that minimize the distances. indices = np.unravel_index(np.argmin(totals), totals.shape) return indices, totals[indices] def _assign_pair(self, vehicle_pair, closest_pair, distance): # Determine the synchronization (waits) between the two vehicles in the # chosen vehicle pair. There are always two permutations here. syncs = list(itertools.permutations(self._vehicle_pairs[vehicle_pair])) waits = [ self._number_of_waits[other_vehicle-1] for _, other_vehicle in syncs ] for i, sync_pair in enumerate(syncs): vehicle, other_vehicle = sync_pair wait_waypoint = waits[i] # The coordinates of the next position for the given vehicle, and # the direction of the vehicle after moving to this position. new_position = list(self._positions[closest_pair, i, :]) new_direction = self._get_new_direction(vehicle, new_position) # Check whether the new position is problematic according to the # collision avoidance algorithm, i.e., it crosses other current # routes. yaw_direction = self._current_directions[vehicle-1].yaw yaw_turning_cost = self._turning_cost / (math.pi/2) route, direction = \ self._collision_avoider.update(self._home_locations, new_position, vehicle, other_vehicle, distance, direction=yaw_direction, turning_cost=yaw_turning_cost) if self._collision_avoider.distance > distance: distance = self._collision_avoider.distance if distance == np.inf: return distance for point in route: self._add_waypoint(vehicle, point, Waypoint_Type.PASS) if direction is not None: new_direction = Line_Follower_Direction.from_yaw(direction) # Track the new position and direction self._current_positions[vehicle-1] = new_position self._current_directions[vehicle-1] = new_direction self._number_of_waits[vehicle-1] += 1 # Assign the sensor waypoint, including the position and the other # vehicle's wait ID. self._add_waypoint(vehicle, new_position, Waypoint_Type.WAIT, wait_id=other_vehicle, wait_count=1, wait_waypoint=wait_waypoint) return distance def assign(self, positions_pairs, export=True): """ Assign the vehicles with current positions `home_positions` an ordering of the position pairs to be visited. `positions_pairs` must be a numpy array of size (Nx2x2), where N is the number of pairs, and the other dimensions encompass the pairs and the coordinates of each position, respectively. The returned values are the assignment, which is a dictionary with vehicle indexes and an ordered list of waypoints to visit, and the total distance needed for this assignment according to the algorithm. If `export` is `True`, then the waypoints are lists that can be exported as JSON. Set `export` to `False` to receive `Waypoint` objects instead. """ self._export = export self._collision_avoider.reset() self._positions = np.array(positions_pairs, dtype=np.int) self._current_positions = list(self._home_locations) self._current_directions = [ Line_Follower_Direction(d) for d in self._home_directions ] self._number_of_waits = dict([ (i, 0) for i in range(self._number_of_vehicles) ]) self._assignment = dict([ (i, []) for i in range(1, self._number_of_vehicles + 1) ]) for vehicle, home_location in enumerate(self._current_positions): self._add_waypoint(vehicle + 1, home_location, Waypoint_Type.HOME, home_direction=self._current_directions[vehicle]) total_distance = 0 while len(self._positions) > 0: # The index of the distances matrix and the distance value itself. idx, distance = self._get_closest_pair() # The chosen vehicle pair and the chosen measurement positions pair vehicle_pair, closest_pair = idx distance = self._assign_pair(vehicle_pair, closest_pair, distance) if distance == np.inf: return {}, distance total_distance += distance self._positions = np.delete(self._positions, closest_pair, axis=0) return self._assignment, total_distance
class Greedy_Assignment(object): """ Synchronized traveling salesmen's tasks assignment problem algorithm. We use a greedy strategy to appoint a number of vehicles to visit a number of waypoint pairs. It does not matter which vehicle visits which waypoint, but each waypoint within a pair must be visited by different vehicles at the same moment in time (or they have to wait for the other vehicle to arrive). We want to minimize the time loss due to this synchronization as well as the time needed to visit the waypoint pairs, which can be done in any order. The time is calculated using the Manhattan distance in this class. Other implementations could add in the time needed for turning and synchronization or use a different distance function. """ def __init__(self, arguments, geometry, import_manager): self._settings = arguments.get_settings("planning_assignment") self._geometry = geometry self._import_manager = import_manager self._home_locations = self._settings.get("vehicle_home_locations") self._home_directions = self._settings.get("vehicle_home_directions") self._turning_cost = self._settings.get("turning_cost") self._number_of_vehicles = len(self._home_locations) self._vehicle_pairs = list( itertools.permutations(range(1, self._number_of_vehicles + 1), r=2)) self._collision_avoider = Collision_Avoidance(arguments, geometry) self._export = True self._assignment = None self._positions = None self._current_positions = None self._current_directions = None self._number_of_waits = None def _add_waypoint(self, vehicle, position, waypoint_type, wait_id=0, wait_count=1, wait_waypoint=-1): """ Create a waypoint for the vehicle with sensor ID `vehicle`, based on the given coordinate tuple `position` and additional type-specific data, and add it to the assignment. The assignment can be altered to add different waypoint types using the `waypoint_type` keyword argument, which expects a `Waypoint_Type` enum value. For wait types, `wait_id`, `wait_count` and `wait_waypoint` integers are accepted if they are known at this point. """ if self._export: waypoint = list(position) + [ 0, waypoint_type, wait_id, wait_count, wait_waypoint ] else: location = self._geometry.make_location(*position) waypoint = Waypoint.create(self._import_manager, waypoint_type, vehicle, self._geometry, location, wait_id=wait_id, wait_count=wait_count, wait_waypoint=wait_waypoint) self._assignment[vehicle].append(waypoint) def _calculate_vehicle_distances(self): V = np.full((self._number_of_vehicles, 2, len(self._positions)), np.nan) for vehicle in range(self._number_of_vehicles): for i in range(2): # The traveling distance: Manhattan grid distance D = self._positions[:, i, :] - self._current_positions[vehicle] # Indications of whether we travel upward/downward/neutral and # leftward/rightward/neutral. S = np.sign(D) # Determine how many turns the vehicle needs to make to get to # the positions, so whether we need to turn left/right and then # optionally turn in the same direction again, or turn around # completely. cur = self._current_directions[vehicle] right = abs(S[:, (cur.axis + 1) % 2]) straight = (2 - right) * (S[:, cur.axis] == -cur.sign) T = straight + right V[vehicle, i, :] = abs(D).sum(axis=1) + self._turning_cost * T return V def _get_new_direction(self, vehicle, new_position): up = new_position[0] - self._current_positions[vehicle - 1][0] right = new_position[1] - self._current_positions[vehicle - 1][1] cur = [self._current_directions[vehicle - 1] == d for d in range(4)] if up == 0 or (up > 0 and cur[Line_Follower_Direction.UP]) or \ (up < 0 and cur[Line_Follower_Direction.DOWN]): if right > 0: return Line_Follower_Direction.RIGHT if right < 0: return Line_Follower_Direction.LEFT return self._current_directions[vehicle - 1] if right == 0 or (right > 0 and cur[Line_Follower_Direction.RIGHT]) or \ (right < 0 and cur[Line_Follower_Direction.LEFT]): if up > 0: return Line_Follower_Direction.UP return Line_Follower_Direction.DOWN # up != 0, right != 0, and either up or right is in the wrong direction # compared to the current direction. thus the next direction is the # inverse of the current direction. return self._current_directions[vehicle - 1].invert() def _get_closest_pair(self): V = self._calculate_vehicle_distances() distances = np.array( [[V[vehicle - 1, i, :] for i, vehicle in enumerate(vehicle_pair)] for vehicle_pair in self._vehicle_pairs]) # Given that both vehicles operate at the same time and synchronize at # the next waypoint, the time needed depends on the longest distance # that either vehicle needs to move. Thus take the maximum. totals = distances.max(axis=1) # Determine the indices of the combination of vehicle and sensor pair # that minimize the distances. indices = np.unravel_index(np.argmin(totals), totals.shape) return indices, totals[indices] def _assign_pair(self, vehicle_pair, closest_pair, distance): # Determine the synchronization (waits) between the two vehicles in the # chosen vehicle pair. There are always two permutations here. syncs = list(itertools.permutations(self._vehicle_pairs[vehicle_pair])) waits = [ self._number_of_waits[other_vehicle - 1] for _, other_vehicle in syncs ] for i, sync_pair in enumerate(syncs): vehicle, other_vehicle = sync_pair wait_waypoint = waits[i] # The coordinates of the next position for the given vehicle, and # the direction of the vehicle after moving to this position. new_position = list(self._positions[closest_pair, i, :]) new_direction = self._get_new_direction(vehicle, new_position) # Check whether the new position is problematic according to the # collision avoidance algorithm, i.e., it crosses other current # routes. yaw_direction = self._current_directions[vehicle - 1].yaw yaw_turning_cost = self._turning_cost / (math.pi / 2) route, direction = \ self._collision_avoider.update(self._home_locations, new_position, vehicle, other_vehicle, distance, direction=yaw_direction, turning_cost=yaw_turning_cost) if self._collision_avoider.distance > distance: distance = self._collision_avoider.distance if distance == np.inf: return distance for point in route: self._add_waypoint(vehicle, point, Waypoint_Type.PASS) if direction is not None: new_direction = Line_Follower_Direction.from_yaw(direction) # Track the new position and direction self._current_positions[vehicle - 1] = new_position self._current_directions[vehicle - 1] = new_direction self._number_of_waits[vehicle - 1] += 1 # Assign the sensor waypoint, including the position and the other # vehicle's wait ID. self._add_waypoint(vehicle, new_position, Waypoint_Type.WAIT, wait_id=other_vehicle, wait_count=1, wait_waypoint=wait_waypoint) return distance def assign(self, positions_pairs, export=True): """ Assign the vehicles with current positions `home_positions` an ordering of the position pairs to be visited. `positions_pairs` must be a numpy array of size (Nx2x2), where N is the number of pairs, and the other dimensions encompass the pairs and the coordinates of each position, respectively. The returned values are the assignment, which is a dictionary with vehicle indexes and an ordered list of waypoints to visit, and the total distance needed for this assignment according to the algorithm. If `export` is `True`, then the waypoints are lists that can be exported as JSON. Set `export` to `False` to receive `Waypoint` objects instead. """ self._export = export self._collision_avoider.reset() self._positions = np.array(positions_pairs, dtype=np.int) self._current_positions = list(self._home_locations) self._current_directions = [ Line_Follower_Direction(d) for d in self._home_directions ] self._number_of_waits = dict([ (i, 0) for i in range(self._number_of_vehicles) ]) self._assignment = dict([ (i, []) for i in range(1, self._number_of_vehicles + 1) ]) for vehicle, home_location in enumerate(self._current_positions): self._add_waypoint(vehicle + 1, home_location, Waypoint_Type.HOME) total_distance = 0 while len(self._positions) > 0: # The index of the distances matrix and the distance value itself. idx, distance = self._get_closest_pair() # The chosen vehicle pair and the chosen measurement positions pair vehicle_pair, closest_pair = idx distance = self._assign_pair(vehicle_pair, closest_pair, distance) if distance == np.inf: return {}, distance total_distance += distance self._positions = np.delete(self._positions, closest_pair, axis=0) return self._assignment, total_distance