def process_message(self, message): """ Take action on the received message. """ if isinstance(message, protocol.KillImmediately): util.log_msg(util.Logging.INFO, f"Killed robot {self.robot.getName()}") self.queued_task = Tasks.DEAD elif isinstance(message, protocol.GiveRobotTarget): self.requested_target = message.target # Wake the robot if its not doing anything if self.current_task == Tasks.NONE: self.queued_task = Tasks.FOLLOWING_CONTROLLER elif isinstance(message, protocol.AskRobotSearch): self.requested_target = message.target # Ensure robot isn't already searching if self.current_task == Tasks.NONE or self.current_task == Tasks.FOLLOWING_CONTROLLER: self.queued_task = Tasks.FACE_TARGET_SEARCH elif isinstance(message, protocol.AskRobotDeposit): # Robot should only attempt to deposit block if all other actions are finished if self.current_task == Tasks.NONE or self.current_task == Tasks.FOLLOWING_CONTROLLER: self.queued_task = Tasks.DEPOSITING_BLOCK else: raise NotImplementedError(f"Could not process message {message}")
def predict_block_locations(self, cluster_candidates, occupancy_map): # Smooth clusters into single block clusters = get_cluster_average(cluster_candidates, occupancy_map) # Some block colors are known, mark these in the clusters object for block in self.confirmed_blocks: block_location, block_color = block # Check if block is in radius of any clusters color_consumed = False # NOTE: this is a back hackfix ClosestCluster = namedtuple("ClosestBlock", ["distance", "cluster"]) closest_cluster = ClosestCluster(np.inf, None) for cluster in clusters: cluster_distance = util.get_distance(cluster.coord, block_location) if cluster_distance < mapping.CLUSTER_PROXIMITY_THRESHOLD: if (cluster.color is not None): util.log_msg(util.Logging.WARNING, "Multiple blocks identified in the same cluster") color_consumed = True cluster.assign_known_color(block) if cluster_distance < closest_cluster.distance: closest_cluster = ClosestCluster( cluster_distance, cluster ) if not color_consumed: # Hackfix, no cluster identified properly, assign to closest one if closest_cluster.distance < mapping.CLUSTER_FUDGE_DISTANCE: closest_cluster.cluster.assign_known_color(block) continue new_cluster = ClusterLocation( block_location, 20, 10, mapping.BLOCK_OBSTACLE_WIDTH ) new_cluster.assign_known_color(block) clusters.append(new_cluster) # Ensure no clusters are inside the spawn region filtered_clusters = [] for cluster in clusters: # Each robot spawn position is defined here for robot_name in util.ROBOT_SPAWN: spawn_pos = util.ROBOT_SPAWN[robot_name] if util.get_distance(spawn_pos, cluster.coord) < mapping.SPAWN_REGION_RADIUS: break # Block is inside spawn region, don't recollect it else: # Block was not inside any spawn radius, lets save it filtered_clusters.append(cluster) return filtered_clusters
def get_distance(self): """ Returns distance in m by performing linear interpolation of measured IR sensor value. If object is closer than 0.06m or further than 0.8m, returns float('inf') instead, as value is out of range of sensor capability. """ x = self.sensor.getValue() y = float('inf') if x is None: util.log_msg(util.Logging.WARNING, "Attempted IR sensor reading when no data available") return y if x > 654: # maximum received value from sensor, anything more is too close return y for val_pair in self.pairs: (r1, r2), (d1, d2) = val_pair if r2 <= x < r1: y = ((x - r1) / (r2 - r1) * (d2 - d1)) + d1 return y + 0.006 # changed due to a change in noise
def __initialize_queued_task(self, about_to_end, about_to_start): """ Runs when the next robot task is about to start. Note: 'self.current_task' is about_to_end 'self.queued_task' is about_to_start DO NOT MODIFY THESE VARIABLES HERE """ util.log_msg(util.Logging.INFO, f"{self.robot.getName()} starting new task: {about_to_start}") if about_to_start == Tasks.INITIAL_SCAN: self.drive_controller.halt() if about_to_start == Tasks.FULL_SCAN: self.scanning_controller.clockwise = True if about_to_start == Tasks.DEAD: self.drive_controller.halt() self.positioning_system.kill_turret() if about_to_start == Tasks.SEARCHING_BLOCK: util.log_msg(util.Logging.INFO, "Started searching for block") self.block_searching_algorithm = BlockSearch( self.IR_sensor, self.positioning_system, self.drive_controller, self.pincer_controller ) if about_to_start == Tasks.REVERSING: self.reversing_algorithm = Reversing( self.drive_controller, 200 # Reverse for 10 ticks to ensure robot doesn't collide with hidden blocks ) if about_to_start == Tasks.REVERSING_LONG: self.reversing_algorithm = Reversing( self.drive_controller, 300 # Reverse for 10 ticks to ensure robot doesn't collide with hidden blocks ) if about_to_start == Tasks.DEPOSITING_BLOCK: self.depositing_algorithm = BlockDeposit( self.robot.getName(), self.positioning_system, self.drive_controller, self.pincer_controller )
def invalid_region(self, block_locations, position, invalidation_size): """ Invalidates the region surrounding a single block. This should be used after the robot changes the environment for an reason. """ # Remove each block in a known position for cluster in block_locations: invalidation_radius = max(invalidation_size, cluster.radius) if util.get_distance(cluster.coord, position) < invalidation_radius: if(cluster.known_block is not None): if cluster.known_block not in self.confirmed_blocks: util.log_msg(util.Logging.WARNING, "unexpected block not in confirmed_blocks") continue util.log_msg(util.Logging.WARNING, "Block in invalidation cluster, removing") self.confirmed_blocks.remove(cluster.known_block) # Also delete overlapping blocks if they are not in a cluster for index in range(len(self.confirmed_blocks) - 1, 0, -1): block_pos, _ = self.confirmed_blocks[index] if util.get_distance(block_pos, position) < invalidation_size: # Purge this block util.log_msg(util.Logging.INFO, "Block in invalidation region, removing") self.confirmed_blocks.pop(index)
def __call__(self): IR_dist = self.IR_sensor.get_distance() wall_dist = WALL_REJECTION_FRAC * util.get_static_distance( self.positioning_system.get_2D_position(), self.positioning_system.get_world_bearing(), [], []) # The old pincer code didn't work if self.closing_pincer: if self.pincer_controller.close_pincer(): return self.FOUND_BLOCK return self.IN_PROGRESS if not self.block_found: # Run the sweeping search using IR sensor to find direction of block if IR_dist < min(MAX_BLOCK_DIST, wall_dist): self.min_IR_angle = self.positioning_system.get_world_bearing() self.block_found = True self.sweeping_back = False if self.drive_controller.rotate_absolute_angle( self.positioning_system, self.target_angle): if not self.sweeping_back: self.sweeping_back = True self.target_angle = (self.target_angle - SEARCH_ANGLE + 2 * math.pi) % (2 * math.pi) else: if IR_dist < min(MAX_BLOCK_DIST, wall_dist): util.log_msg(util.Logging.INFO, "Block Detected") self.block_found = True self.sweeping_back = False else: util.log_msg(util.Logging.INFO, "No Block Found") return self.FAILED else: # If a block has been found, rotate towards it, then drive until it is within # the gripper, then close the gripper if not self.sweeping_back: # Make sure the robot is facing the right way before driving forward if self.drive_controller.rotate_absolute_angle( self.positioning_system, self.min_IR_angle): self.sweeping_back = True else: if IR_dist > TIMER_SLOW_DISTANCE: self.FUBAR_timer -= 2 else: # Slows the timer to prevent unnecessary fails self.FUBAR_timer -= 1 if self.FUBAR_timer <= 0: util.log_msg(util.Logging.WARNING, "Took too long to reach block") return self.TIMEOUT if IR_dist > BLOCK_IN_GRABBER_DIST: self.drive_controller.drive_forward() else: self.closing_pincer = True return False
def tick(self): # A dead robot can never be revived, don't even try to do anything if self.current_task == Tasks.DEAD: return # Get the lastest information from controller self.process_controller_instructions() # Update the controller self.send_new_scan() # Ensure IR readings are up-to-date self.IR_sensor.get_distance() # Execute the current task if self.current_task == Tasks.FOLLOWING_CONTROLLER: # Ensure robot knows where to go. If not, wait for instructions if self.requested_target is None: self.queued_task = Tasks.NONE # Wait for further instructions self.drive_controller.navigate_toward_point(self.positioning_system, self.requested_target) # Also scan while driving self.scanning_controller.driving_scan(self.positioning_system) # if not self.pincer_controller.is_closed and self.IR_sensor.get_distance() < BlockSearch.MAX_BLOCK_DIST: # self.queued_task = Tasks.SEARCHING_BLOCK elif self.current_task == Tasks.INITIAL_SCAN: if self.scanning_controller.stationary_scan(self.positioning_system): # Only blank the robot controller if nothing is planned self.queued_task = Tasks.NONE elif self.current_task == Tasks.FULL_SCAN: if self.scanning_controller.active_scan(self.positioning_system): # Only blank the robot controller if nothing is planned self.queued_task = Tasks.NONE elif self.current_task == Tasks.NONE: # Scan to update map self.scanning_controller.stationary_scan(self.positioning_system) elif self.current_task == Tasks.SEARCHING_BLOCK: # Scan to (attempt) avoid false positives self.scanning_controller.driving_scan(self.positioning_system) # Algorithm returns code upon completion result = self.block_searching_algorithm() if result == BlockSearch.FOUND_BLOCK: self.queued_task = Tasks.SCANNING_COLOR elif result != BlockSearch.IN_PROGRESS: # Report this search has failed self.radio.send_message(protocol.IRReportFailed( self.robot.getName(), self.positioning_system.block_detection_position() )) if result == BlockSearch.TIMEOUT: # Robot has moved forwards, avoid unintended collisions self.queued_task = Tasks.REVERSING elif result == BlockSearch.FAILED: # Robot is probably in a safe position to scan self.queued_task = Tasks.FULL_SCAN else: raise NotImplementedError() elif self.current_task == Tasks.SCANNING_COLOR: self.drive_controller.halt() block_color = self.color_sensor.scan_color() color_name = util.get_robot_color_string(block_color) # Log this color to the console for point util.log_msg(util.Logging.INFO, f"Robot '{self.robot.getName()}' Found {color_name} block") # Alert the controller of the true block color self.radio.send_message(protocol.ReportBlockColor( self.robot.getName(), self.positioning_system.get_2D_position(), block_color )) if block_color == self.robot.getName(): # Block is correct color and pincer is already closed self.queued_task = Tasks.REVERSING else: # Block color is wrong, open pincer to reject block self.queued_task = Tasks.REJECT_BLOCK elif self.current_task == Tasks.REJECT_BLOCK: # Algorithm returns True upon completion if self.pincer_controller.open_pincer(): self.queued_task = Tasks.REVERSING elif self.current_task == Tasks.FACE_TARGET_SEARCH: # Algorithm returns True upon completion is_finished = self.drive_controller.turn_toward_point( self.positioning_system, self.requested_target ) if is_finished: self.queued_task = Tasks.SEARCHING_BLOCK elif self.current_task == Tasks.REVERSING or self.current_task == Tasks.REVERSING_LONG: # Algorithm returns True upon completion if self.reversing_algorithm(): self.queued_task = Tasks.FULL_SCAN elif self.current_task == Tasks.DEPOSITING_BLOCK: # Algorithm returns True upon completion if self.depositing_algorithm(): util.log_msg(util.Logging.INFO, "Block deposited") self.queued_task = Tasks.REVERSING_LONG else: util.log_msg(util.Logging.ERROR, f"Unimplemented task type: {self.current_task}") raise NotImplementedError() # Tick finished, switch tasks if necessary self.switch_to_queued_task()