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()